Skip to content

Working with Transforms

EmbodiK uses Pinocchio's SE3 class for representing 3D rigid body transformations (combining rotation and translation). This guide covers basic transform operations you'll need when working with EmbodiK.

Quick Reference

For users familiar with spatialmath-python, here are the most common equivalents:

import embodik
import pinocchio as pin
import numpy as np

# Create SE3 from R and t (equivalent to SE3.Rt)
R = np.eye(3)
t = np.array([1, 2, 3])
T = embodik.Rt(R=R, t=t)  # Equivalent to spatialmath's SE3.Rt(R, t)

# Access translation (equivalent to R.t)
t = T.translation  # [1, 2, 3]

# Rotation matrix to quaternion (equivalent to r2q)
R = np.eye(3)
q = embodik.r2q(R)  # [1, 0, 0, 0] (wxyz format)

# Quaternion to rotation matrix (equivalent to q2r)
q = np.array([1, 0, 0, 0])
R = embodik.q2r(q)  # 3x3 identity matrix

Overview

In EmbodiK, transforms are represented using Pinocchio's SE3 class, which combines: - Rotation: A 3×3 rotation matrix (or quaternion) - Translation: A 3D vector

This is similar to spatialmath-python's SE3, but uses Pinocchio's native implementation for better performance and integration.

Creating Transforms

Identity Transform

import pinocchio as pin
import numpy as np

# Create an identity transform (no rotation, no translation)
T = pin.SE3.Identity()

From Rotation Matrix and Translation

# Method 1: Using Pinocchio directly
R = np.eye(3)  # Identity rotation
t = np.array([1.0, 2.0, 3.0])
T = pin.SE3(R, t)

# Method 2: Using embodik.Rt() (spatialmath-python compatible)
import embodik
T = embodik.Rt(R=R, t=t)  # Equivalent to spatialmath's SE3.Rt(R, t)
T = embodik.Rt(t=t)  # Identity rotation, translation only
T = embodik.Rt(R=R)  # Rotation only, zero translation

From 4×4 Homogeneous Matrix

# Create 4x4 homogeneous matrix
H = np.eye(4)
H[:3, 3] = [1.0, 2.0, 3.0]  # Translation
H[:3, :3] = np.eye(3)        # Rotation

# Convert to SE3
T = pin.SE3(H[:3, :3], H[:3, 3])

Accessing Transform Components

Translation

# Get translation vector
translation = T.translation
print(translation)  # [1.0, 2.0, 3.0]

# Set translation
T.translation = np.array([4.0, 5.0, 6.0])

Rotation

# Get rotation matrix (3x3)
rotation = T.rotation
print(rotation.shape)  # (3, 3)

# Set rotation matrix
T.rotation = np.eye(3)

Homogeneous Matrix

# Get 4x4 homogeneous matrix
H = T.homogeneous()
print(H.shape)  # (4, 4)

Basic Operations

Compose Transforms

Compose two transforms using the multiplication operator (*):

T1 = pin.SE3(np.eye(3), np.array([1.0, 0.0, 0.0]))
T2 = pin.SE3(np.eye(3), np.array([0.0, 1.0, 0.0]))

# Compose: T_result = T1 * T2 (apply T2, then T1)
T_result = T1 * T2
print(T_result.translation)  # [1.0, 1.0, 0.0]

Inverse Transform

# Compute inverse transform
T_inv = T.inverse()

# Verify: T * T_inv should be identity
identity_check = T * T_inv
print(identity_check.translation)  # [0.0, 0.0, 0.0]

Transform Points

# Transform a 3D point
point = np.array([1.0, 2.0, 3.0])

# Transform point: p' = T * p
# For points, we need to use homogeneous coordinates
point_homogeneous = np.append(point, 1.0)
transformed_homogeneous = T.homogeneous() @ point_homogeneous
transformed_point = transformed_homogeneous[:3]

Working with Rotations

EmbodiK provides a native Rotation helper that replaces scipy.spatial.transform.Rotation for common use cases. It uses Pinocchio-backed operations and has no SciPy dependency.

Canonical API (primary):

from embodik import Rotation
import numpy as np

# Constructors
R = Rotation.from_matrix(np.eye(3))       # From 3x3 matrix
R = Rotation.from_quat([0, 0, 0, 1])     # From [x,y,z,w] quaternion
R = Rotation.from_rotvec([0, 0, np.pi/2]) # From axis-angle
R = Rotation.from_euler('xyz', [0, 0, np.pi/2])  # From Euler (xyz, x, y, z)
R = Rotation.identity()

# Outputs
mat = R.as_matrix()   # 3x3 rotation matrix
quat = R.as_quat()    # [x, y, z, w]
rotvec = R.as_rotvec() # Axis-angle

# Operations
R_inv = R.inv()
R_composed = R1 * R2
v_rotated = R.apply([1, 0, 0])

Spatialmath-style shorthands (convenience aliases):

from embodik import SO3  # Alias for Rotation

# Single-axis rotations
R = SO3.Rx(np.pi/2)
R = SO3.Ry(np.pi/2)
R = SO3.Rz(np.pi/2)

# Roll-pitch-yaw
R = SO3.RPY([roll, pitch, yaw], order='xyz')

# Angle-axis
R = SO3.AngVec(theta, axis)
R = SO3.EulerVec(omega)  # Same as from_rotvec

SE3 property aliases (spatialmath-style):

from embodik import Rt, SE3

T = Rt(R=np.eye(3), t=[1, 2, 3])  # Module-level helper
T = SE3.Rt(np.eye(3), [1, 2, 3])  # Classmethod (same result)
# T.R  -> rotation matrix (alias for .rotation)
# T.t  -> translation (alias for .translation)
# T.A  -> 4x4 homogeneous matrix (alias for .homogeneous())

Creating Rotation Matrices (Alternative: SciPy)

For basic rotations, you can also use NumPy or SciPy:

import numpy as np
from scipy.spatial.transform import Rotation as R

# Create rotation from roll-pitch-yaw (RPY) angles
roll, pitch, yaw = np.pi/2, 0, 0  # 90 degrees roll
R_rpy = R.from_euler('xyz', [roll, pitch, yaw]).as_matrix()

# Create rotation about X-axis (90 degrees)
R_x = R.from_euler('x', np.pi/2).as_matrix()

# Create rotation about Y-axis
R_y = R.from_euler('y', np.pi/2).as_matrix()

# Create rotation about Z-axis
R_z = R.from_euler('z', np.pi/2).as_matrix()

Alternatively, create simple rotations manually:

import numpy as np

# Rotation about Z-axis (yaw)
angle = np.pi / 2
R_z = np.array([
    [np.cos(angle), -np.sin(angle), 0],
    [np.sin(angle),  np.cos(angle), 0],
    [0,              0,             1]
])

Using Quaternions

from pinocchio import Quaternion

# Create quaternion from rotation matrix
R = np.eye(3)
q = Quaternion(R)

# Access quaternion components (w, x, y, z)
w, x, y, z = q.w, q.x, q.y, q.z

# Create quaternion from components (w, x, y, z)
q2 = Quaternion(1.0, 0.0, 0.0, 0.0)  # Identity quaternion

# Convert quaternion to rotation matrix
R_from_q = q2.matrix()

Quaternion Order Conventions

Critical: Different libraries use different quaternion orders. Always specify the order parameter to prevent errors:

  • 'sxyz' or 'wxyz' (default): [w, x, y, z] — Used by viser, Pinocchio, EmbodiK, most robotics libraries
  • 'xyzs' or 'xyzw': [x, y, z, w] — Used by some libraries (e.g., SciPy in some contexts)

Best Practice: Always explicitly specify the order parameter when converting quaternions:

from embodik import r2q, q2r

# If your data source uses wxyz format (default)
R = np.eye(3)
q = r2q(R, order='sxyz')  # Explicit: [w, x, y, z]

# If your data source uses xyzw format
q_xyzw = r2q(R, order='xyzs')  # Explicit: [x, y, z, w]

# Converting back - MUST match the order!
R1 = q2r(q, order='sxyz')  # Correct - matches r2q order
R2 = q2r(q_xyzw, order='xyzs')  # Correct - matches r2q order

# Common mistake - wrong order produces incorrect rotation!
R_wrong = q2r(q, order='xyzs')  # WRONG - will produce incorrect result

All quaternion functions support the order parameter:

  • r2q(R, order='sxyz') — rotation matrix to quaternion
  • q2r(q, order='sxyz') — quaternion to rotation matrix

Tip: When integrating with other libraries, check their quaternion format documentation and always specify the order parameter explicitly to prevent convention mismatches.

Converting Between Representations

from pinocchio import Quaternion
from embodik import r2q, q2r

# Rotation matrix to quaternion (recommended: use r2q with explicit order)
R = np.eye(3)
q_wxyz = r2q(R, order='sxyz')  # Returns [w, x, y, z] - explicit order prevents errors
q_xyzw = r2q(R, order='xyzs')  # Returns [x, y, z, w] - alternative format

# Quaternion to rotation matrix (recommended: use q2r with explicit order)
q = np.array([1, 0, 0, 0])  # wxyz format
R = q2r(q, order='sxyz')  # Explicit order prevents errors

# Using Pinocchio's Quaternion directly
q_pin = Quaternion(R)
wxyz = np.array([q_pin.w, q_pin.x, q_pin.y, q_pin.z])  # For viser format
R_from_q = q_pin.matrix()

# Rotation matrix to axis-angle (using log3)
axis_angle = pin.log3(R)  # Returns 3D vector (axis * angle)

Common Transform Patterns

Pure Translation

# Create transform with only translation (no rotation)
translation = np.array([1.0, 2.0, 3.0])
T = pin.SE3(np.eye(3), translation)

Pure Rotation

# Create transform with only rotation (no translation)
R = np.eye(3)  # Your rotation matrix
T = pin.SE3(R, np.zeros(3))

Rotation About an Axis

from scipy.spatial.transform import Rotation as R

# Rotate 90 degrees about Z-axis
angle = np.pi / 2
R_z = R.from_euler('z', angle).as_matrix()
T = pin.SE3(R_z, np.zeros(3))

Combining Translation and Rotation

from scipy.spatial.transform import Rotation as R

# First rotate, then translate
R_rot = R.from_euler('z', np.pi/4).as_matrix()  # 45 deg about Z
t = np.array([1.0, 0.0, 0.0])
T = pin.SE3(R_rot, t)

Using Transforms with EmbodiK

Setting Target Poses

import embodik
import pinocchio as pin
import numpy as np

# Create robot model
model = embodik.RobotModel.from_urdf("robot.urdf")

# Create target pose using SE3
target_pose_se3 = pin.SE3(np.eye(3), np.array([0.5, 0.2, 0.3]))

# Convert to 4x4 matrix for EmbodiK
target_pose_matrix = target_pose_se3.homogeneous()

# Use in frame task
frame_task = embodik.FrameTask(
    frame_id="end_effector",
    target_pose=target_pose_matrix
)

Getting Frame Poses

# Get current frame pose (returns SE3)
pose_se3 = model.get_frame_pose("end_effector")

# Access components
translation = pose_se3.translation
rotation = pose_se3.rotation

# Convert to homogeneous matrix if needed
pose_matrix = pose_se3.homogeneous()

Advanced Operations

Exponential Coordinates (Axis-Angle)

Pinocchio provides functions for working with exponential coordinates (axis-angle representation):

import pinocchio as pin
import numpy as np

# Convert rotation matrix to axis-angle (exponential coordinates)
R = np.eye(3)  # Your rotation matrix
axis_angle = pin.log3(R)  # Returns 3D vector: axis * angle

# Convert axis-angle back to rotation matrix
R_from_axis_angle = pin.exp3(axis_angle)

# For SE3 transforms (6D twist)
T = pin.SE3(np.eye(3), np.array([1.0, 2.0, 3.0]))
twist = pin.log6(T.homogeneous())  # 6D twist vector
T_from_twist = pin.exp6(twist)  # Back to SE3

Computing Pose Errors

For IK tasks, you often need to compute the error between two poses:

from embodik.utils import compute_pose_error

# Current and goal poses
pose_current = pin.SE3(R1, t1)
pose_goal = pin.SE3(R2, t2)

# Compute 6D error vector [translation_error, rotation_error]
error = compute_pose_error(pose_current, pose_goal)
# error[:3] is translation error
# error[3:] is rotation error (in axis-angle form)

Utility Functions

EmbodiK provides utility functions for common transform operations, including spatialmath-python compatible functions:

Creating SE3 Transforms

from embodik import Rt
import numpy as np

# Create SE3 from rotation and translation (equivalent to spatialmath's SE3.Rt)
R = np.eye(3)
t = np.array([1.0, 2.0, 3.0])
T = Rt(R=R, t=t)

# Convenience: identity rotation or zero translation
T = Rt(t=t)  # Identity rotation, translation only
T = Rt(R=R)  # Rotation only, zero translation
T = Rt()  # Identity transform

Quaternion Conversions (spatialmath-python compatible)

from embodik import r2q, q2r
import numpy as np

# Convert rotation matrix to quaternion (default: wxyz format)
R = np.eye(3)
q = r2q(R)  # Returns [1, 0, 0, 0] (wxyz format, default)
q_xyzw = r2q(R, order='xyzs')  # Returns [0, 0, 0, 1] (xyzw format)

# Convert quaternion to rotation matrix
q = np.array([1, 0, 0, 0])  # Identity quaternion (wxyz)
R = q2r(q)  # Returns 3x3 identity matrix
R = q2r(q, order='sxyz')  # Same, explicit order

# With xyzw format
q_xyzw = np.array([0, 0, 0, 1])  # Identity quaternion (xyzw)
R = q2r(q_xyzw, order='xyzs')  # Returns 3x3 identity matrix

Viser-Specific Conversions

For viser compatibility, use r2q and q2r directly:

from embodik import r2q, q2r
import numpy as np

# Convert rotation matrix to viser quaternion format (wxyz)
R = np.eye(3)
wxyz = r2q(R)  # Returns [w, x, y, z] array
# If you need a tuple: tuple(r2q(R))

# Convert viser quaternion to rotation matrix
wxyz = np.array([1, 0, 0, 0])  # [w, x, y, z] format
R = q2r(wxyz)  # Returns 3x3 rotation matrix

# With explicit order parameter (recommended)
wxyz = r2q(R, order='sxyz')  # Explicit: [w, x, y, z]
R = q2r(wxyz, order='sxyz')  # Explicit order prevents errors

Accessing Translation from SE3

import pinocchio as pin

# Create SE3 transform
T = pin.SE3(np.eye(3), np.array([1.0, 2.0, 3.0]))

# Access translation (equivalent to spatialmath-python's R.t)
translation = T.translation  # Returns [1.0, 2.0, 3.0]

# Access rotation
rotation = T.rotation  # Returns 3x3 rotation matrix

Computing Pose Errors

from embodik.utils import compute_pose_error

# Compute pose error between two poses
pose_current = pin.SE3(R1, t1)
pose_goal = pin.SE3(R2, t2)
error = compute_pose_error(pose_current, pose_goal)  # 6D error vector
# error[:3] is translation error
# error[3:] is rotation error (in axis-angle form)

Reference

For more advanced transform operations, see:

  • Pinocchio Documentation: Pinocchio Python API
  • SE3 Class: Pinocchio's SE3 class provides additional methods for Lie group operations
  • Quaternion Operations: Pinocchio's Quaternion class for quaternion-based rotations
  • Rotation Utilities: Functions like pin.log3(), pin.exp3() for exponential coordinates
  • embodiK.Rotation: Native Rotation and SO3 helpers (no SciPy); see "embodiK.Rotation (Recommended)" above
  • SciPy Rotations: scipy.spatial.transform.Rotation for creating rotation matrices from various representations

Note: r2q and q2r use native embodiK conversion and do not depend on SciPy.

Comparison with spatialmath-python

If you're familiar with spatialmath-python, here's a quick comparison:

spatialmath-python Pinocchio (EmbodiK)
SE3(R, t) pin.SE3(R, t) or embodik.Rt(R=R, t=t)
SE3.Rt(R, t) embodik.Rt(R=R, t=t)
T1 * T2 T1 * T2
T.inv() T.inverse()
T.t T.translation or T.t
T.R T.rotation or T.R
T.A T.homogeneous() or T.A
r2q(R) embodik.r2q(R)
q2r(q) embodik.q2r(q)

Most Common Functions

The most frequently used functions from spatialmath-python are available in EmbodiK:

import embodik
import pinocchio as pin
import numpy as np

# 1. Create SE3 from R and t (equivalent to SE3.Rt(R, t))
R = np.eye(3)
t = np.array([1, 2, 3])
T = embodik.Rt(R=R, t=t)  # Equivalent to spatialmath's SE3.Rt(R, t)

# 2. Accessing translation (equivalent to R.t)
t = T.translation  # [1, 2, 3] - equivalent to spatialmath's R.t

# 3. Rotation matrix to quaternion (equivalent to r2q)
R = np.eye(3)
q = embodik.r2q(R)  # [1, 0, 0, 0] - equivalent to spatialmath's r2q(R)

# 4. Quaternion to rotation matrix (equivalent to q2r)
q = np.array([1, 0, 0, 0])  # wxyz format
R = embodik.q2r(q)  # 3x3 identity - equivalent to spatialmath's q2r(q)

# With explicit order parameter (recommended to prevent errors)
q = np.array([1, 0, 0, 0])  # wxyz format
R = embodik.q2r(q, order='sxyz')  # Explicit order specification

The main difference is that Pinocchio's SE3 is optimized for robotics applications and integrates seamlessly with EmbodiK's kinematics computations.