Implementation:ARISE Initiative Robosuite TransformUtils
| Knowledge Sources | |
|---|---|
| Domains | Robotics, 3D Geometry |
| Last Updated | 2026-02-15 07:00 GMT |
Overview
The transform_utils module provides a comprehensive set of functions for 3D spatial transformations including quaternion operations, rotation matrix conversions, Euler angle conversions, pose manipulation, and velocity/force frame transformations used throughout robosuite.
Description
This module is the mathematical backbone of robosuite's spatial reasoning. It adopts the (x, y, z, w) quaternion convention throughout and provides over 30 functions organized into several categories.
Quaternion operations include quat_multiply, quat_conjugate, quat_inverse, quat_distance, quat_slerp (spherical linear interpolation), random_quat, and convert_quat (for switching between xyzw and wxyz conventions). Many of these use the @jit_decorator for optional Numba acceleration.
Conversion functions handle transformations between representations: mat2quat and quat2mat convert between rotation matrices and quaternions using eigenvalue decomposition; euler2mat and mat2euler convert between Euler angles and rotation matrices supporting all 24 axis sequence conventions; quat2axisangle and axisangle2quat convert between quaternion and axis-angle representations; mat2pose and pose2mat convert between (position, quaternion) tuples and 4x4 homogeneous matrices.
Pose operations include pose_in_A_to_pose_in_B for transforming poses between coordinate frames, pose_inv for computing the inverse of a homogeneous transformation matrix, make_pose for constructing a 4x4 matrix from translation and rotation components, and rotation_matrix for creating rotation matrices about arbitrary axes.
Velocity and force transformations include vel_in_A_to_vel_in_B and force_in_A_to_force_in_B which handle proper adjoint transformations including the skew-symmetric cross-product terms needed for spatial velocity and wrench transformations.
Utility functions include unit_vector, clip_translation, clip_rotation, get_orientation_error, get_pose_error, random_axis_angle, and rotate_2d_point.
Usage
Use this module whenever you need to convert between rotation representations, transform poses between frames, compute orientation errors for controllers, or perform spatial velocity/force transformations. It is used pervasively by robot controllers, IK solvers, camera utilities, and task reward computations.
Code Reference
Source Location
- Repository: ARISE_Initiative_Robosuite
- File: robosuite/utils/transform_utils.py
Signature
def convert_quat(q, to="xyzw") -> np.array: ...
def quat_multiply(quaternion1, quaternion0) -> np.array: ...
def quat_conjugate(quaternion) -> np.array: ...
def quat_inverse(quaternion) -> np.array: ...
def quat_slerp(quat0, quat1, fraction, shortestpath=True) -> np.array: ...
def mat2quat(rmat) -> np.array: ... # @jit_decorator
def quat2mat(quaternion) -> np.array: ... # @jit_decorator
def euler2mat(euler) -> np.array: ...
def mat2euler(rmat, axes="sxyz") -> np.array: ...
def quat2axisangle(quat) -> np.array: ...
def axisangle2quat(vec) -> np.array: ...
def mat2pose(hmat) -> tuple: ...
def pose2mat(pose) -> np.array: ...
def pose_in_A_to_pose_in_B(pose_A, pose_A_in_B) -> np.array: ...
def pose_inv(pose) -> np.array: ...
def make_pose(translation, rotation) -> np.array: ...
def get_orientation_error(target_orn, current_orn) -> np.array: ...
def get_pose_error(target_pose, current_pose) -> np.array: ...
def vel_in_A_to_vel_in_B(vel_A, ang_vel_A, pose_A_in_B) -> tuple: ...
def force_in_A_to_force_in_B(force_A, torque_A, pose_A_in_B) -> tuple: ...
def clip_translation(dpos, limit) -> tuple: ...
def clip_rotation(quat, limit) -> tuple: ...
Import
import robosuite.utils.transform_utils as T
# Or import specific functions:
from robosuite.utils.transform_utils import (
quat_multiply,
quat2mat,
mat2quat,
euler2mat,
mat2euler,
axisangle2quat,
quat2axisangle,
pose_inv,
make_pose,
get_orientation_error,
)
I/O Contract
Inputs
| Name | Type | Required | Description |
|---|---|---|---|
| q (convert_quat) | np.array (4,) | Yes | Input quaternion to convert between (xyzw) and (wxyz) |
| quaternion1, quaternion0 | np.array (4,) | Yes | Quaternions in (x,y,z,w) format for multiplication |
| rmat (mat2quat) | np.array (3,3) | Yes | 3x3 rotation matrix to convert to quaternion |
| quaternion (quat2mat) | np.array (4,) | Yes | (x,y,z,w) quaternion to convert to rotation matrix |
| euler (euler2mat) | np.array (3,) | Yes | (roll, pitch, yaw) Euler angles in radians |
| hmat (mat2pose) | np.array (4,4) | Yes | 4x4 homogeneous transformation matrix |
| pose (pose2mat) | tuple(np.array, np.array) | Yes | (position, quaternion) tuple |
| target_orn, current_orn | np.array (4,) | Yes | (x,y,z,w) quaternions for error computation |
| dpos (clip_translation) | np.array | Yes | Translation delta to clip |
| limit | float | Yes | Maximum magnitude for clipping |
Outputs
| Name | Type | Description |
|---|---|---|
| quat_multiply | np.array (4,) float32 | (x,y,z,w) product quaternion |
| mat2quat | np.array (4,) float32 | (x,y,z,w) quaternion from rotation matrix |
| quat2mat | np.array (3,3) | 3x3 rotation matrix from quaternion |
| euler2mat | np.array (3,3) float64 | Rotation matrix from Euler angles |
| mat2euler | np.array (3,) float32 | (r,p,y) Euler angles in radians |
| mat2pose | tuple(np.array, np.array) | (position (3,), quaternion (4,)) |
| pose_inv | np.array (4,4) | Inverted homogeneous matrix |
| get_orientation_error | np.array (3,) | 3-DOF orientation error vector |
| clip_translation | tuple(np.array, bool) | Clipped translation and whether clipping occurred |
Usage Examples
import numpy as np
import robosuite.utils.transform_utils as T
# Convert between quaternion conventions
quat_xyzw = np.array([0.0, 0.0, 0.707, 0.707])
quat_wxyz = T.convert_quat(quat_xyzw, to="wxyz")
# Multiply two quaternions
q_result = T.quat_multiply(quat_xyzw, quat_xyzw)
# Convert rotation matrix to quaternion and back
rot_mat = T.quat2mat(quat_xyzw)
quat_back = T.mat2quat(rot_mat)
# Euler angle conversions
euler = T.mat2euler(rot_mat)
mat_back = T.euler2mat(euler)
# Axis-angle conversions
axis_angle = T.quat2axisangle(quat_xyzw)
quat_from_aa = T.axisangle2quat(axis_angle)
# Build and invert a homogeneous pose matrix
pose = T.make_pose(translation=np.array([1.0, 0.0, 0.5]), rotation=rot_mat)
pose_inverse = T.pose_inv(pose)
# Compute orientation error for a controller
target_quat = np.array([0.0, 0.0, 0.0, 1.0])
current_quat = np.array([0.0, 0.0, 0.1, 0.995])
error = T.get_orientation_error(target_quat, current_quat)
# Spherical linear interpolation
q_interp = T.quat_slerp(quat_xyzw, target_quat, fraction=0.5)
# Clip a translation delta
dpos = np.array([0.5, 0.5, 0.5])
clipped_dpos, was_clipped = T.clip_translation(dpos, limit=0.1)