trajectories

Trajectory generation and interpolation utilities.

This module provides a collection of functions to generate, interpolate, and process different types of trajectories for robotic or path planning systems. The main functionalities include creating Cartesian and joint space trajectories, performing interpolation on spatial poses (SE3), computing velocities and accelerations, and generating auxiliary points on paths. It supports various interpolation 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), used for path planning in Cartesian space.
Joint space trajectory: Describes movements in a robot’s joint space, typically used for articulated robot arm movements.
Spline trajectory: Smoothing or interpolation between points using cubic splines or other methods to ensure smooth transitions.
RBF-based trajectory: Trajectories that are generated using Radial Basis Function interpolation, useful for more complex path generation.

The module is designed to be flexible and easily adaptable for various robotic applications, path planners, and trajectory optimization tasks. It can handle 2D and 3D space and supports various interpolation techniques to meet different planning requirements.

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

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

The arc is centered at pC, starting at point p0 and ending at point 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, the shortest rotation is used (default is True)

Returns:

Points on the arc (3,) or (n, 3)

Return type:

Vector3DType

Raises:

ValueError – If the points are not distinct or collinear.

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

Generates a Cartesian trajectory on an arc from x0 to x1, with the arc defined by the initial and final poses and an arc center.

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) → 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) – Trajectory time (nsamp,), where nsamp is the number of time samples.

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) → 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) – is evaluated.
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, qd0: ndarray | None = None, qd1: ndarray | None = None) → 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 (qd0 and qd1) 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) – is evaluated.
qd0 (JointConfigurationType, optional) – Initial joint velocities (n,). Defaults to zero if not provided.
qd1 (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, qd0, and qd1 do not have the same size.
ValueError – If the trajectory time values are non-positive or incorrect.

robotblockset.trajectories.jtraj(q0: ndarray, q1: ndarray, t: ndarray, traj: str = 'poly', qd0: ndarray | None = None, qd1: ndarray | None = None, **kwargs) → 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) – Trajectory time (nsamp,).
traj (str, optional) – Trajectory type. Possible values are: - ‘poly’: Polynomial trajectory. - ‘trap’: Trapezoidal trajectory. - ‘line’: Linear trajectory. By default, ‘poly’.
qd0 (JointConfigurationType, optional) – Initial joint velocities (n,). Required for ‘trap’ and ‘poly’ types with velocity constraints.
qd1 (JointConfigurationType, optional) – Final joint velocities (n,). Required for ‘trap’ and ‘poly’ types with velocity constraints.
**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) → 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) – is evaluated.
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) → 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) – is evaluated.
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) → 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) – is evaluated.
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.ctraj(x0: ndarray, x1: ndarray, t: ndarray, traj: str = 'poly', short: bool = True) → 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) – is evaluated.
traj (str, optional) – The type of trajectory to generate. Options are: - “poly” for polynomial (default), - “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: “poly”, “trap”, or “line”.

Perform multidimensional linear interpolation between two sets of data points.

This function linearly interpolates between two sets of data points y1 and y2 for a set of query points s. The interpolation is performed element-wise.

Parameters:

y1 (ArrayLike) – Initial data points (n,). A 1-dimensional array of floats representing the initial data points.
y2 (ArrayLike) – Final data points (n,). A 1-dimensional array of floats representing the final data points.
s (ArrayLike) – Query data points (ns,). A 1-dimensional array of floats representing the query points at which the interpolation is to be evaluated.

Returns:

Interpolated data points (ns, n). A 2-dimensional array of floats containing the interpolated data points for each query in s.

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 are represented as arrays of 3 positions and 4 quaternion elements (position + orientation).

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]

Linear interpolation of spatial poses (SE3) along an arc.

This function interpolates between two spatial poses x1 and x2 along an arc with a given center point pC. The positions are interpolated along the arc using LERP (Linear Interpolation), and the rotations are interpolated using SLERP (Spherical Linear Interpolation) for quaternions.

Parameters:

x1 (Pose3DType) – Initial Cartesian pose represented as an array of 7 elements (3 positions and 4 quaternion elements).
x2 (Pose3DType) – Final Cartesian pose represented as an array of 7 elements (3 positions and 4 quaternion elements).
pC (Vector3DType) – Arc center position, an array of 3 elements representing the center of the arc.
s (ArrayLike) – Query data points (s,). 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 along the arc (n, 7), where each pose is a combination of interpolated position and rotation.

Return type:

Poses3DType

Notes

The translation part of the transformation is interpolated along the arc using LERP.
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 data points y at query points s and returns the interpolated values at new query points si.

Parameters:

s (ArrayLike) – for which the values in y are provided.
y (ArrayLike) – to the query points s.
si (ArrayLike) – the new x-coordinates where you want to interpolate the corresponding values.

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.

Generates auxiliary points for path points.

This function generates additional points along a path for refinement. The auxiliary points can be created using either absolute or relative distances from the original path points. If specified, the function can also generate viapoints for more detailed path representation.

Parameters:

pnt (ArrayLike) – Waypoints for the path, which can be of shape (n, 3), (n, 7), or (n, 6). Each row represents a point in 3D, Cartesian coordinates (position and orientation as quaternion), or other representations of poses.
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) – The distance of auxiliary points, default is [0.1, 0.1], where the first value is for position and the second value is for orientation (if applicable).
viapoints (bool, optional) – Whether to include viapoints when generating auxiliary points, by default 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 include viapoints to ensure 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]) → ndarray[source]

Generates a path over points using spline interpolation.

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, 3), (n, 7), or (n, 6) where each row represents a point in 3D, Cartesian coordinates (with position and orientation as quaternion), or some other 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) – The minimal number of path points. If 0, step is used for subdivision, by default 0.
auxpoints (str, optional) –
Auxiliary points generation method, used only 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.
auxdistance (list, optional) – The distance of auxiliary points, default is [0.1, 0.1], where the first value is for position and the second value is for orientation (if applicable).
viapoints (bool, optional) – Whether to include viapoints when auxiliary points are used, by default False.
natural (bool, optional) – If True, makes the path parameter natural (i.e., uses path length for parameterization), by default False.
normscale (list, optional) – Scaling factor for rotation norm, by default [1, 1], where the first value is for position and the second for rotation.
plot (bool, optional) – If True, plots the generated path, by default False.
ori_sel (list, optional) – Selection of quaternion angles for 2D plotting, by default [1, 2], which represents the first and second quaternion components.

Returns:

np.ndarray – Interpolated path (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.
np.ndarray – Path parameter (m, ). The parameterization of the generated path.

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.

Calculates the path length (natural path parameter) for a given path in m-dimensional space.

This function computes the natural 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) – A flag to indicate if the path is in Cartesian space. If True, the function computes the path length considering both position and orientation using L2 norms for position and orientation differences. If False, the path is assumed to be in general m-dimensional space, and the function computes the path length by considering only the position differences.

Returns:

The natural path parameter (n,), where each entry corresponds to 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 (7D), 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.

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) – (3,) for 3D points or (1, 7) for Cartesian points (position and quaternion).
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) – The path parameter associated with each point in the path (n, 1). These parameters typically represent the progression along the path.
scale (float, optional) – A scaling factor for rotation norm, used only for Cartesian paths (when path has shape (n, 7)). Default is 1.0, meaning no scaling is applied to the orientation component.

Returns:

np.ndarray – The closest point on the path, either in 3D or Cartesian coordinates (3,) or (1, 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])	Generates points on an arc defined by two points and a center point.
`carctraj`(x0, x1, pC, t[, traj, short])	Generates a Cartesian trajectory on an arc from x0 to x1, with the arc defined by the initial and final poses and an arc center.
`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, *args)	Find the closest point on a given path and compute the distance to it.
`interp`(y1, y2, s)	Perform multidimensional linear interpolation between two sets of data points.
`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).
`jline`(q0, q1, t)	Generates a trajectory from joint position q0 to q1 with constant velocity.
`jpoly`(q0, q1, t[, qd0, qd1])	Generates a trajectory from q0 to q1 using a 5th order polynomial.
`jtraj`(q0, q1, t[, traj, qd0, qd1])	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, ...])	Generates auxiliary points for path points.
`pathlen`(path[, scale, Cartesian])	Calculates the path length (natural path parameter) for a given path in m-dimensional space.
`pathoverpoints`(pnt[, interp, order, step, ...])	Generates a path over points using spline interpolation.
`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])	Linear interpolation of spatial poses (SE3) along an arc.
`xinterp`(x1, x2, s[, short])	Linear interpolation of spatial poses (SE3).