Nonholonomic Systems I — Controllability

Step 1 — one flow. Taylor-expand $\phi^g_\epsilon(x)$ in $\epsilon$: $\dot x = g(x)$ and $\ddot x = \frac{\partial g}{\partial x}\dot x = \frac{\partial g}{\partial x}g$, so $\phi^g_\epsilon(x) = x + \epsilon g(x) + \tfrac{\epsilon^2}{2}\frac{\partial g}{\partial x}(x)\,g(x) + O(\epsilon^3)$.

Step 2 — compose $g_2$ after $g_1$. Write $x_1 = \phi^{g_1}_\epsilon(x_0)$. Then $\phi^{g_2}_\epsilon(x_1) = x_1 + \epsilon g_2(x_1) + \tfrac{\epsilon^2}{2}\frac{\partial g_2}{\partial x}g_2 + O(\epsilon^3)$, and re-expanding $g_2(x_1)$ at $x_0$ gives $g_2(x_1) = g_2(x_0) + \epsilon\frac{\partial g_2}{\partial x}g_1(x_0) + O(\epsilon^2)$. The cross term $\epsilon^2\frac{\partial g_2}{\partial x}g_1$ appears; the self terms $\tfrac{\epsilon^2}{2}\frac{\partial g_i}{\partial x}g_i$ are carried along.

Step 3 — compose $-g_1$. The $\epsilon g_1$ term cancels the one from step 1. Re-expanding $-g_1$ at the current point produces $-\epsilon^2\frac{\partial g_1}{\partial x}(g_1 + g_2)$ from the first-order displacement $\epsilon(g_1 + g_2)$ accumulated so far, and the self term $+\tfrac{\epsilon^2}{2}\frac{\partial g_1}{\partial x}g_1$ from the flow of $-g_1$ itself (the sign of $g$ enters twice). The two $g_1$-self contributions cancel: $\tfrac{\epsilon^2}{2} - \epsilon^2 + \tfrac{\epsilon^2}{2} = 0$. What survives from this leg is $-\epsilon^2\frac{\partial g_1}{\partial x}g_2$.

Step 4 — compose $-g_2$. Likewise the $\epsilon g_2$ term cancels, the $g_2$-self terms cancel, and the re-expansion of $-g_2$ at a point displaced by $\epsilon g_2$ (the $g_1$ displacement is gone) gives $-\epsilon^2\frac{\partial g_2}{\partial x}g_2 + \tfrac{\epsilon^2}{2}\frac{\partial g_2}{\partial x}g_2$, which together with step 2's $\tfrac{\epsilon^2}{2}\frac{\partial g_2}{\partial x}g_2$ sums to zero.

Step 5 — collect. Every first-order term and every self term is gone; the survivors are the two cross terms, $\epsilon^2\big(\frac{\partial g_2}{\partial x}g_1 - \frac{\partial g_1}{\partial x}g_2\big)$ — the bracket.

The unicycle in closed form. Drive by $\epsilon$ at heading $x_3$, spin by $\epsilon$, drive back by $\epsilon$ at heading $x_3 + \epsilon$, spin back:

The $\epsilon^2$ term is $[g_1, g_2]$; the $\epsilon^3/2$ term points along the heading, so the loop also drifts forward by half a cube — the leak the hook measures and the number w20.1 plots.

The linear case and the sign (Choset Problem 8). For $g_1 = Ax$, $g_2 = Bx$ equation (12.5) gives $[g_1, g_2] = BAx - ABx = (BA - AB)x$, the negative of the matrix commutator $AB - BA$ that the sister book's Lie-group chapter uses. The sign is immaterial for spans and determinants' zero sets, and material for the direction of the loop residue; the library keeps Choset's and pins it in a check.

Step 1 — necessity. If the fields of $\mathcal{D}$ are tangent to a submanifold $N$, their flows stay in $N$, so the four-flow loop stays in $N$, so its residue $\epsilon^2[g_1, g_2] + O(\epsilon^3)$ is tangent to $N$ in the limit: the bracket lies in $TN = \mathcal{D}$. Involutive is forced.

Step 2 — sufficiency, in one picture. Choose $m$ independent fields of an involutive $\mathcal{D}$ near $x$. Because their brackets stay in $\mathcal{D}$, the fields can be re-combined (pointwise, by an invertible $m \times m$ matrix) into $m$ commuting fields, and commuting flows can be straightened into coordinate flows $\partial/\partial y_1, \dots, \partial/\partial y_m$ by the inverse function theorem. The leaf through $x$ is then $\{y_{m+1} = \text{const}, \dots, y_n = \text{const}\}$.

Step 3 — regularity and termination. If every $\mathcal{D}_i$ is regular, $\dim \mathcal{D}_i$ is constant in $x$ and non-decreasing in $i$, bounded by $n$; so it strictly increases or the recursion stops, after at most $n - \dim\mathcal{D}_1$ steps. Without regularity nothing bounds the degree a priori — the library runs to a stated maximum and reports a stall separately from a closure.

Step 4 — the planning consequence. A system whose closure has dimension $k < n$ at $x$ never leaves a $k$-dimensional leaf: it is neither controllable nor accessible, and no amount of cleverness with the inputs changes that. The unicycle with drive only ($k = 1$) is the canonical case.

Example 12.1.7, by the numbers. $g_1 = [x_1\cos x_3, x_2\sin x_3, 0]^\top$, $g_2 = \partial_3$, $[g_1, g_2] = [x_1\sin x_3, -x_2\cos x_3, 0]^\top$ and $\det[g_1\ g_2\ [g_1, g_2]] = x_1x_2$. The library's larc returns STLC at $(0.7, -1.1, 0.4)$, a two-dimensional leaf at $(0, 1.3, 0.4)$ — the ladder stays at $(2, 2, 2, 2, 2)$ through degree five — and a one-dimensional leaf on the $x_3$-axis. Nine leaves: a line, four half-planes, four quadrants (Choset Figure 12.9).

The Philip Hall basis. Skew-symmetry and Jacobi mean most products in $\mathcal{G}_i$ are redundant; the Hall basis chooses the smallest set at each degree. The library prunes only the trivial redundancies ($[g, g] = 0$, $[g_j, g_k] = -[g_k, g_j]$ at degree two) and products that vanish identically near the test point, and never prunes a product merely because it is dependent at the point — Example 12.1.7 on a coordinate plane is exactly a field that is zero at $x$ with nonzero brackets. Exercise 6 asks for the Hall basis proper.

Step 1 — every Lie product is a motion. A degree-$k$ product is realized by nesting four-flow loops: $[g_1, [g_1, g_2]]$ is a loop whose second leg is itself a loop. The net motion is $O(\epsilon^k)$ in time $O(\epsilon)$ — slower by one power per degree, which is why parking into a space $\epsilon$ longer than the car costs $\Theta(1/\epsilon^2)$ reversals (Chapter 21).

Step 2 — full rank gives interior. If the products span $T_xM$, there are $n$ independent motion directions available in arbitrarily small time inside arbitrarily small $V$, and small combinations of them sweep an open set: STLA. For analytic fields the converse holds because an analytic system's reachable set is determined by all derivatives at $x$, which is the Lie algebra.

Step 3 — symmetry gives both signs. Reverse every leg of a loop and the residue flips sign; for a symmetric system the reversed controls are available, so each bracket direction comes with both signs and the open set of step 2 can be centred on $x$: STLC.

Step 4 — bad brackets. Write the loop for a bracket with its control amplitudes $u_i$ explicit. A control appearing an even number of times contributes $u_i^2$ to the net motion, so flipping $u_i$ changes nothing: the direction can be followed one way only, like a drift. A bad bracket is a one-way direction. It is harmless if a faster motion (a good bracket of lower degree) can cancel it — the bad bracket is neutralized — and fatal otherwise.

Step 5 — the degree-one bad bracket. $g_0$ itself is bad (drift once, controls zero times) and there is nothing of lower degree to neutralize it, hence the hypothesis $g_0(x) = 0$: Sussmann's theorem speaks only at equilibria.

Step 6 — from local to global. On a connected $M$, STLC everywhere gives controllability: chain neighbourhoods along any curve. The converse fails — the forward-only unicycle is controllable in the open plane and STLC nowhere.

Choset's four counterexamples, each in the library. The one-way watch knob (accessible, controllable, STLA, not STLC). $g_0 = [x_2^2, 0]^\top$, $g_1 = \partial_2$: at the origin $[g_0, g_1] = 0$ and $[g_1, [g_0, g_1]] = [2, 0]^\top$ is bad — sussmannStlc reports the LARC satisfied at degree three and a neutralization residual of exactly $1$ (nothing of lower degree points along $x_1$), hence STLA only; indeed $x_1$ never decreases. The double integrator: STLC at $(0.5, 0)$, undecided at $(0.5, 0.3)$ because $g_0 \ne 0$. The unicycle with $U \in \mathcal{U}^+$: STLA by Chow, STLC unavailable because symmetry is unavailable.

Step 1. The LARC gives forward local reachability: an open set of states is reached from any $x$ in short time (STLA).

Step 2. To move against the drift, wait: recurrence returns the unforced flow arbitrarily close to where it started, so a neighbourhood of any state is reached again later without controls, and forward-only motions plus waiting generate backward displacements too.

Step 3. Chain neighbourhoods along any path, as in the connected-and-STLC argument.

The example. $g_0 = \tfrac12[-x_2, x_1]^\top$ (circular orbits — WPPS), $g_1 = \partial_1$: $[g_0, g_1] = [0, -\tfrac12]^\top$ and $\det[g_1\ [g_0, g_1]] = -\tfrac12$ everywhere, so the system is controllable, though STLC only at the origin where $g_0$ vanishes. The library returns STLA by the LARC and $[0, -0.5]$ for the bracket. The robotics case: energy-conserving drift on a compact configuration space is WPPS by Poincaré recurrence — a frictionless Reach on the horizontal Workbench (Exercise 1), a satellite on $SO(3)$ with a single body-fixed torque.

Step 1 — the intrinsic acceleration. On $S^1 \subset \mathbb{R}^2$ a point mass at angle $\theta$ has $\ddot q = \ddot\theta[-\sin\theta, \cos\theta]^\top + \dot\theta^2[-\cos\theta, -\sin\theta]^\top$. Only the $\ddot\theta$ term is visible to an observer confined to the circle; the $\dot\theta^2$ term keeps the mass on it. Subtracting the centripetal term is what $M^{-1}\dot q\T\Gamma\dot q$ does in general coordinates.

Step 2 — the connection in coordinates. For a metric $\langle v_1, v_2\rangle = v_1\T M v_2$ the Levi-Civita connection is $\nabla_{Y_1}Y_2 = \frac{\partial Y_2}{\partial q}Y_1 + M^{-1}Y_1\T\Gamma Y_2$, with the Christoffel symbols of Chapter 17's equation (10.9). Euclidean metric: $\Gamma = 0$ and $\nabla_{Y_1}Y_2$ is the directional derivative.

Step 3 — block-differentiate on $T\mathcal{Q}$. With $g_0 = [\dot q;\ -M^{-1}\dot q\T\Gamma\dot q]$ and $g_i = [0;\ Y_i(q)]$, the Jacobian of $g_i$ has only a lower-left block $\partial Y_i/\partial q$, acting on the upper half of its argument — which is zero for every $g_j$, so $[g_i, g_j] = 0$. For $[g_0, g_i]$ the upper-right block of $\partial g_0/\partial x$ (the identity) hits $Y_i$ and gives the $-Y_i$ in the configuration slot; the velocity slot is linear in $\dot q$ and vanishes at rest. For $[g_i, [g_0, g_j]]$ the velocity-product's second derivative in $\dot q$ survives at rest and is exactly $M^{-1}(Y_i\T\Gamma Y_j + Y_j\T\Gamma Y_i)$, while the two $\partial Y/\partial q$ terms assemble the rest of $\langle Y_i : Y_j\rangle$.

Step 4 — the reading. $\mathrm{Sym}(\mathcal{Y})(q)$ is the set of velocity directions reachable from rest; $\mathrm{Lie}(\mathrm{Sym}(\mathcal{Y}))(q)$ the set of configuration directions. Sussmann's good/bad bookkeeping carries over verbatim because the lift makes $g_0$ appear once in every symmetric product.

Step 5 — in the library. liftToStateSpace builds the six-dimensional fields for the planar body with thrusters, and a check confirms the three identities numerically to $10^{-12}$ against symmetricProduct on $\mathcal{Q}$.

The planar body with thrusters (Choset Example 12.4.3) — and Reach's free third link. Unit mass and inertia, $\Gamma = 0$, $Y_1 = [\cos q_3, \sin q_3, 0]^\top$ (thrust through the centre of mass), $Y_2 = [-\sin q_3, \cos q_3, -d]^\top$ (the offset thruster). Then $\langle Y_1 : Y_2\rangle = [d\sin q_3, -d\cos q_3, 0]^\top$, rank 3: STLA from rest. The bad products are $\langle Y_1 : Y_1\rangle = 0$ and $\langle Y_2 : Y_2\rangle = 2\nabla_{Y_2}Y_2 = 2d\,Y_1$ — Choset prints $dY_1$; the factor is immaterial — neutralized by $Y_1$: STLC at rest, confirming the Lie-bracket computation of Example 12.3.4, where $\det[g_1 \cdots g_6] = d^4$. A single offset thruster gives rank$(Y_2\ \langle Y_2 : Y_2\rangle\ \langle Y_2 : \langle Y_2 : Y_2\rangle\rangle) = 3$, STLA, with nothing to neutralize the bad products: Sussmann is silent, and the system is in fact controllable but not STLC (Choset Figure 12.14). Section "Putting it together" asks whether this body is Reach's third link when the third motor is dead; the honest answer is "kinematically, yes; dynamically, not quite".

The hopper (Example 12.4.4). $M = \mathrm{diag}(I, mq_3^2, m)$, $\Gamma^2_{23} = \Gamma^2_{32} = mq_3$, $\Gamma^3_{22} = -mq_3$, $Y_1 = [1/I, -1/(mq_3^2), 0]^\top$, $Y_2 = [0, 0, 1/m]^\top$. Angular momentum is conserved, so velocities fill at most two dimensions: $\langle Y_1 : Y_1\rangle = [0, 0, -2/(m^2q_3^3)]^\top \in \mathrm{span}(Y_2)$, the other products vanish, and $\mathrm{Sym}$ is two-dimensional (not STLA) while $[Y_1, Y_2] = [0, -2/(m^2q_3^3), 0]^\top$ completes $\mathrm{Lie}(\mathrm{Sym})$: STLEC and equilibrium controllable. The library reports exactly this, and that $\mathrm{Sym}(\mathcal{Y}) = \mathrm{span}(\mathcal{Y})$ — the hopper is maximally reducible to a kinematic system, Chapter 21's Theorem 12.4.5.

Step 1 — the projection. $M = \mathrm{diag}(m, m, I)$, $A = [\sin q_3, -\cos q_3, 0]$ (no sideways velocity), so $P$ has the upper block $\begin{bmatrix}\cos^2 q_3 & \sin q_3\cos q_3\\ \sin q_3\cos q_3 & \sin^2 q_3\end{bmatrix}$ and a $1$ in the corner: it projects planar velocity onto the heading.

Step 2 — the inputs. A force along $q_1$ and a torque about $q_3$: $Y_1 = e_1/m$, $Y_2 = e_3/I$, hence $\tilde Y_1 = [\cos^2 q_3, \sin q_3\cos q_3, 0]^\top/m$ — the forward force projected onto the heading — and $\tilde Y_2 = e_3/I$.

Step 3 — the connection. $\Gamma = 0$, so $\nabla_{\tilde Y_2}\tilde Y_1 = \frac{\partial\tilde Y_1}{\partial q_3}\cdot\frac1I = \frac{1}{mI}[-\sin 2q_3, \cos 2q_3, 0]^\top$ and $\nabla_{\tilde Y_1}\tilde Y_2 = 0$; projecting with $P$ gives $\langle\tilde Y_1 : \tilde Y_2\rangle = \frac{1}{mI}[-\sin q_3\cos q_3, -\sin^2 q_3, 0]^\top = -\frac{\tan q_3}{I}\tilde Y_1$. Both self products vanish. So $\mathrm{Sym}(\tilde{\mathcal{Y}}) = \mathrm{span}(\tilde{\mathcal{Y}}) = \mathcal{D}$: the knife-edge is maximally reducible — it is "secretly kinematic", the unicycle with velocity inputs, which is why Chapter 21 can steer it with Chapter 20's kinematic tools.

Step 4 — configurations. $[\tilde Y_1, \tilde Y_2] = \frac{1}{mI}[\sin 2q_3, -\cos 2q_3, 0]^\top$ and $\det[\tilde Y_1\ \tilde Y_2\ [\tilde Y_1, \tilde Y_2]] = \cos^2 q_3/(m^2I^2)$, which vanishes at $q_3 = \pm\pi/2$ — there the projected force $\tilde Y_1$ itself vanishes, and the distribution is not regular. One more bracket, $[\tilde Y_2, [\tilde Y_1, \tilde Y_2]] = \frac{2}{mI^2}[\cos 2q_3, \sin 2q_3, 0]^\top$, gives $\det[\tilde Y_2\ [\tilde Y_1, \tilde Y_2]\ [\tilde Y_2, [\tilde Y_1, \tilde Y_2]]] = 2/(m^2I^4)$: STLCA and STLEC everywhere by Theorem 12.4.2.

In the library (with $m = 2$, $I = 0.5$ so the numbers are not all ones): every field above matches Choset's closed form to $10^{-9}$, $\det_2 = 0.584984$ at $q_3 = 0.7$ against the formula's $0.584984$, $\det_3 = 8.000000$; larc on the reduction $\{\tilde Y_1, \tilde Y_2\}$ at $q_3 = \pi/2$ returns rank 2 and "undecided" at degree two and STLC at degree three — the LARC refusing to certify the constrained system until it has the bracket that actually moves it.