Unit 1: Coordinate Systems and Matrices

1. Geometric Foundations of Robotic Kinematics

Robotics is fundamentally the mathematical science of representing, predicting, and controlling the spatial pose, velocity, acceleration, and force transmission of physical bodies operating in three-dimensional space. The physical posture of an articulated manipulator, mobile robot, or aerial drone is defined through rigid-body spatial transformations relative to structured reference coordinate frames.

A coordinate system establishes a one-to-one mapping between abstract geometric points and numerical ordered n-tuples in $\mathbb{R}^3$. In robotic manipulation, kinematic state estimation, and sensor fusion, coordinate selection dictates the computational complexity of motion planning, inverse kinematics, and closed-loop dynamic equations of motion.

CODECopy Code
+---------------------------------------------------------------------------------------------------+
|                        ROBOTIC SPATIAL TRANSFORMATION HIERARCHY                                   |
+---------------------------------------------------------------------------------------------------+
|  WORLD / BASE FRAME {W}                                                                           |
|       |                                                                                           |
|       |--> Homogeneous Transform T_0 (Base to Joint 1)                                            |
|       v                                                                                           |
|  LINK 1 FRAME {1}                                                                                 |
|       |                                                                                           |
|       |--> Homogeneous Transform T_1 (Joint 1 to Joint 2)                                         |
|       v                                                                                           |
|  LINK 2 FRAME {2}                                                                                 |
|       |                                                                                           |
|       |--> Homogeneous Transform T_2 ... T_{n-1}                                                  |
|       v                                                                                           |
|  END-EFFECTOR / TOOL FRAME {E}                                                                    |
|       |                                                                                           |
|       |--> Spatial Mapping: ^W p = ^W T_E * ^E p                                                  |
|       v                                                                                           |
|  TARGET OBJECT / SENSOR FRAME {S}                                                                 |
+---------------------------------------------------------------------------------------------------+

2. Curvilinear Coordinate Systems in Robotics

Robotic workspace analyses frequently require transitioning between Cartesian space and curvilinear coordinate systems to exploit natural geometric symmetries (e.g., revolute joint arcs, SCARA cylindrical workspaces, or spherical wrists).

CODECopy Code
+---------------------------------------------------------------------------------------------------+
|                         ROBOTIC COORDINATE SYSTEMS COMPARISON                                     |
+---------------------+-------------------------------+---------------------------------------------+
| COORDINATE SYSTEM   | COORDINATES                   | SCALE FACTORS (h1, h2, h3)                  |
+---------------------+-------------------------------+---------------------------------------------+
| Cartesian (Rect.)   | (x, y, z)                     | (1, 1, 1)                                   |
| Cylindrical         | (r, theta, z)                 | (1, r, 1)                                   |
| Spherical           | (rho, theta, phi)             | (1, rho*sin(phi), rho)                      |
| Polar (2D Planar)   | (r, theta)                    | (1, r)                                      |
+---------------------+-------------------------------+---------------------------------------------+

2.1 Cartesian Coordinate System

The 3D Cartesian frame defines a point $P$ using mutually orthogonal axes $\hat{\mathbf{x}}, \hat{\mathbf{y}}, \hat{\mathbf{z}}$ conforming strictly to the right-hand rule ($\hat{\mathbf{x}} \times \hat{\mathbf{y}} = \hat{\mathbf{z}}$):
$\mathbf{p} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} = x \hat{\mathbf{x}} + y \hat{\mathbf{y}} + z \hat{\mathbf{z}}$

• Metric Differential Length:
  $ds^2 = dx^2 + dy^2 + dz^2$
• Differential Volume Element:
  $dV = dx \, dy \, dz$

2.2 Cylindrical Coordinate System

Cylindrical coordinates $(r, \theta, z)$ are natural for cylindrical-geometry manipulators (e.g., Stanford arm base, gantry systems) where motion consists of planar polar sweeps combined with axial linear elevation:
$x = r \cos\theta, \quad y = r \sin\theta, \quad z = z$
where $r \ge 0$, $0 \le \theta < 2\pi$, and $z \in (-\infty, \infty)$.

• Scale Factors: $h_r = 1$, $h_\theta = r$, $h_z = 1$.
• Differential Length:
  $ds^2 = dr^2 + r^2 d\theta^2 + dz^2$
• Differential Volume Element:
  $dV = r \, dr \, d\theta \, dz$
  The factor $r$ represents the Jacobian determinant of the coordinate transformation, accounting for the tangential arc stretching at increasing radial distance.

2.3 Spherical Coordinate System

Spherical coordinates $(\rho, \theta, \phi)$ model spherical wrists, tracking radar gimbals, and omnidirectional sensor fields where motion radiates from a central pivot:
$x = \rho \sin\phi \cos\theta, \quad y = \rho \sin\phi \sin\theta, \quad z = \rho \cos\phi$
where $\rho \ge 0$ is radial distance, $\theta \in [0, 2\pi)$ is azimuthal angle in the $xy$-plane, and $\phi \in [0, \pi]$ is zenith / polar angle measured from the $+z$ axis.

• Scale Factors: $h_\rho = 1$, $h_\phi = \rho$, $h_\theta = \rho \sin\phi$.
• Differential Volume Element:
  $dV = \rho^2 \sin\phi \, d\rho \, d\phi \, d\theta$

3. Differential Geometry & Field Integrals in Robotics

Robotic links possess distributed continuous mass and experience distributed hydrodynamic, aerodynamic, and contact forces. Transforming continuous spatial mechanics into rigid-body dynamics requires evaluating line, surface, and volume integrals.

CODECopy Code
+---------------------------------------------------------------------------------------------------+
|                            CALCULUS OF CONTINUOUS ROBOT LINK MECHANICS                            |
+---------------------------------------------------------------------------------------------------+
|  1. LINE INTEGRALS (Actuator Work & Trajectory Workspaces)                                        |
|     W = \int_C \mathbf{F} \cdot d\mathbf{r} = \int_a^b \mathbf{F}(\mathbf{r}(t)) \cdot \mathbf{r}'(t) dt |
|     - Accumulates mechanical work along the end-effector trajectory curve C                       |
|                                                                                                   |
|  2. SURFACE INTEGRALS & FLUX (Gripper Contact Pressures & Sensor Fields)                         |
|     \Phi = \iint_S \mathbf{F} \cdot \hat{\mathbf{n}} \, dA                                        |
|     - Computes net fluid thrust or tactile pressure normal to the sensor pad surface              |
|                                                                                                   |
|  3. VOLUME INTEGRALS (Link Mass, Center of Mass, & Inertia Tensors)                               |
|     Mass: m = \iiint_V \rho(\mathbf{r}) \, dV                                                     |
|     Center of Mass: \mathbf{r}_c = \frac{1}{m} \iiint_V \mathbf{r} \rho(\mathbf{r}) \, dV         |
|     Inertia Tensor: I_{ij} = \iiint_V \rho(\mathbf{r}) [\delta_{ij} r^2 - x_i x_j] \, dV          |
+---------------------------------------------------------------------------------------------------+

3.1 Divergence Theorem (Gauss's Theorem)

The Divergence Theorem equates the net outward flux of a vector field across a closed surface to the volume integral of its spatial divergence:
$\iint_{\partial V} \mathbf{F} \cdot \hat{\mathbf{n}} \, dA = \iiint_V (\nabla \cdot \mathbf{F}) \, dV$
In robotic continuum mechanics, this converts surface contact stresses into internal volume stress tensor divergences.

3.2 Stokes' Theorem

Stokes' Theorem equates the circulation of a vector field around a closed boundary curve $\partial S$ to the surface integral of its curl:
$\oint_{\partial S} \mathbf{F} \cdot d\mathbf{r} = \iint_S (\nabla \times \mathbf{F}) \cdot \hat{\mathbf{n}} \, dA$
In magnetic robotic actuation and conservative potential field path planning, Stokes' theorem validates whether a force field is conservative ($\nabla \times \mathbf{F} = \mathbf{0} \iff \oint \mathbf{F} \cdot d\mathbf{r} = 0$).

4. Matrix Algebra & Spatial Transformations in SE(3)

Rigid-body kinematics operates on the Special Euclidean Group $SE(3)$, composed of rotations in the Special Orthogonal Group $SO(3)$ combined with translations in $\mathbb{R}^3$.

CODECopy Code
+---------------------------------------------------------------------------------------------------+
|                        HOMOGENEOUS TRANSFORMATION MATRIX STRUCTURE                                |
+---------------------------------------------------------------------------------------------------+
|                                                                                                   |
|          ^A T_B  =  [   ^A R_B (3x3)   |   ^A p_B (3x1)   ]                                       |
|                     [ -----------------+----------------- ]                                       |
|                     [    0   0   0     |        1         ]                                       |
|                                                                                                   |
|  WHERE:                                                                                           |
|  - ^A R_B : 3x3 Orthogonal Rotation Matrix in SO(3)  (R^T * R = I,  det(R) = +1)                  |
|  - ^A p_B : 3x1 Translation Vector in R^3 locating origin of frame B relative to frame A          |
|  - [0 0 0]: Perspective scaling row (zero for affine rigid transformations)                       |
|  - 1      : Homogeneous scale factor                                                              |
|                                                                                                   |
|  INVERSE HOMOGENEOUS TRANSFORMATION:                                                             |
|          (^A T_B)^{-1} = [   (^A R_B)^T   |   - (^A R_B)^T * ^A p_B   ]                           |
|                          [ ---------------+----------------------- ]                              |
|                          [    0   0   0   |            1          ]                               |
+---------------------------------------------------------------------------------------------------+

4.1 Rotation Matrices in SO(3)

A rotation matrix $R \in \mathbb{R}^{3 \times 3}$ transforms coordinate representations between two orthonormal bases without altering lengths or angles:

• Orthogonality: $R^T R = R R^T = I \implies R^{-1} = R^T$
• Proper Orientation: $\det(R) = +1$ (distinguishing pure rotations from reflections where $\det(R) = -1$).
• Canonical Elementary Rotations: R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}, \quad R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

5. Matrix Decompositions in Robotic Computation

Modern robotics algorithms rely on structured matrix factorizations rather than direct matrix inversion to maximize numerical stability and compute efficiency.

CODECopy Code
+---------------------------------------------------------------------------------------------------+
|                        TAXONOMY OF MATRIX DECOMPOSITIONS IN ROBOTICS                              |
+---------------------+-------------------+---------------------+-----------------------------------+
| DECOMPOSITION       | FACTORIZATION     | MATRIX PROPERTIES   | ROBOTIC APPLICATION               |
+---------------------+-------------------+---------------------+-----------------------------------+
| LU Decomposition    | P * A = L * U     | Square A, general   | Forward dynamics, joint torques   |
| Cholesky            | A = L * L^T       | Symmetric Pos.-Def. | Manipulator inertia matrix M(q)   |
| QR Decomposition    | A = Q * R         | Orthogonal Q, tri. R| Numerically stable least squares  |
| Eigen Decomposition | A = V * Lambda*V^-1| Square, diagonalizable | Dynamic mode analysis, stability  |
| SVD                 | A = U * Sigma*V^T | Arbitrary m x n     | Kinematic singularities, Jacobian |
+---------------------+-------------------+---------------------+-----------------------------------+

5.1 LU Decomposition

Any nonsingular square matrix $A$ can be factored into a lower triangular matrix $L$ and upper triangular matrix $U$ with row permutation matrix $P$:
$P A = L U$

• Robotic Relevance: Solves systems $A \mathbf{x} = \mathbf{b}$ via forward substitution ($L \mathbf{y} = P \mathbf{b}$) and back substitution ($U \mathbf{x} = \mathbf{y}$) in $\mathcal{O}(n^2)$ time after an initial $\mathcal{O}(\frac{2}{3}n^3)$ factorization, avoiding numerical errors inherent in computing $A^{-1}$.

5.2 Cholesky Decomposition

For real symmetric positive-definite (SPD) matrices where $\mathbf{x}^T A \mathbf{x} > 0$ for all $\mathbf{x} \ne \mathbf{0}$, $A$ factors uniquely into:
$A = L L^T$
where $L$ is a lower triangular matrix with strictly positive diagonal elements.

• Robotic Relevance: The manipulator mass/inertia matrix $M(q)$ in the Lagrangian dynamics equation:
  $M(q) \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau$
  is guaranteed by physics to be symmetric positive-definite. Cholesky decomposition computes forward accelerations $\ddot{q}$ twice as fast as LU decomposition with guaranteed numerical stability.

5.3 QR Decomposition

Factors an $m \times n$ matrix $A$ into an orthogonal matrix $Q \in \mathbb{R}^{m \times m}$ ($Q^T Q = I$) and an upper triangular matrix $R \in \mathbb{R}^{m \times n}$:
$A = Q R$

• Robotic Relevance: Solves overdetermined kinematic calibration and sensor estimation problems $\min_{\mathbf{x}} \|A\mathbf{x} - \mathbf{b}\|_2$. Because orthogonal transformations preserve Euclidean vector norms ($\|Q\mathbf{v}\|_2 = \|\mathbf{v}\|_2$), QR factorization avoids squaring the condition number $\kappa(A)$, unlike normal equations ($A^T A \mathbf{x} = A^T \mathbf{b}$).

5.4 Singular Value Decomposition (SVD) & Robot Jacobians

Every real matrix $A \in \mathbb{R}^{m \times n}$ has a Singular Value Decomposition:
$A = U \Sigma V^T$
where $U \in \mathbb{R}^{m \times m}$ and $V \in \mathbb{R}^{n \times n}$ are orthogonal matrices, and $\Sigma = \text{diag}(\sigma_1, \sigma_2, \dots, \sigma_p)$ contains non-negative singular values sorted in descending order: $\sigma_1 \ge \sigma_2 \ge \dots \ge \sigma_p \ge 0$.

CODECopy Code
+---------------------------------------------------------------------------------------------------+
|                         SVD & MANIPULABILITY ELLIPSOID ANALYSIS                                   |
+---------------------------------------------------------------------------------------------------+
|  ROBOT GEOMETRIC JACOBIAN:  v_e = J(q) * q_dot                                                    |
|                                                                                                   |
|  SVD FACTORIZATION:        J = U * Sigma * V^T                                                    |
|                                                                                                   |
|  MANIPULABILITY METRICS:                                                                          |
|  - Yoshikawa Manipulability Measure:  w = sqrt(det(J * J^T)) = sigma_1 * sigma_2 * ... * sigma_m  |
|  - Ellipsoid Principal Axes: Directed along columns of U (u_1, u_2, ..., u_m)                     |
|  - Semi-Axis Lengths: Scaled directly by singular values (sigma_1, sigma_2, ..., sigma_m)         |
|  - Singularity Condition: sigma_m -> 0 (Ellipsoid collapses, loss of directional mobility)        |
+---------------------------------------------------------------------------------------------------+

5.5 The Moore-Penrose Pseudoinverse

For redundant manipulators ($n > m$), the Moore-Penrose pseudoinverse $J^+$ provides minimum-norm joint velocity solutions:
$J^+ = V \Sigma^+ U^T = J^T (J J^T)^{-1}$
where $\Sigma^+$ is formed by inverting non-zero singular values:
$\sigma_i^+ = \begin{cases} \frac{1}{\sigma_i} & \text{if } \sigma_i > \epsilon \\ 0 & \text{if } \sigma_i \le \epsilon \end{cases}$

• Damped Least Squares (Levenberg-Marquardt): Near kinematic singularities where $\sigma_m \to 0$, standard pseudoinverse causes joint velocities to diverge to infinity ($\dot{q} \to \infty$). Damped least squares regularizes the inversion:
  $J^* = J^T (J J^T + \lambda^2 I)^{-1}$
  smoothly bounding velocity while trading off instantaneous path tracking precision.