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:

  • None – This constructor initializes the path kinematics object in place.

  • Kinematic quantities along a parameterized path.

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

  • None – This constructor initializes the path kinematics object in place.

  • Kinematic quantities along a parameterized path.

calc(s: float) → None[source]

Calculate path quantities at path parameter s.

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]

Transform a path trajectory into a task trajectory.

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]

Transform a Cartesian path trajectory into joint space.

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

Raises:

RuntimeError – If no feasible time-optimal trajectory can be found.

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

Raises:

RuntimeError – If no feasible time-optimal trajectory can be found.

robotblockset.optimal.smooth_traj_start_end(xi: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], tend: float, path_con: path_constraints, tsamp: float | None = None, Cartesian: bool = True, scale: float | Sequence[float] = (1.0, 1.0)) → Tuple[ndarray, ndarray][source]

Add acceleration-limited transitions to the start and end of a trajectory.

The input path is initially parameterized to run for tend seconds at its nominal path velocity. Forward and backward integrations of the acceleration bounds are then used to replace the beginning and end with smooth transitions from and to zero path velocity. The middle portion retains the nominal timing.

Parameters:

  • xi (ArrayLike) – Sampled input path with shape (n, 7) for a Cartesian pose path or (n, m) for a non-Cartesian path.

  • tend (float) – Nominal duration of the input path in seconds.

  • path_con (path_constraints) – Velocity and acceleration constraints used to calculate the admissible path-acceleration bounds.

  • tsamp (float, optional) – Integration step for the start and end transitions. If omitted, the spacing of the nominal trajectory time samples is used.

  • Cartesian (bool, optional) – Interpret xi as Cartesian poses when True, by default True.

  • scale (float or sequence of float, optional) – Position and orientation scale factors used for Cartesian path length, by default (1.0, 1.0). A scalar specifies the orientation scale while retaining a position scale of one.

Returns:

  • tti (np.ndarray) – Time samples of the smoothed trajectory in seconds.

  • xti (np.ndarray) – Smoothed trajectory samples evaluated at tti.

Raises:

  • ValueError – If the input path, scale, or a path coordinate generated during integration is invalid.

  • RuntimeError – If the requested nominal trajectory velocity cannot be reached within the configured acceleration limits, or if the acceleration and deceleration transitions overlap so that no feasible middle segment remains.

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.

smooth_traj_start_end(xi, tend, path_con[, ...])

Add acceleration-limited transitions to the start and end of a trajectory.

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.