trajectories

Trajectory generation and interpolation utilities.

This module provides functions for generating, interpolating, and processing different types of trajectories for robotics and path-planning systems. Its main capabilities include creating Cartesian and joint-space trajectories, interpolating spatial poses (SE(3)), computing velocities and accelerations, and generating auxiliary points along paths. It supports methods such as linear, cubic splines, and radial basis functions (RBF), along with rotation interpolation (SLERP) for quaternion-based orientations.

Trajectory types:

Cartesian trajectory: Includes both position and orientation (quaternion) for path planning in Cartesian space.
Joint-space trajectory: Describes motion in a robot’s joint space, typically for articulated robot arms.
Spline trajectory: Smooths or interpolates between points using cubic splines or related methods.
RBF-based trajectory: Uses radial basis function interpolation to generate more complex paths.

The module is designed for a range of robotics applications, path planners, and trajectory-optimization tasks. It supports 2D and 3D spaces and provides several interpolation techniques for different planning requirements.

robotblockset.trajectories.arc(p0: ndarray, p1: ndarray, pC: ndarray, s: float, short: bool = True) → ndarray[source]

Generate points on an arc defined by two endpoints and a center point.

The arc is centered at pC, starts at p0, and ends at p1. If the distances from pC to p0 and p1 are not equal, pC is projected to a point on the midline between p0 and p1.

If s < 0, the long path is used for the arc. If short is True, the shortest rotation is taken.

Parameters:

p0 (Vector3DType) – Initial point on the arc (3,)
p1 (Vector3DType) – Final point on the arc (3,)
pC (Vector3DType) – Center point of the arc (3,)
s (float) – Normalized arc distance in the range [0, 1].
short (bool, optional) – If True, use the shortest rotation. The default is True.

Returns:

Point or points on the arc, with shape (3,) or (n, 3).

Return type:

Vector3DType

Raises:

ValueError – If the points are not distinct or are collinear.

robotblockset.trajectories.carctraj(x0: ndarray, x1: ndarray, pC: ndarray, t: ndarray, traj: str = 'Poly', short: bool = True) → Tuple[ndarray, ndarray, ndarray][source]

Generate a Cartesian trajectory along an arc from x0 to x1.

The arc is parameterized by time t, and the trajectory type (Poly, Trap, or Line) determines the interpolation method. The trajectory considers the shortest rotation if short is True.

Parameters:

x0 (Pose3DType) – Initial Cartesian pose (7,)
x1 (Pose3DType) – Final Cartesian pose (7,)
pC (Vector3DType) – Arc center position (3,)
t (TimesType) – Time array (nsamp,)
traj (str, optional) – Trajectory type: “Poly” for polynomial, “Trap” for trapezoidal, or “Line” for linear interpolation. Default is “Poly”.
short (bool, optional) – If True, the shortest rotation path is used. Default is True.

Returns:

xt : Poses3DType Cartesian trajectory - pose (nsamp, 7)
xdt : Velocities3DType Cartesian trajectory - velocity (nsamp, 6)
xddt : Accelerations3DType Cartesian trajectory - acceleration (nsamp, 6)

Return type:

tuple

Raises:

ValueError – If the trajectory time t is invalid or if the arc parameters are incompatible.

robotblockset.trajectories.jline(q0: ndarray, q1: ndarray, t: ndarray, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generates a trajectory from joint position q0 to q1 with constant velocity.

This function calculates a linear trajectory from an initial joint position q0 to a final joint position q1 with constant velocity over the provided time interval t. It also computes the joint velocities and accelerations, which are constant for the entire trajectory.

Parameters:

q0 (JointConfigurationType) – Initial joint positions (n,), where n is the number of joints.
q1 (JointConfigurationType) – Final joint positions (n,), where n is the number of joints.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.

Returns:

qt : JointPathType Interpolated joint positions (nsamp, n).
qdt : JointPathType Interpolated joint velocities (nsamp, n), constant throughout the trajectory.
qddt : JointPathType Interpolated joint accelerations (nsamp, n), which are zero since velocity is constant.

Return type:

tuple

Raises:

ValueError – If the trajectory time values are invalid (i.e., non-positive or incorrect).
TypeError – If the input vectors q0 and q1 do not have the same size.

robotblockset.trajectories.jtrap(q0: ndarray, q1: ndarray, t: ndarray, ta: float = 0.1, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generates a trajectory from q0 to q1 using trapezoidal velocity profile.

This function generates a trajectory from an initial joint position q0 to a final joint position q1 using a trapezoidal velocity profile. The trajectory consists of three phases: acceleration, constant velocity, and deceleration. The time spent on acceleration and deceleration is specified by ta.

Parameters:

q0 (JointConfigurationType) – Initial joint positions (n,). The number of elements corresponds to the number of joints.
q1 (JointConfigurationType) – Final joint positions (n,). The number of elements corresponds to the number of joints.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
ta (float, optional) – Acceleration/deceleration time (default is 0.1). This parameter controls the duration of the acceleration and deceleration phases.

Returns:

qt : JointPathType Interpolated joint positions (nsamp, n).
qdt : JointPathType Interpolated joint velocities (nsamp, n).
qddt : JointPathType Interpolated joint accelerations (nsamp, n).

Return type:

tuple

Raises:

ValueError – If the trajectory time values are non-positive or incorrect.
TypeError – If the input vectors q0 and q1 do not have the same size.

robotblockset.trajectories.jpoly(q0: ndarray, q1: ndarray, t: ndarray, v0: ndarray | None = None, v1: ndarray | None = None, **kwargs) → Tuple[ndarray, ndarray, ndarray][source]

Generates a trajectory from q0 to q1 using a 5th order polynomial.

This function computes a smooth trajectory from an initial joint position q0 to a final joint position q1 using a 5th order polynomial. The polynomial is determined such that the joint positions, velocities, and accelerations are continuous and smooth. Optional initial and final joint velocities (v0 and v1) can be provided to control the velocity at the start and end.

Parameters:

q0 (JointConfigurationType) – Initial joint positions (n,). The number of elements corresponds to the number of joints.
q1 (JointConfigurationType) – Final joint positions (n,). The number of elements corresponds to the number of joints.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
v0 (JointConfigurationType, optional) – Initial joint velocities (n,). Defaults to zero if not provided.
v1 (JointConfigurationType, optional) – Final joint velocities (n,). Defaults to zero if not provided.

Returns:

qt : JointPathType Interpolated joint positions (nsamp, n).
qdt : JointPathType Interpolated joint velocities (nsamp, n).
qddt : JointPathType Interpolated joint accelerations (nsamp, n).

Return type:

tuple

Raises:

TypeError – If the input vectors q0, q1, v0, and v1 do not have the same size.
ValueError – If the trajectory time values are non-positive or incorrect.

robotblockset.trajectories.jjerk(q0: ndarray, q1: ndarray, t: ndarray, qd0: ndarray | None = None, qd1: ndarray | None = None, qdd0: ndarray | None = None, qdd1: ndarray | None = None, qd_max: ndarray | None = None, qd_min: ndarray | None = None, qdd_max: ndarray | None = None, qdd_min: ndarray | None = None, jerk_max: ndarray | None = None, resample: bool = True, **kwargs) → Tuple[ndarray, ndarray, ndarray][source]

Generate a trajectory from q0 to q1 using a jerk-limited profile.

This function computes a smooth trajectory from an initial joint position q0 to a final joint position q1 using a jerk-limited profile generated by Ruckig. The profile ensures that the joint positions, velocities, and accelerations are continuous and smooth. Optional initial and final joint velocities (v0 and v1) can be provided to control the velocity at the start and end.

Parameters:

q0 (JointConfigurationType) – Initial joint positions (n,). The number of elements corresponds to the number of joints.
q1 (JointConfigurationType) – Final joint positions (n,). The number of elements corresponds to the number of joints.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
v0 (JointConfigurationType, optional) – Initial joint velocities (n,). Defaults to zero if not provided.
v1 (JointConfigurationType, optional) – Final joint velocities (n,). Defaults to zero if not provided.
a0 (JointConfigurationType, optional) – Initial joint accelerations (n,). Defaults to zero if not provided.
a1 (JointConfigurationType, optional) – Final joint accelerations (n,). Defaults to zero if not provided.
qd_max (JointConfigurationType, optional) – Maximum joint velocities (n,). Defaults to 1 if not provided.
qd_min (JointConfigurationType, optional) – Minimum joint velocities (n,). Defaults to -1 if not provided.
qdd_max (JointConfigurationType, optional) – Maximum joint accelerations (n,). Defaults to 2 if not provided.
jerk_max (JointConfigurationType, optional) – Maximum joint jerks (n,). Defaults to 10 if not provided.
resample (bool, optional) – Whether to resample when the generated trajectory exceeds the requested duration. The default is True.

Returns:

qt : JointPathType Interpolated joint positions (nsamp, n).
qdt : JointPathType Interpolated joint velocities (nsamp, n).
qddt : JointPathType Interpolated joint accelerations (nsamp, n).

Return type:

tuple

Raises:

TypeError – If the input vectors q0, q1, v0, and v1 do not have the same size.
VadlueError – If the trajectory time values are non-positive or incorrect.

robotblockset.trajectories.jtraj(q0: ndarray, q1: ndarray, t: ndarray, traj: str = 'Poly', v0: ndarray | None = None, v1: ndarray | None = None, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generate a trajectory from initial joint positions q0 to final joint positions q1 over time t.

Parameters:

q0 (JointConfigurationType) – Initial joint positions (n,).
q1 (JointConfigurationType) – Final joint positions (n,).
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
traj (str, optional) – Trajectory type. Supported values are: - “Poly”: Polynomial trajectory. - “Jerk”: Jerk-limited trajectory generated by Ruckig. - “Trap”: Trapezoidal trajectory. - “Line”: Linear trajectory. The default is “Poly”.
v0 (JointConfigurationType, optional) – Initial joint velocities (n,). Used by trajectory types that support boundary velocities.
v1 (JointConfigurationType, optional) – Final joint velocities (n,). Used by trajectory types that support boundary velocities.
**kwargs (dict, optional)

Returns:

qtJointPathType
Interpolated joint positions (nsamp, n).
qdtJointPathType
Interpolated joint velocities (nsamp, n).
qddtJointPathType
Interpolated joint accelerations (nsamp, n).

Return type:

tuple

Raises:

ValueError – If the trajectory type is unsupported.

robotblockset.trajectories.cline(x0: ndarray, x1: ndarray, t: ndarray, short: bool = True, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generate a Cartesian trajectory from x0 to x1 with constant velocity.

This function generates a trajectory that interpolates between an initial Cartesian pose x0 and a final Cartesian pose x1. The trajectory is computed with constant velocity, and the poses are defined by both position and quaternion. Optionally, the shortest rotation can be chosen.

Parameters:

x0 (Pose3DType) – Initial Cartesian pose (7,). The pose is represented by a 7-element vector, where the first three elements are the position, and the last four are the quaternion orientation.
x1 (Pose3DType) – Final Cartesian pose (7,). Similar to x0, this is a 7-element vector representing the final pose.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
short (bool, optional) – If True (default), the shortest rotation is taken between the initial and final orientations. If False, the long path is taken.

Returns:

xt : Poses3DType Cartesian trajectory - pose (nsamp, 7).
xdt : Velocities3DType Cartesian trajectory - velocity (nsamp, 6).
xddt : Accelerations3DType Cartesian trajectory - acceleration (nsamp, 6).

Return type:

tuple

Raises:

ValueError – If the time array t contains non-positive values.

robotblockset.trajectories.ctrap(x0: ndarray, x1: ndarray, t: ndarray, short: bool = True, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generate a Cartesian trajectory from x0 to x1 with trapezoidal velocity profile.

This function generates a trajectory between the initial Cartesian pose x0 and the final Cartesian pose x1, using a trapezoidal velocity profile. The poses are defined by both position and quaternion. Optionally, the shortest rotation can be chosen between the two poses.

Parameters:

x0 (Pose3DType) – Initial Cartesian pose (7,). The pose is represented by a 7-element vector, where the first three elements are the position, and the last four are the quaternion orientation.
x1 (Pose3DType) – Final Cartesian pose (7,). Similar to x0, this is a 7-element vector representing the final pose.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
short (bool, optional) – If True (default), the shortest rotation is taken between the initial and final orientations. If False, the long path is taken.

Returns:

xt : Poses3DType Cartesian trajectory - pose (nsamp, 7).
xdt : Velocities3DType Cartesian trajectory - velocity (nsamp, 6).
xddt : Accelerations3DType Cartesian trajectory - acceleration (nsamp, 6).

Return type:

tuple

Raises:

ValueError – If the time array t contains non-positive values.

robotblockset.trajectories.cpoly(x0: ndarray, x1: ndarray, t: ndarray, short: bool = True, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generate a Cartesian trajectory from x0 to x1 using a 5th order polynomial.

This function generates a trajectory between the initial Cartesian pose x0 and the final Cartesian pose x1, using a 5th order polynomial. The poses are defined by both position and quaternion. Optionally, the shortest rotation can be chosen between the two poses.

Parameters:

x0 (Pose3DType) – Initial Cartesian pose (7,). The pose is represented by a 7-element vector, where the first three elements are the position, and the last four are the quaternion orientation.
x1 (Pose3DType) – Final Cartesian pose (7,). Similar to x0, this is a 7-element vector representing the final pose.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
short (bool, optional) – If True (default), the shortest rotation is taken between the initial and final orientations. If False, the long path is taken.

Returns:

xt : Poses3DType Cartesian trajectory - pose (nsamp, 7).
xdt : Velocities3DType Cartesian trajectory - velocity (nsamp, 6).
xddt : Accelerations3DType Cartesian trajectory - acceleration (nsamp, 6).

Return type:

tuple

Raises:

ValueError – If the time array t contains non-positive values.

robotblockset.trajectories.cjerk(x0: ndarray, x1: ndarray, t: ndarray, v0: ndarray = None, v1: ndarray = None, a0: ndarray = None, a1: ndarray = None, short: bool = True, v_max: ndarray | None = None, v_min: ndarray | None = None, a_max: ndarray | None = None, a_min: ndarray | None = None, jerk_max: ndarray | None = None, resample: bool = True, **kwargs) → Tuple[ndarray, ndarray, ndarray][source]

Generate a Cartesian trajectory from x0 to x1 using a jerk-limited profile.

This function generates a trajectory between the initial Cartesian pose x0 and the final Cartesian pose x1 using a jerk-limited profile. Ruckig treats every DOF as an independent scalar trajectory. That is problematic because quaternions must satisfy the unit-norm constraint. Therefore, the function generates a scalar trajectory:

s(t) ∈ [0,1]

and applies Ruckig trajectory generation to it.

The poses are defined by both position and quaternion. Optionally, the shortest rotation can be chosen between the two poses.

Parameters:

x0 (Pose3DType) – Initial Cartesian pose (7,). The pose is represented by a 7-element vector, where the first three elements are the position, and the last four are the quaternion orientation.
x1 (Pose3DType) – Final Cartesian pose (7,). Similar to x0, this is a 7-element vector representing the final pose.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
v0 (Velocity3DType, Optional) – Initial Cartesian velocity (6,).
v1 (Velocity3DType) – Final Cartesian velocity (6,).
a0 (Velocity3DType) – Initial Cartesian acceleration (6,).
a1 (Velocity3DType) – Final Cartesian acceleration (6,).
short (bool, optional) – If True (default), the shortest rotation is taken between the initial and final orientations. If False, the long path is taken.
v_max (Velocity3DType, optional) – Maximum Cartesian velocities. A scalar or six-element vector can be provided; if not provided, defaults are used.
v_min (Velocity3DType, optional) – Minimum Cartesian velocities (6,). If not provided, no minimum velocity limit is used.
a_max (Acceleration3DType, optional) – Maximum Cartesian accelerations. A scalar or six-element vector can be provided; if not provided, defaults are used.
a_min (Acceleration3DType, optional) – Minimum Cartesian accelerations (6,). If not provided, no minimum acceleration limit is used.
jerk_max (Acceleration3DType, optional) – Maximum Cartesian jerk. A scalar or six-element vector can be provided; if not provided, defaults are used.
resample (bool, optional) – Whether to resample when the generated trajectory exceeds the requested duration. The default is True.

Returns:

xt : Poses3DType Cartesian trajectory - pose (nsamp, 7).
xdt : Velocities3DType Cartesian trajectory - velocity (nsamp, 6).
xddt : Accelerations3DType Cartesian trajectory - acceleration (nsamp, 6).

Return type:

tuple

Raises:

ValueError – If the time array t contains non-positive values.

robotblockset.trajectories.ctraj(x0: ndarray, x1: ndarray, t: ndarray, traj: str = 'Poly', short: bool = True, **kwargs: Any) → Tuple[ndarray, ndarray, ndarray][source]

Generate a Cartesian trajectory from x0 to x1 based on the specified trajectory type.

This function generates a trajectory between two Cartesian poses, x0 and x1, with options for different types of trajectories: polynomial, trapezoidal, or linear. The poses are defined by both position and quaternion.

Parameters:

x0 (Pose3DType) – Initial Cartesian pose (7,). The pose is represented by a 7-element vector, where the first three elements are the position, and the last four are the quaternion orientation.
x1 (Pose3DType) – Final Cartesian pose (7,). Similar to x0, this is a 7-element vector representing the final pose.
t (TimesType) – Time array (nsamp,) for the trajectory evaluation.
traj (str, optional) – The type of trajectory to generate. Options are: - “Poly” for polynomial (default), - “Jerk” for jerk-limited trajectory using Ruckig, - “Trap” for trapezoidal velocity, - “Line” for linear velocity.
short (bool, optional) – If True (default), the shortest rotation is taken between the initial and final orientations. If False, the long path is taken.

Returns:

xt : Poses3DType Cartesian trajectory - pose (nsamp, 7).
xdt : Velocities3DType Cartesian trajectory - velocity (nsamp, 6).
xddt : Accelerations3DType Cartesian trajectory - acceleration (nsamp, 6).

Return type:

tuple

Raises:

ValueError – If the trajectory type is not one of the supported types: “Jerk”, “Poly”, “Trap”, or “Line”.

Perform multidimensional linear interpolation between two values or vectors.

This function linearly interpolates from y1 to y2 at the normalized interpolation parameters s. Interpolation is performed element-wise.

Parameters:

y1 (ArrayLike) – Initial value or vector (n,).
y2 (ArrayLike) – Final value or vector (n,).
s (ArrayLike) – Normalized interpolation parameters (ns,).

Returns:

Interpolated values with shape (ns, n), or (ns,) for scalar endpoints.

Return type:

np.ndarray

Raises:

TypeError – If the input vectors y1 and y2 do not have the same size.

robotblockset.trajectories.slerp(Q1: ndarray, Q2: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], short: bool = True) → ndarray[source]

Interpolate unit quaternions with spherical linear interpolation.

This function interpolates between two unit quaternions Q1 and Q2 at the normalized interpolation points specified by s.

Parameters:

Q1 (QuaternionType) – Initial quaternion (4,).
Q2 (QuaternionType) – Final quaternion (4,).
s (ArrayLike) – Query interpolation points (n,).
short (bool, optional) – If True, use the shortest rotation path. If False, use the longer path. Default is True.

Returns:

Interpolated quaternions with shape (n, 4).

Return type:

QuaternionsType

Notes

SLERP preserves unit length and provides smooth interpolation on the quaternion sphere. When the two quaternions are nearly identical, the function falls back to a numerically safe limit case.

robotblockset.trajectories.qspline(Q: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], mode: str) → ndarray[source]

Spline interpolation of N quaternions in the spherical space of SO(3).

This function computes spline interpolation between quaternions given as input, in the space of SO(3) (3D rotation group). The interpolation is performed using either Hermite cubic or Squad interpolation, based on the specified mode.

Parameters:

Q (QuaternionsType) – Quaternion array of shape (n, 4), where each row represents a quaternion (4 elements).
s (ArrayLike) – Path parameters as a numpy array of shape (m,). These parameters define the interpolation points between [0..1].
mode (str) – Mode of spline interpolation. Can be ‘hermite_cubic’ or ‘squad’. Default is ‘squad’.

Returns:

Interpolated quaternions as a numpy array of shape (m, 4).

Return type:

QuaternionsType

Raises:

ValueError – If quaternion vector ‘Q’ does not have 4 columns or if path parameters ‘s’ are not in the range [0, 1].
TypeError – If input quaternion vectors are not of the same size.

robotblockset.trajectories.qinterp(Q1: ndarray, Q2: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], short: bool = True) → ndarray[source]

Spherical Linear Interpolation (SLERP) of unit quaternion-like arrays.

This function returns interpolated quaternion data points between two quaternions Q1 and Q2 using Spherical Linear Interpolation (SLERP), which is useful for smoothly interpolating rotations in 3D space.

Parameters:

Q1 (QuaternionType) – The initial quaternion (4,). A unit quaternion representing the starting rotation.
Q2 (QuaternionType) – The final quaternion (4,). A unit quaternion representing the ending rotation.
s (ArrayLike) – Query data points (n,). These values range from 0 to 1 and define the interpolation progression between Q1 and Q2.
short (bool, optional) – If True (default), the shortest rotation path will be chosen. If False, the longer path is used for interpolation.

Returns:

Interpolated quaternions at the requested s values, with shape (n, 4), where n is the length of the s array. Each row represents an interpolated quaternion.

Return type:

QuaternionsType

Notes

SLERP provides a smooth, constant velocity interpolation between two unit quaternions and is commonly used for smooth rotation transitions.

robotblockset.trajectories.rinterp(R1: ndarray, R2: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], short: bool = True) → ndarray[source]

Spherical Linear Interpolation (SLERP) of rotation matrices.

This function interpolates between two rotation matrices R1 and R2 using Spherical Linear Interpolation (SLERP), by first converting the rotation matrices into quaternions, performing the interpolation, and then converting back to rotation matrices.

Parameters:

R1 (RotationMatrixType) – The initial rotation matrix (3, 3). A 3x3 matrix representing the starting rotation.
R2 (RotationMatrixType) – The final rotation matrix (3, 3). A 3x3 matrix representing the ending rotation.
s (ArrayLike) – Query data points (n,). These values range from 0 to 1 and define the interpolation progression between R1 and R2.
short (bool, optional) – If True (default), the shortest rotation path will be chosen. If False, the longer path is used for interpolation.

Returns:

Interpolated rotation matrices at the requested s values, with shape (n, 3, 3), where n is the length of the s array. Each matrix represents an interpolated rotation.

Return type:

RotationMatricesType

Notes

SLERP provides a smooth, constant velocity interpolation between two quaternions, which is then mapped back to rotation matrices. The function internally converts the rotation matrices to quaternions, performs SLERP, and converts back to matrices.

This method ensures that the interpolation is geometrically correct and smooth for 3D rotations.

robotblockset.trajectories.xinterp(x1: ndarray, x2: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], short: bool = True) → ndarray[source]

Linear interpolation of spatial poses (SE3).

This function interpolates between two spatial poses x1 and x2 using linear interpolation (LERP) for the positions and spherical linear interpolation (SLERP) for the rotations. Spatial poses contain three position coordinates and four quaternion components.

Parameters:

x1 (Pose3DType) – Initial Cartesian pose (7,). The first three elements represent the position, and the last four elements represent the rotation as a quaternion.
x2 (Pose3DType) – Final Cartesian pose (7,). The first three elements represent the position, and the last four elements represent the rotation as a quaternion.
s (ArrayLike) – Query data points (n,). These values range from 0 to 1 and define the interpolation progression between x1 and x2.
short (bool, optional) – If True (default), the shortest rotation path will be chosen for SLERP. If False, the longer path is used.

Returns:

Interpolated Cartesian poses (n, 7), where each row represents an interpolated pose: [position, quaternion].

Return type:

Poses3DType

Notes

The positions are interpolated using linear interpolation (LERP).
The rotations (quaternions) are interpolated using spherical linear interpolation (SLERP).
The resulting interpolated poses combine the position and rotation at each query point s.

Example

# Example usage of the function: x1 = np.array([1.0, 2.0, 3.0, 0.7071, 0.7071, 0.0, 0.0]) # Initial pose (position + quaternion) x2 = np.array([4.0, 5.0, 6.0, 0.0, 0.7071, 0.7071, 0.0]) # Final pose (position + quaternion) s = np.linspace(0, 1, 10) # 10 query points for interpolation interpolated_poses = xinterp(x1, x2, s) print(interpolated_poses)

robotblockset.trajectories.tinterp(T1: ndarray, T2: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], short: bool = True) → ndarray[source]

Linear interpolation of spatial poses (SE3) represented as homogeneous transformation matrices.

This function interpolates between two spatial poses T1 and T2, which are represented as 4x4 homogeneous matrices. The positions are interpolated using linear interpolation (LERP), and the rotations are interpolated using spherical linear interpolation (SLERP).

Parameters:

T1 (HomogeneousMatrixType) – Initial Cartesian pose represented as a homogeneous transformation matrix (4, 4).
T2 (HomogeneousMatrixType) – Final Cartesian pose represented as a homogeneous transformation matrix (4, 4).
s (ArrayLike) – Query data points (ns,). These values range from 0 to 1 and define the interpolation progression between T1 and T2.
short (bool, optional) – If True (default), the shortest rotation path will be chosen for SLERP. If False, the longer path is used.

Returns:

Interpolated Cartesian poses (ns, 4, 4), where each pose is a 4x4 homogeneous transformation matrix.

Return type:

HomogeneousMatricesType

Notes

The translation part of the transformation matrix is interpolated using linear interpolation (LERP).
The rotation part of the transformation matrix is interpolated using spherical linear interpolation (SLERP) for quaternions.
The resulting interpolated poses combine the interpolated translation and rotation for each query point s.

Example

# Example usage of the function: T1 = np.eye(4) # Initial pose (identity matrix as an example) T2 = np.eye(4) # Final pose (identity matrix as an example) s = np.linspace(0, 1, 10) # 10 query points for interpolation interpolated_poses = tinterp(T1, T2, s) print(interpolated_poses)

robotblockset.trajectories.xarcinterp(x1: ndarray, x2: ndarray, pC: ndarray, s: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], short: bool = True) → ndarray[source]

Interpolate spatial poses (SE(3)) along an arc.

This function interpolates between spatial poses x1 and x2 along an arc centered at pC. Positions follow the circular arc, while orientations use spherical linear interpolation (SLERP).

Parameters:

x1 (Pose3DType) – Initial Cartesian pose represented by three position coordinates and four quaternion components.
x2 (Pose3DType) – Final Cartesian pose represented by three position coordinates and four quaternion components.
pC (Vector3DType) – Arc center position, an array of 3 elements representing the center of the arc.
s (ArrayLike) – Interpolation parameters (n,). Values from 0 to 1 define the progression from x1 to x2.
short (bool, optional) – If True (default), the shortest rotation path will be chosen for SLERP. If False, the longer path is used.

Returns:

Interpolated Cartesian poses along the arc (n, 7), where each pose is a combination of interpolated position and rotation.

Return type:

Poses3DType

Notes

The translation follows the circular arc.
The rotation part of the transformation is interpolated using SLERP for quaternions.
The resulting interpolated poses combine the interpolated translations and rotations for each query point s.

Example

# Example usage of the function: x1 = np.array([0, 0, 0, 1, 0, 0, 0]) # Initial pose (position and quaternion) x2 = np.array([1, 1, 1, 0, 1, 0, 0]) # Final pose (position and quaternion) pC = np.array([0.5, 0.5, 0.5]) # Arc center position s = np.linspace(0, 1, 10) # 10 query points for interpolation interpolated_poses = xarcinterp(x1, x2, pC, s) print(interpolated_poses)

Wrapper for SciPy’s interp1d function to perform 1D interpolation.

This function interpolates values y, sampled at coordinates s, at the query coordinates si.

Parameters:

s (ArrayLike) – Sample coordinates corresponding to the values in y.
y (ArrayLike) – Values sampled at s.
si (ArrayLike) – Query coordinates at which to evaluate the interpolation.

Returns:

Interpolated data points at the new query points (ni, n). The shape of the result matches the shape of si, with interpolated values for each corresponding query point.

Return type:

np.ndarray

Example

# Example usage of the function: s = np.array([0, 1, 2, 3, 4]) y = np.array([0, 1, 4, 9, 16]) si = np.array([1.5, 2.5, 3.5]) interpolated_values = interp1(s, y, si) print(interpolated_values) # Output: array([2.25, 6.25, 12.25])

Interpolate path for query path values.

This function interpolates the path data for given query points using interp1 for linear interpolation. The path data is assumed to be defined for a path parameter s, and the query points are provided in squery. The function will return the path values at the query points.

Parameters:

s (ArrayLike) – Path parameter (ns,). The path parameter defines the progression along the path.
path (ArrayLike) – Path data (ns, n). Each row corresponds to a data point along the path.
squery (ArrayLike) – Query path points (ni,). These are the values at which interpolation is performed.

Returns:

Path values at query points (ni, n). These are the path values interpolated at the specified query path points.

Return type:

np.ndarray

Raises:

TypeError – If s and path do not have the same first dimension.

Interpolate quaternion path for query path values using spherical linear interpolation (SLERP).

This function returns interpolated quaternion data points at the specified query path values based on a path of quaternions. Linear interpolation is performed sequentially between path points.

Parameters:

s (ArrayLike) – Path parameter (ns,). The path parameter defines the progression along the path.
path (QuaternionsType) – Path quaternions (ns, 4). Each row corresponds to a quaternion representing a point along the path.
squery (ArrayLike) – Query path points (ni,). These are the values at which interpolation is performed.
short (bool, optional) – If True, the shortest rotation is taken between quaternions during interpolation. Default is True.

Returns:

Interpolated quaternions at query points (ni, 4). These are the quaternions corresponding to the query path points, interpolated between the path quaternions.

Return type:

QuaternionsType

Raises:

TypeError – If the path does not have the expected shape of (ns, 4).

Interpolate Cartesian path for query path values.

This function performs linear interpolation for position values (using LERP) and quaternion interpolation (using SLERP) for rotations along a Cartesian path.

Parameters:

s (ArrayLike) – Path parameter (ns,). The parameter defining the path progression.
path (Poses3DType) – Cartesian path poses (ns, 7). The Cartesian poses should be a matrix where each row corresponds to a spatial pose (3 positions and 4 quaternion elements).
squery (ArrayLike) – Query path points (ni,). These are the values at which interpolation is performed.
short (bool, optional) – If True, the shortest rotation (SLERP) will be used for quaternion interpolation. Defaults to True.

Returns:

Cartesian path poses at query points (ni, 7). This is the interpolated path where each row contains the position and rotation (quaternion) at the respective query point.

Return type:

Poses3DType

Raises:

TypeError – If path does not have the expected shape.

Generate auxiliary points for a path.

This function generates additional points along a path for refinement. It can place auxiliary points at absolute or relative distances from the original waypoints and optionally retain the intermediate via points.

Parameters:

pnt (ArrayLike) – Path waypoints with shape (n, 3), (n, 6), or (n, 7). Each row represents a 3D point or a Cartesian pose.
auxpoints (str, optional) –
Auxiliary points generation method. Options are:
- ”absolute”: Use absolute distance between points
- ”relative”: Use relative distance of path segments
- ”none” (default): No auxiliary points are added.
auxdistance (ArrayLike, optional) – Distances used to place auxiliary points. The default is [0.1, 0.1], where the first value applies to position and the second to orientation, when applicable.
viapoints (bool, optional) – Whether to retain via points when generating auxiliary points. The default is False.

Returns:

Path with auxiliary points, shape (m, 7) or (m, 3), depending on the input dimensions. The returned path contains positions and optionally orientations (in quaternion form) for each interpolated point.

Return type:

np.ndarray

Raises:

ValueError – If the input parameters or shapes are incorrect or incompatible.

Notes

The function supports both 3D paths and Cartesian paths with orientations (7D).
Auxiliary points can be generated based on the specified distance and method.
The function can retain via points to provide finer control of the path.

robotblockset.trajectories.pathoverpoints(pnt: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], interp: str = 'inner', order: int = 4, step: float = 0.02, n_points: int = 0, auxpoints: str = 'none', auxdistance: list = [0.1, 0.1], viapoints: bool = False, natural: bool = False, normscale: list = [1, 1], plot: bool = False, ori_sel: list = [1, 2], bc_type: str | tuple = 'clamped') → ndarray[source]

Generate a path through a set of waypoints.

This function takes a set of waypoints and generates a smooth path that interpolates between them using different methods such as cubic splines, radial basis functions (RBF), or uniform subdivision. It can also generate auxiliary points to refine the path and return the corresponding path parameterization.

Parameters:

pnt (np.ndarray) – Waypoints for the path, which can be of shape (n, 7), (n, 4, 4), (n, 3), or (n, 6) where each row represents a point in 3D, Cartesian coordinates (with position and orientation as quaternion), homogenous matrices, positions only, or positions and RPY representation of poses.
interp (str, optional) –
Interpolation method. Options include:
- ”inner” (default): Spline by uniform subdivision
- ”spline”: Cubic spline interpolation
- ”RBF”: Radial basis function interpolation
- ”none”: No interpolation
order (int, optional) – The order of the inner spline, by default 4.
step (float, optional) – The maximum difference in the path parameter, by default 0.02.
n_points (int, optional) – Minimum number of path points. If 0, step controls the subdivision. The default is 0.
auxpoints (str, optional) –
Auxiliary points generation method, used primary for the “inner” interpolation method. Options are:
- ”absolute”: Use absolute distance between points
- ”relative”: Use relative distance of path segments
- ”none” (default): No auxiliary points are added.
For “spline” and “RBF” methods, auxiliary points are considered only if viapoints = True.
auxdistance (list, optional) – Distances used to place auxiliary points. The default is [0.1, 0.1], where the first value applies to position and the second to orientation, when applicable.
viapoints (bool, optional) – Whether to retain via points when auxiliary points are used. The default is False.
natural (bool, optional) – If True, use arc length as the path parameter. The default is False.
normscale (list, optional) – Scaling factors for the position and rotation norms. The default is [1, 1].
plot (bool, optional) – If True, plot the generated path. The default is False.
ori_sel (list, optional) – Indices of the quaternion components used for 2D plotting. The default is [1, 2].
bc_type (Union[str, tuple], optional) – Boundary condition type for cubic spline interpolation. The default is “clamped”.

Returns:

path : np.ndarray Interpolated path with shape (m, 3) or (m, 7), depending on the input dimensions.
parameter : np.ndarray Path parameter with shape (m,).

Return type:

tuple

Raises:

ValueError – If the input parameters or shapes are incorrect or incompatible.

Notes

The function supports both 3D paths and Cartesian paths with orientations (7D).
When using “inner” interpolation, auxiliary points are generated between the given waypoints to refine the path.
The path parameterization can be adjusted to use either natural path lengths or custom intervals based on step or n_points.

Calculate the cumulative path length in an m-dimensional space.

This function computes the arc-length parameterization of a path based on its geometry. The path can either be in Cartesian space (with 3D positions and quaternion orientations) or general m-dimensional space. If Cartesian space is used, scaling factors are applied to the position and orientation components to compute the distance.

Parameters:

path (ArrayLike) – The path data, where each row represents a point in the path. The shape of the path should be (n, m), where n is the number of points, and m is the dimensionality of each point. For Cartesian paths, m should be 7 (3D position and 4D quaternion). For other types of paths, m can vary based on the path’s structure.
scale (Union[float, ArrayLike], optional) – Scaling factors for the position and orientation norms, by default [1.0, 1.0]. If the path is in Cartesian space (7D), this parameter scales the position and orientation components differently. The first value scales the position (default is 1.0) and the second value scales the orientation (default is 1.0).
Cartesian (bool, optional) – If True, treat the path as Cartesian and compute its length considering both position and orientation using L2 norms for position and orientation differences. If False, treat the path as a general m-dimensional path and compute its length by considering only the position differences.

Returns:

Arc-length parameter (n,), where each entry contains the cumulative path length up to the respective point in the path.

Return type:

np.ndarray

Raises:

ValueError – If the path data is invalid (e.g., mismatched dimensions or unsupported path format).

Notes

If the path is a Cartesian path, the function computes the distance using both position and orientation.
The path length is calculated by accumulating the distances between consecutive points, considering the specified scaling.
A one-point path returns 0.

Find the closest point on a given path and compute the distance to it.

This function computes the closest point on a path to a given point x, along with the distance to that point and the path parameter associated with it. The path can either be represented in Cartesian coordinates or in a more general space.

Parameters:

x (ArrayLike) – Query point with shape (3,) for a 3D point or (7,) for a Cartesian pose.
path (ArrayLike) – The path, represented as an array of points. The shape can be either (n, 3) for 3D points or (n, 7) for Cartesian points (position and quaternion).
s (ArrayLike) – Path parameter associated with each point in the path (n,). These values typically represent the progression along the path.
scale (float, optional) – Scaling factor for the orientation-error norm, used only for Cartesian paths with shape (n, 7). The default is 1.0.

Returns:

np.ndarray – Closest point on the path, with shape (3,) or (7,).
float – The distance to the closest point on the path.
float – The path parameter of the closest point.

Raises:

ValueError – If the input dimensions are incorrect or if x or path have incompatible shapes.

Notes

The distance is calculated using the Euclidean norm for the position and orientation (if applicable).
For Cartesian paths (7D), both position and orientation are considered in the distance calculation.
The function handles 3D and Cartesian paths differently.

Functions

`arc`(p0, p1, pC, s[, short])	Generate points on an arc defined by two endpoints and a center point.
`carctraj`(x0, x1, pC, t[, traj, short])	Generate a Cartesian trajectory along an arc from x0 to x1.
`cjerk`(x0, x1, t[, v0, v1, a0, a1, short, ...])	Generate a Cartesian trajectory from x0 to x1 using a jerk-limited profile.
`cline`(x0, x1, t[, short])	Generate a Cartesian trajectory from x0 to x1 with constant velocity.
`cpoly`(x0, x1, t[, short])	Generate a Cartesian trajectory from x0 to x1 using a 5th order polynomial.
`ctraj`(x0, x1, t[, traj, short])	Generate a Cartesian trajectory from x0 to x1 based on the specified trajectory type.
`ctrap`(x0, x1, t[, short])	Generate a Cartesian trajectory from x0 to x1 with trapezoidal velocity profile.
`distance2path`(x, path, s[, scale])	Find the closest point on a given path and compute the distance to it.
`interp`(y1, y2, s)	Perform multidimensional linear interpolation between two values or vectors.
`interp1`(s, y, si)	Wrapper for SciPy's interp1d function to perform 1D interpolation.
`interpCartesianPath`(s, path, squery[, short])	Interpolate Cartesian path for query path values.
`interpPath`(s, path, squery)	Interpolate path for query path values.
`interpQuaternionPath`(s, path, squery[, short])	Interpolate quaternion path for query path values using spherical linear interpolation (SLERP).
`jjerk`(q0, q1, t[, qd0, qd1, qdd0, qdd1, ...])	Generate a trajectory from q0 to q1 using a jerk-limited profile.
`jline`(q0, q1, t, **kwargs)	Generates a trajectory from joint position q0 to q1 with constant velocity.
`jpoly`(q0, q1, t[, v0, v1])	Generates a trajectory from q0 to q1 using a 5th order polynomial.
`jtraj`(q0, q1, t[, traj, v0, v1])	Generate a trajectory from initial joint positions q0 to final joint positions q1 over time t.
`jtrap`(q0, q1, t[, ta])	Generates a trajectory from q0 to q1 using trapezoidal velocity profile.
`pathauxpoints`(pnt[, auxpoints, auxdistance, ...])	Generate auxiliary points for a path.
`pathlen`(path[, scale, Cartesian])	Calculate the cumulative path length in an m-dimensional space.
`pathoverpoints`(pnt[, interp, order, step, ...])	Generate a path through a set of waypoints.
`qinterp`(Q1, Q2, s[, short])	Spherical Linear Interpolation (SLERP) of unit quaternion-like arrays.
`qspline`(Q, s, mode)	Spline interpolation of N quaternions in the spherical space of SO(3).
`rinterp`(R1, R2, s[, short])	Spherical Linear Interpolation (SLERP) of rotation matrices.
`slerp`(Q1, Q2, s[, short])	Interpolate unit quaternions with spherical linear interpolation.
`tinterp`(T1, T2, s[, short])	Linear interpolation of spatial poses (SE3) represented as homogeneous transformation matrices.
`xarcinterp`(x1, x2, pC, s[, short])	Interpolate spatial poses (SE(3)) along an arc.
`xinterp`(x1, x2, s[, short])	Linear interpolation of spatial poses (SE3).