Sources and References

Primary textbook — B. Siciliano, L. Sciavicco, L. Villani & G. Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009. Reference texts — M.W. Spong, S. Hutchinson & M. Vidyasagar, Robot Modeling and Control, Wiley, 2006; J.J. Craig, Introduction to Robotics, 4th ed., Prentice-Hall, 2018. Online resources — Stanford CS223A (Khatib); Murray, Li & Sastry, A Mathematical Introduction to Robotic Manipulation (open access); MIT OCW 6.832.

---

Chapter 1: Introduction to Robot Manipulators

1.1 What Is a Robot Manipulator?

A robot manipulator is a mechanically programmable device consisting of a series of rigid bodies — called links — connected by joints that permit relative motion. Unlike mobile platforms that move through an environment, a manipulator is typically fixed to a base and moves its end-effector through a prescribed workspace. The end-effector may be a gripper, welding torch, surgical tool, or any task-specific device. The goal of manipulation is to position and orient that end-effector with prescribed accuracy, speed, and force — tasks that demand a unified treatment of geometry, mechanics, and control.

Industrial robots have been in widespread use since the 1960s (Unimate, 1961). Modern applications span automotive assembly, semiconductor fabrication, laparoscopic surgery (the da Vinci system), deep-sea inspection, and planetary exploration (the Mars rovers). In every case, the same mathematical foundations apply: coordinate geometry to describe poses, differential geometry to relate velocities, Lagrangian or Newtonian mechanics to derive equations of motion, and control theory to ensure accurate tracking.

This course treats all four layers in depth. We begin with the geometry of rigid-body motion, progress through forward and inverse kinematics, differential kinematics and the Jacobian, trajectory planning, and then dynamics — both Lagrangian and Newton-Euler — before addressing motion control and force control.

1.2 Joint Types and Robot Classification

1.2.1 Revolute and Prismatic Joints

Every joint in a serial manipulator provides one degree of freedom (DOF). The two dominant types are:

• A revolute (R) joint allows rotation about a fixed axis. The joint variable is an angle \(\theta_i \in \mathbb{R}\), typically constrained to a finite range.
• A prismatic (P) joint allows translation along a fixed axis. The joint variable is a displacement \(d_i \in \mathbb{R}\), again range-limited by mechanical stops.

Spherical joints (3 DOF) and universal joints (2 DOF) also appear in parallel mechanisms and in wrist assemblies, but serial robots are most commonly built from R and P joints.

1.2.2 Serial vs. Parallel Architectures

A serial (open-chain) manipulator has links connected in a single chain from base to end-effector. Each joint contributes one DOF and the structure is kinematically straightforward to analyze. The PUMA 560, KUKA KR 6, and most industrial arms are serial.

A parallel manipulator connects the base to the end-effector through multiple independent kinematic chains (legs). The Stewart-Gough platform, used in flight simulators, and the Delta robot, used in high-speed pick-and-place, are canonical examples. Parallel robots offer greater stiffness and higher payload-to-weight ratios but have smaller workspaces and more complex kinematics.

This course focuses on serial manipulators, which remain the dominant architecture in industrial and research settings.

1.2.3 Common Configurations

Standard robot configurations are named by their joint sequence:

• RRR (anthropomorphic/articulated): Three revolute joints mimic the human shoulder, elbow, and wrist. The PUMA 560 uses this arrangement in the first three joints.
• RRP (SCARA): Selective Compliance Assembly Robot Arm. Two revolute joints provide planar positioning; one prismatic joint provides vertical stroke. Ideal for assembly tasks.
• PRR (cylindrical): One prismatic base joint with two revolute joints. The workspace is a portion of a cylinder.
• PRP / PPP (Cartesian): Prismatic joints along three orthogonal axes. The workspace is a rectangular box; kinematics are trivial but the structure is bulky.
• RRP + RRR (Stanford arm): The first three joints (RRP) position the wrist center; the final three revolute joints (spherical wrist, RRR) orient the end-effector.

1.3 Workspace

The workspace is determined by link lengths, joint ranges, and mechanical interference. For a 2-DOF planar arm with link lengths \(l_1, l_2\) and unlimited joint ranges, the reachable workspace is an annulus with inner radius \(|l_1 - l_2|\) and outer radius \(l_1 + l_2\). Joint limits and self-collision reduce this to a subset.

1.4 Robot Specifications

When selecting or designing a manipulator, the key specifications are:

• Payload: maximum load (including end-effector) the robot can carry at rated speed and accuracy.
• Reach: maximum radial distance from the base to the wrist.
• Repeatability: the ability to return to a previously taught position; typically ±0.02 mm for precision assembly robots.
• Accuracy: how closely the robot reaches a commanded position; often worse than repeatability due to model errors.
• Speed: maximum joint or Cartesian velocity.
• Footprint and mounting: floor-mounted, ceiling-mounted (inverted), or wall-mounted.

---

Chapter 2: Spatial Descriptions and Transformations

2.1 Coordinate Frames and Position Vectors

To describe the pose (position and orientation) of a rigid body, we attach a coordinate frame — three mutually orthogonal unit vectors \(\{\hat{x}, \hat{y}, \hat{z}\}\) together with an origin point — to that body. The pose of frame \(\{B\}\) relative to frame \(\{A\}\) is then specified by:

1. The position of the origin of \(\{B\}\) expressed in \(\{A\}\): a vector \({}^A\mathbf{p}_{B} \in \mathbb{R}^3\).
2. The orientation of \(\{B\}\) relative to \(\{A\}\): encoded by a rotation matrix, Euler angles, or quaternion.

Throughout these notes, a leading superscript denotes the frame in which a quantity is expressed. Thus \({}^A\mathbf{v}\) is a vector expressed in frame \(\{A\}\).

2.2 Rotation Matrices and SO(3)

2.2.1 Elementary Rotation Matrices

Rotation by angle \(\theta\) about the \(x\)-, \(y\)-, and \(z\)-axes:

\[ \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}. \]

These satisfy \(\mathbf{R}_i(-\theta) = \mathbf{R}_i(\theta)^\top\) and are the generators of one-parameter subgroups of \(SO(3)\).

2.3 Representations of Orientation

2.3.1 Euler Angles

Any rotation can be decomposed as a sequence of three elementary rotations about specified axes. The ZYZ Euler angles \((\phi, \vartheta, \psi)\) represent:

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

The ZYX (roll-pitch-yaw) convention is common in aerospace and robotics:

\[ \mathbf{R} = \mathbf{R}_z(\psi)\,\mathbf{R}_y(\vartheta)\,\mathbf{R}_x(\phi), \]

where \(\phi\) is roll (about \(x\)), \(\vartheta\) is pitch (about \(y\)), and \(\psi\) is yaw (about \(z\)).

2.3.2 Angle-Axis Representation

Euler’s rotation theorem states that any rotation in \(SO(3)\) is equivalent to a single rotation by angle \(\theta \in [0, \pi]\) about a unit axis \(\hat{k} \in \mathbb{R}^3\). The corresponding rotation matrix is given by the Rodrigues formula:

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

where \(\left[\hat{k}\right]_\times\) is the skew-symmetric matrix associated with \(\hat{k} = (k_x, k_y, k_z)^\top\):

\[ \left[\hat{k}\right]_\times = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix}. \]

The Lie algebra \(\mathfrak{so}(3)\) is precisely the space of \(3\times 3\) real skew-symmetric matrices. The exponential map \(\exp: \mathfrak{so}(3) \to SO(3)\) sends \(\theta[\hat{k}]_\times \mapsto \mathbf{R}(\hat{k},\theta)\), recovering the Rodrigues formula.

2.3.3 Unit Quaternions

A unit quaternion \(\mathbf{q} = (\eta, \boldsymbol{\varepsilon}) = (\eta, \varepsilon_1, \varepsilon_2, \varepsilon_3)\) with \(\eta^2 + \|\boldsymbol{\varepsilon}\|^2 = 1\) represents a rotation by angle \(\theta\) about axis \(\hat{k}\) via:

\[ \eta = \cos\tfrac{\theta}{2}, \qquad \boldsymbol{\varepsilon} = \sin\tfrac{\theta}{2}\,\hat{k}. \]

The corresponding rotation matrix is:

\[ \mathbf{R}(\mathbf{q}) = (2\eta^2 - 1)\mathbf{I} + 2\boldsymbol{\varepsilon}\boldsymbol{\varepsilon}^\top + 2\eta\left[\boldsymbol{\varepsilon}\right]_\times. \]

Quaternion multiplication \(\mathbf{q}_1 \otimes \mathbf{q}_2\) composes rotations without the trigonometric overhead of matrix multiplication, making quaternions preferred in real-time implementations. Note that \(\mathbf{q}\) and \(-\mathbf{q}\) represent the same rotation — the unit sphere \(S^3\) double-covers \(SO(3)\).

2.4 Homogeneous Transformations and SE(3)

To simultaneously describe rotation and translation in a single matrix operation, we use homogeneous coordinates. A point \(\mathbf{p} \in \mathbb{R}^3\) is embedded as \(\tilde{\mathbf{p}} = (\mathbf{p}^\top, 1)^\top \in \mathbb{R}^4\).

The group operation is matrix multiplication. Given frames \(\{A\}\), \(\{B\}\), \(\{C\}\):

[

]

The inverse of \(\mathbf{T} = \begin{bmatrix}\mathbf{R} & \mathbf{p} \\ \mathbf{0}^\top & 1\end{bmatrix}\) is:

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

Transforming a point: \({}^A\tilde{\mathbf{p}} = {}^A\mathbf{T}_B \; {}^B\tilde{\mathbf{p}}\), i.e.,

[

]

---

Chapter 3: Forward Kinematics — Denavit-Hartenberg Convention

3.1 The Forward Kinematics Problem

Forward kinematics (FK) maps joint variables \(\mathbf{q} = (q_1, \ldots, q_n)^\top \in \mathbb{R}^n\) to the pose of the end-effector frame in the base frame:

\[ \mathbf{T}_0^n(\mathbf{q}) = {}^0\mathbf{T}_1(q_1) \; {}^1\mathbf{T}_2(q_2) \cdots {}^{n-1}\mathbf{T}_n(q_n) \in SE(3). \]

Each local transformation \({}^{i-1}\mathbf{T}_i\) depends on exactly one joint variable (\(\theta_i\) for revolute, \(d_i\) for prismatic) and on fixed geometric parameters of the link. The Denavit-Hartenberg (DH) convention provides a systematic, minimal parameterization: each link-to-link transformation is described by exactly four parameters.

3.2 Denavit-Hartenberg Parameters

3.2.1 Frame Assignment Rules

1. Assign \(z_{i-1}\) along the axis of joint \(i\).
2. Locate the origin of frame \(\{i-1\}\) at the intersection of \(z_{i-1}\) with the common normal to \(z_{i-1}\) and \(z_i\) (or at the intersection of axes if they intersect).
3. \(x_{i-1}\) points along the common normal from \(z_{i-1}\) toward \(z_i\).
4. \(y_{i-1}\) completes the right-hand frame.

The base frame \(\{0\}\) and end-effector frame \(\{n\}\) may be chosen freely (commonly aligned with the base and tool flanges, respectively), with any mismatch absorbed into constant offset transformations \(\mathbf{T}_{base}\) and \(\mathbf{T}_{tool}\).

3.2.2 DH Transformation Matrix

The transformation from frame \(\{i-1\}\) to frame \(\{i\}\) is the composition of four elementary operations — rotate about \(z_{i-1}\) by \(\theta_i\), translate along \(z_{i-1}\) by \(d_i\), translate along \(x_i\) by \(a_i\), rotate about \(x_i\) by \(\alpha_i\):

[

]

Evaluating explicitly:

[

\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}. ]

3.3 Worked Examples

---

Chapter 4: Robot Vision Geometry

4.1 Pinhole Camera Model

A camera maps 3D scene points to 2D image points. The pinhole model assumes all rays pass through a single focal point (the optical center). Let the camera frame have its optical axis along \(z_c\) and the image plane at \(z_c = f\) (the focal length). A 3D point \(\mathbf{p}^c = (X, Y, Z)^\top\) expressed in the camera frame projects to image coordinates:

\[ u = f\,\frac{X}{Z}, \qquad v = f\,\frac{Y}{Z}. \]

In homogeneous coordinates, this is a linear map:

\[ \lambda \begin{bmatrix}u \\ v \\ 1\end{bmatrix} = \begin{bmatrix}f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0\end{bmatrix} \begin{bmatrix}X \\ Y \\ Z \\ 1\end{bmatrix}, \quad \lambda = Z. \]

4.2 Camera Intrinsic and Extrinsic Parameters

The full projection model includes:

• Intrinsic parameters: focal length \(f\), pixel scaling \(k_u, k_v\) (pixels/mm), principal point offset \((u_0, v_0)\) in pixels. These form the calibration matrix \(\mathbf{K} \in \mathbb{R}^{3\times 3}\):

\[ \mathbf{K} = \begin{bmatrix}fk_u & 0 & u_0 \\ 0 & fk_v & v_0 \\ 0 & 0 & 1\end{bmatrix}. \]

• Extrinsic parameters: the pose of the camera frame in the world frame, represented by \(\left[\mathbf{R}_c \,|\, \mathbf{t}_c\right]\).

The full projection is:

\[ \lambda \tilde{\mathbf{s}} = \mathbf{K}\left[\mathbf{R}_c \,|\, \mathbf{t}_c\right] \tilde{\mathbf{P}}, \]

where \(\tilde{\mathbf{P}} = (X_w, Y_w, Z_w, 1)^\top\) is the world point and \(\tilde{\mathbf{s}} = (u, v, 1)^\top\) is the image point.

4.3 Image Jacobian

In visual servoing, we control joint velocities to drive image features to desired values. Let \(\mathbf{s} = (u, v)^\top\) be image feature coordinates. The image Jacobian (interaction matrix) \(\mathbf{L}_s \in \mathbb{R}^{2\times 6}\) relates the camera’s spatial velocity \(\boldsymbol{\xi} = (\mathbf{v}_c^\top, \boldsymbol{\omega}_c^\top)^\top\) to the rate of change of features:

\[ \dot{\mathbf{s}} = \mathbf{L}_s \, \boldsymbol{\xi}. \]

For a point feature \((u, v)\) at depth \(Z\):

\[ \mathbf{L}_s = \begin{bmatrix} -f/Z & 0 & u/Z & uv/f & -(f^2+u^2)/f & v \\ 0 & -f/Z & v/Z & (f^2+v^2)/f & -uv/f & -u \end{bmatrix}. \]

4.3.1 Visual Servoing Overview

In image-based visual servoing (IBVS), the control law drives the feature error \(\mathbf{e} = \mathbf{s} - \mathbf{s}^*\) to zero:

\[ \dot{\mathbf{s}} = -\lambda_s \mathbf{e} \implies \boldsymbol{\xi} = -\lambda_s \mathbf{L}_s^+ \mathbf{e}, \]

where \(\mathbf{L}_s^+ = \mathbf{L}_s^\top(\mathbf{L}_s \mathbf{L}_s^\top)^{-1}\) is the pseudoinverse. Joint velocities follow from the manipulator Jacobian: \(\dot{\mathbf{q}} = \mathbf{J}^+(\mathbf{q})\boldsymbol{\xi}\). In position-based visual servoing (PBVS), 3D pose is estimated from image measurements and Cartesian errors are used for control.

---

Chapter 5: Inverse Kinematics

5.1 Problem Statement

Inverse kinematics (IK) seeks joint configurations \(\mathbf{q}\) such that the forward kinematics gives a desired end-effector pose \(\mathbf{T}_d\):

\[ \mathbf{T}_0^n(\mathbf{q}) = \mathbf{T}_d. \]

In contrast to forward kinematics — which has a unique solution given joint values — inverse kinematics raises three fundamental questions:

1. Existence: Does a solution exist? (Is \(\mathbf{T}_d\) in the reachable workspace?)
2. Uniqueness: If solutions exist, how many? (Multiple configurations may achieve the same pose.)
3. Computation: How do we find the solutions efficiently?

5.2 Existence, Multiplicity, and Workspace

For a 6-DOF manipulator, the task space is 6-dimensional (position + orientation). If the robot’s workspace encloses \(\mathbf{T}_d\), solutions generically exist. However:

• Near workspace boundaries, solutions may coincide (tangency) or disappear.
• At kinematic singularities, the Jacobian loses rank and the manipulator loses the ability to move in certain Cartesian directions — the number of solutions may become infinite.
• A 6-DOF robot typically has a finite number of IK solutions at a generic interior point — for the PUMA 560, up to eight distinct configurations can achieve the same end-effector pose.

5.3 Geometric and Algebraic Approaches

5.3.1 Geometric Decomposition for Planar Arms

5.3.2 Pieper’s Theorem and Wrist Decoupling

Wrist center computation: Given the desired end-effector frame \(\mathbf{T}_d = \begin{bmatrix}\mathbf{R}_d & \mathbf{p}_d \\ \mathbf{0}^\top & 1\end{bmatrix}\) and the distance \(d_6\) from the wrist center to the end-effector along the approach axis \(\hat{z}_6 = \mathbf{R}_d \hat{e}_3\):

\[ \mathbf{p}_{wc} = \mathbf{p}_d - d_6 \mathbf{R}_d \hat{e}_3. \]

The first three joints are then solved to position the wrist center at \(\mathbf{p}_{wc}\). The desired orientation of the wrist frame in the frame of joint 3 is:

[

]

This matrix is then matched to the Euler-angle parameterization of the spherical wrist to extract \(\theta_4, \theta_5, \theta_6\).

5.4 Redundant Manipulators and Pseudoinverse

A manipulator with \(n > 6\) joints is kinematically redundant. The forward kinematics \(\mathbf{f}: \mathbb{R}^n \to \mathbb{R}^m\) with \(n > m\) (where \(m\) is the dimension of the task space) has an underdetermined Jacobian at almost every configuration.

The minimum-norm joint-velocity solution is:

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

where \(\mathbf{J}^+ = \mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top)^{-1}\) is the Moore-Penrose pseudoinverse (valid when \(\mathbf{J}\) has full row rank), \((\mathbf{I} - \mathbf{J}^+\mathbf{J})\) is the null-space projection, and \(\mathbf{z} \in \mathbb{R}^n\) is an arbitrary vector used to optimize secondary objectives (e.g., joint-limit avoidance, singularity avoidance). Integrating \(\dot{\mathbf{q}}\) yields a trajectory in joint space consistent with the Cartesian task.

---

Chapter 6: Differential Kinematics and Statics

6.1 The Geometric Jacobian

The Jacobian is the central analytical tool relating infinitesimal joint motions to end-effector velocity. The geometric Jacobian \(\mathbf{J}(\mathbf{q}) \in \mathbb{R}^{6\times n}\) satisfies:

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

where \(\mathbf{v}_e \in \mathbb{R}^3\) is the linear velocity of the end-effector origin and \(\boldsymbol{\omega}_e \in \mathbb{R}^3\) is its angular velocity.

6.1.1 Derivation via Velocity Propagation

6.2 Singularity Analysis

Singularities arise from three geometric causes:

1. Arm singularities: The wrist center lies on the \(z_1\) axis (shoulder singularity) or the elbow is fully extended or fully folded.
2. Wrist singularities: Two wrist axes become aligned (e.g., \(\theta_5 = 0\) for a ZYZ wrist), losing one orientation DOF.
3. Boundary singularities: At the workspace boundary.

6.3 Manipulability

6.4 Analytic Jacobian

The analytic Jacobian \(\mathbf{J}_A\) relates joint velocities to the time derivative of a minimal orientation representation \(\boldsymbol{\phi}\) (e.g., Euler angles):

\[ \dot{\mathbf{x}}_A = \begin{bmatrix}\dot{\mathbf{p}}_e \\ \dot{\boldsymbol{\phi}}\end{bmatrix} = \mathbf{J}_A(\mathbf{q})\dot{\mathbf{q}}, \]

where \(\mathbf{J}_A = \mathbf{T}_A(\boldsymbol{\phi})^{-1} \mathbf{J}\) and \(\mathbf{T}_A\) maps Euler-angle rates to angular velocity: \(\boldsymbol{\omega}_e = \mathbf{T}_A(\boldsymbol{\phi})\dot{\boldsymbol{\phi}}\). The analytic Jacobian is used in resolved-rate control and task-space control laws when working with minimal representations, but note that \(\mathbf{T}_A\) is singular at Euler-angle representation singularities.

6.5 Statics: Force-Torque Duality

This elegant result reveals a force-velocity duality: the Jacobian maps joint velocities to end-effector velocity, and its transpose maps end-effector forces to joint torques. At singularities, \(\mathbf{J}^\top\) loses rank, meaning certain external wrenches produce no joint torque (the robot cannot balance them), while unbounded end-effector forces may be produced by bounded joint torques — a property exploited in force amplification.

---

Chapter 7: Trajectory Planning

7.1 Overview

Trajectory planning generates time-parameterized joint or Cartesian paths \(\mathbf{q}(t)\) or \(\mathbf{x}(t)\) that move the robot from an initial configuration to a goal while respecting kinematic constraints (velocity, acceleration limits) and dynamic constraints (torque limits). We distinguish:

• Path: the geometric locus of configurations, without time information.
• Trajectory: a path with an associated time law (position, velocity, acceleration as functions of time).

7.2 Joint-Space Trajectories

7.2.1 Cubic Polynomial

Given start \(q_0 = q(0)\), goal \(q_f = q(T)\), and zero velocity at endpoints (\(\dot{q}(0) = \dot{q}(T) = 0\)), a cubic polynomial:

\[ q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 \]

with four boundary conditions determines four coefficients:

\[ a_0 = q_0,\quad a_1 = 0,\quad a_2 = \frac{3(q_f - q_0)}{T^2},\quad a_3 = -\frac{2(q_f - q_0)}{T^3}. \]

The velocity profile is quadratic with a single peak at \(t = T/2\); the acceleration profile is linear, implying infinite jerk at \(t=0\) and \(t=T\) — acceptable for many applications but undesirable for precision tasks.

7.2.2 LSPB (Linear Segment with Parabolic Blends)

The trapezoidal velocity profile (LSPB) uses:

1. A parabolic acceleration phase from \(0\) to \(t_b\) (blend time).
2. A constant-velocity (linear position) phase from \(t_b\) to \(T - t_b\).
3. A symmetric parabolic deceleration phase from \(T - t_b\) to \(T\).

The peak velocity is \(\dot{q}_{max}\) and the acceleration is \(\ddot{q} = \dot{q}_{max}/t_b\). The blend time satisfies:

\[ t_b = \frac{T}{2} - \frac{1}{2}\sqrt{T^2 - \frac{4(q_f - q_0)}{\ddot{q}_{max}}}. \]

LSPB profiles are continuous in position and velocity (but not acceleration), making them practical for industrial robots.

7.2.3 Via-Points

When the trajectory must pass through intermediate configurations \(q_{k}\) at times \(t_k\), piecewise polynomial segments are joined with continuity conditions. For cubic splines, continuity of position, velocity, and acceleration at each via-point gives a tridiagonal system of equations for the coefficients.

7.3 Cartesian-Space Trajectories

7.3.1 Straight-Line Interpolation

For a straight-line path from \(\mathbf{p}_i\) to \(\mathbf{p}_f\) in Cartesian space, parameterize by \(s(t) \in [0,1]\):

\[ \mathbf{p}(t) = (1 - s(t))\mathbf{p}_i + s(t)\mathbf{p}_f, \]

where \(s(t)\) follows any desired time law (cubic, LSPB). The joint trajectory is then obtained by applying inverse kinematics at each time step, which requires real-time IK computation and careful singularity handling.

7.3.2 SLERP for Orientation Interpolation

For orientation, Spherical Linear Interpolation (SLERP) between quaternions \(\mathbf{q}_i\) and \(\mathbf{q}_f\) provides constant angular velocity rotation:

\[ \mathbf{q}(s) = \mathbf{q}_i \otimes \left(\mathbf{q}_i^{-1} \otimes \mathbf{q}_f\right)^s = \frac{\sin((1-s)\Omega)}{\sin\Omega}\,\mathbf{q}_i + \frac{\sin(s\Omega)}{\sin\Omega}\,\mathbf{q}_f, \]

where \(\Omega = \arccos(\mathbf{q}_i \cdot \mathbf{q}_f)\) is the half-angle between the two orientations, and the dot product is the standard inner product in \(\mathbb{R}^4\). SLERP ensures constant angular speed along the shortest great-circle arc on \(S^3\), unlike naive linear interpolation of rotation matrices (which does not stay in \(SO(3)\)).

---

Chapter 8: Robot Dynamics — Lagrangian Formulation

8.1 Motivation and Setup

The equations of motion (EOM) of a robot manipulator describe how joint torques \(\boldsymbol{\tau}\) relate to joint accelerations \(\ddot{\mathbf{q}}\), velocities \(\dot{\mathbf{q}}\), and positions \(\mathbf{q}\). Two complementary approaches exist:

• Lagrangian (energy-based): Elegant, compact, yields the EOM directly in generalized coordinates. Ideal for control design.
• Newton-Euler (recursive): Computationally efficient for numerical simulation and real-time control; propagates forces and torques link by link.

This chapter develops the Lagrangian approach; Chapter 9 covers Newton-Euler.

8.2 Lagrangian Mechanics

The Lagrangian of the robot is \(\mathcal{L}(\mathbf{q}, \dot{\mathbf{q}}) = \mathcal{T}(\mathbf{q}, \dot{\mathbf{q}}) - \mathcal{V}(\mathbf{q})\), where \(\mathcal{T}\) is the total kinetic energy and \(\mathcal{V}\) is the total potential energy. The Euler-Lagrange equations give:

\[ \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. \]

8.2.1 Kinetic Energy of a Rigid Link

For link \(i\) with mass \(m_i\) and inertia tensor \(\mathbf{I}_{C_i}\) about its center of mass \(C_i\), the kinetic energy is:

\[ \mathcal{T}_i = \frac{1}{2}m_i \mathbf{v}_{C_i}^\top \mathbf{v}_{C_i} + \frac{1}{2}\boldsymbol{\omega}_i^\top \mathbf{I}_{C_i} \boldsymbol{\omega}_i. \]

Both \(\mathbf{v}_{C_i}\) and \(\boldsymbol{\omega}_i\) are linear in \(\dot{\mathbf{q}}\) through the partial Jacobians. Define the Jacobian of the center of mass of link \(i\): \(\mathbf{J}_{v,i}(\mathbf{q})\) for translation and \(\mathbf{J}_{\omega,i}(\mathbf{q})\) for rotation, each of size \(3\times n\) with nonzero columns only for joints \(1\) through \(i\). Then:

\[ \mathbf{v}_{C_i} = \mathbf{J}_{v,i}\dot{\mathbf{q}}, \qquad \boldsymbol{\omega}_i = \mathbf{J}_{\omega,i}\dot{\mathbf{q}}. \]

Substituting:

\[ \mathcal{T}_i = \frac{1}{2}\dot{\mathbf{q}}^\top \left(m_i \mathbf{J}_{v,i}^\top \mathbf{J}_{v,i} + \mathbf{J}_{\omega,i}^\top \mathbf{I}_{C_i}^0 \mathbf{J}_{\omega,i}\right)\dot{\mathbf{q}}, \]

where \(\mathbf{I}_{C_i}^0 = {}^0\mathbf{R}_i \mathbf{I}_{C_i} {}^0\mathbf{R}_i^\top\) is the inertia tensor expressed in the base frame. Summing over all links:

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

where the mass matrix (also called the inertia matrix) is:

\[ \mathbf{M}(\mathbf{q}) = \sum_{i=1}^n \left(m_i \mathbf{J}_{v,i}^\top \mathbf{J}_{v,i} + \mathbf{J}_{\omega,i}^\top \mathbf{I}_{C_i}^0 \mathbf{J}_{\omega,i}\right). \]

8.2.2 Potential Energy

The gravitational potential energy of link \(i\) is:

\[ \mathcal{V}_i = m_i g_0 h_i(\mathbf{q}), \]

where \(g_0\) is gravitational acceleration and \(h_i(\mathbf{q})\) is the height of the center of mass of link \(i\) above a reference plane. Total: \(\mathcal{V} = \sum_i \mathcal{V}_i\).

8.3 Equations of Motion

8.4 Properties of the Dynamic Model

---

Chapter 9: Newton-Euler Dynamics

9.1 Newton-Euler Formulation

The Newton-Euler formulation computes robot dynamics by applying Newton’s second law (linear momentum) and Euler’s equation (angular momentum) to each rigid link, then propagating forces and torques through the chain. The result is identical to the Lagrangian EOM but obtained via a recursive algorithm that is computationally efficient for simulation and real-time control.

9.2 Forward Recursion: Velocity and Acceleration Propagation

Starting from the base (link 0: \(\boldsymbol{\omega}_0 = \mathbf{0}\), \(\dot{\boldsymbol{\omega}}_0 = \mathbf{0}\), \(\ddot{\mathbf{p}}_0 = \mathbf{g}\) where \(\mathbf{g}\) is the gravitational acceleration treated as a pseudo-acceleration acting upward), propagate outward for \(i = 1, \ldots, n\):

Revolute joint \(i\):

[

]

[

]

[

]

where \({}^{i-1}\mathbf{p}_{i-1,i}\) is the vector from the origin of frame \(\{i-1\}\) to the origin of frame \(\{i\}\).

The acceleration of the center of mass of link \(i\) (at position \({}^i\mathbf{r}_i\) from frame \(\{i\}\) origin):

[

]

Prismatic joint \(i\): Replace the angular velocity update by \({}^i\boldsymbol{\omega}_i = {}^i\mathbf{R}_{i-1}{}^{i-1}\boldsymbol{\omega}_{i-1}\) and add translational acceleration contributions from \(\dot{d}_i\).

9.3 Backward Recursion: Force and Torque Propagation

---

Chapter 10: Motion Control

10.1 Control Architecture

The robot control problem is: given a desired trajectory \(\mathbf{q}_d(t)\), design \(\boldsymbol{\tau}(t)\) so that \(\mathbf{q}(t) \to \mathbf{q}_d(t)\). The dynamics \(\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}\) are coupled and nonlinear, which complicates control design. Three levels of sophistication are considered: independent joint control, computed-torque (inverse dynamics), and adaptive/robust control.

10.2 Independent Joint PD/PID Control

The simplest approach treats each joint as an independent SISO system and applies a PD or PID controller:

\[ \tau_i = k_{P,i}(q_{d,i} - q_i) + k_{D,i}(\dot{q}_{d,i} - \dot{q}_i) + k_{I,i}\int_0^t (q_{d,i} - q_i)\,ds. \]

Gravity compensation augments the control law by feeding forward the gravity torque:

\[ \boldsymbol{\tau} = \mathbf{K}_P \mathbf{e} + \mathbf{K}_D \dot{\mathbf{e}} + \hat{\mathbf{g}}(\mathbf{q}), \]

where \(\mathbf{e} = \mathbf{q}_d - \mathbf{q}\) and \(\hat{\mathbf{g}}\) is the estimated gravity vector. Gravity compensation removes the steady-state error that would otherwise require large integral gains.

Stability of PD with gravity compensation: Let \(\mathbf{q}_d\) be a constant setpoint. Using the Lyapunov function:

\[ V = \frac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q})\dot{\mathbf{q}} + \frac{1}{2}\mathbf{e}^\top \mathbf{K}_P \mathbf{e}, \]

one can show that \(\dot{V} = -\dot{\mathbf{q}}^\top \mathbf{K}_D \dot{\mathbf{q}} \leq 0\), and by LaSalle’s invariance principle, \(\mathbf{e} \to \mathbf{0}\) as \(t \to \infty\) if \(\hat{\mathbf{g}} = \mathbf{g}\) exactly. In practice, model errors require integral action or robust terms.

10.3 Computed-Torque Control (Inverse Dynamics)

Computed-torque control (also called inverse dynamics control or feedback linearization) cancels the robot dynamics exactly to produce a linear closed-loop system.

The critical assumption is that the dynamic model \(\hat{\mathbf{M}}, \hat{\mathbf{C}}, \hat{\mathbf{g}}\) used in the controller equals the true dynamics exactly. In practice, model errors and unmodeled dynamics (joint flexibility, friction) lead to imperfect cancellation, requiring robustification.

10.4 Feedforward Control

Rather than computing the full inverse dynamics at every time step, feedforward control precomputes the nominal torque along the planned trajectory:

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

combined with a feedback term:

\[ \boldsymbol{\tau} = \boldsymbol{\tau}_{ff} + \mathbf{K}_D\dot{\mathbf{e}} + \mathbf{K}_P\mathbf{e}. \]

Near the desired trajectory, the residual dynamics are small and PD feedback handles the remaining error. This reduces real-time computation compared to full computed-torque.

10.5 Adaptive and Robust Control Overview

10.5.1 Adaptive Control

When dynamic parameters (masses, inertias, friction) are uncertain, adaptive control estimates them online. The key observation is that the robot dynamics are linear in parameters:

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

where \(\boldsymbol{\Phi}\) is the regressor matrix (computable from measurements) and \(\boldsymbol{\pi} \in \mathbb{R}^p\) is the vector of unknown inertial parameters. The adaptive computed-torque law uses:

\[ \boldsymbol{\tau} = \boldsymbol{\Phi}(\mathbf{q}, \dot{\mathbf{q}}, \mathbf{a})\hat{\boldsymbol{\pi}} + \mathbf{K}_D\dot{\mathbf{e}} + \mathbf{K}_P\mathbf{e}, \]\[ \dot{\hat{\boldsymbol{\pi}}} = -\boldsymbol{\Gamma}\boldsymbol{\Phi}^\top \mathbf{s}, \]

where \(\boldsymbol{\Gamma} \succ 0\) is the adaptation gain, \(\mathbf{s} = \dot{\mathbf{e}} + \lambda\mathbf{e}\) is the sliding variable. Under persistency of excitation, \(\hat{\boldsymbol{\pi}} \to \boldsymbol{\pi}\) and tracking error converges to zero.

10.5.2 Robust Control

Robust control (sliding mode, \(H_\infty\)) bounds the effect of model uncertainty without estimating parameters. Sliding mode control augments the nominal control with a discontinuous term:

\[ \boldsymbol{\tau} = \hat{\boldsymbol{\tau}}_{nom} - \mathbf{K}_{rob}\,\text{sgn}(\mathbf{s}), \]

where \(\mathbf{K}_{rob}\) is chosen to dominate the uncertainty bound. The price is chattering — high-frequency switching — which is mitigated by replacing \(\text{sgn}\) with a saturation function or using higher-order sliding modes.

---

Chapter 11: Force Control and Compliant Motion

11.1 Motivation

Pure motion control is inadequate when the robot contacts the environment: attempting to follow a trajectory that penetrates a surface generates large, uncontrolled contact forces. Force control explicitly regulates the interaction wrench, while compliant motion combines position and force objectives.

11.2 Wrist F/T Sensors and Joint Torque Sensing

A six-axis force/torque (F/T) sensor mounted between the robot wrist and end-effector measures the wrench \(\mathbf{f} = (f_x, f_y, f_z, \mu_x, \mu_y, \mu_z)^\top\) exerted by the environment on the end-effector. These sensors use strain gauges or piezoelectric elements arranged to provide full 6-DOF wrench measurement.

Joint torque sensors estimate external loads by measuring deformation in the joint transmission (via strain gauges on the joint structure). They are noisier than wrist F/T sensors but provide distributed force information along the kinematic chain.

11.3 Impedance Control

Impedance control (Hogan, 1985) does not attempt to independently regulate position or force, but rather specifies the dynamic relationship between position error and contact force — the mechanical impedance of the robot as seen from the environment.

The impedance control law that realizes this target uses the full robot dynamics model. In joint space, the control law is:

\[ \boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\left[\ddot{\mathbf{q}}_d + \mathbf{J}^+\left(\ddot{\mathbf{x}}_d + \mathbf{M}_d^{-1}\left(\mathbf{f}_{ext} - \mathbf{B}_d\dot{\mathbf{e}} - \mathbf{K}_d\mathbf{e}\right) - \dot{\mathbf{J}}\dot{\mathbf{q}}\right)\right] + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g}. \]

Stiffness control is the special case \(\mathbf{M}_d = \mathbf{0}\), \(\mathbf{B}_d = \mathbf{0}\): the robot behaves as a pure spring. Damping control sets \(\mathbf{M}_d = \mathbf{0}\), \(\mathbf{K}_d = \mathbf{0}\).

11.3.1 Stability of Impedance Control

The stability argument uses the storage function \(V = \tfrac{1}{2}\mathbf{e}^\top\mathbf{K}_d\mathbf{e} + \tfrac{1}{2}\dot{\mathbf{e}}^\top\mathbf{M}_d\dot{\mathbf{e}}\), which satisfies \(\dot{V} \leq \dot{\mathbf{x}}_e^\top \mathbf{f}_{ext}\) — the power inequality defining passivity.

11.4 Hybrid Position/Force Control

Hybrid position/force control (Raibert & Craig, 1981) partitions the task space into a position subspace (where position is controlled) and a force subspace (where force is controlled), using a selection matrix \(\mathbf{S}\) to separate the two objectives.

The hybrid control law in task space:

\[ \mathbf{f}_{cmd} = (\mathbf{I} - \mathbf{S})\left(\mathbf{f}_{ff} + \mathbf{K}_{Pp}\mathbf{e}_p + \mathbf{K}_{Dp}\dot{\mathbf{e}}_p\right) + \mathbf{S}\left(\mathbf{f}_{ref} + \mathbf{K}_{If}\int\mathbf{e}_f\,dt\right), \]

where \(\mathbf{e}_p = \mathbf{x}_d - \mathbf{x}_e\) is the position error and \(\mathbf{e}_f = \mathbf{f}_{ref} - \mathbf{f}_{meas}\) is the force error. The joint torque is computed via \(\boldsymbol{\tau} = \mathbf{J}^\top \mathbf{f}_{cmd} + \mathbf{g}(\mathbf{q})\).

11.5 Comparison of Force Control Strategies

The three main approaches have distinct characteristics:

Stiffness control: Simplest implementation; no explicit force sensing needed; performance degrades with environmental stiffness uncertainty. Suitable for tasks with predictable contact conditions.

Impedance control: Specifies dynamic response; naturally handles contact transitions; requires accurate dynamic model; does not guarantee a specific contact force.

Hybrid position/force control: Directly controls force in constrained directions; requires task-frame specification and knowledge of constraint geometry; decoupled design of position and force loops simplifies gain tuning; sensitive to constraint-frame errors.

---

Summary of Key Results

The following table collects the principal equations developed in this course:

Topic	Equation
Rotation group	\(\mathbf{R} \in SO(3): \mathbf{R}^\top\mathbf{R} = \mathbf{I},\, \det\mathbf{R} = +1\)
Rodrigues formula	\(\mathbf{R} = \cos\theta\,\mathbf{I} + (1-\cos\theta)\hat{k}\hat{k}^\top + \sin\theta[\hat{k}]_\times\)
DH matrix	\({}^{i-1}\mathbf{T}_i\) given in Chapter 3
Geometric Jacobian	\(\mathbf{J}_i = [\hat{z}_{i-1}\times(\mathbf{p}_e-\mathbf{p}_{i-1});\,\hat{z}_{i-1}]\) (revolute)
Statics duality	\(\boldsymbol{\tau} = \mathbf{J}^\top\mathbf{f}\)
Manipulability	\(w = \sqrt{\det(\mathbf{J}\mathbf{J}^\top)}\)
EOM	\(\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}\)
Computed torque	\(\boldsymbol{\tau} = \mathbf{M}\mathbf{a} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g},\; \mathbf{a} = \ddot{\mathbf{q}}_d + \mathbf{K}_D\dot{\mathbf{e}} + \mathbf{K}_P\mathbf{e}\)
Target impedance	\(\mathbf{M}_d\ddot{\tilde{\mathbf{x}}} + \mathbf{B}_d\dot{\tilde{\mathbf{x}}} + \mathbf{K}_d\tilde{\mathbf{x}} = \mathbf{f}_{ext}\)
Statics (hybrid)	\(\boldsymbol{\tau} = \mathbf{J}^\top\mathbf{f}_{cmd} + \mathbf{g}\)

This course has developed a unified analytical framework — from coordinate geometry through dynamics to control — that enables systematic design and analysis of robot manipulator systems. The Lagrangian and Newton-Euler formulations are computationally dual: the former provides structural insight for control design (linearity in parameters, passivity), while the latter provides efficient real-time computation. Model-based controllers (computed torque, adaptive) achieve high tracking performance at the cost of model complexity; simpler controllers (PD + gravity compensation) are robust to model uncertainty but sacrifice performance at high speeds. Force control closes the loop on physical interaction, enabling safe and dexterous manipulation in partially structured environments.