Polynomial¶
src.python_motion_planning.curve_generator.polynomial_curve.Polynomial
¶
Bases: Curve
Class for polynomial curve generation(Quintic).
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
step
|
float
|
Simulation or interpolation size |
required |
max_acc
|
float
|
Maximum acceleration |
required |
max_jerk
|
float
|
Maximum jerk |
required |
Examples:
Python Console Session
>>> from python_motion_planning.curve_generation import Polynomial
>>> points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
>>> generator = Polynomial(step, max_acc, max_jerk)
>>> generator.run(points)
Source code in src\python_motion_planning\curve_generator\polynomial_curve.py
Python
class Polynomial(Curve):
"""
Class for polynomial curve generation(Quintic).
Parameters:
step (float): Simulation or interpolation size
max_acc (float): Maximum acceleration
max_jerk (float): Maximum jerk
Examples:
>>> from python_motion_planning.curve_generation import Polynomial
>>> points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
>>> generator = Polynomial(step, max_acc, max_jerk)
>>> generator.run(points)
"""
def __init__(self, step: float, max_acc: float, max_jerk: float) -> None:
super().__init__(step)
self.max_acc = max_acc
self.max_jerk = max_jerk
self.dt = 0.1
self.t_min = 1
self.t_max = 30
def __str__(self) -> str:
return "Quintic Polynomial Curve"
class Poly:
"""
Polynomial interpolation solver
"""
def __init__(self, state0: tuple, state1: tuple, t: float) -> None:
x0, v0, a0 = state0
xt, vt, at = state1
A = np.array([[t ** 3, t ** 4, t ** 5],
[3 * t ** 2, 4 * t ** 3, 5 * t ** 4],
[6 * t, 12 * t ** 2, 20 * t ** 3]])
b = np.array([xt - x0 - v0 * t - a0 * t ** 2 / 2,
vt - v0 - a0 * t,
at - a0])
X = np.linalg.solve(A, b)
# Quintic polynomial coefficient
self.p0 = x0
self.p1 = v0
self.p2 = a0 / 2.0
self.p3 = X[0]
self.p4 = X[1]
self.p5 = X[2]
def x(self, t):
return self.p0 + self.p1 * t + self.p2 * t ** 2 + \
self.p3 * t ** 3 + self.p4 * t ** 4 + self.p5 * t ** 5
def dx(self, t):
return self.p1 + 2 * self.p2 * t + 3 * self.p3 * t ** 2 + \
4 * self.p4 * t ** 3 + 5 * self.p5 * t ** 4
def ddx(self, t):
return 2 * self.p2 + 6 * self.p3 * t + 12 * self.p4 * t ** 2 + 20 * self.p5 * t ** 3
def dddx(self, t):
return 6 * self.p3 + 24 * self.p4 * t + 60 * self.p5 * t ** 2
class Trajectory:
"""
Polynomial interpolation solver
"""
def __init__(self):
self.clear()
def clear(self):
self.time = []
self.x = []
self.y = []
self.yaw = []
self.v = []
self.a = []
self.jerk = []
@property
def size(self):
assert len(self.time) == len(self.x) and \
len(self.x) == len(self.y) and \
len(self.y) == len(self.yaw) and \
len(self.yaw) == len(self.v) and \
len(self.v) == len(self.a) and \
len(self.a) == len(self.jerk), \
"Unequal dimensions of each attribute, this should not happen."
return len(self.time)
def generation(self, start_pose: tuple, goal_pose: tuple):
"""
Generate the polynomial Curve.
Parameters:
start_pose (tuple): Initial pose (x, y, yaw)
goal_pose (tuple): Target pose (x, y, yaw)
Returns:
traj (Traj): The first trajectory that satisfies the acceleration and jerk constraint
"""
sx, sy, syaw, sv, sa = start_pose
gx, gy, gyaw, gv, ga = goal_pose
sv_x = sv * math.cos(syaw)
sv_y = sv * math.sin(syaw)
gv_x = gv * math.cos(gyaw)
gv_y = gv * math.sin(gyaw)
sa_x = sa * math.cos(syaw)
sa_y = sa * math.sin(syaw)
ga_x = ga * math.cos(gyaw)
ga_y = ga * math.sin(gyaw)
traj = self.Trajectory()
for T in np.arange(self.t_min, self.t_max, self.step):
x_psolver = self.Poly((sx, sv_x, sa_x), (gx, gv_x, ga_x), T)
y_psolver = self.Poly((sy, sv_y, sa_y), (gy, gv_y, ga_y), T)
for t in np.arange(0.0, T + self.dt, self.dt):
traj.time.append(t)
traj.x.append(x_psolver.x(t))
traj.y.append(y_psolver.x(t))
vx = x_psolver.dx(t)
vy = y_psolver.dx(t)
traj.v.append(math.hypot(vx, vy))
traj.yaw.append(math.atan2(vy, vx))
ax = x_psolver.ddx(t)
ay = y_psolver.ddx(t)
a = math.hypot(ax, ay)
if len(traj.v) >= 2 and traj.v[-1] - traj.v[-2] < 0.0:
a *= -1
traj.a.append(a)
jx = x_psolver.dddx(t)
jy = y_psolver.dddx(t)
j = math.hypot(jx, jy)
if len(traj.a) >= 2 and traj.a[-1] - traj.a[-2] < 0.0:
j *= -1
traj.jerk.append(j)
if max(np.abs(traj.a)) <= self.max_acc and \
max(np.abs(traj.jerk)) <= self.max_jerk:
return traj
else:
traj.clear()
return traj
Poly
¶
Polynomial interpolation solver
Source code in src\python_motion_planning\curve_generator\polynomial_curve.py
Python
class Poly:
"""
Polynomial interpolation solver
"""
def __init__(self, state0: tuple, state1: tuple, t: float) -> None:
x0, v0, a0 = state0
xt, vt, at = state1
A = np.array([[t ** 3, t ** 4, t ** 5],
[3 * t ** 2, 4 * t ** 3, 5 * t ** 4],
[6 * t, 12 * t ** 2, 20 * t ** 3]])
b = np.array([xt - x0 - v0 * t - a0 * t ** 2 / 2,
vt - v0 - a0 * t,
at - a0])
X = np.linalg.solve(A, b)
# Quintic polynomial coefficient
self.p0 = x0
self.p1 = v0
self.p2 = a0 / 2.0
self.p3 = X[0]
self.p4 = X[1]
self.p5 = X[2]
def x(self, t):
return self.p0 + self.p1 * t + self.p2 * t ** 2 + \
self.p3 * t ** 3 + self.p4 * t ** 4 + self.p5 * t ** 5
def dx(self, t):
return self.p1 + 2 * self.p2 * t + 3 * self.p3 * t ** 2 + \
4 * self.p4 * t ** 3 + 5 * self.p5 * t ** 4
def ddx(self, t):
return 2 * self.p2 + 6 * self.p3 * t + 12 * self.p4 * t ** 2 + 20 * self.p5 * t ** 3
def dddx(self, t):
return 6 * self.p3 + 24 * self.p4 * t + 60 * self.p5 * t ** 2
Trajectory
¶
Polynomial interpolation solver
Source code in src\python_motion_planning\curve_generator\polynomial_curve.py
Python
class Trajectory:
"""
Polynomial interpolation solver
"""
def __init__(self):
self.clear()
def clear(self):
self.time = []
self.x = []
self.y = []
self.yaw = []
self.v = []
self.a = []
self.jerk = []
@property
def size(self):
assert len(self.time) == len(self.x) and \
len(self.x) == len(self.y) and \
len(self.y) == len(self.yaw) and \
len(self.yaw) == len(self.v) and \
len(self.v) == len(self.a) and \
len(self.a) == len(self.jerk), \
"Unequal dimensions of each attribute, this should not happen."
return len(self.time)
generation(start_pose, goal_pose)
¶
Generate the polynomial Curve.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
start_pose
|
tuple
|
Initial pose (x, y, yaw) |
required |
goal_pose
|
tuple
|
Target pose (x, y, yaw) |
required |
Returns:
| Name | Type | Description |
|---|---|---|
traj |
Traj
|
The first trajectory that satisfies the acceleration and jerk constraint |
Source code in src\python_motion_planning\curve_generator\polynomial_curve.py
Python
def generation(self, start_pose: tuple, goal_pose: tuple):
"""
Generate the polynomial Curve.
Parameters:
start_pose (tuple): Initial pose (x, y, yaw)
goal_pose (tuple): Target pose (x, y, yaw)
Returns:
traj (Traj): The first trajectory that satisfies the acceleration and jerk constraint
"""
sx, sy, syaw, sv, sa = start_pose
gx, gy, gyaw, gv, ga = goal_pose
sv_x = sv * math.cos(syaw)
sv_y = sv * math.sin(syaw)
gv_x = gv * math.cos(gyaw)
gv_y = gv * math.sin(gyaw)
sa_x = sa * math.cos(syaw)
sa_y = sa * math.sin(syaw)
ga_x = ga * math.cos(gyaw)
ga_y = ga * math.sin(gyaw)
traj = self.Trajectory()
for T in np.arange(self.t_min, self.t_max, self.step):
x_psolver = self.Poly((sx, sv_x, sa_x), (gx, gv_x, ga_x), T)
y_psolver = self.Poly((sy, sv_y, sa_y), (gy, gv_y, ga_y), T)
for t in np.arange(0.0, T + self.dt, self.dt):
traj.time.append(t)
traj.x.append(x_psolver.x(t))
traj.y.append(y_psolver.x(t))
vx = x_psolver.dx(t)
vy = y_psolver.dx(t)
traj.v.append(math.hypot(vx, vy))
traj.yaw.append(math.atan2(vy, vx))
ax = x_psolver.ddx(t)
ay = y_psolver.ddx(t)
a = math.hypot(ax, ay)
if len(traj.v) >= 2 and traj.v[-1] - traj.v[-2] < 0.0:
a *= -1
traj.a.append(a)
jx = x_psolver.dddx(t)
jy = y_psolver.dddx(t)
j = math.hypot(jx, jy)
if len(traj.a) >= 2 and traj.a[-1] - traj.a[-2] < 0.0:
j *= -1
traj.jerk.append(j)
if max(np.abs(traj.a)) <= self.max_acc and \
max(np.abs(traj.jerk)) <= self.max_jerk:
return traj
else:
traj.clear()
return traj