Utils Module

Utilities Module - ManipulaPy

This module contains essential utility functions for working with rigid body motions, transformations, and related operations in robotic manipulation. Functions cover screw theory, matrix operations, Lie algebra computations, time scaling, and coordinate transformations for kinematics and dynamics calculations.

The functions in this module support: - Extracting and manipulating screw vectors and twists - Computing transformation matrices from twists and joint angles - Matrix logarithms and exponentials for SE(3) and SO(3) - Converting between rotation matrices and Euler angles - Skew-symmetric matrix operations - Time scaling functions for trajectory generation

Copyright (c) 2025 Mohamed Aboelnar

This file is part of ManipulaPy.

ManipulaPy is free software: you can redistribute it and/or modify it under the terms of the GNU Affero General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

ManipulaPy is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License along with ManipulaPy. If not, see <https://www.gnu.org/licenses/>.

ManipulaPy.utils.extract_r_list(Slist)[source]

Extracts the r_list from the given Slist.

Parameters:

Slist (list) – A list of S vectors representing the joint screws.

Returns:

An array of r vectors.

Return type:

np.ndarray

ManipulaPy.utils.extract_omega_list(Slist)[source]

Extracts the first three elements from each sublist in the given list and returns them as a numpy array.

Parameters:

Slist (list) – A list of sublists.

Returns:

A numpy array containing the first three elements from each sublist.

Return type:

np.array

ManipulaPy.utils.extract_screw_list(omega_list, r_list)[source]

Build a 6xn screw-axis matrix from (3xn) angular velocities β€˜omega_list’ and (3xn) positions β€˜r_list’. For each column i:

S[:3, i] = w = omega_list[:, i] S[3:, i] = v = - w x r

Returns a 6xn array of [wx, wy, wz, vx, vy, vz] in each column.

ManipulaPy.utils.NearZero(z)[source]

Determines if a given number is near zero.

Parameters:

z (float) – The number to check.

Returns:

True if the number is near zero, False otherwise.

Return type:

bool

ManipulaPy.utils.skew_symmetric(v)[source]

Returns the skew symmetric matrix of a 3D vector.

Parameters:

v (array-like) – A 3D vector.

Returns:

The corresponding skew symmetric matrix.

Return type:

np.ndarray

ManipulaPy.utils.transform_from_twist(S, theta)[source]

Computes the transformation matrix from a twist and a joint angle.

Parameters:

  • S (array-like) – A 6D twist vector.

  • theta (float) – The joint angle.

Returns:

The corresponding transformation matrix.

Return type:

np.ndarray

ManipulaPy.utils.adjoint_transform(T)[source]

Computes the adjoint transformation matrix for a given transformation matrix.

Parameters:

T (np.ndarray) – A 4x4 transformation matrix.

Returns:

The corresponding adjoint transformation matrix.

Return type:

np.ndarray

ManipulaPy.utils.logm(T)[source]

Computes the logarithm of a transformation matrix.

Parameters:

T (np.ndarray) – A 4x4 transformation matrix.

Returns:

The logarithm of the transformation matrix.

Return type:

np.ndarray

ManipulaPy.utils.rotation_logm(R)[source]

Computes the logarithm of a rotation matrix.

Parameters:

R (np.ndarray) – A 3x3 rotation matrix.

Returns:

A tuple containing the rotation vector and the angle.

Return type:

tuple

ManipulaPy.utils.logm_to_twist(logm)[source]

Convert the logarithm of a transformation matrix to a twist vector.

Parameters:

logm (np.ndarray) – The logarithm of a transformation matrix.

Returns:

The corresponding twist vector.

Return type:

np.ndarray

ManipulaPy.utils.skew_symmetric_to_vector(skew_symmetric)[source]

Convert a skew-symmetric matrix to a vector.

Parameters:

skew_symmetric (np.ndarray) – A 3x3 skew-symmetric matrix.

Returns:

The corresponding vector.

Return type:

np.ndarray

ManipulaPy.utils.se3ToVec(se3_matrix)[source]

Convert an se(3) matrix to a twist vector.

Parameters:

se3_matrix (np.ndarray) – A 4x4 matrix from the se(3) Lie algebra.

Returns:

A 6-dimensional twist vector.

Return type:

np.ndarray

ManipulaPy.utils.TransToRp(T)[source]

Converts a homogeneous transformation matrix into a rotation matrix and position vector.

Parameters:

T (np.ndarray) – A 4x4 transformation matrix.

Returns:

A tuple containing the rotation matrix and position vector.

Return type:

tuple

ManipulaPy.utils.TransInv(T)[source]

Inverts a homogeneous transformation matrix.

Parameters:

T (np.ndarray) – A 4x4 transformation matrix.

Returns:

The inverse of the transformation matrix.

Return type:

np.ndarray

ManipulaPy.utils.MatrixLog6(T)[source]

Compute the matrix logarithm of a given transformation matrix T.

Parameters:

T (np.ndarray) – The transformation matrix of shape (4, 4).

Returns:

The matrix logarithm of T, with shape (4, 4).

Return type:

np.ndarray

ManipulaPy.utils.MatrixExp6(se3mat)[source]

Computes the matrix exponential of a matrix in se(3).

Parameters:

se3mat (np.ndarray) – A 4x4 matrix representing a twist in se(3).

Returns:

The corresponding 4x4 transformation matrix in SE(3).

Return type:

np.ndarray

ManipulaPy.utils.MatrixLog3(R)[source]

Computes the matrix logarithm of a rotation matrix.

Parameters:

R (np.ndarray) – A 3x3 rotation matrix.

Returns:

The matrix logarithm of the rotation matrix.

Return type:

np.ndarray

ManipulaPy.utils.VecToso3(omega)[source]

Converts a 3D angular velocity vector to a skew-symmetric matrix.

Parameters:

omega (array-like) – A 3D angular velocity vector.

Returns:

The corresponding skew-symmetric matrix.

Return type:

np.ndarray

ManipulaPy.utils.VecTose3(V)[source]

Converts a spatial velocity vector to an se(3) matrix.

Parameters:

V (array-like) – A 6D spatial velocity vector.

Returns:

The corresponding 4x4 matrix in se(3).

Return type:

np.ndarray

ManipulaPy.utils.MatrixExp3(so3mat)[source]

Computes the matrix exponential of a matrix in so(3).

Parameters:

so3mat (np.ndarray) – A 3x3 skew-symmetric matrix.

Returns:

The corresponding 3x3 rotation matrix in SO(3).

Return type:

np.ndarray

ManipulaPy.utils.CubicTimeScaling(Tf, t)[source]

Compute the cubic time scaling factor.

Parameters:

  • Tf (float) – The total time of the motion.

  • t (float) – The current time.

Returns:

The cubic time scaling factor.

Return type:

float

ManipulaPy.utils.QuinticTimeScaling(Tf, t)[source]

Compute the quintic time scaling factor.

Parameters:

  • Tf (float) – The total time of the motion.

  • t (float) – The current time.

Returns:

The quintic time scaling factor.

Return type:

float

ManipulaPy.utils.rotation_matrix_to_euler_angles(R)[source]

Convert a rotation matrix to Euler angles (roll, pitch, yaw).

Parameters:

R (numpy.ndarray) – A 3x3 rotation matrix.

Returns:

A 3-element array representing the Euler angles (roll, pitch, yaw).

Return type:

numpy.ndarray

ManipulaPy.utils.euler_to_rotation_matrix(euler_deg)[source]

Convert Euler angles (roll_deg, pitch_deg, yaw_deg) in degrees to a 3x3 rotation matrix. ZYX convention is typical in robotics: yaw -> pitch -> roll, but adapt as needed.

Parameters:

euler_deg (array-like) – [roll_deg, pitch_deg, yaw_deg]

Returns:

A 3x3 rotation matrix (float64 by default).

Return type:

np.ndarray