Sources and References

Primary textbooks — M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and Control, 2nd ed., Wiley, 2020; B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009. Supplementary texts — S. Boyd and L. Vandenberghe, Convex Optimization, Cambridge University Press, 2004 (free PDF: web.stanford.edu/~boyd/cvxbook); M. Egerstedt, Robot Ecology: Constraint-Based Design for Long-Duration Autonomy, Princeton University Press, 2021. Online resources — MIT OpenCourseWare 6.832 Underactuated Robotics (Tedrake); Stanford CS223A Introduction to Robotics (Khatib).

---

Chapter 1: Introduction to Robot Dynamics and Control

1.1 Scope of the Course

A robotic manipulator is a mechanical system whose motion is described by a chain of rigid bodies (links) connected by actuated joints. Understanding how such a system moves, how forces and torques relate to accelerations, and how to design feedback laws that make the robot track desired trajectories constitutes the core of robot dynamics and control.

The course is organized around five major themes. First, the kinematics of manipulators: relating joint variables to the pose (position and orientation) of the end-effector and, conversely, finding joint variables that achieve a desired pose. Second, the dynamics of manipulators: deriving the equations of motion. Third, the control of manipulators: designing torque laws that stabilize desired trajectories. Fourth, the kinematics and control of mobile robots, which introduce non-holonomic constraints absent from fixed-base manipulators. Fifth, optimization-based control: framing stability and safety requirements as quadratic programs (QPs) using control Lyapunov functions (CLFs) and control barrier functions (CBFs).

1.2 Notation and Conventions

Throughout these notes:

• Scalars are denoted by lowercase italic letters: \( q, \theta, \tau \).
• Column vectors are lowercase bold: \( \mathbf{q}, \boldsymbol{\omega} \).
• Matrices are uppercase bold: \( \mathbf{R}, \mathbf{J} \).
• The \( n \times n \) identity matrix is \( \mathbf{I}_n \).
• The skew-symmetric matrix associated with a vector \( \mathbf{a} = (a_1, a_2, a_3)^T \) is

\[ [\mathbf{a}]_\times = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}. \]

---

Chapter 2: Rigid Body Transformations

2.1 Rotation Matrices and the Group SO(3)

A rotation in three-dimensional space can be represented by a \( 3 \times 3 \) real matrix \( \mathbf{R} \) satisfying

\[ \mathbf{R}^T \mathbf{R} = \mathbf{I}_3, \quad \det(\mathbf{R}) = +1. \]

The set of all such matrices forms the special orthogonal group \( SO(3) \). This is a Lie group: it is a smooth manifold (dimension 3) with a group structure. The corresponding Lie algebra \( \mathfrak{so}(3) \) consists of all \( 3 \times 3 \) skew-symmetric matrices; the exponential map \( \exp: \mathfrak{so}(3) \to SO(3) \) is surjective.

Key properties of rotation matrices:

• Closure: If \( \mathbf{R}_1, \mathbf{R}_2 \in SO(3) \), then \( \mathbf{R}_1 \mathbf{R}_2 \in SO(3) \).
• Identity: \( \mathbf{I}_3 \in SO(3) \) (no rotation).
• Inverse: \( \mathbf{R}^{-1} = \mathbf{R}^T \in SO(3) \).
• Non-commutativity: In general \( \mathbf{R}_1 \mathbf{R}_2 \neq \mathbf{R}_2 \mathbf{R}_1 \).

2.1.1 Elementary Rotations

The basic rotations about the \(x\)-, \(y\)-, and \(z\)-axes by angle \(\theta\) are

\[ \mathbf{R}_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix}, \quad \mathbf{R}_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}, \]\[ \mathbf{R}_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}. \]

2.2 Euler Angle Representations

Since \(SO(3)\) is a 3-dimensional manifold, any rotation can be parameterized by three angles. Several conventions exist; the most common in robotics are ZYZ Euler angles and ZYX (roll-pitch-yaw) angles.

2.2.1 ZYZ Euler Angles

Given angles \(\phi, \theta, \psi\), the rotation is

\[ \mathbf{R}_{ZYZ}(\phi, \theta, \psi) = \mathbf{R}_z(\phi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_z(\psi). \]

Expanding explicitly and denoting \(c_\phi = \cos\phi\), \(s_\phi = \sin\phi\), etc.:

\[ \mathbf{R}_{ZYZ} = \begin{bmatrix} c_\phi c_\theta c_\psi - s_\phi s_\psi & -c_\phi c_\theta s_\psi - s_\phi c_\psi & c_\phi s_\theta \\ s_\phi c_\theta c_\psi + c_\phi s_\psi & -s_\phi c_\theta s_\psi + c_\phi c_\psi & s_\phi s_\theta \\ -s_\theta c_\psi & s_\theta s_\psi & c_\theta \end{bmatrix}. \]

The ZYZ representation has a gimbal lock singularity when \(\theta = 0\) or \(\theta = \pi\), where \(\phi\) and \(\psi\) are not individually recoverable.

2.2.2 Roll-Pitch-Yaw (ZYX) Angles

In the ZYX convention, the rotation is \(\mathbf{R} = \mathbf{R}_z(\psi)\mathbf{R}_y(\varphi)\mathbf{R}_x(\vartheta)\) where \(\vartheta\) (roll), \(\varphi\) (pitch), \(\psi\) (yaw) are fixed-axis rotations. This convention is widely used in aerospace and mobile robotics.

2.3 Axis-Angle Representation

Given unit axis \(\hat{\mathbf{k}} = (k_x, k_y, k_z)^T\) and angle \(\theta\), the rotation matrix is given by Rodrigues’ rotation formula:

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

This is the matrix exponential of the Lie algebra element \(\theta[\hat{\mathbf{k}}]_\times \in \mathfrak{so}(3)\):

\[ \mathbf{R}(\hat{\mathbf{k}}, \theta) = \exp\!\bigl(\theta[\hat{\mathbf{k}}]_\times\bigr). \]

Recovering axis and angle from \(\mathbf{R}\): The angle satisfies \(\cos\theta = \frac{\text{tr}(\mathbf{R}) - 1}{2}\), and for \(\theta \neq 0, \pi\) the axis is extracted from the skew-symmetric part:

\[ [\hat{\mathbf{k}}]_\times = \frac{1}{2\sin\theta}\bigl(\mathbf{R} - \mathbf{R}^T\bigr). \]

2.4 Unit Quaternions

A unit quaternion is a 4-tuple \(\mathbf{q} = (q_0, \mathbf{q}_v) = (q_0, q_1, q_2, q_3)\) with \(q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1\). It represents a rotation about axis \(\hat{\mathbf{k}}\) by angle \(\theta\) as

\[ q_0 = \cos\frac{\theta}{2}, \quad \mathbf{q}_v = \sin\frac{\theta}{2}\,\hat{\mathbf{k}}. \]

The corresponding rotation matrix is

\[ \mathbf{R}(\mathbf{q}) = (q_0^2 - \|\mathbf{q}_v\|^2)\mathbf{I} + 2\mathbf{q}_v\mathbf{q}_v^T + 2q_0[\mathbf{q}_v]_\times. \]

Quaternion composition \(\mathbf{q}_1 \otimes \mathbf{q}_2\) corresponds to composing the two rotations and avoids gimbal lock. Note that \(\mathbf{q}\) and \(-\mathbf{q}\) represent the same rotation (double cover of \(SO(3)\)).

2.5 Homogeneous Transformations

A rigid body transformation combines a rotation \(\mathbf{R} \in SO(3)\) and a translation \(\mathbf{p} \in \mathbb{R}^3\). It is convenient to represent it as a \( 4 \times 4 \) homogeneous transformation matrix:

\[ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3). \]

The set \(SE(3)\) of all such matrices forms the special Euclidean group. Composition of transformations is matrix multiplication. The inverse is

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

A point \(\mathbf{x} \in \mathbb{R}^3\) in one frame transforms to another via

\[ \begin{bmatrix} \mathbf{x}' \\ 1 \end{bmatrix} = \mathbf{T} \begin{bmatrix} \mathbf{x} \\ 1 \end{bmatrix}. \]

---

Chapter 3: Forward Kinematics

3.1 The Kinematic Chain

A serial manipulator consists of \(n\) rigid links connected by \(n\) joints (revolute or prismatic). Joint \(i\) connects link \(i-1\) (proximal) to link \(i\) (distal). The base frame is frame 0; the end-effector frame is frame \(n\). The vector of joint variables is \(\mathbf{q} = (q_1, \ldots, q_n)^T \in \mathbb{R}^n\).

Forward kinematics is the mapping \(\mathbf{q} \mapsto \mathbf{T}_0^n(\mathbf{q})\) giving the end-effector pose in the base frame.

3.2 Denavit-Hartenberg Convention

The Denavit-Hartenberg (DH) convention assigns coordinate frames to each link such that the transformation from frame \(i-1\) to frame \(i\) depends on only four parameters.

The homogeneous transformation from frame \(i-1\) to frame \(i\) is

\[ \mathbf{T}_{i-1}^i(\theta_i) = \mathbf{R}_z(\theta_i)\,\mathbf{T}_z(d_i)\,\mathbf{T}_x(a_i)\,\mathbf{R}_x(\alpha_i), \]

which expands to

\[ \mathbf{T}_{i-1}^i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}. \]

The overall forward kinematics is

\[ \mathbf{T}_0^n(\mathbf{q}) = \mathbf{T}_0^1(\theta_1)\,\mathbf{T}_1^2(\theta_2)\cdots\mathbf{T}_{n-1}^n(\theta_n). \]

3.3 Example: Planar 2R Manipulator

3.4 Product of Exponentials (PoE) Formulation

The PoE approach (Murray, Li, Sastry) avoids the frame-assignment conventions of DH and instead expresses each joint transformation as the exponential of a twist.

A twist \(\boldsymbol{\xi} \in \mathbb{R}^6\) represents an instantaneous screw motion. For revolute joint \(i\) with axis direction \(\hat{\boldsymbol{\omega}}_i\) and a point \(\mathbf{r}_i\) on the axis:

\[ \boldsymbol{\xi}_i = \begin{pmatrix} -\hat{\boldsymbol{\omega}}_i \times \mathbf{r}_i \\ \hat{\boldsymbol{\omega}}_i \end{pmatrix} \in \mathbb{R}^6. \]

The matrix form is the \(4\times4\) element of \(\mathfrak{se}(3)\):

\[ \hat{\boldsymbol{\xi}}_i = \begin{bmatrix} [\hat{\boldsymbol{\omega}}_i]_\times & \mathbf{v}_i \\ \mathbf{0}^T & 0 \end{bmatrix}. \]

The forward kinematics in PoE form is

\[ \mathbf{T}_0^n(\mathbf{q}) = e^{\hat{\boldsymbol{\xi}}_1 \theta_1} e^{\hat{\boldsymbol{\xi}}_2 \theta_2} \cdots e^{\hat{\boldsymbol{\xi}}_n \theta_n}\,\mathbf{T}_0^n(\mathbf{0}), \]

where \(\mathbf{T}_0^n(\mathbf{0})\) is the end-effector pose at the zero (reference) configuration.

---

Chapter 4: Differential Kinematics

4.1 The Geometric Jacobian

The Jacobian relates joint velocities \(\dot{\mathbf{q}}\) to the end-effector velocity. The geometric Jacobian \(\mathbf{J}_g(\mathbf{q}) \in \mathbb{R}^{6 \times n}\) is defined by

\[ \begin{pmatrix} \mathbf{v}_e \\ \boldsymbol{\omega}_e \end{pmatrix} = \mathbf{J}_g(\mathbf{q})\,\dot{\mathbf{q}}, \]

where \(\mathbf{v}_e\) is the linear velocity and \(\boldsymbol{\omega}_e\) is the angular velocity of the end-effector, both expressed in the base frame.

4.1.1 Column Construction

For a serial chain with \(n\) joints, each column \(\mathbf{j}_i\) of \(\mathbf{J}_g\) depends on the joint type:

• Revolute joint \(i\):

\[ \mathbf{j}_i = \begin{pmatrix} \hat{\mathbf{z}}_{i-1} \times (\mathbf{p}_e - \mathbf{p}_{i-1}) \\ \hat{\mathbf{z}}_{i-1} \end{pmatrix}, \]

where \(\hat{\mathbf{z}}_{i-1}\) is the unit vector along the \(z\)-axis of frame \(i-1\) expressed in frame 0, and \(\mathbf{p}_e - \mathbf{p}_{i-1}\) is the vector from the origin of frame \(i-1\) to the end-effector.

• Prismatic joint \(i\):

\[ \mathbf{j}_i = \begin{pmatrix} \hat{\mathbf{z}}_{i-1} \\ \mathbf{0} \end{pmatrix}. \]

4.1.2 Example: 2R Planar Arm

4.2 The Analytical Jacobian

If the end-effector orientation is parameterized by Euler angles \(\boldsymbol{\phi} = (\phi_1, \phi_2, \phi_3)^T\), then \(\boldsymbol{\omega}_e = \mathbf{T}(\boldsymbol{\phi})\dot{\boldsymbol{\phi}}\) for some transformation matrix \(\mathbf{T}(\boldsymbol{\phi})\). The analytical Jacobian relates \(\dot{\mathbf{q}}\) directly to \((\dot{\mathbf{p}}_e, \dot{\boldsymbol{\phi}})^T\):

\[ \mathbf{J}_a(\mathbf{q}) = \begin{bmatrix} \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{T}^{-1}(\boldsymbol{\phi}) \end{bmatrix} \mathbf{J}_g(\mathbf{q}). \]

This is useful when the task specification involves Euler angles rather than angular velocities.

4.3 Singularities

At singularities:

1. Certain end-effector velocities become unachievable regardless of joint velocities.
2. Small Cartesian velocities may require unbounded joint velocities.
3. The inverse kinematics solution becomes ill-conditioned.

Types of singularities:

• Boundary singularities: occur when the arm is fully extended or retracted (workspace boundary).
• Interior singularities: occur inside the workspace, often due to alignment of joint axes.

4.4 Manipulability

Yoshikawa’s manipulability measure quantifies how far a configuration is from singular:

\[ w(\mathbf{q}) = \sqrt{\det\!\bigl(\mathbf{J}_g \mathbf{J}_g^T\bigr)}. \]

At singularities \(w = 0\). The manipulability ellipsoid at configuration \(\mathbf{q}\) is defined by the singular value decomposition of \(\mathbf{J}_g\): its principal axes have lengths equal to the singular values \(\sigma_i\) of \(\mathbf{J}_g\), and they represent the directions and magnitudes of achievable end-effector velocities for unit joint velocities.

---

Chapter 5: Inverse Kinematics

5.1 Problem Statement

Inverse kinematics (IK) asks: given a desired end-effector pose \(\mathbf{T}_d\), find \(\mathbf{q}\) such that \(\mathbf{T}_0^n(\mathbf{q}) = \mathbf{T}_d\). This is generally a nonlinear problem admitting multiple solutions, a unique solution, or no solution.

The number of solutions depends on the robot geometry and workspace. A 6-DOF robot generically has up to 16 solutions for a given pose.

5.2 Closed-Form Solutions

Closed-form (analytical) solutions are preferred for real-time control. They exist for special geometries, notably when:

• Three consecutive joint axes intersect (e.g., a spherical wrist), or
• Three consecutive joint axes are parallel.

5.2.1 Wrist Decoupling

The PUMA-type robot has a spherical wrist (joints 4, 5, 6 intersect at the wrist center \(\mathbf{p}_w\)). The IK decouples:

1. Position: Solve for joints 1–3 using \(\mathbf{p}_w = \mathbf{p}_e - d_6 \hat{\mathbf{z}}_5\).
2. Orientation: Solve for joints 4–6 using the ZYZ Euler angles of \(\mathbf{R}_0^3{}^T\mathbf{R}_d\).

5.2.2 2R Planar Closed-Form

5.3 Numerical Inverse Kinematics

For general robots, IK is solved iteratively. Define the task error

\[ \mathbf{e}(\mathbf{q}) = \mathbf{x}_d - \mathbf{f}(\mathbf{q}), \]

where \(\mathbf{f}(\mathbf{q})\) is the forward kinematics function mapping to task space.

5.3.1 Newton-Raphson Method

The linearization \(\mathbf{e} \approx -\mathbf{J}\Delta\mathbf{q}\) gives the update rule

\[ \mathbf{q}_{k+1} = \mathbf{q}_k + \mathbf{J}^{-1}(\mathbf{q}_k)\,\mathbf{e}(\mathbf{q}_k) \quad (n=6, \mathbf{J} \text{ square and invertible}). \]

Convergence is quadratic near the solution but the method fails near singularities.

5.3.2 Damped Least-Squares (DLS)

Near singularities, small task errors can produce large joint velocities. The damped least-squares (Levenberg-Marquardt) solution replaces the exact inverse with

\[ \mathbf{J}^\dagger_\lambda = \mathbf{J}^T\!\bigl(\mathbf{J}\mathbf{J}^T + \lambda^2 \mathbf{I}\bigr)^{-1}, \]

yielding the update

\[ \Delta\mathbf{q} = \mathbf{J}^\dagger_\lambda\,\mathbf{e}. \]

The parameter \(\lambda > 0\) limits joint velocities at the cost of task accuracy. As \(\lambda \to 0\) the DLS reduces to the Moore-Penrose pseudoinverse.

5.3.3 Pseudoinverse and Null Space

For a redundant robot (\(n > 6\)), the minimum-norm joint velocity solution is

\[ \dot{\mathbf{q}} = \mathbf{J}^\dagger \dot{\mathbf{x}}_d + \bigl(\mathbf{I} - \mathbf{J}^\dagger \mathbf{J}\bigr)\mathbf{z}, \]

where \(\mathbf{J}^\dagger = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T)^{-1}\) is the right pseudoinverse (valid when \(\mathbf{J}\) has full row rank) and \(\mathbf{z}\) is an arbitrary vector. The term \((\mathbf{I} - \mathbf{J}^\dagger\mathbf{J})\mathbf{z}\) lies in the null space of \(\mathbf{J}\) and can be used to satisfy secondary objectives (avoid joint limits, avoid obstacles, maximize manipulability) without affecting the primary task.

---

Chapter 6: Dynamic Model of Manipulators

6.1 Overview

The dynamic model describes how torques and forces applied at the joints produce motion. For an \(n\)-DOF manipulator, the equations of motion take the standard form

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]

where \(\mathbf{M}\) is the symmetric positive-definite mass (inertia) matrix, \(\mathbf{C}\) contains Coriolis and centrifugal terms, \(\mathbf{g}\) is the gravity vector, and \(\boldsymbol{\tau}\) is the vector of joint torques.

6.2 Lagrangian Dynamics

6.2.1 Kinetic and Potential Energy

The kinetic energy of link \(i\) expressed in frame 0 is

\[ \mathcal{K}_i = \frac{1}{2}m_i \|\mathbf{v}_{c_i}\|^2 + \frac{1}{2}\boldsymbol{\omega}_i^T \,{}^0\mathbf{I}_{c_i}\, \boldsymbol{\omega}_i, \]

where \(m_i\) is the mass, \(\mathbf{v}_{c_i}\) is the velocity of the center of mass, \(\boldsymbol{\omega}_i\) is the angular velocity of the link, and \({}^0\mathbf{I}_{c_i}\) is the inertia tensor expressed in frame 0.

Expressing velocities via the Jacobian, the total kinetic energy is

\[ \mathcal{K}(\mathbf{q},\dot{\mathbf{q}}) = \frac{1}{2}\dot{\mathbf{q}}^T \mathbf{M}(\mathbf{q})\dot{\mathbf{q}}, \]

where

\[ \mathbf{M}(\mathbf{q}) = \sum_{i=1}^n \bigl(m_i \mathbf{J}_{v_i}^T \mathbf{J}_{v_i} + \mathbf{J}_{\omega_i}^T \,{}^0\mathbf{I}_{c_i}\, \mathbf{J}_{\omega_i}\bigr), \]

with \(\mathbf{J}_{v_i}\) and \(\mathbf{J}_{\omega_i}\) being the Jacobians mapping joint velocities to linear and angular velocities of the center of mass of link \(i\).

The potential energy is \(\mathcal{V}(\mathbf{q}) = -\sum_{i=1}^n m_i \mathbf{g}_0^T \mathbf{p}_{c_i}(\mathbf{q})\), where \(\mathbf{g}_0\) is the gravity vector.

6.2.2 Euler-Lagrange Equations

The Lagrangian is \(\mathcal{L} = \mathcal{K} - \mathcal{V}\). The equations of motion follow from

\[ \frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}_i} - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i, \quad i = 1, \ldots, n. \]

Carrying out the differentiation yields \(\mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} = \boldsymbol{\tau}\), with the Christoffel symbol form:

\[ c_{ijk} = \frac{1}{2}\!\left(\frac{\partial m_{ij}}{\partial q_k} + \frac{\partial m_{ik}}{\partial q_j} - \frac{\partial m_{jk}}{\partial q_i}\right), \quad C_{ij}(\mathbf{q},\dot{\mathbf{q}}) = \sum_{k=1}^n c_{ijk}\dot{q}_k. \]

An important property: \(\dot{\mathbf{M}} - 2\mathbf{C}\) is skew-symmetric, which is central to passivity-based control.

6.3 Newton-Euler Recursive Algorithm

The Newton-Euler method is computationally efficient (\(O(n)\) operations vs. \(O(n^4)\) for Lagrange in naive form) and proceeds in two passes.

6.3.1 Outward Recursion (velocities and accelerations)

Starting from the base (\(i=0\): \(\boldsymbol{\omega}_0 = \ddot{\mathbf{p}}_0 = \mathbf{0}\), \(\dot{\boldsymbol{\omega}}_0 = \mathbf{0}\), \(\ddot{\mathbf{p}}_0 = -\mathbf{g}_0\) to absorb gravity), propagate outward for \(i = 1, \ldots, n\):

Revolute joint:

\[ \boldsymbol{\omega}_i = \mathbf{R}_{i-1}^i{}^T \boldsymbol{\omega}_{i-1} + \dot{\theta}_i\hat{\mathbf{z}}_i, \]\[ \dot{\boldsymbol{\omega}}_i = \mathbf{R}_{i-1}^i{}^T \dot{\boldsymbol{\omega}}_{i-1} + \mathbf{R}_{i-1}^i{}^T\boldsymbol{\omega}_{i-1} \times \dot{\theta}_i\hat{\mathbf{z}}_i + \ddot{\theta}_i\hat{\mathbf{z}}_i, \]\[ \ddot{\mathbf{p}}_{c_i} = \ddot{\mathbf{p}}_i + \dot{\boldsymbol{\omega}}_i \times \mathbf{r}_{c_i} + \boldsymbol{\omega}_i \times (\boldsymbol{\omega}_i \times \mathbf{r}_{c_i}). \]

6.3.2 Inward Recursion (forces and torques)

Propagate inward for \(i = n, \ldots, 1\):

\[ \mathbf{f}_i = m_i \ddot{\mathbf{p}}_{c_i} + \mathbf{R}_i^{i+1}\mathbf{f}_{i+1}, \]\[ \boldsymbol{\mu}_i = \mathbf{I}_{c_i}\dot{\boldsymbol{\omega}}_i + \boldsymbol{\omega}_i \times \mathbf{I}_{c_i}\boldsymbol{\omega}_i + \mathbf{r}_{c_i} \times \mathbf{f}_i + \mathbf{R}_i^{i+1}\boldsymbol{\mu}_{i+1} + \mathbf{r}_{i+1} \times \mathbf{R}_i^{i+1}\mathbf{f}_{i+1}, \]

and the joint torque is \(\tau_i = \boldsymbol{\mu}_i^T \hat{\mathbf{z}}_i\) for revolute joints.

---

Chapter 7: Control of Manipulators

7.1 Decentralized Control

Decentralized (independent joint) control treats each joint as an independent SISO system, ignoring the coupling between joints. This is a practical first approximation, especially at low speeds where Coriolis terms are small.

7.1.1 PD Control

The simplest joint-space controller is proportional-derivative (PD):

\[ \tau_i = K_{p_i}(q_{d_i} - q_i) + K_{d_i}(\dot{q}_{d_i} - \dot{q}_i). \]

In matrix form: \(\boldsymbol{\tau} = \mathbf{K}_p \mathbf{e} + \mathbf{K}_d \dot{\mathbf{e}}\), where \(\mathbf{e} = \mathbf{q}_d - \mathbf{q}\) and \(\mathbf{K}_p, \mathbf{K}_d\) are positive-definite diagonal matrices.

7.1.2 PD with Gravity Compensation

Pure PD control has steady-state error in the presence of gravity. Adding a feedforward gravity term eliminates this:

\[ \boldsymbol{\tau} = \mathbf{K}_p \mathbf{e} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{g}(\mathbf{q}). \]

7.2 Computed-Torque Control (Inverse Dynamics)

Computed-torque control achieves exact linearization of the manipulator dynamics. Define

\[ \boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\mathbf{a} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}), \]

where \(\mathbf{a}\) is an auxiliary control input. Substituting into \(\mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} = \boldsymbol{\tau}\) yields the double-integrator

\[ \ddot{\mathbf{q}} = \mathbf{a}. \]

Now choose \(\mathbf{a} = \ddot{\mathbf{q}}_d + \mathbf{K}_d(\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) + \mathbf{K}_p(\mathbf{q}_d - \mathbf{q})\) to get the closed-loop error dynamics

\[ \ddot{\mathbf{e}} + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e} = \mathbf{0}. \]

With \(\mathbf{K}_p, \mathbf{K}_d\) chosen so the characteristic polynomial is Hurwitz, the error converges exponentially to zero. The gain matrices decouple: each joint error evolves independently as a linear second-order system.

7.3 Operational Space Control

7.3.1 Task-Space Dynamics

In many applications, the task is specified in Cartesian (operational) space. Define the task-space coordinates \(\mathbf{x} = \mathbf{f}(\mathbf{q}) \in \mathbb{R}^m\) and the task-space error \(\mathbf{e}_x = \mathbf{x}_d - \mathbf{x}\).

The task-space dynamics are obtained by differentiating \(\dot{\mathbf{x}} = \mathbf{J}\dot{\mathbf{q}}\):

\[ \ddot{\mathbf{x}} = \mathbf{J}\ddot{\mathbf{q}} + \dot{\mathbf{J}}\dot{\mathbf{q}}. \]

Substituting the manipulator dynamics:

\[ \boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}) = \mathbf{F}, \]

where the task-space inertia matrix \(\boldsymbol{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}\), and \(\mathbf{F} = (\mathbf{J}^T)^\dagger \boldsymbol{\tau}\) is the operational-space generalized force.

7.3.2 Pseudoinverse Control

The joint torque that realizes a desired task-space force \(\mathbf{F}_d\) is

\[ \boldsymbol{\tau} = \mathbf{J}^T\mathbf{F}_d + (\mathbf{I} - \mathbf{J}^T(\mathbf{J}^T)^\dagger)\boldsymbol{\tau}_0, \]

where \((\mathbf{J}^T)^\dagger = (\mathbf{J}\mathbf{J}^T)^{-1}\mathbf{J}\) (left pseudoinverse of \(\mathbf{J}^T\)) and \(\boldsymbol{\tau}_0\) is an arbitrary null-space torque used for secondary objectives.

7.3.3 Task-Priority Framework

When there are multiple tasks with different priorities, the task-priority framework (Nakamura, Hanafusa) handles conflicts. For two tasks with Jacobians \(\mathbf{J}_1\) (primary) and \(\mathbf{J}_2\) (secondary):

\[ \dot{\mathbf{q}} = \mathbf{J}_1^\dagger \dot{\mathbf{x}}_1 + (\mathbf{I} - \mathbf{J}_1^\dagger \mathbf{J}_1)\mathbf{J}_2^\dagger \bigl(\dot{\mathbf{x}}_2 - \mathbf{J}_2\mathbf{J}_1^\dagger\dot{\mathbf{x}}_1\bigr). \]

The primary task is executed exactly; the secondary task is pursued in the null space of \(\mathbf{J}_1\).

7.4 Impedance Control

Impedance control (Hogan 1985) regulates the dynamic relationship (mechanical impedance) between the end-effector and the environment rather than tracking a trajectory directly. The desired behavior is a mass-spring-damper:

\[ \mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{D}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{F}_{ext}, \]

where \(\mathbf{M}_d, \mathbf{D}_d, \mathbf{K}_d\) are desired virtual inertia, damping, and stiffness matrices, and \(\mathbf{F}_{ext}\) is the contact force from the environment. The controller shapes the robot to behave like this spring-damper, making it compliant. Impedance control subsumes both pure force and pure position control as limiting cases.

---

Chapter 8: Kinematics of Mobile Robots

8.1 Non-Holonomic Constraints

A mechanical system with generalized coordinates \(\mathbf{q} \in \mathbb{R}^n\) has a non-holonomic constraint if its velocity is restricted by a condition of the form \(\mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = \mathbf{0}\) that cannot be integrated into a constraint on \(\mathbf{q}\) alone (i.e., it is not of the form \(h(\mathbf{q}) = 0\)).

The canonical example is a wheel rolling without slipping: the wheel can roll forward and turn, but cannot slide sideways. The constraint is non-integrable: despite the instantaneous restriction on velocity, the wheel can reach any position and orientation in the plane — the system is controllable but not accessible in the state space by straightforward integration.

8.2 Unicycle Model

The simplest mobile robot model is the unicycle: a point mass constrained to move in the direction it is pointing. The configuration is \(\mathbf{q} = (x, y, \theta)^T\), where \((x,y)\) is the position and \(\theta\) is the heading angle. The kinematic model is

\[ \begin{pmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{pmatrix} = \begin{pmatrix} \cos\theta \\ \sin\theta \\ 0 \end{pmatrix} v + \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \omega, \]

where \(v\) is the forward (linear) velocity and \(\omega\) is the angular velocity. This is a driftless control-affine system:

\[ \dot{\mathbf{q}} = g_1(\mathbf{q})\,v + g_2(\mathbf{q})\,\omega. \]

The non-holonomic constraint is the no-sideslip condition: \(-\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0\).

8.3 Differential Drive Model

A differential-drive robot has two independently driven wheels of radius \(r\), separated by a distance \(2b\). The wheel angular velocities \(\omega_R, \omega_L\) relate to \(v\) and \(\omega\) by

\[ v = \frac{r(\omega_R + \omega_L)}{2}, \quad \omega = \frac{r(\omega_R - \omega_L)}{2b}. \]

The configuration kinematics are identical to the unicycle above.

8.4 Bicycle Model

The bicycle model (Ackermann steering) has a front steerable wheel and a rear driving wheel, with wheelbase \(L\). Configuration \((x, y, \theta, \phi)\) where \(\phi\) is the steering angle:

\[ \dot{x} = v\cos\theta, \quad \dot{y} = v\sin\theta, \quad \dot{\theta} = \frac{v}{L}\tan\phi, \quad \dot{\phi} = u_\phi, \]

where \(v\) is the rear wheel speed and \(u_\phi\) is the steering rate input. This is the standard model for car-like robots.

---

Chapter 9: Controllability and Control of Mobile Robots

9.1 Lie Brackets and the Controllability of Driftless Systems

Consider a driftless control-affine system

\[ \dot{\mathbf{q}} = \sum_{i=1}^m g_i(\mathbf{q})\,u_i, \quad \mathbf{q} \in \mathbb{R}^n, \; \mathbf{u} \in \mathbb{R}^m. \]

With \(m < n\) (underactuated), the system has fewer inputs than states. The Lie bracket of two vector fields \(g_1, g_2\) is

\[ [g_1, g_2](\mathbf{q}) = \frac{\partial g_2}{\partial \mathbf{q}} g_1(\mathbf{q}) - \frac{\partial g_1}{\partial \mathbf{q}} g_2(\mathbf{q}). \]

Physically, the Lie bracket of two input directions corresponds to the net infinitesimal displacement obtained by applying \(g_1\) for \(\epsilon\), then \(g_2\) for \(\epsilon\), then \(-g_1\) for \(\epsilon\), then \(-g_2\) for \(\epsilon\); to first order the robot returns to its start, but at second order there is a net displacement in the direction \([g_1, g_2]\).

9.1.1 Unicycle Controllability Verification

For the unicycle, \(g_1 = (\cos\theta, \sin\theta, 0)^T\) and \(g_2 = (0, 0, 1)^T\). Compute:

\[ [g_1, g_2] = \frac{\partial g_2}{\partial \mathbf{q}} g_1 - \frac{\partial g_1}{\partial \mathbf{q}} g_2 = \begin{bmatrix}0\\0\\0\end{bmatrix}\cdot g_1 - \begin{bmatrix} 0 & 0 & -\sin\theta \\ 0 & 0 & \cos\theta \\ 0 & 0 & 0 \end{bmatrix} g_2 = \begin{pmatrix} \sin\theta \\ -\cos\theta \\ 0 \end{pmatrix}. \]

The three vector fields \(\{g_1, g_2, [g_1,g_2]\}\) span \(\mathbb{R}^3\) for all \(\theta\) (the determinant is \(\pm 1 \neq 0\)). By Chow’s theorem, the unicycle is STLC.

9.2 Brockett’s Obstruction

Despite being controllable, the unicycle (and non-holonomic systems generally) cannot be asymptotically stabilized to a point by a smooth (or even continuous) static state-feedback law.

For the unicycle, the map \((x,y,\theta,v,\omega)\mapsto (v\cos\theta, v\sin\theta, \omega)\) does not surject onto a neighborhood of the origin in \(\mathbb{R}^3\) (the image misses vectors not in the span of \((\cos\theta,\sin\theta,0)^T\) and \((0,0,1)^T\) for any fixed \(\theta\)). Therefore, smooth static feedback stabilization is impossible.

This necessitates the use of:

• Time-varying feedback (sinusoidal inputs),
• Discontinuous feedback (hybrid/switched controllers), or
• Trajectory tracking (which bypasses the obstruction).

9.3 Sinusoidal Steering and Open-Loop Maneuvers

One classical approach for non-holonomic systems is sinusoidal steering (Murray and Sastry). For a chained-form system, inputs are chosen as sinusoids at different frequencies to decouple the effect on each state.

For the unicycle, an open-loop maneuver to move from \((0,0,0)\) to \((x_f, y_f, 0)\) can be constructed by a sequence of:

1. Rotate to face \((x_f, y_f)\).
2. Drive straight to the target.
3. Rotate to the original orientation.

Such maneuvers are concatenations of primitives (straight lines and in-place rotations), exploiting the controllability.

---

Chapter 10: Differential Flatness and Trajectory Planning

10.1 Differentially Flat Systems

If a system is flat, trajectory planning reduces to choosing a smooth curve for \(\mathbf{y}(t)\) in the flat output space (without solving a differential equation) and then recovering the state and input trajectories algebraically.

10.2 Unicycle Flat Output

The unicycle \((\dot{x}, \dot{y}, \dot{\theta}) = (v\cos\theta, v\sin\theta, \omega)\) is differentially flat with flat output \(\mathbf{y} = (x, y)^T\) (the position of the center). Given a trajectory \((x(t), y(t))\):

\[ v = \sqrt{\dot{x}^2 + \dot{y}^2}, \quad \theta = \text{atan2}(\dot{y}, \dot{x}), \quad \omega = \frac{\dot{x}\ddot{y} - \dot{y}\ddot{x}}{\dot{x}^2 + \dot{y}^2}. \]

Thus, planning a smooth path for \((x,y)\) directly gives \(v, \omega\) without integration of the kinematic equations.

10.3 Flatness-Based Tracking Control

Given a desired flat output trajectory \(\mathbf{y}_d(t)\) and the corresponding nominal input \(\mathbf{u}_d(t)\) and state \(\mathbf{q}_d(t)\), design a tracking controller as follows:

1. Compute the nominal feedforward: \(\mathbf{u}_{ff}(t) = \phi_u(\mathbf{y}_d, \dot{\mathbf{y}}_d, \ldots)\).
2. Add a feedback correction in the flat output space to correct for deviations: \(\mathbf{u} = \mathbf{u}_{ff} + \mathbf{u}_{fb}(\mathbf{y} - \mathbf{y}_d, \dot{\mathbf{y}} - \dot{\mathbf{y}}_d)\).

For a unicycle near the nominal trajectory, linearize the error dynamics in the flat output space to get a pair of decoupled double integrators (position errors), and design \(\mathbf{u}_{fb}\) using standard linear control.

---

Chapter 11: Optimization-Based Robot Control

11.1 Mathematical Optimization Background

A general optimization problem has the form

\[ \min_{\mathbf{u} \in \mathbb{R}^m} \quad f_0(\mathbf{u}) \quad \text{subject to} \quad f_i(\mathbf{u}) \leq 0, \; i=1,\ldots,p, \quad h_j(\mathbf{u}) = 0, \; j=1,\ldots,q. \]

A convex program has convex \(f_0, f_1, \ldots, f_p\) and affine equality constraints. Convex programs have the property that every local minimum is a global minimum, and they can be solved efficiently (polynomial time in problem size for interior-point methods).

A quadratic program (QP) is a convex program with a quadratic objective and linear constraints:

\[ \min_{\mathbf{u}} \quad \frac{1}{2}\mathbf{u}^T\mathbf{H}\mathbf{u} + \mathbf{c}^T\mathbf{u} \quad \text{s.t.} \quad \mathbf{A}\mathbf{u} \leq \mathbf{b}, \quad \mathbf{C}\mathbf{u} = \mathbf{d}, \]

where \(\mathbf{H} \succ 0\). QPs arise naturally in robotics when stability and safety constraints are affine in the control input.

11.2 Control Lyapunov Functions (CLFs)

The existence of a CLF is equivalent to the existence of a smooth stabilizing feedback (Artstein’s theorem). Denote

\[ L_f V = \frac{\partial V}{\partial \mathbf{x}} f(\mathbf{x}), \quad L_g V = \frac{\partial V}{\partial \mathbf{x}} g(\mathbf{x}). \]

Then the CLF condition requires \(\inf_\mathbf{u} [L_f V + L_g V \mathbf{u}] < 0\) for all \(\mathbf{x} \neq 0\), which when \(L_g V \neq 0\) can always be satisfied by choosing \(\mathbf{u}\) to make \(L_g V \mathbf{u}\) sufficiently negative.

11.2.1 Sontag’s Universal Formula

Given a CLF \(V\), an explicit stabilizing feedback is given by Sontag’s universal formula:

\[ k(\mathbf{x}) = \begin{cases} -\dfrac{L_f V + \sqrt{(L_f V)^2 + (L_g V)(L_g V)^T}}{(L_g V)(L_g V)^T}(L_g V)^T & \text{if } L_g V \neq \mathbf{0}, \\ \mathbf{0} & \text{if } L_g V = \mathbf{0}. \end{cases} \]

This formula is smooth away from the origin and produces the minimum-norm \(\mathbf{u}\) that achieves \(\dot{V} < 0\).

11.2.2 CLF as a QP Constraint

The CLF decay condition \(\dot{V} \leq -\gamma V\) (exponential convergence rate \(\gamma > 0\)) is affine in \(\mathbf{u}\):

\[ L_f V(\mathbf{x}) + L_g V(\mathbf{x})\,\mathbf{u} \leq -\gamma V(\mathbf{x}). \]

This single affine inequality in \(\mathbf{u}\) is a QP constraint. A CLF-based QP minimizes control effort subject to this stability constraint:

\[ \min_{\mathbf{u}} \quad \frac{1}{2}\|\mathbf{u}\|^2 \quad \text{s.t.} \quad L_f V + L_g V\,\mathbf{u} \leq -\gamma V(\mathbf{x}). \]

11.3 Control Barrier Functions (CBFs)

While CLFs encode stability (go to a desired point), CBFs encode safety (stay within a safe set).

Proof sketch: Consider \(\dot{h} = L_f h + L_g h \mathbf{u} \geq -\alpha(h)\). This is a differential inequality of the form \(\dot{h} \geq -\alpha(h)\). By comparison lemma, if \(h(\mathbf{x}(0)) \geq 0\), then \(h(\mathbf{x}(t)) \geq 0\) for all \(t \geq 0\). Hence \(\mathbf{x}(t) \in \mathcal{C}\).

The CBF constraint is affine in \(\mathbf{u}\): \(L_g h\,\mathbf{u} \geq -(L_f h + \alpha(h))\). Common choices are \(\alpha(s) = \kappa s\) for \(\kappa > 0\) (linear class-\(\mathcal{K}\)).

11.3.1 CBF-QP for Safety

The minimum-norm safe controller is

\[ \mathbf{u}^*(\mathbf{x}) = \arg\min_{\mathbf{u}} \frac{1}{2}\|\mathbf{u}\|^2 \quad \text{s.t.} \quad L_f h + L_g h\,\mathbf{u} + \kappa h(\mathbf{x}) \geq 0. \]

By KKT conditions, the solution is

\[ \mathbf{u}^* = \begin{cases} \mathbf{0} & \text{if } L_f h + \kappa h \geq 0, \\ -\dfrac{L_f h + \kappa h}{\|L_g h\|^2}(L_g h)^T & \text{otherwise.} \end{cases} \]

This is a smooth safety filter: it does nothing if the system is already safe, and applies the minimum correction to maintain safety otherwise.

11.4 CLF-CBF Quadratic Programs

The most powerful formulation combines stability and safety by simultaneously enforcing both CLF and CBF constraints in a single QP:

\[ \mathbf{u}^*(\mathbf{x}) = \arg\min_{\mathbf{u},\, \delta} \quad \frac{1}{2}\mathbf{u}^T\mathbf{H}\mathbf{u} + p\delta^2 \]\[ \text{s.t.} \quad L_f V + L_g V\,\mathbf{u} \leq -\gamma V + \delta \quad \text{(CLF, relaxed)}, \]\[ \quad\quad\; L_f h + L_g h\,\mathbf{u} + \alpha(h) \geq 0 \quad\quad \text{(CBF, hard)}, \]

where \(\delta \in \mathbb{R}\) is a slack variable with large penalty \(p \gg 1\). The CLF constraint is soft (relaxed by \(\delta\)) to ensure feasibility when safety and stability conflict; the CBF constraint is hard (always enforced).

11.4.1 Multiple CBF Constraints

Multiple safety constraints \(h_1(\mathbf{x}) \geq 0, \ldots, h_k(\mathbf{x}) \geq 0\) can be added as separate CBF constraints:

\[ L_f h_i + L_g h_i\,\mathbf{u} + \alpha_i(h_i) \geq 0, \quad i = 1, \ldots, k. \]

Each constraint is affine in \(\mathbf{u}\), so the overall QP remains a QP and can be solved in real time.

11.4.2 Connection to Egerstedt’s Robot Ecology

M. Egerstedt’s framework of constraint-based design views robot behavior as the intersection of multiple safety and performance constraints (a “constraint zoo”), with the CBF-QP as the unifying computational tool. Long-duration autonomy requires safety guarantees that persist over time, which CBFs provide by design: the safe set is positively invariant under any controller satisfying the CBF inequality.

---

Chapter 12: Synthesis and Advanced Topics

12.1 Putting It All Together: A CLF-CBF Controller for a Mobile Manipulator

Consider a mobile manipulator: a robot arm mounted on a differential-drive base. The full system state is \(\mathbf{x} = (\mathbf{q}_{base}, \mathbf{q}_{arm}, \dot{\mathbf{q}}_{arm})\). A unified controller can be designed as:

1. End-effector tracking CLF: Define \(V_{ee}(\mathbf{x}) = \|\mathbf{p}_e(\mathbf{x}) - \mathbf{p}_d(t)\|^2 + \text{orientation error}\). The CLF constraint enforces convergence of the end-effector to the desired pose.

2. Obstacle avoidance CBF: For each obstacle at position \(\mathbf{p}_{obs}\) with radius \(r_{obs}\), define \(h_{obs}(\mathbf{x}) = \|\mathbf{p}_e - \mathbf{p}_{obs}\|^2 - (r_{obs} + r_{safe})^2\). The CBF constraint keeps the end-effector outside a safety buffer.

3. Joint limit CBF: For each joint \(i\), define \(h_i^+(\mathbf{x}) = q_{i,max} - q_i\) and \(h_i^-(\mathbf{x}) = q_i - q_{i,min}\). These CBF constraints prevent joint limit violations.

The combined CLF-CBF QP solves for the minimum-effort control simultaneously satisfying all constraints.

12.2 Summary of Key Results

Topic	Key Result
SO(3)	Rotation matrix: \(\mathbf{R}^T\mathbf{R}=\mathbf{I}\), \(\det\mathbf{R}=1\)
Rodrigues	\(\exp(\theta[\hat{\mathbf{k}}]_\times) = \mathbf{I} + \sin\theta[\hat{\mathbf{k}}]_\times + (1-\cos\theta)[\hat{\mathbf{k}}]_\times^2\)
DH	4 parameters per joint; \(\mathbf{T}_0^n = \prod_i \mathbf{T}_{i-1}^i\)
Jacobian	\((\mathbf{v}_e, \boldsymbol{\omega}_e)^T = \mathbf{J}_g \dot{\mathbf{q}}\); singular when \(\text{rank}(\mathbf{J}_g) < 6\)
Dynamics	\(\mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} = \boldsymbol{\tau}\); \(\dot{\mathbf{M}} - 2\mathbf{C}\) skew-symmetric
PD+g	Lyapunov stable via \(V = \frac{1}{2}\dot{\mathbf{q}}^T\mathbf{M}\dot{\mathbf{q}} + \frac{1}{2}\mathbf{e}^T\mathbf{K}_p\mathbf{e}\)
CTC	Exact linearization; \(\ddot{\mathbf{e}} + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e} = 0\)
Unicycle	Non-holonomic; Brockett: no smooth static stabilizer
Chow	LARC \(\Rightarrow\) STLC for driftless systems
Flatness	\((x,y)\) flat output for unicycle; trajectory planning without ODE integration
CLF	\(V > 0\), \(\inf_\mathbf{u} [L_f V + L_g V \mathbf{u}] < 0\) \(\Rightarrow\) stabilizable
CBF	\(h \geq 0\) safe set; \(\dot{h} + \kappa h \geq 0\) \(\Rightarrow\) forward invariance
CLF-CBF QP	Real-time, minimum-norm control satisfying stability + safety

12.3 Worked Example: CLF-CBF QP for Unicycle

12.4 Connections Between Parts

The five parts of ECE 486 form a coherent progression:

• Parts I–II (Manipulator dynamics and control) build the foundation: how to model and control rigid-body mechanical systems with exact kinematic and dynamic models.

• Part III (Mobile robot kinematics) introduces non-holonomic constraints, which fundamentally change what is achievable with smooth feedback controllers. This motivates studying Lie algebraic tools for analyzing controllability.

• Part IV (Mobile robot control) develops constructive control methods: sinusoidal steering exploits the Lie bracket structure to achieve motion in otherwise inaccessible directions; differential flatness enables systematic trajectory planning.

• Part V (Optimization-based control) provides a unified computational framework. Rather than designing task-specific controllers for each situation, one encodes stability (CLF) and safety (CBF) as affine constraints and solves a QP in real time. This is particularly powerful for systems with multiple simultaneous objectives, which is the norm for real-world robots operating in complex, dynamic environments.

---

Appendix A: Mathematical Prerequisites

A.1 Matrix Calculus

The gradient of a scalar function \(f: \mathbb{R}^n \to \mathbb{R}\) is the row vector \(\frac{\partial f}{\partial \mathbf{x}} \in \mathbb{R}^{1 \times n}\). The Jacobian of a vector function \(\mathbf{f}: \mathbb{R}^n \to \mathbb{R}^m\) is the \(m \times n\) matrix \(\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\) with \((i,j)\) entry \(\frac{\partial f_i}{\partial x_j}\).

A.2 Skew-Symmetric Matrices

For \(\mathbf{a}, \mathbf{b} \in \mathbb{R}^3\): \([\mathbf{a}]_\times \mathbf{b} = \mathbf{a} \times \mathbf{b}\). Key identities:

• \([\mathbf{a}]_\times^T = -[\mathbf{a}]_\times\).
• \([\mathbf{a}]_\times^2 = \mathbf{a}\mathbf{a}^T - \|\mathbf{a}\|^2\mathbf{I}\) (for unit \(\hat{\mathbf{a}}\): \([\hat{\mathbf{a}}]_\times^2 = \hat{\mathbf{a}}\hat{\mathbf{a}}^T - \mathbf{I}\)).
• \(\mathbf{R}[\mathbf{a}]_\times\mathbf{R}^T = [\mathbf{R}\mathbf{a}]_\times\) for any \(\mathbf{R} \in SO(3)\).

A.3 Lyapunov Stability Theory

A.4 Class K and Class KL Functions

A function \(\alpha: [0, a) \to \mathbb{R}_{\geq 0}\) is of class \(\mathcal{K}\) if it is continuous, strictly increasing, and \(\alpha(0) = 0\). It is of class \(\mathcal{K}_\infty\) if additionally \(a = \infty\) and \(\alpha(s) \to \infty\) as \(s \to \infty\). These appear in the CBF definition as the comparison function governing the rate at which the safety margin is allowed to decrease.

---

Appendix B: Quick Reference — DH Parameters for Common Robots

B.1 PUMA 560 (Approximate)

\(i\)	\(a_i\) (m)	\(\alpha_i\) (rad)	\(d_i\) (m)	\(\theta_i\)
1	0	\(\pi/2\)	0	\(\theta_1\)
2	0.4318	0	0	\(\theta_2\)
3	-0.0203	\(\pi/2\)	0.1502	\(\theta_3\)
4	0	\(-\pi/2\)	0.4318	\(\theta_4\)
5	0	\(\pi/2\)	0	\(\theta_5\)
6	0	0	0	\(\theta_6\)

The PUMA has a spherical wrist (joints 4–5–6 axes intersect), enabling wrist-decoupled inverse kinematics.

B.2 Planar 3R Robot

\(i\)	\(a_i\)	\(\alpha_i\)	\(d_i\)	\(\theta_i\)
1	\(l_1\)	0	0	\(\theta_1\)
2	\(l_2\)	0	0	\(\theta_2\)
3	\(l_3\)	0	0	\(\theta_3\)

Forward kinematics:

\[ x_e = l_1 c_1 + l_2 c_{12} + l_3 c_{123}, \quad y_e = l_1 s_1 + l_2 s_{12} + l_3 s_{123}, \quad \phi_e = \theta_1 + \theta_2 + \theta_3, \]

where \(c_{12} = \cos(\theta_1+\theta_2)\), etc. This is redundant (\(n=3\), task dimension 3 for planar position + orientation) and admits a 1-dimensional null space for any non-singular configuration.