pythonoptimizationmathematical-optimizationpolynomialspolynomial-math

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:

Region_Of_Interest

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):

Output

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
    

    path example