Сгенерировать ограниченный путь, используя полином кубической или пятой степени

Определенно возможно создать ограниченный путь, но это может быть невозможно реализовать как путь роботизированной руки, если вы также не добавите ограничения на ускорение.

Вам также необходимо выбрать цель, выходящую за рамки просто «реального пути». Например, после первого прохода, который устанавливает возможный путь, вы можете выполнить второй проход, который ужесточает ограничения на стоимость радиуса и представляет полную энергию как стоимость. Полную энергию можно вывести из интеграла абсолютного ускорения, которое, в свою очередь, можно рассчитать на основе второго вызова gradient.

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