A Mathematical Introduction to Robotic Manipulation, Taught as One Idea

The whole book in one sentence: a robot is not a set of link lengths and angles to memorize — it is a curve in a group of matrices, every joint is a screw, and forward kinematics, the Jacobian, singularities, dynamics, and control are all manifestations of a single object: the exponential of a constant twist \(\exp(\hat{\xi}\,\theta)\). This note walks the whole book — chapters 2–4 (rigid motions, kinematics, dynamics, control) in full, then chapters 5–8 (grasping, hand dynamics, nonholonomic behavior, motion planning) as the same machinery spent — building every formula from a picture first. Every computation shown has been re-derived and verified symbolically.

0. What problem is this book solving?

Suppose you must program a 6-axis industrial arm to insert a chip into a socket. You need, in order: a way to describe where the tool is (configuration), a way to predict where it lands when joints rotate (forward kinematics), a way to choose joint angles that put the tool somewhere (inverse kinematics), the map from joint speeds to tool speed (the Jacobian), the map from tool forces to joint torques (its transpose), the equations that predict how the arm responds to torques (dynamics), and a feedback law that makes it track a path despite all of the above being wrong in small ways (control). That is exactly the sequence of the book — and each item looks like a separate engineering course.

MLS94’s bet is that they are one subject. The move that collapses them is to represent the configuration of a rigid body as a \(4\times4\) matrix \(g\in SE(3)\) and each joint as a constant matrix \(\hat{\xi}\) so that one turn of a joint is the matrix exponential \(e^{\hat{\xi}\theta}\). Composition of motions becomes matrix multiplication; velocity becomes a logarithmic derivative; and control becomes algebra on the same matrices. The price is a little Lie-group vocabulary up front; the return is that nothing in kinematics or dynamics ever needs to be re-derived for a new robot.

Notation used throughout (all defined again on first use)

The book at a glance

1. Rigid motions form a group (MLS §2.1–2.3)

Why start here? Because everything else is a property of this group. A rigid body cannot stretch, so a motion of the body must preserve distances; it cannot reflect itself, so it must preserve orientation (the handedness of crossed vectors). MLS turns these two physical facts into a definition, and the definition has a surprising consequence: to record the motion of a body you may as well record the motion of a coordinate frame glued to it, because distances and handedness mean the body is rigidly bolted to that frame.

Concretely: glue frame \(B\) to the body, fix frame \(A\) to the ground, and write the body’s axes \(x_{ab},y_{ab},z_{ab}\) as they appear in \(A\)’s coordinates. Stacking them as columns,

The same matrix does double duty, and it is worth being explicit about why. As a pose, its columns are literally where the body’s axes point. As a coordinate changer, the product \(R_{ab}q_b\) rebuilds the point from body projections: \(q_a = x_{ab}x_b + y_{ab}y_b + z_{ab}z_b\). Orthonormality of the columns (\(R^TR=I\)) comes free because they are orthonormal axes; the determinant is \(+1\), not \(-1\), because the frame is right-handed: \(\det R = r_1^T(r_2\times r_3)=r_1^Tr_1=1\). These two conditions define the set

Both are groups under matrix multiplication: composing poses (move \(B\to C\), then \(A\to B\)) is \(R_{ac}=R_{ab}R_{bc}\), the identity and inverses (\(R^{-1}=R^T\)) are poses, and associativity is matrix associativity. The homogeneous \(4\times4\) form makes the group law uniform: the bottom row \(\begin{bmatrix}0&1\end{bmatrix}\) never gets disturbed, and rotations about off-center axes — the rule, not the exception, for real joints — are handled by the same multiplication.

2. Exponential coordinates for rotation: why \(e^{\hat{\omega}\theta}\) is the answer (MLS §2.2)

The question: a link rotates about a fixed axis \(\omega\in\mathbb{R}^3\) (a unit vector) through an angle \(\theta\). What is the resulting attitude \(R\)? Any answer should be built from \(\omega\) and \(\theta\) alone — the axis is all the geometry there is.

The trick that makes it inevitable: imagine spinning about \(\omega\) at unit speed. Every point \(q\) of the body then travels on a circle about the axis, and its velocity is the cross product \(\dot q = \omega\times q = \hat{\omega}q\) — this is just the definition of angular velocity: direction tangent to the circle, magnitude \(\|q\|\sin\alpha\) (the distance to the axis), and the cross product encodes both. That is a constant-coefficient linear ODE, and such an ODE is solved by a matrix exponential:

The infinite series is exact but slow; a closed form falls out of two algebraic facts about skew matrices (verified symbolically): for unit \(\omega\), \(\hat{\omega}^2=\omega\omega^T-I\) and \(\hat{\omega}^3=-\hat{\omega}\). Squaring feeds on itself, so every power collapses to \(\hat{\omega}\) or \(\hat{\omega}^2\); splitting the series into even and odd powers gives

Figure 2. Rotation about a fixed axis, viewed in the plane perpendicular to it. The velocity of any body point is \(\omega\times q\): tangent to the circle. Integrating \(\dot q=\hat{\omega}q\) for exactly \(\theta\) seconds sweeps the arc from \(q(0)\) to \(q(\theta)\) — that is all the matrix exponential is doing. (Computed and plotted in Wolfram Language.)

Worked example. Take \(\omega=(0,0,1)\), \(\theta\) arbitrary. Then \(\hat{\omega}^2=\mathrm{diag}(-1,-1,0)\) and Rodrigues gives instantly

Why this is called “exponential coordinates”: the three numbers \(\omega\theta\in\mathbb{R}^3\) label rotations, matching the 3 degrees of freedom of \(SO(3)\). The match is imperfect by design of the space, not the notation: rotations by \(\theta\) about \(\omega\) and by \(-\theta\) about \(-\omega\) agree, and \(\theta=\pi\) is reached twice (the ball-with-glued-antipodes picture). So \(\exp:so(3)\to SO(3)\) is onto (every rotation is a rotation about some axis — Euler’s theorem) but many-to-one; the \(\log\) is a branch, not a function. For building forward kinematics we only ever need the exponential direction, so the non-uniqueness costs nothing.

3. Twists and screws: one generator for rotation and translation (MLS §2.3)

Why we need more than \(\hat{\omega}\): real joints combine rotation and translation, and even a pure rotation about an off-origin axis moves points both ways. The fix is not a new theory but a bigger matrix. Pack an angular velocity \(\omega\) and a linear velocity \(v\) into one \(4\times4\) object,

and define the exponential exactly as before. Carrying out the series (the \(4\times4\) structure makes every power telescope into \(\begin{bmatrix}(\hat{\omega})^k&(\hat{\omega})^{k-1}v\\0&0\end{bmatrix}\)) gives the closed form, with the top-left block the familiar Rodrigues formula and the top-right block the integral \(\int_0^\theta e^{\hat{\omega}s}v\,ds\) evaluated using \(\hat{\omega}v=\omega\times v\) and \(\hat{\omega}^2v=\omega(\omega^Tv)-v\) (for unit \(\omega\)):

What does \(\xi\) mean geometrically? Chasles’ theorem says: every rigid motion is a rotation about some axis plus a translation along that same axis — the motion of a screw. Give the screw an axis \(l=\{q+\lambda\omega:\lambda\in\mathbb{R}\}\) (point \(q\), direction unit \(\omega\)), a pitch \(h\) = translation per radian, and a magnitude \(\theta\); then the screw motion moves a point \(p\) to

Comparing with the boxed exponential: the two agree exactly when \(v=-\,\omega\times q + h\,\omega\) — a two-line check, since \((I-e^{\hat{\omega}\theta})\omega=0\) kills the \(\omega\)-parallel part and \(\omega\omega^Tv\theta = h\theta\omega\). So every twist is a screw and every screw is a twist, with the dictionary (MLS eq.~2.42–2.44)

The two joint types of every robot arm are the two extreme screws (MLS §3.2):

• Revolute joint (zero pitch, \(h=0\)): \(\ \xi=\begin{bmatrix}-\,\omega\times q\\ \omega\end{bmatrix}\), with \(\omega\) the axis direction and \(q\) any point on the axis.
• Prismatic joint (infinite pitch, \(\omega=0\)): \(\ \xi=\begin{bmatrix}v\\0\end{bmatrix}\), with unit \(v\) the direction of sliding.

Note that \(q\) may be replaced by \(q+\lambda\omega\) — any point on the axis — without changing the screw: the exponential is invariant to that choice, another small but liberating fact.

Figure 3. A point carried by a screw motion with pitch \(h>0\): each radian of rotation about the axis advances the body by \(h\) along it, tracing a helix. The dashed circle is the \(h=0\) trace — a pure rotation. This is Chasles’ theorem you can watch. (Wolfram Language.)

Worked example (MLS Example 2.3). Rotate about the fixed axis in the \(\hat z\) direction through \(q=(0,l_1,0)\). The twist is \(\xi=(-\,\omega\times q,\ \omega)=(l_1,\,0,\,0,\,0,\,0,\,1)\), and the exponential acts on a point as

which you should read as: the matrix rotates the world by \(\theta\) about the offset axis, so a point sitting on the axis at \((0,l_1,0)\) maps to itself while a point at the origin swings a distance \(l_1\). If the body started at pose \(g_{ab}(0)\), its pose after turning \(\theta\) is \(g_{ab}(\theta)=e^{\hat{\xi}\theta}g_{ab}(0)\) — motion first applied to the body’s reference pose. This left-multiplication pattern is the seed of the product-of-exponentials formula in section 6.

4. Velocity: two twists per motion, glued by \(\mathrm{Ad}\) (MLS §2.4–2.5)

Why this needs care: for a particle, velocity is \(\dot q\). For a body whose pose is a matrix \(g(t)\), the derivative \(\dot g\) is… not a pose, not a velocity, not an element of anything useful. The book’s move: don’t ask what \(\dot g\) is; ask which constant twist, run for one time unit, would produce the same instantaneous motion. Formally, push \(\dot g\) back to the identity, from the left or from the right — two equally legitimate choices:

Writing \(g=(R,p)\) and differentiating \(RR^T=I\) (which forces \(\dot R R^T\) to be skew, i.e. an angular velocity) unpacks the six coordinates:

The interpretations are the crux, and they are not what a first guess would be:

• \(\omega^s\) (resp. \(\omega^b\)): the angular velocity of the body as seen in the fixed (resp. current body) frame.
• \(v^s\) is not \(\dot p\). It is the velocity of the — possibly imaginary — body point currently passing through the spatial origin. Meanwhile \(\dot p\) is the velocity of the body-frame origin, and \(v^b = R^T\dot p\) is that same quantity written in body coordinates.

The two twists are the same physical motion, so they are related by a change of coordinates. That change of coordinate is the adjoint \(\mathrm{Ad}_g\), the \(6\times6\) matrix

and its deepest form is the conjugation identity \(\;g\,\hat{\xi}\,g^{-1} = \big(\mathrm{Ad}_g\,\xi\big)^{\wedge}\): the adjoint is how a twist looks after the world (or the body) has been moved by \(g\). Apply it to a joint axis and you get the rule that will generate every Jacobian column later: to move a screw axis rigidly by \(g\), replace \(\xi\) by \(\mathrm{Ad}_g\,\xi\) (MLS eq.~2.64).

Two consequences we will use constantly. First, for a pure screw motion \(g(\theta)=e^{\hat{\xi}\theta}g(0)\) the spatial velocity is a constant twist times a rate: \(\hat V^s=\dot g g^{-1}=\hat{\xi}\,\dot\theta\), i.e. \(V^s=\xi\,\dot\theta\) — the twist is the velocity (MLS §2.4.3). Second, velocities compose across a chain by \(V^s_{ac}=V^s_{ab}+\mathrm{Ad}_{g_{ab}}V^s_{bc}\): the downstream velocity must be expressed in the upstream frame before adding. Both follow from the product and chain rules in two lines each.

Wrenches: the dual side (MLS §2.5). A force/torque pair \(F=(f,\tau)\in\mathbb{R}^6\) pairs with a twist to give instantaneous work, \(\delta W = V\cdot F = v\cdot f + \omega\cdot\tau\). Demanding that the work be the same number no matter which frame you compute it in forces wrenches to transform by the transpose adjoint:

Velocities flow \(V^s=\mathrm{Ad}_gV^b\) (upstream), forces flow the opposite way with a transpose (downstream) — the \( -R^T\hat p\) term is literally the torque \(-p\times f\) of a force applied at a lever arm. Twists and wrenches that satisfy \(V\cdot F=0\) are reciprocal: you can move along such a twist while the force does no work. This single sentence powers grasp analysis (chapter 5) and one of the singularity proofs in section 7.

5. Forward kinematics = a product of exponentials (MLS §3.2)

The goal: given joint angles \(\theta=(\theta_1,\dots,\theta_n)\), where is the tool? Classical textbooks answer with Denavit–Hartenberg parameters: four magic numbers per link, a mandatory bookkeeping frame on every link, and conventions so error-prone that two books disagree. The geometric answer is cleaner: a joint is a screw, and motions compose by multiplication.

Build-up. Fix a reference configuration \(\theta=0\) (you choose it — typically arm extended) and let \(g_{st}(0)\) be the tool pose there. For joint \(i\), write its screw as a constant twist \(\xi_i\) in base coordinates, evaluated at the reference pose: a revolute joint about axis \((\omega_i, q_i)\) gives \(\xi_i=(-\omega_i\times q_i,\ \omega_i)\); a prismatic joint along \(v_i\) gives \(\xi_i=(v_i,0)\). Moving only joint \(i\) is exactly the screw motion \(e^{\hat{\xi}_i\theta_i}\). Now ask: can we move joints one at a time in any order and always get the same final pose? Yes — because moving an earlier joint drags the later axis along, and dragging an axis is an adjoint, under which the exponential conjugates: \(e^{\hat{\xi}_1\theta_1}\,e^{\hat{\xi}_2'\theta_2}\) with \(\xi_2'=\mathrm{Ad}_{e^{\hat{\xi}_1\theta_1}}\xi_2\) equals \(e^{\hat{\xi}_1\theta_1}e^{\hat{\xi}_2\theta_2}e^{-\hat{\xi}_1\theta_1}\cdot e^{\hat{\xi}_1\theta_1}=e^{\hat{\xi}_1\theta_1}e^{\hat{\xi}_2\theta_2}\). The composition below is therefore order-independent and each \(\xi_i\) is computed once, at \(\theta=0\), from geometry you can read off a drawing:

The rightmost factor acts on the tool first, then joint \(n\), then \(n-1\), … then joint 1 — the opposite of the arm’s mechanical order, which is exactly what keeps every \(\xi_i\) constant.

Worked example: SCARA (MLS Example 3.1). Three revolute joints about parallel vertical axes and one prismatic. At \(\theta=0\) the arm lies along the \(+y\) axis with joints at \(q_1=(0,0,0)\), \(q_2=(0,l_1,0)\), \(q_3=(0,l_1+l_2,0)\), all with \(\omega_i=\hat z\), and the prismatic joint slides along \(\hat z\):

Multiplying the four exponentials (symbolically verified) gives the tidy forward map

with \(\theta_3\) absent from the position — as it must be, since joint 3 spins the tool about its own vertical axis. Notice what just happened: no link frames, no DH table, four twists read off the drawing.

6. The Jacobian: read it off, don’t differentiate (MLS §3.4)

Why not just differentiate \(g(\theta)\)? Because \(\partial g/\partial\theta_i\) is a \(4\times4\) matrix that is neither a pose nor a velocity — the derivative of a group element is not in the group. The correct object is the twist of the end-effector, \(V^s=\widehat{\dot g\,g^{-1}}\). Differentiate the PoE product joint by joint: when you differentiate \(e^{\hat{\xi}_i\theta_i}\) the factors to its right cancel against \(g^{-1}\), and the factors to its left are left holding a conjugated twist — which the adjoint turns back into a vector. Line by line:

The structure is the theorem: column \(i\) is joint \(i\) ’s own screw, rigidly carried along by the joints before it — and it does not depend on the joints after it. For a revolute joint that means the by-inspection recipe: point the axis where it points now (\(\omega_i'=R_1\cdots R_{i-1}\omega_i\)), place it where its axis passes now (\(q_i'\)), and write \(\xi_i'=(-\omega_i'\times q_i',\ \omega_i')\). The whole Jacobian, no calculus.

SCARA by inspection (MLS Example 3.8; matches the adjoint formula to the last digit — verified): the axes stay vertical, only their intersection points move, so

And the earlier subtlety pays off: the tool-origin velocity is not the top three entries of \(J^s\dot\theta\) but \(\dot p = v^s+\omega^s\times p\) — also verified symbolically. The twist is the primary object; positions are recovered from it.

Forces: the transpose rule. Equate the work done at the joints, \(\dot\theta^T\tau\), with the work done at the tool, \(V^{b}\cdot F^{b}=\dot\theta^T(J^b)^TF^b\); since \(\dot\theta\) is arbitrary,

For the SCARA, the bottom half of every revolute column is \(\hat z\), so a wrench whose torque part points along \(x\) or \(y\) (try to twist the tool about a horizontal axis) lies in the null space of \((J^s)^T\): it requires zero joint torque and produces zero motion — the mechanism’s structure silently resists it (MLS Example 3.10). Remember this asymmetric bookkeeping: \(F\in\ker J^T\) = no torque needed, while \(V\in\ker J\) = unreachable direction.

Singularities: when columns lean on each other (MLS §3.4.3)

Definition with consequences: a singular configuration is one where \(J\) loses rank. There, some twist direction is unreachable (big joint speeds for small motion), and some wrench direction is unresistable-by-motors. Because columns are screws, singularities are geometric coincidences between axes, catalogued by MLS:

For a 6-DOF arm each coincidence costs one degree of freedom. Spherical wrists hit singularity (a) routinely; an elbow arm fully stretched hits (b); the shoulder-wrist alignment hits (c).

Manipulability: the Jacobian as a lens (MLS §3.4.4)

Why ellipsoids: feed the unit ball of joint speeds \(\{\dot\theta:\|\dot\theta\|=1\}\) through the (position part of the) Jacobian and look at the image — an ellipsoid whose long axis is the direction of easy motion and whose short axis is the direction where the arm is stiffest. The singular values \(\sigma_{\min}\le\sigma_{\max}\) of \(J\) measure those axes; \(\sigma_{\min}=0\) is singularity; \(\sigma_{\min}/\sigma_{\max}\) (inverse condition number) is a configuration-quality score used to place robot bases and choose redundant-arm postures.

Figure 7. Unit joint-speed ball mapped to workspace velocity ellipses for \(l_1=l_2=1\). At \(\theta_2=90^\circ\) the arm is isotropic-ish; folding to \(150^\circ\) trades reach for speed; stretching to \(\theta_2=8^\circ\) the ellipse collapses to a sliver: near the boundary you can only move perpendicular to the arm. (Wolfram Language.)

7. Inverse kinematics as geometry: Paden–Kahan subproblems (MLS §3.3)

Why the PoE form wins here too: an IK problem \(e^{\hat{\xi}_1\theta_1}\cdots e^{\hat{\xi}_n\theta_n}g(0)=g_d\) can be solved by peeling joints off from the outside in: left-multiply by \(e^{-\hat{\xi}_1\theta_1}\) to isolate the subproblem “rotate about one axis until a point reaches a target.” The book reduces essentially all solvable cases to three canonical subproblems; the first is the workhorse:

Why atan2 of (cross, dot): rotation about the axis preserves each point’s distance along the axis and its distance from the axis, so the motion lives in the plane perpendicular to \(\omega\); in that plane \(p'\cdot w'=\|p'\|\|w'\|\cos\theta\) and \(\omega\cdot(p'\times w')=\|p'\|\|w'\|\sin\theta\). atan2 reassembles the angle with the correct quadrant; zero, one, or infinitely many solutions depending on whether the radii match.

Subproblem 2 (rotate about two successive axes — two circles intersect) and Subproblem 3 (rotate about one axis until a point reaches a prescribed distance \(\delta\) from a target — circle meets sphere) complete the toolkit. A full elbow-arm solution then reads like a recipe: use 3 to fold the elbow until the wrist is at distance \(l_2\) from the target (giving \(\theta_2\), two branches), then 2 for the shoulder pair, then 1 for the final roll. Six-DOF arms with spherical wrists decompose the same way — MLS solves Stanford and elbow arms in a page each. What you give up versus numerical IK: completeness for general geometries; what you gain: all branches, no iterations, and formulas you can differentiate.

8. Dynamics: Lagrange, and the pair \((M, C)\) with superpowers (MLS §4.2–4.3)

Why Lagrange instead of \(F=ma\) per link? An arm is a pile of constraints: each link is bolted to the next, and constraint forces do no work and cancel in pairs. Newton’s method makes you compute those internal forces only to watch them cancel. Lagrange’s method parametrizes the system by \(n\) generalized coordinates — for a robot, simply the joint angles \(\theta\) — and the constraints are absorbed before any force accounting starts. From the Lagrangian \(\mathcal{L}=T-V\) (kinetic minus potential energy), the equations of motion are

with \(\Upsilon_i\) the generalized force — at joint \(i\), the actuator torque \(\tau_i\). (Sanity check: with no constraints and \(q=x\), this restates Newton’s law.)

Kinetic energy via the tools we already own. For link \(i\), its center-of-mass pose is \(g_{sl_i}(\theta)=e^{\hat{\xi}_1\theta_1}\cdots e^{\hat{\xi}_i\theta_i}g_{sl_i}(0)\) — the same PoE formula, truncated. Its body velocity is \(V_i^b=J_i^b(\theta)\dot\theta\) with a link Jacobian of exactly the section-6 form (nonzero only in columns \(1..i\)), so its kinetic energy is \(\tfrac12 V_i^{bT}M_iV_i^b\), where the generalized inertia matrix of a rigid body about its center of mass is the block-diagonal \(M_i=\mathrm{diag}(m_i I,\ \mathcal{I}_i)\) with \(\mathcal{I}_i\) the usual inertia tensor. Summing links and collecting,

Carrying this \(\mathcal L=\tfrac12\dot\theta^TM\dot\theta - V(\theta)\) through Lagrange’s equations and collecting the quadratic-in-velocity terms yields the standard robot equation

where \(N=\partial V/\partial\theta\) collects gravity (plus any friction you model) and the \(\Gamma_{ijk}\) are the Christoffel symbols of the inertia matrix — differential geometry vocabulary that a mechanical engineer may recognize from tensor calculus on curved surfaces; the “curved surface” here is the configuration space of the robot itself. There is a parallel per-link route, the Newton–Euler equations in body coordinates, \(\begin{bmatrix}mI&0\\0&\mathcal I\end{bmatrix}\begin{bmatrix}\dot v^b\\ \dot\omega^b\end{bmatrix} +\begin{bmatrix}\omega^b\times mv^b\\ \omega^b\times\mathcal I\omega^b\end{bmatrix}=F^b\), which recursive algorithms use for speed; it is the same physics with different bookkeeping.

Worked example: the two-link planar arm (MLS §4.2.3)

Links \(m_i,l_i\), centers of mass at \(r_i\) along each link, \(\mathcal I_{z_i}\) the in-plane inertia. Writing out \(T\) (position of each COM by chain rule, differentiate, square) gives three constants that carry the whole story:

Read the physics off the matrix: \(M_{11}=\alpha+2\beta\cos\theta_2\) is the inertia about joint 1 as felt by motor 1 — large when the arm stretches out \((\theta_2=0)\), small when folded \((\theta_2=\pi)\); that is the \(2\beta\cos\theta_2\) term. The off-diagonal \(\delta+\beta\cos\theta_2\) says motor 2’s rotor drags joint 1. The \(C\dot\theta\) terms are the Coriolis/centrifugal pair: \(-\beta s_2\dot\theta_2(2\dot\theta_1+\dot\theta_2)\) on joint 1, and the reaction \(+\beta s_2\dot\theta_1^2\) on joint 2 — a spinning forearm pushes on the elbow. None of this was imposed; it fell out of \(\tfrac12\dot\theta^TM\dot\theta\).

The superpower: \(\dot M - 2C\) is skew-symmetric

For any robot built from rigid links with this particular (Christoffel) choice of \(C\), MLS Lemma 4.2 guarantees — and symbolic computation confirms for the two-link arm, where \(\dot M-2C=\begin{bmatrix}0&\beta s_2(2\dot\theta_1{+}\dot\theta_2)\\ -\beta s_2(2\dot\theta_1{+}\dot\theta_2)&0\end{bmatrix}\) — that \(\dot M-2C\) is skew-symmetric. Why care: differentiate the kinetic energy and substitute the equations of motion,

because \(\dot\theta^T S\,\dot\theta=0\) for skew \(S\). Translation: power in through the motors minus power lost to gravity equals the rate of change of kinetic energy — the equations conserve energy exactly, a structural property no amount of term-by-term algebra makes obvious, and the hinge on which every stability proof in the next section swings.

9. Control: computed torque and the model-free PD (MLS §4.4–4.5)

The task: follow a desired trajectory \(\theta_d(t)\). The naive first idea — invert the dynamics once, \(\tau = M\ddot\theta_d + C\dot\theta_d + N\) — is an open-loop law: it assumes \(\theta(0)=\theta_d(0)\) exactly and corrects nothing afterwards. Real arms start with error, sag under friction, and drift. We need feedback, and the structure of the dynamics tells us exactly how much model we must buy.

Computed torque: buy the whole model, get a linear system

Idea: use the model to cancel the nonlinearities in real time, then inject linear error feedback. Set \(e=\theta-\theta_d\) and

Substituting into \(M\ddot\theta+C\dot\theta+N=\tau\): the \(C\dot\theta\) and \(N\) terms cancel verbatim; \(M\) (positive-definite, hence invertible) cancels around it, leaving

Each step is one cancellation — and the punchline is that the error of a nonlinear, configuration-dependent machine obeys a linear, constant-coefficient ODE. Choose \(K_p=\omega_n^2 I,\ K_v=2\zeta\omega_n I\) and every error component decays like a damped oscillator (exponential tracking, MLS Proposition 4.8). This is feedback linearization: the nonlinear model is not an obstacle but a feedforward you own. Its price: you must know \(M(\theta)\), \(C\), and \(N\) accurately, and you recompute them every control tick.

Plain PD: buy no model at all, get a setpoint guarantee

Now drop the model entirely: \(\tau=-K_v\dot e-K_p e\). For setpoint stabilization (\(\theta_d\equiv\) const) MLS Proposition 4.9 proves global asymptotic stability with total energy as the Lyapunov function:

The middle term dies because \(\dot M-2C\) is skew — the energy-conservation structure of section 8, spent as a control-theoretic weapon. \(\dot{\mathcal V}\le 0\) gives stability; since \(\dot{\mathcal V}\) only sees velocity, LaSalle’s invariance principle finishes the job: trajectories settle into the set where \(\dot\theta\equiv 0\), and there the dynamics force \(K_pe=0\), i.e. \(e=0\). No model of \(M\), \(C\), or gravity was needed — only their structure. The bill for model-freedom: no trajectory tracking (nothing cancels \(C\dot\theta+N\) along the way, so persistent errors persist), and only asymptotic — not exponential — convergence.

The practical compromise (augmented PD, MLS eq.~4.53): feed forward the model along the desired path only, and let skew-symmetry handle the rest: \(\tau=M\ddot\theta_d+C\dot\theta_d+N-K_v\dot e-K_pe\), giving \(M\ddot e+(C+K_v)\dot e+K_p e=0\) — exponentially stable, and robust to the parts of the model you got wrong (they act as disturbances that the PD loop attenuates). The same trick transplants to workspace coordinates \(x\): with effective inertia \(\tilde M=J^{-T}MJ^{-1}\) the error equation keeps its form, and near a singularity \(\tilde M\) blows up — the geometry of section 6 reappearing as a control problem.

Figure 8. The book’s two-link arm (\(\alpha=5/3,\ \beta=1/2,\ \delta=1/3\)) tracking \(\theta_{d}(t)=(0.9\sin t,\ 0.8\sin 0.6t)\) from initial error \((0.15,-0.15)\), gains \(K_p=100I,\ K_v=20I\). Left: computed torque drives the error to numerical zero exponentially (the linear ODE at work); PD alone stalls near \(10^{-2}\) — no feedforward, no cancellation of \(C\dot\theta\). Right: PD (red) visibly lags the desired path at the peaks. Simulated and plotted in Wolfram Language.

10. Grasping: the wrench calculus spent (MLS ch. 5)

Why hands need new theory: a gripper holds an object through contacts, and a contact is a very partial kind of actuator: your fingertip can push the surface but not pull it, and it slips if it pushes too tangentially. The chapter-5 question is: given where and how the fingers touch, which object wrenches can be produced, and can that set be all of them? The answer is built entirely from the chapter-2 machinery — wrenches, adjoints, friction cones.

Contact models: how strong is a fingertip?

Each contact gets a frame \(C_i\) on the object surface and a wrench basis \(B_{c_i}\) (the directions it can push in) plus a friction cone \(\mathcal{FC}_{c_i}\) (how far from the normal it may push). Coulomb’s law says the tangential force is bounded by the normal force, \(|f^t|\le\mu f^n\) — geometrically, all allowable forces lie in a cone of half-angle \(\tan^{-1}\mu\) about the surface normal. The book’s three standard models:

The grasp map: finger forces → object wrench

Why a linear map suffices: wrenches superpose, and each finger’s wrench must be re-expressed at the object frame — which is exactly the transpose-adjoint rule \(F^b=\mathrm{Ad}^T_gF^s\) of section 4. Pull all \(k\) contacts together and the object wrench is

For frictionless contacts this collapses to a beautiful picture: each finger contributes one column \(\begin{bmatrix}n_{c_i}\\ p_{c_i}\times n_{c_i}\end{bmatrix}\) — the contact normal plus the torque it makes about the object origin — with a nonnegative coefficient. The grasp is then just a list of push directions and their lever arms.

Worked example (MLS Example 5.3, verified): planar rectangle, two contacts on the opposite edges at \(p_{c_1}=(-r,0)\), \(p_{c_2}=(r,0)\), contact \(y\)-axes along the inward normals. Carrying the planar wrench transform \(\begin{bmatrix}R_{c_i} & 0\\ (-p_y,\ p_x)\,R_{c_i} & 1\end{bmatrix}\) (force to object coordinates, plus the torque \(p\times f\) of the lever arm) through for each basis direction gives

The two normal columns are equal-and-opposite pushes: \(f_N\) is an internal force — the squeeze. It loads the friction cones (so the fingers don’t slip) while exerting zero net wrench on the object. Every useful grasp leans on internal forces.

Force closure: when the grasp can resist anything

Why the internal force matters rigorously (Prop 5.2): a grasp is force-closure if and only if \(G\) is surjective and there is a strictly internal force \(f_N\in\ker G\cap\operatorname{int}\mathcal{FC}\). The proof is one line of geometry: given any desired \(F_o=Gf_c'\), add \(\alpha f_N\) with huge \(\alpha\) — the sum stays in the cone (cones are closed under positive addition) while \(G(f_c'+\alpha f_N)=F_o\) is unchanged.

Worked check (MLS Example 5.4, verified): four frictionless contacts clustered near the corners of a planar rectangle give \(G=\begin{bmatrix}1&0&-1&0\\0&-1&0&1\\-a&b&-a&b\end{bmatrix}\) with \(a,b>0\); enumerating all candidate separating-hyperplane normals (planes through pairs of columns) shows none has \(v^TG\ge 0\) componentwise — force-closure. Slide all contacts onto two collinear edges (\(a=b=0\)) and \(v=(0,0,1)\) satisfies \(v^TG=0\): no contact can exert torque, so the grasp cannot resist a twist about the object normal. Geometry, not algebra, decides.

How many fingers are enough? Positive-span counting plus two classical convexity theorems (Carathéodory, Steinitz) bracket the answer: a force-closure grasp needs at least \(p{+}1\) frictionless contacts and at most \(2p\). The book’s table (5.3), in wrench-space dimension \(p\):

Friction is worth fingers: with point-contact-with-friction, four fingers grasp essentially any 3-D object (three for most planar ones); the human hand’s five is a redundancy margin, not a necessity.

11. Hands in motion: constraints, multipliers, and rolling (MLS ch. 6)

Why dynamics changes: fingers touch things. Contact means the hand+object system is subjected to velocity constraints (no slipping), and the grip forces we met above are constraint forces. Chapter 6 rebuilds Lagrangian mechanics to handle both honestly.

Pfaffian constraints: a velocity constraint that is linear in \(\dot q\), \(A(q)\,\dot q=0\) with \(A\in\mathbb{R}^{k\times n}\). If it is equivalent to some \(h(q)=0\) (a hypersurface in configuration space) it is holonomic — you could have eliminated a coordinate; if not, it is nonholonomic, and (as chapter 7 will show) it restricts velocities but not reachability.

Why keep redundant coordinates and Lagrange multipliers? For the spherical pendulum MLS keeps \(q=(x,y)\) plus the constraint \(x^2+y^2=l^2\) and solves for the multiplier: \(\lambda=-\frac{m}{l}(gy+\dot x^2+\dot y^2)\) — which is precisely the tension in the rod. Multipliers are not bookkeeping noise; they are the forces your sensors cannot directly read (grip pressure, contact loads). The constrained equations become \(M\ddot q + C\dot q + N = A^T\lambda\): the constraint force is the vector \(A^T\lambda\) along the constraint gradients — normal to the constraint surface, as it must be.

Lagrange–d’Alembert: the principled shortcut. D’Alembert’s principle says constraint forces do no work on virtual displacements — any \(\delta q\) with \(A(q)\delta q=0\), whether or not the actual motion goes that way. So project the equations onto the null space of \(A(q)\) and never solve for \(\lambda\):

Worked example: the rolling disk (MLS Example 6.2). A disk of radius \(\rho\) on the plane, configuration \(q=(x,y,\theta,\phi)\) (position, heading, disk angle), rolling without slipping:

Running Lagrange–d’Alembert: the virtual-displacement condition forces \(\delta x=\rho\cos\theta\,\delta\phi,\ \delta y=\rho\sin\theta\,\delta\phi\), the \(x,y\) rows get absorbed, and after re-substituting the constraints the dynamics collapse to

Second-order dynamics in the reduced variables \((\theta,\phi)\); the position \((x,y)\) follows by integrating the constraints afterwards. This two-tier structure — reduced dynamics plus kinematic reconstruction — is the fingerprint of a nonholonomic system. Note the effective inertia \(I_2+m\rho^2\): the disk must also accelerate the ground contact, exactly the \(m\rho^2\) parallel-axis term.

Controlling a hand: chapter 6 closes by composing the pieces: the hand+object dynamics is an unconstrained Lagrangian system plus contact constraints; internal forces (vectors in \(\ker G\)) are commanded on top of object-motion forces to keep every contact inside its friction cone; and the whole controller is stacked hierarchically — object-level wrench control at the top, finger joint control beneath it, each layer using the Jacobian transposes of chapter 3.

12. Nonholonomic behavior: sideways is impossible, parking is not (MLS ch. 7)

The puzzle: a car’s wheels forbid sideways sliding, yet you can parallel park — ending up displaced sideways with the car pointing the original way. The constraint “no sideways velocity” evidently does not confine the car to some surface in configuration space. When does a velocity constraint fail to restrict configurations, and how do you exploit that?

The translation that makes it a control question: take the null space of the constraint matrix as your set of feasible motion directions. If the rows of \(A(q)\) are \(\omega_i(q)\), choose basis vectors \(g_j(q)\) with \(\omega_i g_j=0\) and write every feasible trajectory as a control system

The constraint is completely nonholonomic exactly when this system can reach everywhere — controllability is the dual of integrability.

The Lie bracket is the sideways move. Run \(g_1\) for \(\epsilon\), then \(g_2\), then \(-g_1\), then \(-g_2\). Each leg returns you nominally, but the flows fail to commute in a very specific small way — Taylor-expanding the four legs,

The two theorems that organize everything:

Worked example: the rolling disk (MLS Example 7.9, verified). From the constraints of section 11, with inputs chosen as rolling rate \(u_1=\dot\phi\) and steering rate \(u_2=\dot\theta\):

\([g_1,g_2]\) is the sideways translation the disk cannot do directly — at any \(\theta\) it is perpendicular to the heading. Bracket once more with \(g_2\) and you recover the forward direction scaled: the four fields span \(\mathbb{R}^4\) everywhere (rank 4). The book summarizes this with the growth vector \((2,3,4)\): 2 input directions, 3 after one bracket, 4 after two — degree of nonholonomy 3. The kinematic car has the same growth \((2,3,4)\) (away from \(\phi=\pm\frac{\pi}{2}\)), which is why it parks like the disk rolls. And a spherical fingertip rolling on a plane gives growth \((2,3,5)\) — the same mathematics that chapter 8 will use to reposition fingers by rolling.

13. Nonholonomic motion planning: sinusoids, chains, and phases (MLS ch. 8)

The gap Chow leaves: controllability says a path exists; it does not hand you one. Chapter 8 is the constructive sequel: steer the system. Its strategy is to solve some model systems exactly, convert real systems into those coordinates, and exploit the one input shape that makes the algebra collapse — sinusoids at integrally related frequencies.

Brockett’s system and why sinusoids are optimal. The canonical degree-2 system is

Minimizing the least-squares control cost \(\int\|u\|^2dt\) subject to reaching a target turns the optimality conditions into a harmonic oscillator, whose solution is sinusoidal: the optimal steering inputs are sinusoids at frequency \(\lambda\). That fact motivates using sinusoids everywhere, even where they are not optimal.

Chained form and the steering algorithm. A two-input system is in chained form if (MLS eq. 8.7)

— a cascade where \(u_1\) drags each state down the chain. It is maximally nonholonomic: \(\mathrm{ad}^k_{g_1}g_2\) (bracket \(g_2\) with \(g_1\), \(k\) times) points along \(\pm q_{k+2}\) alone. The steering algorithm (MLS Alg. 3, verified numerically for \(k=1,2,3\)): choose \(u_1=a\sin 2\pi t\), \(u_2=b\cos 2\pi k t\) for \(t\in[0,1]\). Products of sinusoids shift frequencies, so every state except \(q_{k+2}\) integrates to a periodic signal — and the last one accumulates

One sinusoid period moves exactly one chain coordinate; repeat for each \(k\) and you steer the whole state vector. The kinematic car converts into chained form (MLS Example 8.2), so this is literally an automatic-parking algorithm.

Figure 14. The rolling disk driven by one period of \(u_1=a\sin 2\pi t\) (roll) and \(u_2=b\cos 2\pi t\) (steer), \(a=b=\rho=1\). The disk wiggles out and back (Δx = 0, heading restored) but ends displaced sideways: exactly the bracket motion of Figure 13, and the exact integral evaluates to \(\Delta y = a\rho\, J_1(b/2\pi)\approx 0.44\) (verified numerically). Repeat with adjusted amplitudes and you park anywhere.

Fourier techniques for systems that are not quite chained. For the hopping robot in flight (or a cat falling with no angular momentum to push against), the equations reduce to \(\dot\alpha=f(l)u_1\) after the easy coordinates are driven home. With \(u_1=a_1\sin2\pi t\) and \(u_2\) chosen so \(l=\frac{a_2}{2\pi}\sin2\pi t\), expand \(f(l)\) in a Fourier series: the constant and all harmonics except the first integrate to zero over the period, leaving \(\Delta\alpha=\tfrac12 a_1\beta_1(a_2)\) — solve two scalar equations for the two amplitudes and you have reoriented the body. “Cartwheeling in mid-air” is a phase of \(2\pi\).

Geometric phase and Gauss–Bonnet. The deepest statement in the chapter is qualitative: for a sphere rolling on a plane along a closed path enclosing (spherical) area \(\Omega\), the change in contact angle is

independent of the sphere’s radius. The net rotation is the geometric phase of the loop — the same phenomenon that lets a cat land on its feet and lets a hand reposition a fingertip on a grasped object (MLS §8.4) by tracing the right closed loop. Position space forgets the path; the orientation remembers its area.

14. Future prospects (MLS ch. 9) — and the punchline

Chapter 9 surveys three application frontiers as of 1994: robots in hazardous environments (nuclear service, hazardous-materials handling), medical applications for multifingered hands (minimally invasive surgery, prosthetics), and microrobotics (MEMS-scale manipulation, where friction and surface forces dominate gravity). Three decades on, all three became industries — and all three still run on the mathematics of chapters 2–8.

The punchline of the whole book is the one this page opened with: pick a rigid body and you have picked \(SE(3)\); pick a joint and you have picked a screw \(\hat{\xi}\); differentiate and you get twists and adjoints; differentiate twice and you get \(M\ddot\theta+C\dot\theta+N=\tau\) with its skew-symmetric superpower; constrain the velocities and Lie brackets tell you whether geometry has locked you in (it has not); and every control law, grasp test, and parking maneuver is linear algebra on those same matrices. Abstraction, as the preface promised, saves time in the end.

15. Formula sheet

Source: R. M. Murray, Z. Li, S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994 (2017 electronic edition). All figures computed with Wolfram Language from the book’s own examples; all boxed formulas re-derived and verified symbolically.