Robot Dynamics

Step 1 — the momentum. $\partial L/\partial\dot q = M(q)\dot q$, because $K$ is a quadratic form and $V$ has no $\dot q$ in it.

Step 2 — its time derivative. $\tfrac{d}{dt}(M\dot q) = M\ddot q + \dot M\dot q$, and since $M$ depends on $t$ only through $q$, $\dot M_{ij} = \sum_k (\partial M_{ij}/\partial q_k)\,\dot q_k$.

Step 3 — the configuration derivative. $\partial L/\partial q_i = \tfrac12\dot q\T(\partial M/\partial q_i)\dot q - \partial V/\partial q_i$.

Step 4 — subtract and symmetrize. The $i$-th equation reads

The middle sum is a quadratic form in $\dot q$ whose coefficient matrix $\partial M_{ij}/\partial q_k$ is not symmetric in $(j, k)$. A quadratic form only sees the symmetric part, so replace it by $\tfrac12(\partial M_{ij}/\partial q_k + \partial M_{ik}/\partial q_j)$. Together with the third sum that is exactly $\sum_{jk}\Gamma^i_{jk}\dot q_j\dot q_k$ — the symmetrization is where the three-term Christoffel formula comes from.

Step 5 — read off the pieces. Define $c_i = \sum_{jk}\Gamma^i_{jk}\dot q_j\dot q_k$ (eq. 10.8), $g_i = \partial V/\partial q_i$, and any matrix $C$ with $C\dot q = c$; the choice $C_{ij} = \sum_k\Gamma^i_{jk}\dot q_k$ is the one the rest of the chapter uses. $\blacksquare$

The RP arm as a check (Choset Examples 10.1.2, 10.2.1). With $M = \operatorname{diag}(I_1 + I_2 + m_1 r_1^2 + m_2 q_2^2,\; m_2)$, the only nonzero symbols are $\Gamma^1_{12} = \Gamma^1_{21} = m_2 q_2$ and $\Gamma^2_{11} = -m_2 q_2$, and the velocity-product vector is $(2 m_2 q_2\dot q_1\dot q_2,\; -m_2 q_2\dot q_1^2)$ — eqs. (10.5)–(10.6) exactly. The cast of this book is revolute, so the RP arm survives only here.

$C$ is not unique; $C\dot q$ is. Add to $C$ any matrix $S$ with $S\dot q = 0$ — for $n = 2$, $S = s\,[\dot q_2, -\dot q_1]\T[\dot q_2, -\dot q_1]$ for any scalar $s$ — and the equations of motion are unchanged. What is changed is the property the next derivation proves, which is why the Christoffel choice is the one worth naming.

Step 1 — write both pieces with the same partials. From Step 2 above, $\dot M_{ij} = \sum_k\frac{\partial M_{ij}}{\partial q_k}\dot q_k$. From the definition, $2C_{ij} = \sum_k\left(\frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i}\right)\dot q_k$.

Step 2 — the first terms cancel. Subtracting leaves $(\dot M - 2C)_{ij} = \sum_k\left(\frac{\partial M_{jk}}{\partial q_i} - \frac{\partial M_{ik}}{\partial q_j}\right)\dot q_k$.

Step 3 — what remains is antisymmetric. Swap $i$ and $j$ and the bracket changes sign: $(\dot M - 2C)_{ji} = -(\dot M - 2C)_{ij}$. So $\dot q\T(\dot M - 2C)\dot q = 0$ for every $\dot q$, since a quadratic form of a skew matrix vanishes identically.

Step 4 — differentiate the energy. $E = \tfrac12\dot q\T M\dot q + V$, so $\dot E = \dot q\T M\ddot q + \tfrac12\dot q\T\dot M\dot q + \dot q\T g$. Substitute $M\ddot q = u - C\dot q - g - b$:

Why this is the test the dynamics crate runs. The derivation used only (i) the correct $M$, (ii) the correct $\Gamma$ obtained from it, and (iii) $g = \partial V/\partial q$. An error in any of the three breaks the identity, and an integrator run from rest with $u = b = 0$ will show it as energy drift. That is one assertion covering the whole derivation. The library check dynamics: released from rest … E stays within 1e-6 relative under RK4 is exactly this.

Why dropping $C$ breaks it. With $C \equiv 0$ the Step 4 remainder is $\tfrac12\dot q\T\dot M\dot q$, which is not zero — it is the rate at which the configuration-dependence of $M$ pumps energy in or out. That is the Lagrangian Lab's lie, and the sign of the drift depends on whether $\dot M$ is positive or negative along the motion (Exercise 4).

Step 1 — centers of mass by forward kinematics. Chapter 2's $\varphi$ evaluated at $r_i$ instead of $L_i$: $c_1 = r_1(\cos q_1, \sin q_1)$ and $c_2 = (L_1\cos q_1 + r_2\cos(q_1 + q_2),\; L_1\sin q_1 + r_2\sin(q_1 + q_2))$.

Step 2 — center-of-mass Jacobians. Differentiating, $J_{c_1} = r_1\begin{pmatrix}-\sin q_1 & 0\\ \cos q_1 & 0\end{pmatrix}$ and

These are Chapter 4's Jacobian construction applied to interior points of the links.

Step 3 — kinetic energy. Each link contributes translation of its center of mass plus rotation about it: $K = \sum_i \tfrac12\left(m_i\|J_{c_i}\dot q\|^2 + I_i\omega_i^2\right)$ with $\omega_1 = \dot q_1$ and $\omega_2 = \dot q_1 + \dot q_2$, i.e. $J_{\omega_1} = (1, 0)$, $J_{\omega_2} = (1, 1)$. Hence

Multiplying out: $\|J_{c_2}\dot q\|^2 = (L_1^2 + r_2^2 + 2L_1 r_2\cos q_2)\dot q_1^2 + 2(r_2^2 + L_1 r_2\cos q_2)\dot q_1\dot q_2 + r_2^2\dot q_2^2$, using $\cos q_1\cos(q_1{+}q_2) + \sin q_1\sin(q_1{+}q_2) = \cos q_2$. Collect by $\dot q_i\dot q_j$ and the three entries of $M$ in the statement appear.

Step 4 — differentiate for $\Gamma$. Only $q_2$ appears in $M$, and only through $\cos q_2$: $\partial M_{11}/\partial q_2 = -2h$, $\partial M_{12}/\partial q_2 = -h$, every other partial is zero. Feeding these into the three-term formula: $\Gamma^1_{12} = \tfrac12(\partial M_{11}/\partial q_2) = -h$; $\Gamma^1_{22} = \tfrac12(2\,\partial M_{12}/\partial q_2 - \partial M_{22}/\partial q_1) = -h$; $\Gamma^2_{11} = \tfrac12(2\,\partial M_{21}/\partial q_1 - \partial M_{11}/\partial q_2) = h$; $\Gamma^2_{12} = \tfrac12(\partial M_{21}/\partial q_2 + \partial M_{22}/\partial q_1 - \partial M_{12}/\partial q_2) = 0$. Contracting, $c_1 = -2h\dot q_1\dot q_2 - h\dot q_2^2$ and $c_2 = h\dot q_1^2$ (Problem 10.3).

Step 5 — gravity. $V = m_1 a_g r_1\sin q_1 + m_2 a_g\left(L_1\sin q_1 + r_2\sin(q_1 + q_2)\right)$, and $g = \partial V/\partial q$ is the statement. $\blacksquare$

The same recipe for 3R. Nothing in Steps 1–5 used $n = 2$ except the trigonometric collapse in Step 3. For Reach's three-link variant there are three center-of-mass Jacobians, nine entries of $M$ and 27 symbols — too many to differentiate by hand without error, so the library assembles $M$ from the Jacobians and takes the Christoffel symbols by central differences. The 2R case is the check: the finite-difference symbols agree with the closed form above to $10^{-7}$ at fifty random states.

Step 1 — differentiate the constraint. $A\dot q = 0$ holds for all time, so $\tfrac{d}{dt}(A\dot q) = A\ddot q + \dot A\dot q = 0$ (eq. 10.17).

Step 2 — solve the force balance for $\ddot q$. From (10.16), $\ddot q = M^{-1}(u + A\T\lambda - C\dot q - g)$.

Step 3 — substitute into the differentiated constraint. $\dot A\dot q + AM^{-1}(u + A\T\lambda - C\dot q - g) = 0$.

Step 4 — solve for $\lambda$. The $k \times k$ matrix $AM^{-1}A\T$ is invertible because $A$ has full row rank and $M \succ 0$: $\lambda = (AM^{-1}A\T)^{-1}\left(-\dot A\dot q + AM^{-1}(C\dot q + g - u)\right)$.

Step 5 — substitute back and factor. Using $-\dot A\dot q = A\ddot q$ and collecting, $(\mathbb{1} - A\T(AM^{-1}A\T)^{-1}AM^{-1})(M\ddot q + C\dot q + g - u) = 0$. Call the bracket $P_u$ (eq. 10.18); then $P = M^{-1}P_u M$ (eq. 10.19) and rearranging gives the statement (eq. 10.20). $\blacksquare$

Orthogonality with respect to $M$. $P$ and $\mathbb{1} - P$ split a velocity into a part that satisfies the constraints and a part in the constrained directions, and $(P\dot q)\T M(\mathbb{1} - P)\dot q = 0$ for every $\dot q$. The inner product is $M$, not the identity: Choset is explicit that this is "the appropriate one when discussing dynamics," because $M$ carries the metric of coordinates that mix lengths and angles. Also $P = P_u\T$, which the library checks numerically alongside the rank.

The knife-edge (Example 10.3.1). $q = (q_1, q_2, q_3)$ is contact point and heading, $M = \operatorname{diag}(m, m, I)$, $A = [\sin q_3, -\cos q_3, 0]$, $\dot A = \dot q_3[\cos q_3, \sin q_3, 0]$. Then $P = \begin{pmatrix}\cos^2 q_3 & \sin q_3\cos q_3 & 0\\ \sin q_3\cos q_3 & \sin^2 q_3 & 0 \\ 0 & 0 & 1\end{pmatrix}$, rank 2, and the closed forms (10.25)–(10.28) follow — including the constraint force $\lambda_1 = (u_2 - m\dot q_1\dot q_3)\cos q_3 - (u_1 + m\dot q_2\dot q_3)\sin q_3$. The library solves the $(n + k) \times (n + k)$ system directly and reproduces those four formulas to $10^{-9}$ at thirty random states. This is Rusty's no-side-slip constraint, and Chapter 20 will show it is nonholonomic.

Step 1 — kinetic energy in the spatial frame. A point at $r_s$ moves at $\omega_s \times r_s$, so $K = \tfrac12\int_V\rho\,\|\omega_s \times r_s\|^2\,dV = \tfrac12\omega_s\T\mathcal{I}_s\omega_s$ with $\mathcal{I}_s = \int_V\rho(\|r_s\|^2\mathbb{1} - r_s r_s\T)\,dV$ (eqs. 10.34–10.36). Because it is written in a fixed frame, $\mathcal{I}_s$ changes as the body turns.

Step 2 — angular momentum and its rate. $P = \mathcal{I}_s\omega_s$ and $\tau_s = \dot P$. The density does not change, so $\dot{\mathcal{I}}_s = \omega_s \times \mathcal{I}_s$ comes only from the rotation, giving $\tau_s = \omega_s \times \mathcal{I}_s\omega_s + \mathcal{I}_s\dot\omega_s$ — Euler's equation in the inertial frame (10.37), or $\tau_s = \hat\omega_s\mathcal{I}_s\omega_s + \mathcal{I}_s\dot\omega_s$ in matrix form (10.38).

Step 3 — change frames. $\omega_s = R\omega$, $\tau_s = R\tau$, $\mathcal{I}_s = R\mathcal{I}R\T$, $\hat\omega_s = \dot R R\T$, and $\dot R = R\hat\omega$ (10.39, 10.41).

Step 4 — substitute. $R\tau = \dot R R\T R\mathcal{I}R\T R\omega + R\mathcal{I}R\T(\dot R\omega + R\dot\omega)$.

Step 5 — kill one term. $\dot R\omega = R\hat\omega\omega = R(\omega \times \omega) = 0$.

Step 6 — premultiply by $R\T$. $\tau = R\T\dot R\,\mathcal{I}\omega + \mathcal{I}\dot\omega = \hat\omega\mathcal{I}\omega + \mathcal{I}\dot\omega$. $\blacksquare$

The principal-axis form (10.45). Align the body frame with the eigenvectors of $\mathcal{I}$ and $\tau_x = I_{xx}\dot\omega_x + (I_{zz} - I_{yy})\omega_y\omega_z$, with the two cyclic permutations. Set $\tau = 0$: $\dot\omega_x = (I_{yy} - I_{zz})\omega_y\omega_z/I_{xx}$ is zero only if two of the components vanish. Spin about the axis of intermediate inertia and a small perturbation in the other two components grows exponentially — the flip the next widget shows. The parallel-axis theorem (10.33 planar, 10.46 spatial) and the composite-body rule $\mathcal{I} = \sum_i R_i(\mathcal{I}_i + m_i(\|r_i\|^2\mathbb{1} - r_i r_i\T))R_i\T$ are functions in the library, not sections here.