CubicSpline¶
src.python_motion_planning.curve_generator.cubic_spline.CubicSpline
¶
Bases: Curve
Class for cubic spline generation.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
step
|
float
|
Simulation or interpolation size |
required |
Examples:
Python Console Session
>>> from python_motion_planning.curve_generation import CubicSpline
>>> points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
>>> generator = CubicSpline(step)
>>> generator.run(points)
Source code in src\python_motion_planning\curve_generator\cubic_spline.py
Python
class CubicSpline(Curve):
"""
Class for cubic spline generation.
Parameters:
step (float): Simulation or interpolation size
Examples:
>>> from python_motion_planning.curve_generation import CubicSpline
>>> points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
>>> generator = CubicSpline(step)
>>> generator.run(points)
"""
def __init__(self, step: float) -> None:
super().__init__(step)
def __str__(self) -> str:
return "Cubic Spline"
def spline(self, x_list: list, y_list: list, t: list):
"""
Running both generation and animation.
Parameters:
x_list (list[tuple]): path points x-direction
y_list (list[tuple]): path points y-direction
t (list): parameter
Returns:
p (list): The (x, y) of curve with given t
dp (list): The derivative (dx, dy) of curve with given t
"""
# cubic polynomial functions
a, b, c, d = y_list, [], [], []
h = np.diff(x_list)
num = len(x_list)
# calculate coefficient matrix
A = np.zeros((num, num))
for i in range(1, num - 1):
A[i, i - 1] = h[i - 1]
A[i, i] = 2.0 * (h[i - 1] + h[i])
A[i, i + 1] = h[i]
A[0, 0] = 1.0
A[num - 1, num - 1] = 1.0
B = np.zeros(num)
for i in range(1, num - 1):
B[i] = 3.0 * (a[i + 1] - a[i]) / h[i] - \
3.0 * (a[i] - a[i - 1]) / h[i - 1]
c = np.linalg.solve(A, B)
for i in range(num - 1):
d.append((c[i + 1] - c[i]) / (3.0 * h[i]))
b.append((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2.0 * c[i]) / 3.0)
# calculate spline value and its derivative
p, dp = [], []
for it in t:
if it < x_list[0] or it > x_list[-1]:
continue
i = bisect.bisect(x_list, it) - 1
dx = it - x_list[i]
p.append(a[i] + b[i] * dx + c[i] * dx**2 + d[i] * dx**3)
dp.append(b[i] + 2.0 * c[i] * dx + 3.0 * d[i] * dx**2)
return p, dp
def generation(self, start_pose: tuple, goal_pose: tuple):
pass
spline(x_list, y_list, t)
¶
Running both generation and animation.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
x_list
|
list[tuple]
|
path points x-direction |
required |
y_list
|
list[tuple]
|
path points y-direction |
required |
t
|
list
|
parameter |
required |
Returns:
| Name | Type | Description |
|---|---|---|
p |
list
|
The (x, y) of curve with given t |
dp |
list
|
The derivative (dx, dy) of curve with given t |
Source code in src\python_motion_planning\curve_generator\cubic_spline.py
Python
def spline(self, x_list: list, y_list: list, t: list):
"""
Running both generation and animation.
Parameters:
x_list (list[tuple]): path points x-direction
y_list (list[tuple]): path points y-direction
t (list): parameter
Returns:
p (list): The (x, y) of curve with given t
dp (list): The derivative (dx, dy) of curve with given t
"""
# cubic polynomial functions
a, b, c, d = y_list, [], [], []
h = np.diff(x_list)
num = len(x_list)
# calculate coefficient matrix
A = np.zeros((num, num))
for i in range(1, num - 1):
A[i, i - 1] = h[i - 1]
A[i, i] = 2.0 * (h[i - 1] + h[i])
A[i, i + 1] = h[i]
A[0, 0] = 1.0
A[num - 1, num - 1] = 1.0
B = np.zeros(num)
for i in range(1, num - 1):
B[i] = 3.0 * (a[i + 1] - a[i]) / h[i] - \
3.0 * (a[i] - a[i - 1]) / h[i - 1]
c = np.linalg.solve(A, B)
for i in range(num - 1):
d.append((c[i + 1] - c[i]) / (3.0 * h[i]))
b.append((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2.0 * c[i]) / 3.0)
# calculate spline value and its derivative
p, dp = [], []
for it in t:
if it < x_list[0] or it > x_list[-1]:
continue
i = bisect.bisect(x_list, it) - 1
dx = it - x_list[i]
p.append(a[i] + b[i] * dx + c[i] * dx**2 + d[i] * dx**3)
dp.append(b[i] + 2.0 * c[i] * dx + 3.0 * d[i] * dx**2)
return p, dp