optimal

Optimal Trajectory Generation Module.

This module provides utilities for the generation of time-optimal trajectories with bounded position, velocity, and acceleration. It includes functions for calculating and optimizing trajectories using constraints for both Cartesian and joint space. The module leverages numerical optimization and interpolation to create smooth paths that satisfy given constraints, ensuring that the motion is as fast as possible without violating the constraints.

class robotblockset.optimal.path_constraints[source]

Bases: object

A class to define constraints on the path motion, including: - Maximum velocity and acceleration in Cartesian and joint spaces.

Attributes:

  • xdnmax (float, optional) – Maximum path velocity in Cartesian space.

  • xddnmax (float, optional) – Maximum path acceleration in Cartesian space.

  • xdmax (np.ndarray, optional) – Maximum velocity in Cartesian space.

  • xddmax (np.ndarray, optional) – Maximum acceleration in Cartesian space.

  • qdmax (np.ndarray, optional) – Maximum joint velocity.

  • qddmax (np.ndarray, optional) – Maximum joint acceleration.

Initialize an empty set of path velocity and acceleration constraints.

Returns:

This constructor initializes the constraint container in place.

Return type:

None

__init__() → None[source]

Initialize an empty set of path velocity and acceleration constraints.

Returns:

This constructor initializes the constraint container in place.

Return type:

None

robotblockset.optimal.splinedif(s: float, path_s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], path: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], ds: float = 0.001, Cartesian: bool = False) → Tuple[ndarray, ndarray, ndarray][source]

Numerically calculates path Jacobian and its derivative.

Parameters:

  • s (float) – Path parameter.

  • path_s (ArrayLike) – Path parameters for path (n, ).

  • path (ArrayLike) – Path task positions (n, m).

  • ds (float, optional) – Path step, by default 0.001.

  • Cartesian (bool, optional) – Whether the path is Cartesian task poses, by default False.

Returns:

  • x (m, ): Task position.

  • sJ (m, ): Jacobian dx/ds.

  • sJd (m, ): Second derivative d2(x)/ds2.

Return type:

tuple

class robotblockset.optimal.path_kinematics(path: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], path_s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, path_q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, Cartesian: bool = False, dkin: Callable[[...], Tuple[ndarray, ndarray]] | None = None, ds: float = 0.001, scale: float | Sequence[float] = (1.0, 1.0))[source]

Bases: object

A class that handles the kinematics of a path, including calculating task positions, velocities, and accelerations, along with their corresponding Jacobians.

Initialize the path kinematics model used for trajectory optimization.

Parameters:

  • path (ArrayLike) – Path samples in task space or joint space.

  • path_s (ArrayLike, optional) – Path parameter values associated with the samples. If omitted, they are computed from the path.

  • path_q (ArrayLike, optional) – Joint-space samples associated with a Cartesian path.

  • Cartesian (bool, optional) – If True, interpret the path as Cartesian poses.

  • dkin (callable, optional) – Direct kinematics function used to map joint states to task space and Jacobians.

  • ds (float, optional) – Step size used for numerical differentiation along the path.

  • scale (float or sequence of float, optional) – Scale factors used in SE(3) path-length and error computations.

Returns:

This constructor initializes the path kinematics object in place.

Return type:

None

__init__(path: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], path_s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, path_q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, Cartesian: bool = False, dkin: Callable[[...], Tuple[ndarray, ndarray]] | None = None, ds: float = 0.001, scale: float | Sequence[float] = (1.0, 1.0)) → None[source]

Initialize the path kinematics model used for trajectory optimization.

Parameters:

  • path (ArrayLike) – Path samples in task space or joint space.

  • path_s (ArrayLike, optional) – Path parameter values associated with the samples. If omitted, they are computed from the path.

  • path_q (ArrayLike, optional) – Joint-space samples associated with a Cartesian path.

  • Cartesian (bool, optional) – If True, interpret the path as Cartesian poses.

  • dkin (callable, optional) – Direct kinematics function used to map joint states to task space and Jacobians.

  • ds (float, optional) – Step size used for numerical differentiation along the path.

  • scale (float or sequence of float, optional) – Scale factors used in SE(3) path-length and error computations.

Returns:

This constructor initializes the path kinematics object in place.

Return type:

None

calc(s: float) → None[source]

Calculates task position, velocity, and acceleration at a given path parameter s.

Parameters:

s (float) – Path parameter at which to compute the task position, velocity, and acceleration.

s2x(sp: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], sv: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, sa: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None) → Tuple[ndarray, ndarray, ndarray][source]

Transforms path trajectory to task trajectory.

Parameters:

  • sp (ArrayLike) – Path positions (nsamp, ).

  • sv (ArrayLike, optional) – Path velocities (nsamp, ), by default None.

  • sa (ArrayLike, optional) – Path accelerations (nsamp, ), by default None.

Returns:

  • x (nsamp, m): Task positions.

  • xd (nsamp, m): Task velocities.

  • xdd (nsamp, m): Task accelerations.

Return type:

tuple

s2q_x(sp: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], sv: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, sa: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None) → Tuple[ndarray, ndarray, ndarray][source]

Transforms path trajectory via Cartesian task space into joint space.

Parameters:

  • sp (ArrayLike) – Path positions (nsamp, ).

  • sv (ArrayLike, optional) – Path velocities (nsamp, ), by default None.

  • sa (ArrayLike, optional) – Path accelerations (nsamp, ), by default None.

Returns:

  • q (nsamp, nj): Joint positions.

  • qd (nsamp, nj): Joint velocities.

  • qdd (nsamp, nj): Joint accelerations.

Return type:

tuple

robotblockset.optimal.accbounds(sd: float, path_kin: path_kinematics, path_con: path_constraints, calc_option: int = 7) → ndarray[source]

Calculates the path acceleration bounds in (s, sd) space.

Parameters:

  • sd (float) – Path velocity.

  • path_kin (path_kinematics) – Path kinematics parameters.

  • path_con (path_constraints) – Velocity and acceleration constraints in Cartesian and joint space.

  • calc_option (int, optional) – Bit selection for considered bounds (default is 0b111), where: - 0: maximal Cartesian acceleration norm - 1: maximal Cartesian acceleration - 2: maximal joint acceleration.

Returns:

[sddmax, sddmin]: Maximum and minimum path acceleration bounds.

Return type:

np.ndarray

robotblockset.optimal.velbounds(path_kin: path_kinematics, path_con: path_constraints) → ndarray[source]

Calculates path velocity bounds as path position s due to Cartesian and joint constraints.

Parameters:

  • path_kin (path_kinematics) – Path kinematics parameters.

  • path_con (path_constraints) – Velocity and acceleration constraints in Cartesian and joint space.

Returns:

[sd_xdn, sd_xd, sd_xdd, sd_qd, sd_qdd]: Path velocity bounds due to: - maximal Cartesian velocity norm - maximal Cartesian velocity - maximal Cartesian acceleration - maximal joint velocity - maximal joint acceleration

Return type:

np.ndarray

robotblockset.optimal.lineIntersection(L1x1: float, L1y1: float, L1x2: float, L1y2: float, L2x1: float, L2y1: float, L2x2: float, L2y2: float) → Tuple[float, float][source]

Calculates the intersection point of two 2D lines defined by their endpoints.

Parameters:

  • L1x1 (float) – Coordinates of the first endpoint of the first line.

  • L1y1 (float) – Coordinates of the first endpoint of the first line.

  • L1x2 (float) – Coordinates of the second endpoint of the first line.

  • L1y2 (float) – Coordinates of the second endpoint of the first line.

  • L2x1 (float) – Coordinates of the first endpoint of the second line.

  • L2y1 (float) – Coordinates of the first endpoint of the second line.

  • L2x2 (float) – Coordinates of the second endpoint of the second line.

  • L2y2 (float) – Coordinates of the second endpoint of the second line.

Returns:

  • tuple –

    • x (float): x-coordinate of the intersection point.

    • y (float): y-coordinate of the intersection point.

  • If the lines are parallel (i.e., no intersection), the function returns NaN for both coordinates.

robotblockset.optimal.timeopttraj(path_kin: path_kinematics, path_con: path_constraints, s0: float = 0, send: float | None = None, sd0: float = 0, sdend: float = 0, tsamp: float = 0.01, plot: bool = False, sd_bounds: ndarray | None = None) → Tuple[ndarray, ndarray, ndarray, ndarray][source]

Generates a time-optimal trajectory for bounded position, acceleration, and velocity.

Parameters:

  • path_kin (path_kinematics) – Path kinematics parameters.

  • path_con (path_constraints) – Velocity and acceleration constraints in Cartesian and joint space.

  • s0 (float, optional) – Path start position, by default 0.

  • send (float, optional) – Path end position, by default None.

  • sd0 (float, optional) – Path start velocity, by default 0.

  • sdend (float, optional) – Path end velocity, by default 0.

  • tsamp (float, optional) – Sample time, by default 0.01.

  • plot (bool, optional) – Flag for plotting, by default False.

  • sd_bounds (np.ndarray, optional) – Path velocity bounds for plot, by default None.

Returns:

  • t (time): Time array.

  • sp (path parameter): Path parameter array.

  • sv (path velocity): Path velocity array.

  • sa (path acceleration): Path acceleration array.

Return type:

tuple

robotblockset.optimal.plot_acc_bounds(s: float, sd: float, path_kin: path_kinematics, path_con: path_constraints, tsamp: float = 0.001, fig: int | None = None) → None[source]

Plots max and min acceleration directions to the current s-sd plot.

Parameters:

  • s (float) – Path position.

  • sd (float) – Path velocity.

  • path_kin (path_kinematics) – Path kinematics parameters.

  • path_con (path_constraints) – Path constraints due to velocity and acceleration.

  • tsamp (float, optional) – Sample time, by default 0.001.

  • fig (int, optional) – Figure number, by default None.

robotblockset.optimal.plot_path_bounds(path_kin: path_kinematics, path_con: path_constraints, s0: float | None = None, send: float | None = None) → ndarray[source]

Plots path velocity bounds in the s-sd plane.

Parameters:

  • path_kin (path_kinematics) – Path kinematics parameters.

  • path_con (path_constraints) – Path constraints due to velocity and acceleration.

  • s0 (float, optional) – Path start position, by default None.

  • send (float, optional) – Path end position, by default None.

Returns:

Path velocity bounds.

Return type:

np.ndarray

robotblockset.optimal.timeopt_joint_traj(path_q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], path_con: path_constraints, dkin: Callable[[...], Tuple[ndarray, ndarray]] | None = None, scale: float | Sequence[float] = (1.0, 1.0), s0: float = 0, send: float | None = None, sd0: float = 0, sdend: float = 0, tsamp: float = 0.01, plot: bool = False, sd_bounds: ndarray | None = None) → Tuple[ndarray, ndarray, ndarray, ndarray][source]

Time optimal joint trajectory generation for bounded position, acceleration, and velocity.

Parameters:

  • path_q (ArrayLike) – Path positions in joint space (n, nj).

  • path_con (path_constraints) – Velocity and acceleration constraints in Cartesian and joint space.

  • dkin (Callable[..., Tuple[np.ndarray, np.ndarray]], optional) – Direct kinematic function, by default None.

  • scale (Union[float, Sequence[float]], optional) – SE3 norm scale factors, by default [1.0, 1.0].

  • s0 (float, optional) – Path start position, by default 0.

  • send (float, optional) – Path end position, by default None.

  • sd0 (float, optional) – Path start velocity, by default 0.

  • sdend (float, optional) – Path end velocity, by default 0.

  • tsamp (float, optional) – Sample time, by default 0.01.

Returns:

  • t (time): Time array.

  • q (joint positions): Joint position array.

  • qd (joint velocities): Joint velocity array.

  • qdd (joint accelerations): Joint acceleration array.

Return type:

tuple

Functions

accbounds(sd, path_kin, path_con[, calc_option])

Calculates the path acceleration bounds in (s, sd) space.

lineIntersection(L1x1, L1y1, L1x2, L1y2, ...)

Calculates the intersection point of two 2D lines defined by their endpoints.

plot_acc_bounds(s, sd, path_kin, path_con[, ...])

Plots max and min acceleration directions to the current s-sd plot.

plot_path_bounds(path_kin, path_con[, s0, send])

Plots path velocity bounds in the s-sd plane.

splinedif(s, path_s, path[, ds, Cartesian])

Numerically calculates path Jacobian and its derivative.

timeopt_joint_traj(path_q, path_con[, dkin, ...])

Time optimal joint trajectory generation for bounded position, acceleration, and velocity.

timeopttraj(path_kin, path_con[, s0, send, ...])

Generates a time-optimal trajectory for bounded position, acceleration, and velocity.

velbounds(path_kin, path_con)

Calculates path velocity bounds as path position s due to Cartesian and joint constraints.

Classes

path_constraints()

A class to define constraints on the path motion, including: - Maximum velocity and acceleration in Cartesian and joint spaces.

path_kinematics(path[, path_s, path_q, ...])

A class that handles the kinematics of a path, including calculating task positions, velocities, and accelerations, along with their corresponding Jacobians.