generate constrained path using cubic or quintic polynomial
I need to write a program to generate a path using cubic/quintic polynomials.
I have written the following code to generate path for 3D space. It plots a path(using cubic polynomial) with constraints on starting point, goal point, initial velocity and goal velocity.
import numpy as np
import matplotlib.pyplot as plt
def cubic_trajectory(x0, xf, v0, vf, T):
# Calculate coefficients for cubic polynomial
a0 = x0
a1 = v0
a2 = (3 * (xf - x0) - (2 * v0 + vf) * T) / T ** 2
a3 = (2 * (x0 - xf) + (v0 + vf) * T) / T ** 3
return a0, a1, a2, a3
def generate_trajectory(a0, a1, a2, a3, T, num_points=100):
t = np.linspace(0, T, num_points)
x_t = a0 + a1 * t + a2 * t ** 2 + a3 * t ** 3
return t, x_t
def plot_trajectories_3d(trajectories):
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for traj in trajectories:
t, x_t, y_t, z_t = traj
ax.plot(x_t, y_t, z_t,
label=f'Trajectory from ({x_t[0]}, {y_t[0]}, {z_t[0]}) to ({x_t[-1]}, {y_t[-1]}, {z_t[-1]})')
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_zlabel('Z Position')
ax.set_title('3D Cubic Polynomial Trajectories')
ax.legend()
plt.show()
# Define multiple start and end conditions for 3D
conditions_3d = [
{'x0': (-0.5, 0.0, 0.5), 'xf': (0.5, 0.0, 1.0), 'v0': (0, 0, 0), 'vf': (0, 10, 0), 'T': 5},
#{'x0': (5, 5, 5), 'xf': (15, 15, 15), 'v0': (1, 1, 1), 'vf': (-1, -1, -1), 'T': 6},
#{'x0': (-3, -3, -3), 'xf': (7, 7, 7), 'v0': (0, 0, 0), 'vf': (2, 0, 0), 'T': 4},
]
trajectories_3d = []
for cond in conditions_3d:
a0_x, a1_x, a2_x, a3_x = cubic_trajectory(cond['x0'][0], cond['xf'][0], cond['v0'][0], cond['vf'][0], cond['T'])
a0_y, a1_y, a2_y, a3_y = cubic_trajectory(cond['x0'][1], cond['xf'][1], cond['v0'][1], cond['vf'][1], cond['T'])
a0_z, a1_z, a2_z, a3_z = cubic_trajectory(cond['x0'][2], cond['xf'][2], cond['v0'][2], cond['vf'][2], cond['T'])
t, x_t = generate_trajectory(a0_x, a1_x, a2_x, a3_x, cond['T'])
_, y_t = generate_trajectory(a0_y, a1_y, a2_y, a3_y, cond['T'])
_, z_t = generate_trajectory(a0_z, a1_z, a2_z, a3_z, cond['T'])
trajectories_3d.append((t, x_t, y_t, z_t))
plot_trajectories_3d(trajectories_3d)
Now I need to extend this code such that the generated path is within a particular region. For example in the blue region in the image below.
QUESTION:
- How can I add constraints to cubic or quintic polynomial such that the generated line is within a region. For example in the blue colored region below:
For example if I generate a line from (3,4) to (8,5), the output line should look like the red line in the image below(means not passing through the white portion):
Thanks
Solution
It's definitely possible to generate a constrained path, but this may not be realizable as a robotic arm path unless you also add constraints on acceleration.
You also need to choose an objective beyond just "feasible path". For instance, after a first pass that establishes a feasible path, you can do a second pass that hardens the radius costs to constraints and represents total energy as a cost. Total energy can be inferred from integral of the absolute acceleration, which in turn can be calculated from a second gradient call.
import dataclasses
import functools
import typing
import numpy as np
import matplotlib.pyplot as plt
import scipy.optimize
from numpy.polynomial import Polynomial
if typing.TYPE_CHECKING:
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Path3DCollection
class Path(typing.NamedTuple):
space: 'PathSpace'
polys: tuple[Polynomial, ...]
position: np.ndarray
@classmethod
def from_array(cls, space: 'PathSpace', params: np.ndarray) -> 'typing.Self':
by_dim = params.reshape((len(space.p0), -1))
polys = tuple(
Polynomial(symbol='t', coef=coef) for coef in by_dim
)
return cls(
space=space, polys=polys,
position=np.array([
poly(space.t) for poly in polys
]),
)
def velocity(self) -> np.ndarray:
# Numerical, less-accurate alternative:
# return np.gradient(self.position, self.space.dt, axis=1)
series = [
poly.deriv()(self.space.t)
for poly in self.polys
]
return np.array(series)
def position_endpoints(self) -> np.ndarray:
return self.position[:, [0, -1]]
def velocity_endpoints(self) -> np.ndarray:
return self.velocity()[:, [0, -1]]
def endpoints(self) -> np.ndarray:
return np.concatenate((self.position_endpoints().T, self.velocity_endpoints().T))
def lower_radii(self) -> np.ndarray:
return np.linalg.norm(self.position.T - self.space.inner_sphere_centre, axis=1)
def upper_radii(self) -> np.ndarray:
return np.linalg.norm(self.position.T - self.space.outer_sphere_centre, axis=1)
def energy(self) -> float:
accel_polys = [
poly.deriv(2)
for poly in self.polys
]
abs_accel = np.abs([
poly(self.space.t)
for poly in accel_polys
])
return abs_accel.sum(axis=1)
def dump(self) -> None:
print('Polynomials:')
for sym, poly in zip('xyz', self.polys):
print(' ', sym, '=', poly)
print('Position endpoints approximating', self.space.p0, self.space.p1)
print(self.position_endpoints().round(6))
print('Velocity endpoints approximating', self.space.v0, self.space.v1)
print(self.velocity_endpoints().round(6))
print(
f'Worst-case radii: '
f'{self.space.inner_sphere_radius} <= {self.lower_radii().min():.2f}, '
f'{self.upper_radii().max():.2f} <= {self.space.outer_sphere_radius}'
)
def plot(self, ax: 'Axes3D', title: str) -> None:
ax.plot3D(*self.position, label=title)
@dataclasses.dataclass(frozen=True)
class PathSpace:
p0: tuple[float, float, float] # Start position
p1: tuple[float, float, float] # End position
v0: tuple[float, float, float] # Start velocity
v1: tuple[float, float, float] # End velocity
inner_sphere_centre: tuple[float, float, float] # Lower bound centroid
outer_sphere_centre: tuple[float, float, float] # Upper bound centroid
inner_sphere_radius: float # Lower bound radius
outer_sphere_radius: float # Upper bound radius
duration: float # Total time
resolution: int = 50 # Number of points in the generated path
order: int = 5 # of the path polynomials
@functools.cached_property
def t(self) -> np.ndarray:
return np.linspace(start=0, stop=self.duration, num=self.resolution + 1)
@functools.cached_property
def dt(self) -> float:
return self.duration/self.resolution
def initial_polys(self) -> np.ndarray:
coefs = np.zeros((len(self.p0), 1 + self.order))
coefs[:, 0] = self.p0
coefs[:, 1] = (np.array(self.p1) - self.p0)/self.duration
return coefs
def solve(self) -> tuple[Path, Path]:
flat_endpoints = np.concatenate((self.p0, self.p1, self.v0, self.v1))
endpoint_constraint = scipy.optimize.NonlinearConstraint(
fun=self.endpoints, lb=flat_endpoints, ub=flat_endpoints,
)
feasible_result = scipy.optimize.minimize(
fun=self.lstsq_error,
x0=self.initial_polys().ravel(),
constraints=endpoint_constraint,
)
if not feasible_result.success:
raise ValueError(feasible_result.message)
energy_result = scipy.optimize.minimize(
fun=self.energy_cost,
x0=feasible_result.x,
constraints=(
endpoint_constraint,
scipy.optimize.NonlinearConstraint(
fun=self.lower_radii, lb=self.inner_sphere_radius, ub=float('inf'),
),
scipy.optimize.NonlinearConstraint(
fun=self.upper_radii, lb=-float('inf'), ub=self.outer_sphere_radius,
),
),
)
return (
Path.from_array(space=self, params=feasible_result.x),
Path.from_array(space=self, params=energy_result.x),
)
def lstsq_error(self, params: np.ndarray) -> float:
path = Path.from_array(space=self, params=params)
lower_errors = (
self.inner_sphere_radius - path.lower_radii()
).clip(min=0)
upper_errors = (
path.upper_radii() - self.outer_sphere_radius
).clip(min=0)
errors = np.concatenate((lower_errors, upper_errors))
# convert error vector to least-squares scalar
return errors.dot(errors)
def endpoints(self, params: np.ndarray) -> np.ndarray:
path = Path.from_array(space=self, params=params)
return path.endpoints().ravel()
def energy_cost(self, params: np.ndarray) -> float:
path = Path.from_array(space=self, params=params)
energy = path.energy()
return energy.sum()
def lower_radii(self, params: np.ndarray) -> np.ndarray:
return Path.from_array(space=self, params=params).lower_radii()
def upper_radii(self, params: np.ndarray) -> np.ndarray:
return Path.from_array(space=self, params=params).upper_radii()
def plot(self, ax: 'Axes3D') -> None:
ax.scatter3D(*self.p0, label='p0')
ax.scatter3D(*self.p1, label='p1')
sphere(
ax=ax, u_count=20j, j_count=10j, label='inner bound',
radius=self.inner_sphere_radius, centre=self.inner_sphere_centre,
)
sphere(
ax=ax, u_count=40j, j_count=20j, label='outer bound',
radius=self.outer_sphere_radius, centre=self.outer_sphere_centre, alpha=0.3,
)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
def sphere(
ax: 'Axes3D',
u_count: complex,
j_count: complex,
radius: float,
centre: tuple[float, float, float],
label: str,
alpha: float = 1,
) -> 'Path3DCollection':
u, v = np.mgrid[0:2*np.pi:u_count, 0:np.pi:j_count]
x = np.cos(u) * np.sin(v)
y = np.sin(u) * np.sin(v)
z = np.cos(v)
xi = radius*x + centre[0]
yi = radius*y + centre[1]
zi = radius*z + centre[2]
return ax.scatter3D(xi, yi, zi, s=1, alpha=alpha, label=label)
def main() -> None:
# These do NOT reflect the reality of a 1.2m four-link robotic arm.
all_spaces = (
PathSpace(
# v1=(0, 10, 0) is not practical
p0=(-0.5, 0., 0.5), v0=(0., 0., 0.),
p1=( 0.5, 0., 1. ), v1=(0., 5., 0.),
duration=5.,
inner_sphere_centre=(0.1, -0.1, 0.2), inner_sphere_radius=0.6,
outer_sphere_centre=(0. , 0. , 1. ), outer_sphere_radius=1. ,
),
PathSpace(
p0=( 5., 5., 5.), v0=( 1., 1., 1.),
p1=(15., 15., 15.), v1=(-1., -1., -1.),
duration=6.,
inner_sphere_centre=(0.1, -0.1, 0.2), inner_sphere_radius= 7.,
outer_sphere_centre=(0. , 0. , 1. ), outer_sphere_radius=28.,
),
PathSpace(
p0=(-3., -3., -3.), v0=(0., 0., 0.),
p1=( 7., 7., 7.), v1=(2., 0., 0.),
duration=4.,
inner_sphere_centre=(0.1, -0.1, 0.2), inner_sphere_radius= 3.5,
outer_sphere_centre=(0. , 0. , 1. ), outer_sphere_radius=15. ,
),
)
for space in all_spaces:
feasible_path, energy_path = space.solve()
print('Feasible path:')
feasible_path.dump()
print('Energy-efficient path:')
energy_path.dump()
print()
fig, ax = plt.subplots(subplot_kw={'projection': '3d'})
space.plot(ax)
feasible_path.plot(ax, title='Feasible')
energy_path.plot(ax, title='Efficient')
ax.legend()
plt.show()
if __name__ == '__main__':
main()
Feasible path:
Polynomials:
x = -0.5 + (1.62772267e-23) t + 0.3503209 t**2 - 0.28280783 t**3 +
0.07908462 t**4 - 0.00698718 t**5
y = 0.0 + (1.20507891e-23) t - 2.63144819 t**2 + 2.45195622 t**3 -
0.7050087 t**4 + 0.06397508 t**5
z = 0.5 + (1.6085689e-18) t + 0.16248468 t**2 - 0.0582134 t**3 +
0.0077872 t**4 - 0.00036878 t**5
Position endpoints approximating (-0.5, 0.0, 0.5) (0.5, 0.0, 1.0)
[[-0.5 0.5]
[ 0. -0. ]
[ 0.5 1. ]]
Velocity endpoints approximating (0.0, 0.0, 0.0) (0.0, 5.0, 0.0)
[[ 0. -0.]
[ 0. 5.]
[ 0. 0.]]
Worst-case radii: 0.6 <= 0.66, 1.00 <= 1.0
Energy-efficient path:
Polynomials:
x = -0.5 + (5.87576423e-25) t + 0.39036819 t**2 - 0.29438286 t**3 +
0.07890896 t**4 - 0.00680942 t**5
y = 0.0 - (1.23326358e-25) t - 2.69354583 t**2 + 2.50058162 t**3 -
0.71700715 t**4 + 0.06492653 t**5
z = 0.5 + (4.02918561e-22) t + 0.17961151 t**2 - 0.07440046 t**3 +
0.0122068 t**4 - 0.00074223 t**5
Position endpoints approximating (-0.5, 0.0, 0.5) (0.5, 0.0, 1.0)
[[-0.5 0.5]
[ 0. -0. ]
[ 0.5 1. ]]
Velocity endpoints approximating (0.0, 0.0, 0.0) (0.0, 5.0, 0.0)
[[ 0. 0.]
[-0. 5.]
[ 0. 0.]]
Worst-case radii: 0.6 <= 0.66, 1.00 <= 1.0
Feasible path:
Polynomials:
x = 5.0 + 1.0 t + 0.00186909 t**2 + 0.0074451 t**3 + 0.0220539 t**4 -
0.00337671 t**5
y = 5.0 + 1.0 t + 0.00186909 t**2 + 0.0074451 t**3 + 0.0220539 t**4 -
0.00337671 t**5
z = 5.0 + 1.0 t + 0.00186909 t**2 + 0.0074451 t**3 + 0.0220539 t**4 -
0.00337671 t**5
Position endpoints approximating (5.0, 5.0, 5.0) (15.0, 15.0, 15.0)
[[ 5. 15.]
[ 5. 15.]
[ 5. 15.]]
Velocity endpoints approximating (1.0, 1.0, 1.0) (-1.0, -1.0, -1.0)
[[ 1. -1.]
[ 1. -1.]
[ 1. -1.]]
Worst-case radii: 7.0 <= 8.55, 25.61 <= 28.0
Energy-efficient path:
Polynomials:
x = 5.0 + 1.0 t + 0.89433314 t**2 - 0.36750624 t**3 + 0.07266568 t**4 -
0.00552847 t**5
y = 5.0 + 1.0 t + 0.89429503 t**2 - 0.36746431 t**3 + 0.07265488 t**4 -
0.00552766 t**5
z = 5.0 + 1.0 t + 0.89426736 t**2 - 0.3674362 t**3 + 0.07264781 t**4 -
0.00552713 t**5
Position endpoints approximating (5.0, 5.0, 5.0) (15.0, 15.0, 15.0)
[[ 5. 15.]
[ 5. 15.]
[ 5. 15.]]
Velocity endpoints approximating (1.0, 1.0, 1.0) (-1.0, -1.0, -1.0)
[[ 1. -1.]
[ 1. -1.]
[ 1. -1.]]
Worst-case radii: 7.0 <= 8.55, 25.65 <= 28.0
Feasible path:
Polynomials:
x = -3.0 + (4.4408921e-16) t + 0.53461923 t**2 + 1.44358752 t**3 -
0.65797237 t**4 + 0.07568107 t**5
y = -3.0 + (4.4408921e-16) t + 3.96838631 t**2 + 2.24463206 t**3 -
1.67107596 t**4 + 0.22523908 t**5
z = -3.0 + 0.0 t - 1.07776477 t**2 + 1.07596496 t**3 - 0.14058908 t**4 -
0.00549484 t**5
Position endpoints approximating (-3.0, -3.0, -3.0) (7.0, 7.0, 7.0)
[[-3. 7.]
[-3. 7.]
[-3. 7.]]
Velocity endpoints approximating (0.0, 0.0, 0.0) (2.0, 0.0, 0.0)
[[0. 2.]
[0. 0.]
[0. 0.]]
Worst-case radii: 3.5 <= 4.05, 14.62 <= 15.0
Energy-efficient path:
Polynomials:
x = -3.0 - (5.55905085e-22) t + 2.69660108 t**2 - 1.23659809 t**3 +
0.27674884 t**4 - 0.0242686 t**5
y = -3.0 + (7.33606319e-22) t + 5.00901898 t**2 - 2.89516277 t**3 +
0.70370283 t**4 - 0.06347833 t**5
z = -3.0 - (2.98328286e-20) t - 1.40354787 t**2 + 1.2119847 t**3 -
0.14751462 t**4 - 0.00717433 t**5
Position endpoints approximating (-3.0, -3.0, -3.0) (7.0, 7.0, 7.0)
[[-3. 7.]
[-3. 7.]
[-3. 7.]]
Velocity endpoints approximating (0.0, 0.0, 0.0) (2.0, 0.0, 0.0)
[[-0. 2.]
[ 0. -0.]
[-0. -0.]]
Worst-case radii: 3.5 <= 3.50, 11.58 <= 15.0
- Activating Anaconda Environment in VsCode
- How to implement the Softmax function in Python?
- Dataframe - Select the minimum set of rows to cover all possible values of each columns
- Non-equi join in polars
- Tkinter and matplotlib - NavigationToolbar2Tk - cursor position precision
- Media Image is not loaded in the output of Django Project
- selenium upload part not working on windows. Path related?
- How to encode UTF8 filename for HTTP headers? (Python, Django)
- Split columns in Csv file
- In the Django admin site, how do I change the display format of time fields?
- Condition in pytest fixture depending on some test-suite results
- How to make a class JSON serializable
- Apache Airflow unit and integration test
- TypeError: 'MultiPolygon' object is not iterable with Plotly Choropleth
- How do I get the current time in milliseconds in Python?
- Python: Decoding base64 encoded strings
- How to fix the error when try to plot data with pandas?
- Python Image - Finding largest branch from image skeleton
- Enumerate is Returning A None Type
- Python finite difference functions?
- How to extract hex color codes from a colormap
- Regex: Get "London, UK" from "Evolution Recruitment (Agency) (London, UK)"
- Visual Studio Code - How to add multiple paths to python path?
- What are the Spark transformations that causes a Shuffle?
- How to find the second lowest lists into a nested list by their second value?
- python-docx: Parse a table to Panda Dataframe
- Deposit and Withdraw on Account
- Why does this code snippet give the error list object is not callable?
- How to find out if argparse argument has been actually specified on command line?
- Print current call stack from a method in code