17 from scipy.misc
import factorial
22 Implementation of [Bezier curves](https://en.wikipedia.org/wiki/B%C3%A9zier_curve) 23 of orders 3, 4 and 5 based on [1]. 27 * `pnts` (*type:* `list`): List of 3D points as a vector (example: `[[0, 0, 0], [0, 1, 2]]`) 28 * `order` (*type:* `int`): Order of the Bezier curve, options are 3, 4 or 5 29 * `tangents` (*type:* `list`, *default:* `None`): Optional input of the tangent 30 vectors for each of the input points. In case only two points are provided, 31 the tangents have to be provided, too. Otherwise, the tangents will be calculated. 32 * `normals` (*type:* `list`, *default:* `None`): Optional input of the normal 33 vectors for each of the input points. In case only two points are provided, 34 the normals have to be provided, too. Otherwise, the normals will be calculated. 38 [1] Biagiotti, Luigi, and Claudio Melchiorri. Trajectory planning for 39 automatic machines and robots. Springer Science & Business Media, 2008. 41 def __init__(self, pnts, order, tangents=None, normals=None):
42 assert order
in [3, 4, 5],
'Invalid Bezier curve order' 43 assert type(pnts) == list
and len(pnts) >= 2,
'At least two points are needed to calculate the curve' 48 assert len(pnt) == 3,
'Point must have three elements' 49 self._pnts.append(np.array(pnt))
50 elif type(pnt) == np.ndarray:
51 assert pnt.size == 3,
'Point must have three elements' 52 self._pnts.append(pnt)
54 raise TypeError(
'Point in list is neither a list or an array')
56 if tangents
is not None:
57 assert type(tangents) == list
and len(tangents) == 2,
'Tangent vectors must be provided' 60 assert len(t) == 3,
'Tangent vector must have three elements' 61 elif type(t) == np.ndarray:
62 assert t.size == 3,
'Tangent vector must have three elements' 64 raise TypeError(
'Tangent vector is neither a list or an array')
70 assert len(self.
_pnts) == 2,
'Two points are needed for the curve to be computed' 75 a = 16 - np.linalg.norm(tangents[0] + tangents[1])**2
78 alpha = np.roots([a, b, c]).max()
84 assert len(self.
_pnts) == 3,
'Three points are needed for the curve to be computed' 90 radius = np.linalg.norm(self.
_pnts[0] - self.
_pnts[1])
92 tangents.append((self.
_pnts[1] - self.
_pnts[0]) / radius)
93 tangents.append((self.
_pnts[2] - self.
_pnts[1]) / radius)
96 a = 4 - (1.0 / 4) * np.linalg.norm(tangents[0] + tangents[1])**2
99 alpha = np.roots([a, b, c]).max()
105 if len(self.
_pnts) == 3:
110 radius = np.linalg.norm(self.
_pnts[0] - self.
_pnts[1])
112 tangents.append((self.
_pnts[1] - self.
_pnts[0]) / radius)
113 tangents.append((self.
_pnts[2] - self.
_pnts[1]) / radius)
116 a = 256 - 49 * np.linalg.norm(tangents[0] + tangents[1])**2
119 alpha = np.roots([a, b, c]).max()
126 elif len(self.
_pnts) == 2:
127 assert tangents
is not None and normals
is not None 128 assert isinstance(tangents, list)
and len(tangents) == 2,
'Tangent vectors must be provided' 129 assert isinstance(normals, list)
and len(normals) == 2,
'Normal vectors must be provided' 133 a = beta_hat**2 * np.linalg.norm(normals[1] - normals[0])**2
134 b = -28 * beta_hat * np.dot((tangents[0] + tangents[1]), normals[1] - normals[0])
135 c = 196 * np.linalg.norm(tangents[0] + tangents[1])**2 + 120 * beta_hat * np.dot(self.
_pnts[1] - self.
_pnts[0], normals[1] - normals[0]) - 1024
136 d = -1680 * np.dot(self.
_pnts[1] - self.
_pnts[0], tangents[0] + tangents[1])
137 e = 3600 * np.linalg.norm(self.
_pnts[1] - self.
_pnts[0])**2
139 alpha_k = np.real(np.roots([a, b, c, d, e])).max()
152 """Compute the distance between two 3D points. 156 * `p1` (*type:* list of `float` or `numpy.array`): Point 1 157 * `p2` (*type:* list of `float` or `numpy.array`): Point 2 161 Distance between points as a `float` 166 assert p1.size == 3
and p2.size == 3, \
167 'Both input points must be three elements' 168 return np.sqrt(np.sum((p2 - p1)**2))
172 """Generate cubic Bezier curve segments from a list of points. 176 * `pnts` (*type:* list of `float` or of `numpy.array`): List of points 180 List of `BezierCurve` segments 182 assert isinstance(pnts, list),
'List of points is invalid' 183 tangents = [np.zeros(3)
for _
in range(len(pnts))]
185 lengths = [BezierCurve.distance(pnts[i + 1], pnts[i])
for i
in range(len(pnts) - 1)]
186 lengths = [0] + lengths
189 u = [l / np.sum(lengths)
for l
in np.cumsum(lengths)]
190 delta_u =
lambda k: u[k] - u[k - 1]
191 delta_q =
lambda k: pnts[k] - pnts[k - 1]
192 lamb_k =
lambda k: delta_q(k) / delta_u(k)
193 alpha_k =
lambda k: delta_u(k) / (delta_u(k) + delta_u(k + 1))
195 for i
in range(1, len(u) - 1):
196 tangents[i] = (1 - alpha_k(i)) * lamb_k(i) + alpha_k(i) * lamb_k(i + 1)
198 tangents[0] = 2 * lamb_k(i) - tangents[1]
200 tangents[-1] = 2 * lamb_k(len(u) - 1) - tangents[-2]
203 for i
in range(len(tangents)):
204 tangents[i] = tangents[i] / np.linalg.norm(tangents[i])
208 for i
in range(len(tangents) - 1):
209 segments.append(
BezierCurve([pnts[i], pnts[i + 1]], 3, [tangents[i], tangents[i + 1]]))
211 return segments, tangents
215 """Generate quintic Bezier curve segments from a list of points. 219 * `pnts` (*type:* list of `float` or of `numpy.array`): List of points 223 List of `BezierCurve` segments 225 assert isinstance(pnts, list),
'List of points is invalid' 226 tangents = [np.zeros(3)
for _
in range(len(pnts))]
227 normals = [np.zeros(3)
for _
in range(len(pnts))]
229 lengths = [BezierCurve.distance(pnts[i + 1], pnts[i])
for i
in range(len(pnts) - 1)]
230 lengths = [0] + lengths
232 u = np.cumsum(lengths) / np.sum(lengths)
234 delta_u =
lambda k: u[k] - u[k - 1]
235 delta_q =
lambda k: pnts[k] - pnts[k - 1]
236 lamb_k =
lambda k: delta_q(k) / delta_u(k)
237 alpha_k =
lambda k: delta_u(k) / (delta_u(k) + delta_u(k + 1))
238 normal_k =
lambda k: ( ((pnts[k + 1] - pnts[k]) / (u[k + 1] - u[k])) - ((pnts[k] - pnts[k - 1]) / (u[k] - u[k - 1])) ) / (u[k + 1] - u[k - 1])
240 for i
in range(1, len(u) - 1):
241 tangents[i] = (1 - alpha_k(i)) * lamb_k(i) + alpha_k(i) * lamb_k(i + 1)
242 normals[i] = normal_k(i)
244 tangents[0] = 2 * lamb_k(i) - tangents[1]
245 normals[0] = normal_k(i)
247 tangents[-1] = 2 * lamb_k(len(u) - 1) - tangents[-2]
248 normals[-1] = normal_k(len(u) - 3)
251 for i
in range(len(tangents)):
252 tangents[i] /= np.linalg.norm(tangents[i])
253 normals[i] /= np.linalg.norm(normals[i])
257 for i
in range(len(tangents) - 1):
258 segments.append(
BezierCurve([pnts[i], pnts[i + 1]], 5,
259 [tangents[i], tangents[i + 1]],
260 [normals[i], normals[i + 1]]))
265 """Return the list of control points of the Bezier curve. 269 List of 3D points as `list` 274 """Interpolate the Bezier curve using the input parametric variable `u`. 278 * `u` (*type:* `float`): Curve parametric input in the interval `[0, 1]` 282 3D point from the Bezier curve as `numpy.array` 293 """Compute the derivative of the Bezier curve using the input parametric 298 * `u` (*type:* `float`): Curve parametric input in the interval `[0, 1]` 299 * `order` (*type:* `int`, *default:* `1`): Order of the derivative 303 `numpy.array`: 3D derivative value from the Bezier curve 315 """Get length of the Bezier curve segment. 319 `float`: Length of the curve 324 """Compute the Bernstein polynomial 327 \mathbf{B} = {n\choose i} (1 - u)^{(n - i)} u^{i} 332 * `n` (*type:* `int`): Degree of the Bezier curve 333 * `i` (*type:* `int`): Index of the control point 334 * `u` (*type:* `float`): Parametric input of the curve in interval [0, 1] 338 `float`: Bernstein polynomial result 344 """Compute binomial function $\binom{n}{i}$ 348 * `n` (*type:* `int`) 349 * `i` (*type:* `int`) 351 return factorial(n) / (factorial(i) * factorial(n - i))
def generate_cubic_curve(pnts)
def get_derivative(self, u, order=1)
def generate_quintic_curve(pnts)
def compute_polynomial(self, n, i, u)
def __init__(self, pnts, order, tangents=None, normals=None)