Coordinate Transformations

Overview

Coordinate transformations are fundamental to robotics, enabling representation of positions and orientations in different reference frames.

Rotation Representations

Rotation Matrix (3x3)

Orthogonal matrix \(R\) where \(R^T R = I\) and \(\det(R) = 1\)

Interactive visualization — Watch how a 2D rotation matrix rotates a vector:

Properties: - 9 parameters, 6 constraints → 3 DOF - Intuitive for composition - Memory intensive for transmission

Rotation Matrix Construction

2D Rotation

Mathematical Formulation:

A 2D rotation matrix \(R_z(\theta)\) rotates a vector in the XY plane by angle \(\theta\):

\[R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}\]

Code Implementation:

import numpy as np


def rot2d(theta: float) -> np.ndarray:
    """
    Construct a 2D rotation matrix for rotation by angle theta.

    Args:
        theta: Rotation angle in radians

    Returns:
        2x2 rotation matrix
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s],
                     [s,  c]])

Code Breakdown: - Line 9-10: Compute cosine and sine of theta once for efficiency - Line 11-12: Construct the 2x2 rotation matrix using the standard formula

Usage Example:

# Rotate a vector by 45 degrees
theta = np.pi / 4
R = rot2d(theta)

# Apply rotation to vector [1, 0]
v = np.array([1.0, 0.0])
v_rotated = R @ v
print(f"Rotated vector: {v_rotated}")  # [0.707, 0.707]

---

3D Rotation Matrices

Mathematical Formulation:

3D rotations about principal axes:

\[R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix}\]

\[R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}\]

\[R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}\]

Code Implementation:

def rot_x(theta: float) -> np.ndarray:
    """
    Construct 3D rotation matrix about the X-axis.

    Args:
        theta: Rotation angle in radians (right-hand rule)

    Returns:
        3x3 rotation matrix
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1,  0, 0],
                     [0,  c, -s],
                     [0,  s,  c]])


def rot_y(theta: float) -> np.ndarray:
    """
    Construct 3D rotation matrix about the Y-axis.

    Args:
        theta: Rotation angle in radians (right-hand rule)

    Returns:
        3x3 rotation matrix
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[ c, 0, s],
                     [ 0, 1, 0],
                     [-s, 0, c]])


def rot_z(theta: float) -> np.ndarray:
    """
    Construct 3D rotation matrix about the Z-axis.

    Args:
        theta: Rotation angle in radians (right-hand rule)

    Returns:
        3x3 rotation matrix
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0],
                     [s,  c, 0],
                     [0,  0, 1]])

Code Breakdown: - Each function follows the same pattern: compute cos/sin once - rot_x: Rotation in the YZ plane (leaves X unchanged) - rot_y: Rotation in the XZ plane (leaves Y unchanged) - rot_z: Rotation in the XY plane (leaves Z unchanged) - Right-hand rule: positive rotation is counterclockwise when looking along the positive axis

Usage Example:

# Compose rotations: rotate 90° about X, then 45° about Z
R_x_90 = rot_x(np.pi / 2)
R_z_45 = rot_z(np.pi / 4)
R_combined = R_z_45 @ R_x_90  # Matrix multiplication (right-to-left)

---

Euler Angles

Sequential rotations about principal axes.

ZYZ Convention

Mathematical Formulation:

The ZYZ Euler angles representation composes three rotations:

\[R = R_z(\alpha) R_y(\beta) R_z(\gamma)\]

Expanding the full matrix:

\[R = \begin{bmatrix} -\sin\alpha\sin\beta\cos\gamma + \cos\alpha\sin\gamma & -\sin\alpha\sin\beta\sin\gamma - \cos\alpha\cos\gamma & \sin\alpha\cos\beta \\ \cos\alpha\sin\beta\cos\gamma + \sin\alpha\sin\gamma & \cos\alpha\sin\beta\sin\gamma - \sin\alpha\cos\gamma & -\cos\alpha\cos\beta \\ -\cos\beta\cos\gamma & \cos\beta\sin\gamma & \sin\beta \end{bmatrix}\]

Gimbal Lock Condition:

When \(\beta = \pm 90°\) (i.e., \(\beta = \pm \frac{\pi}{2}\)), the first and last rotations become about the same axis, losing one degree of freedom. This is called gimbal lock.

Code Implementation:

import numpy as np


def euler_to_rotation(alpha: float, beta: float, gamma: float) -> np.ndarray:
    """
    Convert ZYZ Euler angles to rotation matrix.

    Args:
        alpha: First rotation about Z-axis (radians)
        beta:  Rotation about Y-axis (radians)
        gamma: Final rotation about Z-axis (radians)

    Returns:
        3x3 rotation matrix R = R_z(alpha) @ R_y(beta) @ R_z(gamma)

    Warning:
        Gimbal lock occurs when beta = +-pi/2, causing numerical instability.
    """
    # Check for gimbal lock condition
    if np.isclose(abs(beta), np.pi / 2):
        print("Warning: Gimbal lock! alpha and gamma become coupled.")

    R_z_alpha = rot_z(alpha)
    R_y_beta = rot_y(beta)
    R_z_gamma = rot_z(gamma)

    # Composition: right-to-left application
    return R_z_alpha @ R_y_beta @ R_z_gamma


def rotation_to_euler(R: np.ndarray) -> tuple[float, float, float]:
    """
    Extract ZYZ Euler angles from a rotation matrix.

    Args:
        R: 3x3 rotation matrix

    Returns:
        Tuple (alpha, beta, gamma) in radians

    Raises:
        ValueError: If gimbal lock is detected (cos(beta) ≈ 0)
    """
    # Check gimbal lock: when cos(beta) ≈ 0
    if np.isclose(R[2, 0], 0) and np.isclose(R[0, 2], 0):
        raise ValueError("Gimbal lock detected: beta = +-90°. "
                         "alpha and gamma are not uniquely determined.")

    # Extract angles from rotation matrix
    beta = np.arctan2(np.sqrt(R[0, 2]**2 + R[1, 2]**2), R[2, 2])
    alpha = np.arctan2(R[1, 2], R[0, 2])
    gamma = np.arctan2(R[2, 1], -R[2, 0])

    return alpha, beta, gamma

Code Breakdown: - Lines 17-18: Gimbal lock check warns when beta approaches ±90° - Line 21-23: Individual rotation matrices for each Euler angle - Line 26: Composition order follows the right-to-left convention (gamma → beta → alpha) - Line 43: Gimbal lock detection based on matrix structure - Lines 47-50: Angle extraction using atan2 for numerical stability

Usage Example:

# Convert Euler angles to rotation matrix
alpha, beta, gamma = np.pi/4, np.pi/3, np.pi/6
R = euler_to_rotation(alpha, beta, gamma)

# Round-trip conversion
alpha2, beta2, gamma2 = rotation_to_euler(R)
print(f"Original: {(alpha, beta, gamma)}")
print(f"Recovered: {(alpha2, beta2, gamma2)}")

---

Axis-Angle (Exponential Coordinates)

Mathematical Formulation:

Any rotation can be represented as a rotation of angle \(\theta\) about axis \(\omega\):

\[R = e^{[\omega]_\times \theta}\]

Where \([\omega]_\times\) is the skew-symmetric matrix:

\[[\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}\]

The exponential map (Rodrigues' formula):

\[e^{[\omega]_\times\theta} = I + \sin\theta [\omega]_\times + (1 - \cos\theta)[\omega]_\times^2\]

Code Implementation:

import numpy as np


def skew_symmetric(omega: np.ndarray) -> np.ndarray:
    """
    Convert 3D rotation axis vector to skew-symmetric matrix.

    Args:
        omega: Rotation axis vector [omega_x, omega_y, omega_z] (unit or scaled)

    Returns:
        3x3 skew-symmetric matrix [omega]_x

    The skew-symmetric matrix satisfies: [omega]_x @ v = omega × v
    """
    wx, wy, wz = omega
    return np.array([[ 0, -wz,  wy],
                     [ wz,   0, -wx],
                     [-wy,  wx,   0]])


def exp_map_axis_angle(omega: np.ndarray, theta: float) -> np.ndarray:
    """
    Exponential map: convert axis-angle to rotation matrix.
    Implements Rodrigues' rotation formula.

    Args:
        omega: Rotation axis (unit vector for pure rotation)
        theta: Rotation angle in radians

    Returns:
        3x3 rotation matrix R = exp([omega]_x * theta)

    Note:
        If omega is not a unit vector, theta should be 0 (omega contains angle).
    """
    omega = np.asarray(omega).flatten()
    omega_hat = omega / np.linalg.norm(omega)  # Ensure unit axis

    # Compute skew-symmetric matrix
    omega_x = skew_symmetric(omega_hat)

    # Rodrigues' formula: R = I + sin(θ)K + (1-cos(θ))K²
    R = (np.eye(3)
         + np.sin(theta) * omega_x
         + (1 - np.cos(theta)) * omega_x @ omega_x)

    return R


def log_map_axis_angle(R: np.ndarray) -> tuple[np.ndarray, float]:
    """
    Logarithmic map: convert rotation matrix to axis-angle.

    Args:
        R: 3x3 rotation matrix

    Returns:
        Tuple of (omega, theta) where omega is the unit rotation axis
        and theta is the rotation angle in radians

    Note:
        Returns (omega, 0) for identity rotation (theta = 0).
    """
    # Ensure R is a valid rotation matrix
    R = np.asarray(R)

    # Compute theta from trace: trace(R) = 1 + 2*cos(theta)
    cos_theta = (np.trace(R) - 1) / 2

    # Clamp to handle numerical errors
    cos_theta = np.clip(cos_theta, -1.0, 1.0)
    theta = np.arccos(cos_theta)

    # Handle identity rotation (theta ≈ 0)
    if np.isclose(theta, 0):
        return np.array([1.0, 0.0, 0.0]), 0.0

    # Extract rotation axis from skew-symmetric part
    # R - R^T = 2*sin(theta)*[omega]_x
    omega_x = (R - R.T) / (2 * np.sin(theta))

    # Reconstruct omega from skew-symmetric matrix
    omega = np.array([-omega_x[1, 2], omega_x[0, 2], -omega_x[0, 1]])

    return omega / np.linalg.norm(omega), theta

Code Breakdown: - Lines 17-20: Skew-symmetric matrix construction from 3D vector - Line 30-33: Rodrigues' formula computes rotation matrix from axis-angle - Line 44-48: Normalizes omega to unit vector for consistent behavior - Lines 67-72: Extracts theta from matrix trace: \(\theta = \arccos(\frac{trace(R)-1}{2})\) - Lines 75-78: Handles the identity rotation case to avoid division by zero - Lines 83-86: Reconstructs rotation axis from the skew-symmetric component

Usage Example:

# Create rotation: 90° about Z-axis
omega = np.array([0, 0, 1])
theta = np.pi / 2
R = exp_map_axis_angle(omega, theta)

# Recover axis-angle from rotation matrix
omega_rec, theta_rec = log_map_axis_angle(R)
print(f"Axis: {omega_rec}, Angle: {theta_rec:.4f} rad ({np.degrees(theta_rec):.1f}°)")

---

Quaternions

Mathematical Formulation:

A unit quaternion represents 3D rotation using 4 parameters:

\[q = [w, x, y, z] = [\cos(\theta/2), \sin(\theta/2)\omega_x, \sin(\theta/2)\omega_y, \sin(\theta/2)\omega_z]\]

Where \(\omega = [\omega_x, \omega_y, \omega_z]\) is the unit rotation axis and \(\theta\) is the rotation angle.

Quaternion Multiplication:

\[q_1 \otimes q_2 = \begin{bmatrix} w_1 w_2 - x_1 x_2 - y_1 y_2 - z_1 z_2 \\ w_1 x_2 + x_1 w_2 + y_1 z_2 - z_1 y_2 \\ w_1 y_2 - x_1 z_2 + y_1 w_2 + z_1 x_2 \\ w_1 z_2 + x_1 y_2 - y_1 x_2 + z_1 w_2 \end{bmatrix}\]

Code Implementation:

import numpy as np


class Quaternion:
    """
    Unit quaternion for representing 3D rotations.

    Quaternion q = [w, x, y, z] where:
    - w is the scalar (real) part
    - [x, y, z] is the vector (imaginary) part
    """

    def __init__(self, w: float = 1.0, x: float = 0.0, y: float = 0.0, z: float = 0.0):
        """
        Initialize quaternion.

        Args:
            w: Scalar component (cos(theta/2))
            x, y, z: Vector components (sin(theta/2) * axis)
        """
        self.w, self.x, self.y, self.z = w, x, y, z

    @classmethod
    def from_axis_angle(cls, omega: np.ndarray, theta: float) -> 'Quaternion':
        """
        Create quaternion from axis-angle representation.

        Args:
            omega: Unit rotation axis [wx, wy, wz]
            theta: Rotation angle in radians

        Returns:
            Quaternion instance
        """
        omega = np.asarray(omega) / np.linalg.norm(omega)
        half_angle = theta / 2
        return cls(np.cos(half_angle),
                    *(np.sin(half_angle) * omega))

    @classmethod
    def from_rotation_matrix(cls, R: np.ndarray) -> 'Quaternion':
        """
        Convert rotation matrix to quaternion.
        Uses Shepperd's method for numerical stability.
        """
        R = np.asarray(R)
        trace = np.trace(R)

        if trace > 0:
            s = np.sqrt(trace + 1.0) * 2  # s = 4*w
            w = 0.25 * s
            x = (R[2, 1] - R[1, 2]) / s
            y = (R[0, 2] - R[2, 0]) / s
            z = (R[1, 0] - R[0, 1]) / s
        elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
            s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2
            w = (R[2, 1] - R[1, 2]) / s
            x = 0.25 * s
            y = (R[0, 1] + R[1, 0]) / s
            z = (R[0, 2] + R[2, 0]) / s
        elif R[1, 1] > R[2, 2]:
            s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2
            w = (R[0, 2] - R[2, 0]) / s
            x = (R[0, 1] + R[1, 0]) / s
            y = 0.25 * s
            z = (R[1, 2] + R[2, 1]) / s
        else:
            s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2
            w = (R[1, 0] - R[0, 1]) / s
            x = (R[0, 2] + R[2, 0]) / s
            y = (R[1, 2] + R[2, 1]) / s
            z = 0.25 * s

        return cls(w, x, y, z)

    def norm(self) -> float:
        """Return quaternion norm (should be 1 for unit quaternions)."""
        return np.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)

    def conjugate(self) -> 'Quaternion':
        """Return quaternion conjugate (negate vector part)."""
        return Quaternion(self.w, -self.x, -self.y, -self.z)

    def __mul__(self, other: 'Quaternion') -> 'Quaternion':
        """
        Quaternion multiplication: self ⊗ other.

        Returns new quaternion representing composition of rotations.
        """
        if isinstance(other, Quaternion):
            return Quaternion(
                self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
                self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
                self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
                self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
            )
        raise TypeError("Can only multiply Quaternion by Quaternion")

    def to_rotation_matrix(self) -> np.ndarray:
        """Convert quaternion to rotation matrix."""
        w, x, y, z = self.w, self.x, self.y, self.z
        return np.array([
            [1 - 2*(y*y + z*z),     2*(x*y - z*w),     2*(x*z + y*w)],
            [    2*(x*y + z*w), 1 - 2*(x*x + z*z),     2*(y*z - x*w)],
            [    2*(x*z - y*w),     2*(y*z + x*w), 1 - 2*(x*x + y*y)]
        ])

    @staticmethod
    def slerp(q1: 'Quaternion', q2: 'Quaternion', t: float) -> 'Quaternion':
        """
        Spherical linear interpolation between two quaternions.

        Args:
            q1: Start quaternion
            q2: End quaternion
            t: Interpolation parameter in [0, 1]

        Returns:
            Interpolated quaternion

        Note:
            Takes shortest path (handles q1 ≈ -q2 case).
        """
        # Ensure shortest path interpolation
        q2_adjusted = q2
        dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z

        if dot < 0:
            q2_adjusted = Quaternion(-q2.w, -q2.x, -q2.y, -q2.z)
            dot = -dot

        # Handle near-identity case with linear interpolation
        if dot > 0.9995:
            result = Quaternion(
                q1.w + t * (q2_adjusted.w - q1.w),
                q1.x + t * (q2_adjusted.x - q1.x),
                q1.y + t * (q2_adjusted.y - q1.y),
                q1.z + t * (q2_adjusted.z - q1.z)
            )
            return result

        # Spherical interpolation
        theta_0 = np.arccos(dot)
        theta = theta_0 * t
        sin_theta = np.sin(theta)
        sin_theta_0 = np.sin(theta_0)

        s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
        s1 = sin_theta / sin_theta_0

        return Quaternion(
            s0 * q1.w + s1 * q2_adjusted.w,
            s0 * q1.x + s1 * q2_adjusted.x,
            s0 * q1.y + s1 * q2_adjusted.y,
            s0 * q1.z + s1 * q2_adjusted.z
        )

    def __repr__(self) -> str:
        return f"Quaternion(w={self.w:.4f}, x={self.x:.4f}, y={self.y:.4f}, z={self.z:.4f})"

Code Breakdown: - Lines 30-35: from_axis_angle creates quaternion from rotation axis and angle - Lines 37-61: from_rotation_matrix handles all 4 cases for numerical stability - Lines 73-76: Conjugate reverses the rotation (useful for inverse) - Lines 78-88: Quaternion multiplication implements the quaternion product formula - Lines 103-132: slerp computes spherical interpolation; handles antipodal case (dot < 0) - Lines 114-120: Near-identity fallback prevents numerical instability at θ ≈ 0

Visualization — SLERP interpolation animation:

Usage Example:

# Create quaternions for 90° rotations about different axes
q1 = Quaternion.from_axis_angle([0, 0, 1], np.pi / 2)  # 90° about Z
q2 = Quaternion.from_axis_angle([0, 1, 0], np.pi / 2)  # 90° about Y

# Compose rotations
q_combined = q1 * q2

# Interpolate between two orientations
trajectory = [Quaternion.slerp(q1, q2, t) for t in np.linspace(0, 1, 10)]

# Convert to rotation matrix
R = q_combined.to_rotation_matrix()

---

Homogeneous Transformations

Combining rotation and translation into a single 4x4 matrix.

Mathematical Formulation:

The homogeneous transformation matrix combines rotation \(R\) and translation \(d\):

\[T = \begin{bmatrix} R & d \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_x \\ r_{21} & r_{22} & r_{23} & d_y \\ r_{31} & r_{32} & r_{33} & d_z \\ 0 & 0 & 0 & 1 \end{bmatrix}\]

Inverse Transformation:

\[T^{-1} = \begin{bmatrix} R^T & -R^T d \\ 0 & 1 \end{bmatrix}\]

Point Transformation:

Given point \(p_B\) in frame B, its coordinates in frame A are:

\[p_A = R_{AB} \cdot p_B + d_{AB}\]

Or in homogeneous form:

\[p_A = T_{AB} \cdot p_B\]

Animation — See how rotation and translation combine:

---

Homogeneous Transform Class

Code Implementation:

import numpy as np


class HomogeneousTransform:
    """
    4x4 homogeneous transformation matrix combining rotation and translation.

    Represents transformation from frame B to frame A: T_AB
    - R: 3x3 rotation matrix (orientation of B in A)
    - d: 3x1 translation vector (origin of B in A)
    """

    def __init__(self, R: np.ndarray = None, d: np.ndarray = None):
        """
        Initialize homogeneous transformation.

        Args:
            R: 3x3 rotation matrix (defaults to identity)
            d: 3x1 translation vector (defaults to zero)
        """
        if R is None:
            R = np.eye(3)
        if d is None:
            d = np.zeros(3)

        self.R = np.asarray(R)
        self.d = np.asarray(d).reshape(3)

        # Construct 4x4 matrix
        self._matrix = np.eye(4)
        self._matrix[:3, :3] = self.R
        self._matrix[:3, 3] = self.d

    @classmethod
    def from_matrix(cls, T: np.ndarray) -> 'HomogeneousTransform':
        """Create from 4x4 homogeneous transformation matrix."""
        T = np.asarray(T)
        R = T[:3, :3]
        d = T[:3, 3]
        return cls(R, d)

    @classmethod
    def from_quaternion(cls, q: 'Quaternion', d: np.ndarray) -> 'HomogeneousTransform':
        """Create from quaternion and translation vector."""
        return cls(q.to_rotation_matrix(), d)

    @property
    def matrix(self) -> np.ndarray:
        """Return 4x4 transformation matrix."""
        return self._matrix.copy()

    def inverse(self) -> 'HomogeneousTransform':
        """
        Compute inverse transformation T_BA = T_AB^(-1).

        Uses formula: T^(-1) = [[R^T, -R^T*d], [0, 1]]
        """
        R_inv = self.R.T
        d_inv = -R_inv @ self.d
        return HomogeneousTransform(R_inv, d_inv)

    def compose(self, other: 'HomogeneousTransform') -> 'HomogeneousTransform':
        """
        Compose transformations: T_AC = T_AB @ T_BC.

        Args:
            other: T_BC - transformation from C to B

        Returns:
            T_AC - transformation from C to A
        """
        # T_AC = T_AB @ T_BC
        R_new = self.R @ other.R
        d_new = self.R @ other.d + self.d
        return HomogeneousTransform(R_new, d_new)

    def transform_point(self, p: np.ndarray) -> np.ndarray:
        """
        Transform a point from frame B to frame A.

        Args:
            p: 3D point in frame B [x, y, z]

        Returns:
            Point coordinates in frame A
        """
        p = np.asarray(p).reshape(3)
        return self.R @ p + self.d

    def transform_vector(self, v: np.ndarray) -> np.ndarray:
        """
        Transform a vector (direction) from frame B to frame A.
        Vectors are not affected by translation.

        Args:
            v: 3D vector in frame B [x, y, z]

        Returns:
            Vector in frame A
        """
        v = np.asarray(v).reshape(3)
        return self.R @ v

    def __matmul__(self, other: 'HomogeneousTransform') -> 'HomogeneousTransform':
        """Matrix multiplication using @ operator."""
        return self.compose(other)

    def __repr__(self) -> str:
        return f"HomogeneousTransform(R={self.R.tolist()}, d={self.d.tolist()})"

Code Breakdown: - Lines 23-28: Constructor stores R and d, builds 4x4 matrix - Lines 46-49: inverse() applies the transpose formula: \(T^{-1} = [[R^T, -R^Td], [0, 1]]\) - Lines 52-58: compose() implements transformation chaining: \(T_{AC} = T_{AB} \cdot T_{BC}\) - Lines 60-66: transform_point() applies rotation then translation - Lines 68-75: transform_vector() applies only rotation (vectors have no origin) - Line 80: Enables using @ operator: T1 @ T2 = composition

Usage Example:

# Create transformation: rotate 90° about Z, translate [1, 2, 0]
R = rot_z(np.pi / 2)
d = np.array([1.0, 2.0, 0.0])
T_AB = HomogeneousTransform(R, d)

# Transform a point from frame B to frame A
p_B = np.array([1.0, 0.0, 0.0])  # Point on X-axis of frame B
p_A = T_AB.transform_point(p_B)
print(f"Point in A: {p_A}")  # [1, 3, 0] (rotated then translated)

# Compose transformations
T_BC = HomogeneousTransform(rot_y(np.pi/4), np.array([0, 0, 1]))
T_AC = T_AB @ T_BC  # Equivalent to T_AB.compose(T_BC)

# Inverse transformation: get frame A coordinates in frame B
T_BA = T_AB.inverse()
p_B_recovered = T_BA.transform_point(p_A)

---

Common Operations

Transformation Composition

\[T_{AC} = T_{AB} \cdot T_{BC}\]

Inverse Transformation

\[T^{-1} = \begin{bmatrix} R^T & -R^T d \\ 0 & 1 \end{bmatrix}\]

Vector Transformation

\[v_A = R_{AB} \cdot v_B\]

---

← Back to Index