From Chapter 2 you know that the orientation of a rigid body in 3D can be represented by a rotation matrix \( \mathbf{R} \in SO(3) \subset \mathbb{R}^{3\times 3} \), where

\[ SO(3) = \left\{ \mathbf{R} \in \mathbb{R}^{3\times 3} \;\middle|\; \mathbf{R}^\top \mathbf{R} = \mathbf{I}_3,\; \det(\mathbf{R}) = 1 \right\}. \]

Lessons 1–3 of this chapter introduced three alternative parameterizations:

• Euler angles \( (\phi,\theta,\psi) \) for a fixed convention (we assume ZYX yaw–pitch–roll: yaw \( \psi \) about z, pitch \( \theta \) about y, roll \( \phi \) about x).
• Axis–angle \( (\hat{\omega}, \theta) \), where \( \hat{\omega} \in \mathbb{R}^3, \|\hat{\omega}\| = 1 \) is a unit rotation axis and \( \theta \) the rotation angle.
• Unit quaternions \( q = (q_0,\mathbf{q}_v) \in \mathbb{R}^4 \) with \( \|q\| = 1 \), where \( q_0 \) is the scalar part and \( \mathbf{q}_v = (q_1,q_2,q_3)^\top \) the vector part.

Each representation defines (almost) a coordinate chart on \( SO(3) \). Conversions between them are smooth maps, except at singularities such as Euler-angle gimbal lock or the antipodal identification \( q \sim -q \) for quaternions.

flowchart TD
  SO["SO(3) orientation"] --> R["Rotation matrix R"]
  SO --> EA["Euler angles (phi, theta, psi)"]
  SO --> AA["Axis-angle (omega_hat, theta)"]
  SO --> Q["Unit quaternion q"]

  R --> EA
  EA --> R

  R --> AA
  AA --> R

  R --> Q
  Q --> R

  EA --> Q
  Q --> EA

  AA --> Q
  Q --> AA
        

In this lesson we derive explicit formulas for all solid arrows in the diagram (using \( \mathbf{R} \) as a central hub) and discuss numerical issues that arise in practical robotic systems.

2.1 Axis–angle \(\rightarrow\) rotation matrix

Given a unit axis \( \hat{\omega} = [\omega_x,\omega_y,\omega_z]^\top \) and angle \( \theta \), the skew-symmetric matrix associated with \( \hat{\omega} \) is

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

The Rodrigues formula (already encountered via the matrix exponential in Chapter 2) gives

\[ \mathbf{R}(\hat{\omega},\theta) = \mathbf{I}_3 + \sin\theta \,[\hat{\omega}]_\times + (1 - \cos\theta)\,[\hat{\omega}]_\times^2. \]

This map is smooth for all \( \theta \) and satisfies \( \mathbf{R}(\hat{\omega},0) = \mathbf{I}_3 \).

2.2 Rotation matrix \(\rightarrow\) axis–angle

Let \( \mathbf{R} \in SO(3) \) represent a rotation with axis–angle parameters \( (\hat{\omega},\theta) \). Using the identity \( \operatorname{tr}([\hat{\omega}]_\times^2) = -2 \|\hat{\omega}\|^2 = -2 \) for a unit axis, we obtain from Rodrigues:

\[ \begin{aligned} \operatorname{tr}(\mathbf{R}) &= \operatorname{tr}\big(\mathbf{I}_3 + \sin\theta[\hat{\omega}]_\times + (1-\cos\theta)[\hat{\omega}]_\times^2\big) \\ &= 3 + (1-\cos\theta)\operatorname{tr}([\hat{\omega}]_\times^2) \\ &= 3 - 2(1 - \cos\theta) = 1 + 2\cos\theta. \end{aligned} \]

Thus the rotation angle is

\[ \theta = \arccos\left(\frac{\operatorname{tr}(\mathbf{R}) - 1}{2}\right), \quad \theta \in [0,\pi]. \]

Whenever \( \theta \neq 0 \) and \( \theta \neq \pi \) (i.e., away from the identity and half-turns), the axis is recovered as

\[ \hat{\omega} = \frac{1}{2\sin\theta} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix}. \]

Special cases:

• If \( \theta \approx 0 \), then \( \mathbf{R} \approx \mathbf{I}_3 \). We can set \( \hat{\omega} \) arbitrarily (e.g., \( [1,0,0]^\top \)) since the rotation is effectively zero.
• If \( \theta \approx \pi \), numerical sensitivity appears because \( \sin\theta \approx 0 \). In practice one uses the diagonal entries of \( \mathbf{R} \) to extract an axis: e.g. \( \omega_x^2 = (R_{11}+1)/2 \), etc., taking care of signs.

3.1 Quaternion \(\leftrightarrow\) axis–angle

For a unit quaternion \( q = (q_0,q_1,q_2,q_3)^\top \), the associated axis–angle parameters are given by

\[ \theta = 2\arccos(q_0), \qquad \sin\frac{\theta}{2} = \sqrt{q_1^2 + q_2^2 + q_3^2}. \]

If \( \sin(\theta/2) \neq 0 \), then

\[ \hat{\omega} = \frac{1}{\sin(\theta/2)} \begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix}. \]

Conversely, given axis–angle \( (\hat{\omega},\theta) \),

\[ q_0 = \cos\frac{\theta}{2}, \qquad \begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix} = \sin\frac{\theta}{2}\,\hat{\omega}. \]

Note the double cover: \( q \) and \( -q \) represent the same rotation, because multiplying all components by −1 leaves \( \theta \) and \( \hat{\omega} \) unchanged.

3.2 Quaternion \(\leftrightarrow\) rotation matrix

Using the axis–angle relation, we can derive a closed-form mapping \( q \mapsto \mathbf{R}(q) \). If \( q = (q_0,q_1,q_2,q_3)^\top \) is unit length, the associated rotation matrix is

\[ \mathbf{R}(q) = \begin{bmatrix} 1 - 2(q_2^2 + q_3^2) & 2(q_1 q_2 - q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & 1 - 2(q_1^2 + q_3^2) & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & 1 - 2(q_1^2 + q_2^2) \end{bmatrix}. \]

A standard algebraic exercise (see Problem 2) shows that if \( \|q\| = 1 \) then \( \mathbf{R}(q)^\top \mathbf{R}(q) = \mathbf{I}_3 \) and \( \det(\mathbf{R}(q)) = 1 \), so \( \mathbf{R}(q) \in SO(3) \).

In the opposite direction, a convenient algorithm is:

• Compute the trace \( t = \operatorname{tr}(\mathbf{R}) = R_{11} + R_{22} + R_{33} \).
• If \( t > 0 \),

  \[ \begin{aligned} q_0 &= \tfrac{1}{2}\sqrt{1 + t}, \\ q_1 &= \frac{R_{32} - R_{23}}{4 q_0},\quad q_2 = \frac{R_{13} - R_{31}}{4 q_0},\quad q_3 = \frac{R_{21} - R_{12}}{4 q_0}. \end{aligned} \]

• Otherwise select the largest diagonal element of \( \mathbf{R} \) and compute the corresponding component of \( q \) first (to avoid dividing by very small numbers); see standard attitude-estimation references.

In all cases, the resulting quaternion should be normalized numerically: \( q \leftarrow q/\|q\| \) to compensate for rounding errors.

We fix the widely-used robotics convention \( \mathbf{R} = R_z(\psi)\,R_y(\theta)\,R_x(\phi) \), where:

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

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

Multiplying these gives

\[ \mathbf{R} = R_z(\psi) R_y(\theta) R_x(\phi) = \begin{bmatrix} c_\psi c_\theta & s_\phi s_\theta c_\psi - s_\psi c_\phi & s_\phi s_\psi + s_\theta c_\phi c_\psi \\ s_\psi c_\theta & s_\phi s_\psi s_\theta + c_\phi c_\psi & -s_\phi c_\psi + s_\psi s_\theta c_\phi \\ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{bmatrix}, \]

where \( c_\psi = \cos\psi \), \( s_\psi = \sin\psi \), etc.

4.1 Rotation matrix \(\rightarrow\) ZYX Euler angles

Let \( \mathbf{R} = [r_{ij}] \). Comparing entries in the last row and first column to the expression above, for the generic (non-singular) case with \( |c_\theta| > 0 \) we obtain:

\[ \theta = \arcsin(-r_{31}), \qquad \phi = \operatorname{atan2}(r_{32}, r_{33}), \qquad \psi = \operatorname{atan2}(r_{21}, r_{11}). \]

Here \( \operatorname{atan2}(y,x) \) is the quadrant-aware arctangent. This parameterization is singular when \( c_\theta = \cos\theta \approx 0 \), i.e. \( \theta \approx \pm \pi/2 \). Then \( r_{31} \approx \mp 1 \) and information about \( \phi \) and \( \psi \) becomes entangled (gimbal lock).

A typical strategy is:

• If \( |r_{31}| < 1 - \varepsilon \) (away from singularity), use the formulas above.
• If \( r_{31} \approx -1 \) (pitch \( \theta \approx \pi/2 \)), set \( \theta = \pi/2 \) and choose \( \phi = 0 \), \( \psi = \operatorname{atan2}(-r_{12}, r_{22}) \).
• If \( r_{31} \approx 1 \) (pitch \( \theta \approx -\pi/2 \)), set \( \theta = -\pi/2 \) and choose \( \phi = 0 \), \( \psi = \operatorname{atan2}(r_{12}, r_{22}) \).

4.2 Euler angles \(\rightarrow\) other representations

Given Euler angles \( (\phi,\theta,\psi) \), we can:

1. Compute \( \mathbf{R} = R_z(\psi)R_y(\theta)R_x(\phi) \) using the formulas above.
2. Convert \( \mathbf{R} \rightarrow (\hat{\omega},\theta) \) using the trace and off-diagonal formulas from Section 2.
3. Convert \( \mathbf{R} \rightarrow q \) using the algorithm in Section 3.

Thus we obtain Euler \(\rightarrow\) axis–angle and Euler \(\rightarrow\) quaternion conversions without deriving additional closed-form expressions: the rotation matrix serves as a universal intermediate.

In software libraries we rarely implement all pairwise conversions independently. Instead, we fix a canonical representation (often the rotation matrix or quaternion) and route all conversions through it. A robust pipeline for a general function \( \texttt{convert(from_type, to_type)} \) can be:

flowchart TD
  S["Input orientation + type"] --> N["Normalize representation"]
  N --> B["Branch on input type"]
  B --> RM["If type is matrix: \nset R = input"]
  B --> EA["If type is Euler: \ncompute R(phi, theta, psi)"]
  B --> AA["If type is axis-angle: \ncompute R(omega_hat, theta) \nvia Rodrigues"]
  B --> Q["If type is quaternion: \nnormalize q, then R = R(q)"]
  RM --> O["Branch on desired output type"]
  EA --> O
  AA --> O
  Q --> O
  O --> O1["If output is matrix: \nreturn R"]
  O --> O2["If output is Euler: \nextract angles from R with atan2 + \nsingular checks"]
  O --> O3["If output is axis-angle: \nuse trace/off-diagonals from R"]
  O --> O4["If output is quaternion: \nuse trace-based R to q"]
        

Key numerical practices in robotics:

• Normalization: always renormalize quaternions, unit axes, and (if necessary) orthogonalize rotation matrices (e.g., via SVD or Gram–Schmidt) when accumulated numerical drift is significant.
• Clamping: when computing \( \theta = \arccos(x) \), clip \( x \) to the interval \( [-1,1] \) to avoid NaNs: e.g. \( x \leftarrow \min(1,\max(-1,x)) \).
• Branch decisions: near singularities, avoid dividing by small numbers (e.g., \( \sin\theta \) or \( 1 + \operatorname{tr}(\mathbf{R}) \)) and switch to alternative formulas.
• Continuity: for time-varying orientations, enforce consistent quaternion sign (e.g., choose the sign that keeps \( \|q_k - q_{k-1}\| \) small) to avoid sudden sign flips.

In Python, robot modeling commonly uses numpy for linear algebra and scipy.spatial.transform.Rotation or libraries like pytransform3d or spatialmath. Below we implement basic conversions from scratch using NumPy, and then show how to use SciPy's higher-level interface.

import numpy as np
from numpy.linalg import norm

def skew(omega):
    """Return skew-symmetric matrix [omega]_x."""
    wx, wy, wz = omega
    return np.array([[0.0, -wz,  wy],
                     [wz,  0.0, -wx],
                     [-wy, wx,  0.0]])

def axis_angle_to_R(omega_hat, theta):
    """Axis-angle to rotation matrix via Rodrigues."""
    omega_hat = np.asarray(omega_hat, dtype=float)
    omega_hat = omega_hat / norm(omega_hat)
    K = skew(omega_hat)
    return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)

def R_to_axis_angle(R, eps=1e-8):
    """Rotation matrix to axis-angle."""
    R = np.asarray(R, dtype=float)
    # Ensure approximate orthogonality if needed
    trace = np.trace(R)
    # Clamp for numerical safety
    cos_theta = (trace - 1.0) / 2.0
    cos_theta = np.clip(cos_theta, -1.0, 1.0)
    theta = np.arccos(cos_theta)
    if theta < eps:
        return np.array([1.0, 0.0, 0.0]), 0.0
    if np.pi - theta < 1e-6:
        # Near pi: use diagonal entries
        diag = np.diag(R)
        omega = np.sqrt(np.maximum((diag + 1.0) / 2.0, 0.0))
        # Fix signs using off-diagonals
        if R[0, 1] < 0.0:
            omega[1] = -omega[1]
        if R[0, 2] < 0.0:
            omega[2] = -omega[2]
        omega_hat = omega / norm(omega)
        return omega_hat, theta
    omega_hat = np.array([
        R[2, 1] - R[1, 2],
        R[0, 2] - R[2, 0],
        R[1, 0] - R[0, 1]
    ]) / (2.0 * np.sin(theta))
    return omega_hat, theta

def quat_to_R(q):
    """Unit quaternion (w, x, y, z) to rotation matrix."""
    q = np.asarray(q, dtype=float)
    q = q / norm(q)
    w, x, y, z = q
    R = np.array([
        [1 - 2*(y*y + z*z), 2*(x*y - w*z),     2*(x*z + w*y)],
        [2*(x*y + w*z),     1 - 2*(x*x + z*z), 2*(y*z - w*x)],
        [2*(x*z - w*y),     2*(y*z + w*x),     1 - 2*(x*x + y*y)]
    ])
    return R

def R_to_quat(R):
    """Rotation matrix to quaternion (w, x, y, z)."""
    R = np.asarray(R, dtype=float)
    t = np.trace(R)
    if t > 0.0:
        s = np.sqrt(1.0 + t) * 2.0
        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
    else:
        # Find the major diagonal term
        if 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.0
            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.0
            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.0
            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
    q = np.array([w, x, y, z])
    q /= norm(q)
    return q

def euler_zyx_to_R(phi, theta, psi):
    """Euler ZYX (roll, pitch, yaw) to rotation matrix."""
    c1, s1 = np.cos(psi), np.sin(psi)
    c2, s2 = np.cos(theta), np.sin(theta)
    c3, s3 = np.cos(phi), np.sin(phi)
    R = np.array([
        [c1*c2,            s3*s2*c1 - s1*c3,  s3*s1 + s2*c3*c1],
        [s1*c2,            s3*s1*s2 + c3*c1, -s3*c1 + s1*s2*c3],
        [-s2,              s3*c2,             c3*c2]
    ])
    return R

def R_to_euler_zyx(R, eps=1e-8):
    """Rotation matrix to Euler ZYX (roll, pitch, yaw)."""
    R = np.asarray(R, dtype=float)
    r31 = R[2, 0]
    if abs(r31) < 1.0 - eps:
        theta = np.arcsin(-r31)
        phi = np.arctan2(R[2, 1], R[2, 2])
        psi = np.arctan2(R[1, 0], R[0, 0])
    else:
        # Gimbal lock
        if r31 < 0:  # theta ~ +pi/2
            theta = np.pi / 2.0
            phi = 0.0
            psi = np.arctan2(-R[0, 1], R[1, 1])
        else:        # theta ~ -pi/2
            theta = -np.pi / 2.0
            phi = 0.0
            psi = np.arctan2(R[0, 1], R[1, 1])
    return phi, theta, psi

# Example: Using SciPy's Rotation class (if available)
try:
    from scipy.spatial.transform import Rotation as Rsc

    def quat_to_R_scipy(q):
        return Rsc.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()

    def R_to_quat_scipy(R):
        r = Rsc.from_matrix(R)
        x, y, z, w = r.as_quat()
        return np.array([w, x, y, z])

except ImportError:
    pass
      

In robotics software (e.g., Python-based simulators or control stacks), these routines are integrated into kinematic solvers, state estimators, and trajectory generators.

In C++, the Eigen library is the de facto standard for linear algebra in robotics (and underlies ROS's tf2 library). Below is a minimalist implementation of quaternion and Euler conversions.

#include <Eigen/Dense>
#include <cmath>

using Matrix3 = Eigen::Matrix3d;
using Vector3 = Eigen::Vector3d;
using Quaternion = Eigen::Quaterniond;

Matrix3 axisAngleToR(const Vector3& omega_hat, double theta) {
    Vector3 w = omega_hat.normalized();
    Eigen::Matrix3d K;
    K << 0.0,   -w.z(),  w.y(),
          w.z(),  0.0,   -w.x(),
         -w.y(),  w.x(),  0.0;
    return Matrix3::Identity()
         + std::sin(theta) * K
         + (1.0 - std::cos(theta)) * K * K;
}

void RToAxisAngle(const Matrix3& R, Vector3& omega_hat, double& theta) {
    double trace = R.trace();
    double cos_theta = (trace - 1.0) / 2.0;
    cos_theta = std::max(-1.0, std::min(1.0, cos_theta));
    theta = std::acos(cos_theta);

    if (theta < 1e-8) {
        omega_hat = Vector3(1.0, 0.0, 0.0);
        theta = 0.0;
        return;
    }

    omega_hat = Vector3(
        R(2,1) - R(1,2),
        R(0,2) - R(2,0),
        R(1,0) - R(0,1)
    ) / (2.0 * std::sin(theta));
    omega_hat.normalize();
}

Quaternion RToQuat(const Matrix3& R) {
    Quaternion q;
    q = Quaternion(R);  // Eigen has built-in constructor
    q.normalize();
    return q;
}

Matrix3 quatToR(const Quaternion& q) {
    Quaternion qn = q.normalized();
    return qn.toRotationMatrix();
}

Matrix3 eulerZYXToR(double phi, double theta, double psi) {
    Matrix3 Rx, Ry, Rz;
    double c1 = std::cos(psi), s1 = std::sin(psi);
    double c2 = std::cos(theta), s2 = std::sin(theta);
    double c3 = std::cos(phi), s3 = std::sin(phi);

    Rz << c1, -s1, 0.0,
           s1,  c1, 0.0,
           0.0, 0.0, 1.0;
    Ry <<  c2, 0.0, s2,
            0.0, 1.0, 0.0,
           -s2, 0.0, c2;
    Rx << 1.0, 0.0, 0.0,
           0.0, c3, -s3,
           0.0, s3,  c3;

    return Rz * Ry * Rx;
}
      

ROS users often rely on geometry_msgs/Quaternion together with tf2::Quaternion utilities, which internally implement the same formulas discussed here.

In Java-based robotics or simulation frameworks (e.g., certain control toolchains, game engines), EJML or Apache Commons Math provide basic matrix operations. Below is a minimal custom implementation using primitive arrays.

public final class SO3Conversions {

    public static double[][] quatToR(double w, double x, double y, double z) {
        // Normalize
        double n = Math.sqrt(w*w + x*x + y*y + z*z);
        w /= n; x /= n; y /= n; z /= n;

        double[][] R = new double[3][3];
        R[0][0] = 1.0 - 2.0*(y*y + z*z);
        R[0][1] = 2.0*(x*y - w*z);
        R[0][2] = 2.0*(x*z + w*y);

        R[1][0] = 2.0*(x*y + w*z);
        R[1][1] = 1.0 - 2.0*(x*x + z*z);
        R[1][2] = 2.0*(y*z - w*x);

        R[2][0] = 2.0*(x*z - w*y);
        R[2][1] = 2.0*(y*z + w*x);
        R[2][2] = 1.0 - 2.0*(x*x + y*y);
        return R;
    }

    public static double[] RToEulerZYX(double[][] R) {
        double r31 = R[2][0];
        double phi, theta, psi;
        if (Math.abs(r31) < 1.0 - 1e-8) {
            theta = Math.asin(-r31);
            phi   = Math.atan2(R[2][1], R[2][2]);
            psi   = Math.atan2(R[1][0], R[0][0]);
        } else {
            // Gimbal lock
            if (r31 < 0) {
                theta = Math.PI / 2.0;
                phi = 0.0;
                psi = Math.atan2(-R[0][1], R[1][1]);
            } else {
                theta = -Math.PI / 2.0;
                phi = 0.0;
                psi = Math.atan2(R[0][1], R[1][1]);
            }
        }
        return new double[]{phi, theta, psi};
    }
}
      

Java robotics libraries for physical simulation (e.g., jMonkeyEngine-based projects) often embed similar routines for camera and body orientation.

9.1 MATLAB / Simulink

In MATLAB, the Robotics System Toolbox and Aerospace Toolbox expose high-level conversion functions. The following script demonstrates conversions for a single orientation:

% Axis-angle to rotation matrix, then to quaternion and Euler ZYX
omega_hat = [0; 0; 1];  % rotation about z
theta = pi/4;           % 45 degrees

R = axang2rotm([omega_hat.' theta]);   % Robotics System Toolbox
q = rotm2quat(R);                      % [w x y z] convention
eulZYX = rotm2eul(R, 'ZYX');           % [phi theta psi] in radians

% Convert back via toolbox routines
R2 = quat2rotm(q);
eul2 = rotm2eul(R2, 'ZYX');

% Numerical consistency check
disp(norm(R - R2));
disp(norm(eulZYX - eul2));

% Simulink:
% Use "Axis-Angle to Rotation Matrix" block, "Rotation Matrix to
% Quaternion" block, etc., in the Robotics System Toolbox library.
      

In Simulink, converting between representations is typically done by interconnecting dedicated blocks, which internally use the same formulas covered in this lesson.

9.2 Wolfram Mathematica

Mathematica provides symbolic and numeric support for rotations. The code below uses RotationMatrix, Quaternion, and EulerAngles:

(* ZYX Euler to rotation matrix *)
phi  = Pi/6;
theta = Pi/4;
psi  = Pi/3;

Rz  = RotationMatrix[psi, {0, 0, 1}];
Ry  = RotationMatrix[theta, {0, 1, 0}];
Rx  = RotationMatrix[phi, {1, 0, 0}];

R   = Rz.Ry.Rx;

(* Rotation matrix to quaternion *)
q   = Quaternion[R];

(* Quaternion back to rotation matrix and Euler ZYX *)
R2  = RotationMatrix[q];
eul = EulerAngles[R2, {3, 2, 1}];  (* {psi, theta, phi} *)

Simplify[R - R2]
      

Symbolic manipulation helps verify identities such as \( \mathbf{R}(q)^\top\mathbf{R}(q)=\mathbf{I}_3 \) and relationships among Euler, axis–angle, and quaternion representations.

In this lesson we treated rotation matrices, Euler angles, axis–angle variables, and unit quaternions as coordinate systems on the same configuration space \( SO(3) \). We derived explicit conversion formulas, emphasized the role of the rotation matrix as a convenient intermediate, and analyzed singularities (Euler gimbal lock) and numerical issues (quaternion normalization, clamping, branch choices).

We then implemented these conversions in Python, C++, Java, MATLAB / Simulink, and Wolfram Mathematica, highlighting common robotics libraries (NumPy/SciPy, Eigen, MATLAB toolboxes) that encapsulate these computations. These conversion routines will be used repeatedly when we express kinematic and dynamic equations in different orientation coordinates in subsequent chapters.