Utilities

Utility functions for working with EmbodiK. Includes pose error computation, quaternion conversions, and SE3 creation.

Pose Error

compute_pose_error

compute_pose_error(pose_current, pose_goal)

Compute 6D pose error (goal - current) using native :func:log3.

Optimized to work directly with SE3 objects without wrapping when possible. Extracts rotation/translation once to minimize Python binding overhead.

Source code in python/embodik/utils.py

def compute_pose_error(pose_current: PoseData | object, pose_goal: PoseData | object) -> np.ndarray:
    """Compute 6D pose error (goal - current) using native :func:`log3`.

    Optimized to work directly with SE3 objects without wrapping when possible.
    Extracts rotation/translation once to minimize Python binding overhead.
    """
    # Fast path for native SE3 objects (most common case)
    if isinstance(pose_current, _native.SE3) and isinstance(pose_goal, _native.SE3):
        # Extract rotation and translation once to avoid repeated attribute access overhead
        t_current = pose_current.translation
        t_goal = pose_goal.translation
        R_current = pose_current.rotation
        R_goal = pose_goal.rotation

        error = np.empty(6, dtype=float)
        error[:3] = t_goal - t_current
        error[3:] = _native.log3(R_goal @ R_current.T)
        return error

    # Fallback to PoseData.wrap for other types
    current = PoseData.wrap(pose_current)
    goal = PoseData.wrap(pose_goal)
    error = np.empty(6, dtype=float)
    error[:3] = goal.t - current.t
    error[3:] = _native.log3(goal.R @ current.R.T)
    return error

get_pose_error_vector

get_pose_error_vector(pose_current, pose_goal)

Pose-error helper function.

Uses Pinocchio's log3 for rotation error computation.

Supports both PoseData objects and Pinocchio SE3 objects.

Source code in python/embodik/utils.py

def get_pose_error_vector(pose_current, pose_goal):
    """Pose-error helper function.

    Uses Pinocchio's log3 for rotation error computation.

    Supports both PoseData objects and Pinocchio SE3 objects.
    """

    pose_error = np.zeros(6, dtype=np.float64)

    # Handle both PoseData (has .t) and Pinocchio SE3 (has .translation)
    if hasattr(pose_goal, "translation"):
        # Pinocchio SE3 object
        t_goal = pose_goal.translation
        t_current = pose_current.translation
        R_goal = pose_goal.rotation
        R_current = pose_current.rotation
    elif hasattr(pose_goal, "t"):
        # PoseData object
        t_goal = pose_goal.t
        t_current = pose_current.t
        R_goal = pose_goal.R
        R_current = pose_current.R
    else:
        raise TypeError(f"Unsupported pose type: {type(pose_goal)}")

    pose_error[:3] = t_goal - t_current
    # Use native log3 for rotation error (equivalent to SO3.log(twist=True))
    # Compute relative rotation: R_error = R_goal * R_current^T
    R_error = R_goal @ R_current.T
    pose_error[3:] = _native.log3(R_error)
    return pose_error

Quaternion Conversions (spatialmath-python compatible)

r2q

r2q(rotation, order='sxyz')

Convert rotation matrix to quaternion (spatialmath-python compatible).

Uses native embodiK/Pinocchio conversion (no SciPy dependency).

Parameters:

Name	Type	Description	Default
rotation	ndarray	3x3 rotation matrix	required
order	str	Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])	'sxyz'

Returns:

Type	Description
ndarray	Quaternion as numpy array:
ndarray	If order='sxyz': [w, x, y, z] (scalar first, default)
ndarray	If order='xyzs': [x, y, z, w] (scalar last)

Examples:

>>> R = np.eye(3)
>>> q = r2q(R)  # Returns [1, 0, 0, 0] (wxyz format)
>>> q = r2q(R, order='xyzs')  # Returns [0, 0, 0, 1] (xyzw format)

Source code in python/embodik/utils.py

def r2q(rotation: np.ndarray, order: str = "sxyz") -> np.ndarray:
    """
    Convert rotation matrix to quaternion (spatialmath-python compatible).

    Uses native embodiK/Pinocchio conversion (no SciPy dependency).

    Args:
        rotation: 3x3 rotation matrix
        order: Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])

    Returns:
        Quaternion as numpy array:
        - If order='sxyz': [w, x, y, z] (scalar first, default)
        - If order='xyzs': [x, y, z, w] (scalar last)

    Examples:
        >>> R = np.eye(3)
        >>> q = r2q(R)  # Returns [1, 0, 0, 0] (wxyz format)
        >>> q = r2q(R, order='xyzs')  # Returns [0, 0, 0, 1] (xyzw format)
    """
    rotation = np.asarray(rotation, dtype=float)
    if rotation.shape != (3, 3):
        raise ValueError(f"Expected 3x3 rotation matrix, got shape {rotation.shape}")

    if order == "sxyz" or order == "wxyz":
        # Scalar first: [w, x, y, z] via native wxyz converter
        quat = _native.matrix_to_quaternion_wxyz(rotation)
        return np.array(quat, dtype=float)
    elif order == "xyzs" or order == "xyzw":
        # Scalar last: [x, y, z, w] via native xyzw converter
        quat = _native.matrix_to_quaternion_xyzw(rotation)
        return np.array(quat, dtype=float)
    else:
        raise ValueError(f"Unknown quaternion order: {order}. Use 'sxyz' or 'xyzs'")

q2r

q2r(quaternion, order='sxyz')

Convert quaternion to rotation matrix (spatialmath-python compatible).

Uses native embodiK/Pinocchio conversion (no SciPy dependency).

Parameters:

Name	Type	Description	Default
quaternion	ndarray	Quaternion as array	required
order	str	Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])	'sxyz'

Returns:

Type	Description
ndarray	3x3 rotation matrix

Examples:

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

Source code in python/embodik/utils.py

def q2r(quaternion: np.ndarray, order: str = "sxyz") -> np.ndarray:
    """
    Convert quaternion to rotation matrix (spatialmath-python compatible).

    Uses native embodiK/Pinocchio conversion (no SciPy dependency).

    Args:
        quaternion: Quaternion as array
        order: Quaternion order, 'sxyz' (default, [w,x,y,z]) or 'xyzs' ([x,y,z,w])

    Returns:
        3x3 rotation matrix

    Examples:
        >>> q = np.array([1, 0, 0, 0])  # Identity quaternion (wxyz)
        >>> R = q2r(q)  # Returns 3x3 identity matrix
        >>> q = np.array([0, 0, 0, 1])  # Identity quaternion (xyzw)
        >>> R = q2r(q, order='xyzs')  # Returns 3x3 identity matrix
    """
    quaternion = np.asarray(quaternion, dtype=float)
    if quaternion.shape != (4,):
        raise ValueError(f"Expected 4-element quaternion, got shape {quaternion.shape}")

    # Normalize quaternion
    norm = float(np.linalg.norm(quaternion))
    if norm < 1e-12:
        return np.eye(3, dtype=float)
    qn = quaternion / norm

    if order == "sxyz" or order == "wxyz":
        # Scalar first: [w, x, y, z]
        R = _native.quaternion_wxyz_to_matrix(
            float(qn[0]), float(qn[1]), float(qn[2]), float(qn[3])
        )
    elif order == "xyzs" or order == "xyzw":
        # Scalar last: [x, y, z, w]
        R = _native.quaternion_xyzw_to_matrix(
            float(qn[0]), float(qn[1]), float(qn[2]), float(qn[3])
        )
    else:
        raise ValueError(f"Unknown quaternion order: {order}. Use 'sxyz' or 'xyzs'")

    return np.array(R)

SE3 Creation

Rt

Rt(R=None, t=None)

Create SE3 transform from rotation matrix and translation (spatialmath-python compatible).

Equivalent to spatialmath-python's SE3.Rt(R, t).

Parameters:

Name	Type	Description	Default
R	Optional[ndarray]	3x3 rotation matrix (default: identity)	None
t	Optional[ndarray]	3D translation vector (default: zero)	None

Returns:

Type	Description
Any	SE3 transform

Examples:

>>> R = np.eye(3)
>>> t = np.array([1, 2, 3])
>>> T = Rt(R=R, t=t)  # Create SE3 from R and t
>>> T = Rt(t=t)  # Create SE3 with identity rotation
>>> T = Rt(R=R)  # Create SE3 with zero translation
>>> T = Rt()  # Create identity transform

Source code in python/embodik/utils.py

def Rt(R: Optional[np.ndarray] = None, t: Optional[np.ndarray] = None) -> Any:
    """
    Create SE3 transform from rotation matrix and translation (spatialmath-python compatible).

    Equivalent to spatialmath-python's SE3.Rt(R, t).

    Args:
        R: 3x3 rotation matrix (default: identity)
        t: 3D translation vector (default: zero)

    Returns:
        SE3 transform

    Examples:
        >>> R = np.eye(3)
        >>> t = np.array([1, 2, 3])
        >>> T = Rt(R=R, t=t)  # Create SE3 from R and t
        >>> T = Rt(t=t)  # Create SE3 with identity rotation
        >>> T = Rt(R=R)  # Create SE3 with zero translation
        >>> T = Rt()  # Create identity transform
    """
    if R is None:
        R = np.eye(3)
    if t is None:
        t = np.zeros(3)

    R = np.asarray(R, dtype=float)
    t = np.asarray(t, dtype=float)

    if R.shape != (3, 3):
        raise ValueError(f"Expected 3x3 rotation matrix, got shape {R.shape}")
    if t.shape != (3,):
        raise ValueError(f"Expected 3D translation vector, got shape {t.shape}")

    return _native.SE3(R, t)

Example

import embodik
import numpy as np

# Compute pose error between two SE3 transforms
pose_current = embodik.Rt(R=np.eye(3), t=[0, 0, 0])
pose_target = embodik.Rt(R=np.eye(3), t=[0.1, 0.2, 0.3])
error = embodik.compute_pose_error(pose_current, pose_target)
# error[:3] = translation error, error[3:] = rotation error (axis-angle)

# Quaternion conversions (native, no SciPy)
R = np.eye(3)
q_wxyz = embodik.r2q(R, order='sxyz')  # [1, 0, 0, 0]
q_xyzw = embodik.r2q(R, order='xyzs')  # [0, 0, 0, 1]
R_back = embodik.q2r(q_wxyz, order='sxyz')

# Create SE3
T = embodik.Rt(R=np.eye(3), t=[1, 2, 3])