Volume II: Transverse Control and the Architecture of Trajectories

Transverse control, trajectory geometry, and motion architecture

Throwing Away the Target

A river does not flow toward the ocean because it knows where the ocean is. It flows because flowing is what rivers do.

— Adapted from Lao Tzu

The Wrong Question

Open any classical control textbook and you will find, within the first few pages, a picture like this: a system, an error signal, a reference point, and a controller whose entire purpose is to drive that error to zero. The implicit promise is seductive—give me a destination and I will take you there.

But consider the golf swing.

A skilled golfer does not steer the clubhead toward the ball the way a thermostat drives a room toward $72^\circ$F. There is no “setpoint” floating in space that the club is chasing. Instead, the golfer’s body executes a motion—a coordinated, whole-body trajectory through a high-dimensional configuration space. The clubhead arrives at the ball not because it was pointed at the ball, but because the entire motion was shaped so that, somewhere along its arc, the club happens to pass through the ball’s location at enormous speed.

Control is motion, not destination. The fundamental object of nonlinear control is not a setpoint, not an equilibrium, and not a fixed target. It is a trajectory—a curve through state space that the system is asked to follow, and around which disturbances are rejected. The quality of control is measured not by how close the system gets to a point, but by how faithfully it reproduces a desired motion.

This is not merely a philosophical reframing. It changes the mathematics. It changes what we optimize. It changes how we define stability. And it changes what “success” means in a controlled system.

The purpose of this book is to develop control theory from this trajectory-first perspective, to show that the classical setpoint framework is a special (and often misleading) case of a much richer theory, and to build the mathematical tools needed to design controllers for systems whose purpose is to move.

[Figure: See PDF edition for TikZ diagram.]

Why Setpoints Are a Special Case

Let us be precise about what we mean. Consider a nonlinear system \[\begin{equation}\label{eq:nonlinear_sys} \dot{\state} = \bm{f}(\state, \control), \qquad \state \in \statespace \subseteq \Reals^n, \quad \control \in \controlspace \subseteq \Reals^m, \end{equation}\] where $\bm{f}$ is smooth and the state space $\statespace$ may carry additional geometric structure (it may be a manifold, as we will see in Chapter 3).

In the classical framework, the control objective is:

Find $\control(t)$ such that $\state(t) \to \state^*$ as $t \to \infty$.

The entire apparatus of Lyapunov stability, LQR, pole placement, and PID control is built around this question. But notice what this objective assumes:

There exists a meaningful equilibrium $\state^*$ where $\bm{f}(\state^*, \control^*) = \bm{0}$.
The system’s purpose is to reach and remain at this equilibrium.
Performance is measured by how quickly and smoothly the error $\bm{e}(t) = \state(t) - \state^*$ decays.

Now consider the systems this book is about:

A golfer executing a swing—the body passes through a continuum of configurations and never “arrives” anywhere.
A gymnast performing a floor routine—the motion is the performance.
A biped walking—there is no single posture that constitutes “walking”; walking is a limit cycle, a periodic orbit through state space.
A robotic arm tracing a weld seam—the task is defined by the path, not the endpoint.

For all of these systems, assumptions A1–A3 fail. The correct control objective is fundamentally different:

Given a reference trajectory $\traj^*: [0, T] \to \statespace$, find a feedback control law $\control = \bm{k}(\state, t)$ such that \[\begin{equation}\label{eq:tracking_obj} \norm{\state(t) - \traj^*(t)} \leq \varepsilon(t) \qquad \text{for all } t \in [0, T], \end{equation}\] where $\varepsilon(t)$ is a prescribed tracking tolerance (the “tube width”), and the system is robust to disturbances $\bm{d}(t)$ satisfying $\norm{\bm{d}(t)} \leq \delta$.

A setpoint is simply the degenerate case where $\traj^*(t) = \state^*$ for all $t$ and $T \to \infty$. The trajectory-tracking framework contains the setpoint framework, but not vice versa.

Given a reference trajectory $\traj^*: [0,T] \to \statespace$ and a tube radius function $\varepsilon: [0,T] \to \Reals_{>0}$, the trajectory tube is the time-varying set \[\begin{equation} \tube(t) = \bigl\{\, \state \in \statespace : \norm{\state - \traj^*(t)} \leq \varepsilon(t) \,\bigr\}. \end{equation}\] A controller achieves tube-invariance if every solution starting in $\tube(0)$ remains in $\tube(t)$ for all $t \in [0, T]$.

This definition is the trajectory-centric replacement for Lyapunov stability. Instead of asking “does the system converge to a point?” we ask “does the system stay inside a moving tube?” The tube can be tight where precision matters (e.g., at ball impact in the golf swing) and loose where it does not (e.g., during the backswing).

Anatomy of a Motion: The Golf Swing as Prototype

We will use the golf swing throughout this book as a running example—not because golf is inherently more interesting than other motions, but because it compresses an extraordinary number of control-theoretic challenges into roughly 1.2~seconds:

High dimensionality. The human body has over 200 degrees of freedom. Even a simplified musculoskeletal model of the golf swing uses 20–40 generalized coordinates.
Extreme nonlinearity. Joint torques couple through the full rigid-body dynamics, including Coriolis and centrifugal terms that dominate at high angular velocities.
Underactuation. Once the club is released in the downswing, the wrist hinge “freewheels”—the golfer cannot directly control the club angle. The clubhead speed at impact comes from passive dynamics, not active torque.
Time-criticality with flexible timing. The swing must produce the right state (clubhead position, velocity, and orientation) at impact, but the exact time of impact is not prescribed. The golfer is free to take the backswing slowly or quickly.
Repeatability over optimality. A touring professional does not execute the theoretically optimal swing. They execute a swing they can repeat 300 times in a tournament, under pressure, with minimal variance.

Example 1 From Setpoint Thinking to Trajectory Thinking

Suppose we model a simplified golf swing with generalized coordinates $\bm{q} = (\theta_{\text{torso}},\, \theta_{\text{shoulder}},\, \theta_{\text{arm}},\, \theta_{\text{wrist}})^\top$ and their velocities $\dot{\bm{q}}$, giving state $\state = (\bm{q}, \dot{\bm{q}}) \in \Reals^8$.

Setpoint approach (wrong): Define a target state $\state^* = (\bm{q}_{\text{impact}}, \dot{\bm{q}}_{\text{impact}})$ and design a controller to drive $\state(t) \to \state^*$. This fails immediately: the system should not stop at $\state^*$. Impact is a waypoint, not a destination. The club must be accelerating through the ball, not decelerating toward it.

Trajectory approach (right): Define a reference motion $\traj^*(s): [0, 1] \to \Reals^8$ parameterized by a phase variable $s$ (not clock time $t$). The controller’s job is to ensure that the system traverses $\traj^*$ with the right sequencing and coordination, with a tight tube near the impact phase ($s \approx 0.7$) and a loose tube during setup ($s \approx 0$).

Point~(v) deserves special emphasis. Classical optimal control asks: “What is the minimum-time or minimum-energy trajectory?” But this is the wrong question for biological and athletic systems. The right question is:

The best trajectory is not the one that maximizes performance on a single trial, but the one that maximizes expected performance over many trials in the presence of noise, fatigue, and uncertainty. Formally, we seek \[\begin{equation}\label{eq:repeatability} \traj^* = \arg\min_{\traj} \; \mathbb{E}\!\left[\, \int_0^T \ell\bigl(\state(t), \control(t)\bigr) \dt \,\right] + \lambda \, \mathrm{Var}\!\left[\, J(\traj, \bm{w}) \,\right], \end{equation}\] where $J$ is the performance cost, $\bm{w}$ represents stochastic disturbances (motor noise, perceptual error), and $\lambda > 0$ penalizes variance.

This is why elite golfers do not all swing the same way. Each golfer finds a trajectory that is locally optimal for their body and their noise characteristics. Robustness is not an afterthought bolted onto an optimal controller—it is the primary design objective.

The Mathematical Landscape

To make the trajectory-first perspective rigorous, we need mathematical machinery that most introductory control courses skip entirely. This section provides a roadmap of the key concepts we will develop in subsequent chapters.

Phase Variables and Time Reparameterization

A crucial distinction separates what the system does from when it does it. Consider two executions of the same golf swing: one with a slow backswing and one with a fast backswing. The spatial path through configuration space may be identical, but the time parameterization differs.

We formalize this by introducing a phase variable $s(t) \in [0, 1]$ that monotonically increases from $0$ (start of motion) to $1$ (end of motion), with dynamics \[\begin{equation}\label{eq:phase} \dot{s} = \alpha(s, \state), \end{equation}\] where $\alpha > 0$ is the phase velocity. The reference trajectory is defined as a function of $s$, not $t$: \[\begin{equation} \traj^* = \traj^*(s), \qquad s \in [0, 1]. \end{equation}\] This decouples the shape of the motion from its temporal execution. The controller has two separate tasks: 1. Path tracking: keep $\state(t)$ close to the reference path $\traj^*(s(t))$, and 2. Timing control: regulate $\dot{s}$ to achieve the desired speed profile along the path.

This decomposition is not naturally represented by a simple setpoint framework, where time and space are usually fused together.

Transverse Dynamics and Orbital Stability

When we shift from point stability to trajectory stability, the relevant notion of “closeness” changes. We do not care about the distance from $\state(t)$ to $\traj^*(t)$ at the same clock time—we care about the distance from $\state(t)$ to the nearest point on the trajectory.

Let $\traj^*: [0,1] \to \statespace$ be a smooth reference trajectory. The transverse distance from a state $\state$ to $\traj^*$ is \[\begin{equation} d_\perp(\state, \traj^*) = \min_{s \in [0,1]} \norm{\state - \traj^*(s)}. \end{equation}\] The minimizing argument $s^*(\state) = \arg\min_s \norm{\state - \traj^*(s)}$ is the projection phase, giving the “closest point on the reference.”

This leads to a decomposition of the state into two components: - The tangential component: where the system is along the trajectory (measured by $s$). - The transverse component: how far the system is from the trajectory (measured by $d_\perp$).

[Figure: See PDF edition for TikZ diagram.]

A system is orbitally stable around $\traj^*$ if the transverse distance $d_\perp(\state(t), \traj^*)$ remains small (or decays) even if the tangential phase $s(t)$ drifts relative to the nominal timing. This is exactly the right notion for the golf swing: we want the motion to follow the right path even if the golfer is slightly faster or slower than planned.

Funnel Control and Time-Varying Tubes

Combining the trajectory tube (Definition~$\ref{def:tube}$) with the transverse/tangential decomposition leads to what we call funnel control: the design of controllers that keep the system inside a prescribed time-varying tube around the reference.

The tube width $\varepsilon(s)$ is a design parameter. For the golf swing: \[\begin{equation}\label{eq:funnel_profile} \varepsilon(s) = \begin{cases} \varepsilon_{\text{loose}} & s \in [0,\, 0.3] \quad \text{(address \& backswing)}, \\[4pt] \varepsilon_{\text{loose}} - (\varepsilon_{\text{loose}} - \varepsilon_{\text{tight}}) \dfrac{s - 0.3}{0.4} & s \in [0.3,\, 0.7] \quad \text{(transition \& downswing)}, \\[6pt] \varepsilon_{\text{tight}} & s \in [0.7,\, 0.8] \quad \text{(impact zone)}, \\[4pt] \text{relaxed} & s \in [0.8,\, 1.0] \quad \text{(follow-through)}. \end{cases} \end{equation}\]

[Figure: See PDF edition for TikZ diagram.]

This funnel profile encodes the physical insight that precision matters most at impact and least during setup. The controller “spends” its control authority where it matters, rather than trying to maintain uniform precision everywhere—a lesson that classical setpoint control cannot teach.

What Changes When Control Is Motion

Adopting the trajectory-first perspective changes nearly every aspect of control system design. Here we preview the major consequences, each of which will be developed in detail in subsequent chapters.

Stability Is Not Convergence—It Is Confinement

In the setpoint framework, stability means convergence: the state approaches the equilibrium and stays near it. In the trajectory framework, stability means confinement: the state remains inside the moving tube.

A reference trajectory $\traj^*$ is stable under the closed-loop dynamics $\dot{\state} = \bm{f}(\state, \bm{k}(\state, t))$ if for every $\varepsilon > 0$ there exists $\delta > 0$ such that \[\begin{equation} d_\perp(\state(0), \traj^*) < \delta \implies d_\perp(\state(t), \traj^*) < \varepsilon \quad \forall\, t \in [0, T]. \end{equation}\] It is asymptotically trajectory-stable if, additionally, $d_\perp(\state(t), \traj^*) \to 0$ as the system evolves.

Note the subtle but critical difference: we use the transverse distance $d_\perp$, not the time-indexed error $\norm{\state(t) - \traj^*(t)}$. This allows the system to be ahead of or behind schedule without being considered “unstable.”

The Cost Function Is a Functional, Not a Function

In setpoint control, the cost is typically a function of the error $\bm{e}(t)$: \[\begin{equation} J_{\text{setpoint}} = \int_0^\infty \bigl(\bm{e}^\top \bm{Q}\, \bm{e} + \control^\top \bm{R}\, \control\bigr) \dt. \end{equation}\] In trajectory control, the cost is a functional that operates on the entire motion: \[\begin{equation}\label{eq:traj_cost} J_{\text{traj}}[\state(\cdot), \control(\cdot)] = \underbrace{\int_0^T w_\perp(s)\, d_\perp^2\bigl(\state(t), \traj^*\bigr) \dt}_{% \text{tracking fidelity (transverse)}} + \underbrace{\int_0^T w_\parallel(s)\, (\dot{s} - \dot{s}_{\text{ref}})^2 \dt}_{% \text{timing fidelity (tangential)}} + \underbrace{\int_0^T \control^\top \bm{R}(s)\, \control \dt}_{% \text{effort}}, \end{equation}\] where the weighting matrices $w_\perp(s)$, $w_\parallel(s)$, and $\bm{R}(s)$ are phase-dependent. This is what allows us to demand high precision at impact and low effort during the backswing.

Feedforward Is Not Optional—It Is Primary

In the setpoint framework, feedforward is a “nice to have” that reduces transient error. In the trajectory framework, feedforward is the primary control action. The reference trajectory $\traj^*(t)$ implicitly defines a feedforward control signal \[\begin{equation} \control_{\text{ff}}(t) = \bm{f}^{-1}\!\left(\dot{\traj}^*(t),\, \traj^*(t)\right), \end{equation}\] where $\bm{f}^{-1}$ denotes the inverse dynamics. The feedback controller handles only the deviations from this feedforward signal: \[\begin{equation}\label{eq:ff_fb_decomp} \control(t) = \underbrace{\control_{\text{ff}}(t)}_{\text{executes the motion}} + \underbrace{\control_{\text{fb}}(\state(t), t)}_{\text{corrects deviations}}. \end{equation}\] For the golf swing, empirical studies indicate the feedforward component accounts for approximately 90% of the total joint torques (Winter 2009). The feedback controller provides the remaining 10% of correction to handle physical disturbances. A control design that treats everything as feedback is asking the feedback loop to do disproportionately more work than necessary, ignoring the baseline deterministic dynamics of the swing.

Underactuation Creates Geometry, Not Just Constraints

Many moving systems are underactuated: they have fewer control inputs than degrees of freedom. In the setpoint framework, underactuation is an obstacle—it means we cannot independently control all states. In the trajectory framework, underactuation creates geometric structure that we can exploit.

The set of states reachable from a given configuration through passive dynamics forms a submanifold of the state space. A well-designed trajectory rides along this manifold, using the passive dynamics to generate motion that active torques alone could not produce. This is how the golf swing works: the wrist hinge releases passively, and the centrifugal acceleration of the downswing automatically squares the clubface through the double pendulum effect.

For a mechanical system with $n$ degrees of freedom and $m < n$ actuators, the set of dynamically feasible trajectories lies on a submanifold of dimension at most $2m$ in the $2n$-dimensional state space. Effective trajectory design requires understanding the geometry of this submanifold.

We will make this precise in Chapter 5 using the language of constraint manifolds and their null-space structures.

A Roadmap for the Book

The remainder of this book develops the trajectory-first theory in a sequence of increasingly sophisticated layers:

Ch.~2 Curves in State Space. The geometry of trajectories: parameterization, arc length, curvature, and torsion in high-dimensional state spaces. Frenet–Serret frames for nonlinear systems.
Ch.~3 Configuration Manifolds. Why state space is often not $\Reals^n$: Lie groups, joint spaces, and the geometry of articulated bodies. The golf swing lives on $SO(3)^k$, not $\Reals^k$.
Ch.~4 Orbital Stability and Transverse Linearization. Rigorous definitions of trajectory stability. Linearization transverse to the orbit. Floquet theory for periodic motions. Moving Poincar'e sections.
Ch.~5 Underactuation and Passive Dynamics. Constraint Jacobians, null-space control, and the exploitation of passive mechanical coupling. The double pendulum and the physics of the wrist release.
Ch.~6 Trajectory Optimization. Direct collocation, shooting methods, and pseudospectral methods for finding optimal reference trajectories. The interplay between optimality and repeatability.
Ch.~7 Funnel Synthesis. Designing time-varying tubes with guaranteed invariance. Sums-of-squares programming for funnel computation. Robust funnels under model uncertainty.
Ch.~8 Phase-Variable Control (Westervelt et al. 2007). Decoupling path shape from timing. Virtual constraints and hybrid zero dynamics for walking and running. Central pattern generators as phase oscillators.
Ch.~9 Stochastic Trajectories and Motor Variability. Signal-dependent noise in biological actuators. The minimum-variance theory of human movement (Todorov and Jordan 2002). Why optimal trajectories are smooth (Flash and Hogan 1985).
Ch.~10 Learning to Move. Iterative learning control, policy search, and trajectory libraries. How to improve a motion over repeated trials without losing stability.
Ch.~11 Case Study: The Complete Golf Swing. Full application of the entire theory to a 15-DOF musculoskeletal model of the golf swing, from trajectory optimization through funnel synthesis to stochastic robustness analysis.

A New Language for Control

Before proceeding, let us establish the conceptual vocabulary that will replace the classical language throughout this book:

**	**
Setpoint / reference	Reference trajectory $\traj^*(s)$
Error $\bm{e}(t) = \state - \state^*$	Transverse deviation $d_\perp(\state, \traj^*)$
Lyapunov stability	Orbital / trajectory stability
Convergence to equilibrium	Confinement to tube
Regulation	Path tracking + timing control
Step response	Trajectory tracking response
Steady-state error	Persistent transverse offset
Gain margin	Funnel robustness margin
Pole placement	Transverse eigenvalue assignment

Exercises

(Conceptual.) Consider a simple pendulum $\ddot{\theta} + \sin\theta = u$.
(Mathematical.) For the system $\dot{x}_1 = x_2$,; $\dot{x}_2 = -x_1 + u$, consider the reference trajectory $\traj^*(t) = (\cos t,\, -\sin t)^\top$ (a unit circle in state space).
Verify that $u_{\text{ff}}(t) = 0$ produces exactly this trajectory.
Compute the transverse linearization about $\traj^*$.
Design a feedback law $u_{\text{fb}}$ that achieves asymptotic orbital stability.
(Design.) A simplified 2-DOF golf swing model has coordinates $\bm{q} = (\theta_{\text{arm}}, \theta_{\text{club}})$ with dynamics \[ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{B}\,\control, \qquad \bm{B} = \begin{pmatrix} 1 \\ 0 \end{pmatrix}. \] This system is underactuated: only $\theta_{\text{arm}}$ is directly actuated.
1. Explain qualitatively how the club can accelerate through passive dynamics even though no torque is applied at the wrist.
2. Define a phase variable $s$ and sketch a funnel profile $\varepsilon(s)$ appropriate for this system.
3. Discuss why a setpoint controller applied at impact ($s = 0.7$) would produce the wrong motion.
(Philosophical.) A thermostat, an autopilot, and a dancer’s body are all “controlled systems.” Classify each according to whether its primary control objective is (i)~setpoint regulation, (ii)~trajectory tracking, or (iii)~something else entirely. Justify your classification.

The river does not arrive at the ocean and stop. It becomes the ocean. The swing does not arrive at impact and stop. It becomes the follow-through. Control is motion.

Curves in State Space

The shortest distance between two points is a straight line, but no interesting motion has ever been straight.

Why Geometry Comes First

In Chapter 1 we argued that control is fundamentally about motion—the faithful execution of a trajectory through state space. Before we can stabilize a trajectory, optimize a trajectory, or confine the system to a tube around a trajectory, we need to understand what a trajectory is as a mathematical object.

This chapter develops the differential geometry of curves in $\Reals^n$, adapted to the needs of control theory. The central insight is that a trajectory has intrinsic geometric properties—curvature, torsion, and a natural moving frame—that exist independently of how fast the system moves along the curve. These intrinsic properties determine how difficult the trajectory is to track, how much control authority is needed to stay on course, and where the system is most vulnerable to disturbances.

The reader may wonder: why not simply work in coordinates? The answer is that coordinate-based descriptions mix up properties of the curve with properties of the parameterization. A slow practice swing and a full-speed swing may trace a similar path in configuration space, but they do not trace the same curve in full state space because the velocity components are different. The geometric tools we develop here allow us to say precisely which claims are about the configuration path, which claims are about timing, and which claims require the full state trajectory.

A trajectory has two kinds of properties: - Intrinsic: properties of the curve itself—its shape, curvature, how it bends and twists through state space. These are independent of parameterization. - Extrinsic: properties that depend on how the curve is traversed—speed, acceleration, timing. These depend on the parameterization. Good control design separates these cleanly. The trajectory planner designs the intrinsic shape. The timing law specifies the extrinsic speed profile. The tracking controller maintains both.

Parameterized Curves

A parameterized curve in $\Reals^n$ is a smooth map \[\begin{equation} \traj: I \to \Reals^n, \qquad \lambda \mapsto \traj(\lambda), \end{equation}\] where $I \subseteq \Reals$ is an interval. The parameter $\lambda$ may be time $t$, arc length $s$, a phase variable $\sigma$, or any other monotone scalar. The curve is regular if $\traj'(\lambda) \neq \bm{0}$ for all $\lambda \in I$, i.e., it never “stops” (though the system traversing it may slow down).

The critical point is that many different parameterizations describe the same geometric curve. If $\traj(\lambda)$ is a parameterized curve and $\lambda = \phi(\mu)$ is a smooth, monotonically increasing reparameterization, then $\tilde{\traj}(\mu) = \traj(\phi(\mu))$ traces the same set of points in $\Reals^n$, but at different “speeds.”

Example 2 The Circle: Three Parameterizations

Consider the unit circle in $\Reals^2$. Three natural parameterizations: \[\begin{align} \traj_1(t) &= (\cos t,\; \sin t), \quad t \in [0, 2\pi] && \text{(constant angular speed)}, \\ \traj_2(t) &= (\cos t^2,\; \sin t^2), \quad t \in [0, \sqrt{2\pi}] && \text{(accelerating)}, \\ \traj_3(s) &= (\cos s,\; \sin s), \quad s \in [0, 2\pi] && \text{(arc-length, same as $\traj_1$ here)}. \end{align}\] All three trace the same circle. The curve is the same; the parameterizations differ. Quantities like curvature are properties of the curve, not the parameterization.

Velocity, Speed, and the Tangent Vector

Given a parameterized curve $\traj(\lambda)$, the velocity vector is \[\begin{equation} \bm{v}(\lambda) = \frac{\dd\traj}{\dd\lambda} = \traj'(\lambda). \end{equation}\] The speed is its magnitude: \[\begin{equation} v(\lambda) = \norm{\traj'(\lambda)}. \end{equation}\] The unit tangent vector is the velocity normalized to unit length: \[\begin{equation}\label{eq:unit_tangent} \tangent(\lambda) = \frac{\traj'(\lambda)}{\norm{\traj'(\lambda)}} = \frac{\traj'(\lambda)}{v(\lambda)}. \end{equation}\] The tangent vector $\tangent$ tells us the direction of the curve at each point—a purely geometric quantity. The speed $v$ tells us how fast we traverse it—a parameterization-dependent quantity. This separation is the first instance of the intrinsic/extrinsic decomposition.

Arc Length: The Natural Parameter

Among all possible parameterizations, one is geometrically privileged: the arc-length parameterization, in which the parameter measures distance traveled along the curve.

The arc length of a curve $\traj(\lambda)$ from $\lambda = a$ to $\lambda = b$ is \[\begin{equation}\label{eq:arc_length} L = \int_a^b \norm{\traj'(\lambda)} \, \dd\lambda = \int_a^b v(\lambda) \, \dd\lambda. \end{equation}\] The arc-length function $s(\lambda)$ measures the distance from the starting point: \[\begin{equation} s(\lambda) = \int_a^{\lambda} \norm{\traj'(\mu)} \, \dd\mu, \qquad \text{so that } \frac{\dd s}{\dd\lambda} = v(\lambda). \end{equation}\]

When a curve is parameterized by arc length, the speed is identically 1: $\norm{\traj'(s)} = 1$ for all $s$. This means the tangent vector and the velocity vector coincide: $\tangent(s) = \traj'(s)$. All geometric information is in the direction of the derivative; none is hidden in its magnitude.

Arc-length parameterization strips away all timing information and reveals the pure geometry of the curve. In control terms, it answers the question: what does the motion look like if we forget how fast it goes?

For a golf swing, the arc-length parameterized trajectory $\traj(s)$ can describe the spatial path of the body through configuration space. The timing law $s(t)$—how the golfer moves along this path in real time—is a separate modeling choice. In full state space, however, changing the timing also changes the velocity coordinates, so the state-space curve must be analyzed with both geometry and speed retained.

Remark

Arc Length in High Dimensions

In $\Reals^n$ with the Euclidean metric, arc length is defined exactly as above. On a manifold, arc length requires an explicitly chosen Riemannian metric (Carmo 1992). When the configuration manifold carries such a metric (as it will in Chapter 3), the formula $\eqref{eq:arc_length}$ generalizes to \[\begin{equation} L = \int_a^b \sqrt{\traj'(\lambda)^\top \bm{G}(\traj(\lambda))\, \traj'(\lambda)} \, \dd\lambda, \end{equation}\] where $\bm{G}$ is the selected metric tensor. For mechanical systems, the kinetic-energy metric on configuration space is represented in local coordinates by the mass-inertia matrix $\bm{M}(\bm{q})$ (Bullo and Lewis 2004). That statement is about configuration velocities $\dot{\bm{q}}$, not automatically about arbitrary state-space coordinates $(\bm{q},\dot{\bm{q}})$. Thus a mass-weighted arc length can make high-inertia motions count as longer than low-inertia motions, but only after the modeling metric has been stated.

Curvature: How Curves Bend

The most important intrinsic property of a curve is its curvature—a measure of how rapidly the tangent direction changes as we move along the curve.

Let $\traj(s)$ be a curve parameterized by arc length in $\Reals^n$. The curvature at each point is \[\begin{equation}\label{eq:curvature_def} \kappa(s) = \norm{\traj''(s)} = \norm{\frac{\dd\tangent}{\dd s}}. \end{equation}\] Since $\norm{\tangent} = 1$, the derivative $\tangent'(s)$ is orthogonal to $\tangent(s)$, and $\kappa$ measures the rate of turning per unit distance. The curvature is always non-negative and is zero if and only if the curve is locally a straight line.

The reciprocal $R(s) = 1/\kappa(s)$ is the radius of curvature—the radius of the circle that best approximates the curve at that point. High curvature means a tight bend; low curvature means a gentle sweep.

Curvature in Arbitrary Parameterization

In practice, we rarely work in arc-length parameterization. Given a curve $\traj(t)$ parameterized by time (or any other parameter), the curvature can be computed directly:

Theorem 1 Curvature Formula

For a regular curve $\traj(t)$ in $\Reals^n$, the curvature is \[\begin{equation}\label{eq:curvature_general} \kappa(t) = \frac{\norm{\traj'(t) \times_n \traj''(t)}}{\norm{\traj'(t)}^3}, \end{equation}\] where for $n = 2$, $\norm{\traj' \times_2 \traj''} = |x'y'' - y'x''|$, and for $n = 3$, $\times_3$ is the usual cross product. For general $n$, this generalizes via \[\begin{equation}\label{eq:curvature_general_n} \kappa(t) = \frac{\sqrt{\norm{\traj'}^2 \norm{\traj''}^2 - (\traj' \cdot \traj'')^2}}{\norm{\traj'}^3}. \end{equation}\]

Proof. Write $\traj(t) = \traj(s(t))$ where $s$ is arc length. Then $\traj' = \dot{s}\, \tangent$ and $\traj'' = \ddot{s}\, \tangent + \dot{s}^2\, \kappa\, \normal$, where $\normal$ is the unit normal (defined below). Since $\tangent \perp \normal$, we have $\norm{\traj'}^2\norm{\traj''}^2 - (\traj'\cdot\traj'')^2 = \dot{s}^2(\ddot{s}^2 + \dot{s}^4\kappa^2) - \dot{s}^2\ddot{s}^2 = \dot{s}^6\kappa^2$. Dividing by $\norm{\traj'}^6 = \dot{s}^6$ and taking the square root yields the result.

Example 3 Curvature of an Elliptical Trajectory

Consider the ellipse $\traj(t) = (a\cos t,\; b\sin t)$ with $a > b > 0$. Then \[\begin{align} \traj'(t) &= (-a\sin t,\; b\cos t), \\ \traj''(t) &= (-a\cos t,\; -b\sin t). \end{align}\] Applying the 2D curvature formula: \[\begin{equation} \kappa(t) = \frac{|(-a\sin t)(-b\sin t) - (b\cos t)(-a\cos t)|} {(a^2\sin^2 t + b^2\cos^2 t)^{3/2}} = \frac{ab}{(a^2\sin^2 t + b^2\cos^2 t)^{3/2}}. \end{equation}\] The curvature is highest at the ends of the minor axis ($t = 0, \pi$, where $\kappa = a/b^2$) and lowest at the ends of the major axis ($t = \pi/2, 3\pi/2$, where $\kappa = b/a^2$). The tighter the bend, the higher the curvature—and, as we will see, the more control authority is needed to track it.

Why Curvature Matters for Control

Curvature is not merely a geometric curiosity. It has direct implications for tracking difficulty.

Consider a system tracking a reference trajectory $\traj^*(s)$ at speed $v$. The centripetal acceleration required to follow the curve is \[\begin{equation}\label{eq:centripetal} a_\perp = \kappa\, v^2. \end{equation}\] This means: 1. High curvature demands high lateral control authority. 2. The required authority grows as the square of the speed. 3. A trajectory that is easy to track slowly may be impossible to track at high speed.

For the golf swing, this explains why the transition from backswing to downswing is so critical: the clubhead path has high curvature precisely where the speed is increasing rapidly.

[Figure: See PDF edition for TikZ diagram.]

The Frenet–Serret Frame

The tangent vector $\tangent$ is only the first element of a natural coordinate frame that moves with the curve. This Frenet–Serret frame (or simply moving frame) provides a complete local coordinate system at every point of the trajectory.

Construction in $\Reals^n$

Let $\traj(s)$ be a curve parameterized by arc length in $\Reals^n$ with $\traj^{(k)}(s)$ denoting the $k$-th derivative with respect to $s$. Assume the first $p \leq n$ derivatives $\{\traj'(s), \traj''(s), \ldots, \traj^{(p)}(s)\}$ are linearly independent at each point. The Frenet–Serret frame $\{\frenet_1(s), \frenet_2(s), \ldots, \frenet_p(s)\}$ is defined by applying the Gram–Schmidt process to these derivatives: \[\begin{align} \frenet_1 &= \traj' = \tangent, \label{eq:frenet_e1} \\[4pt] \tilde{\frenet}_k &= \traj^{(k)} - \sum_{j=1}^{k-1} \inner{\traj^{(k)}}{\frenet_j}\, \frenet_j, \qquad \frenet_k = \frac{\tilde{\frenet}_k}{\norm{\tilde{\frenet}_k}}, \quad k = 2, \ldots, p. \label{eq:frenet_ek} \end{align}\] The frame vectors $\frenet_1, \ldots, \frenet_p$ form an orthonormal set at each point. If $p < n$, the frame can be extended to a full orthonormal basis by appending $n - p$ vectors, but these additional vectors have no intrinsic geometric meaning for the curve.

For the familiar case $n = 3$:

Vector	Name	Interpretation
$\frenet_1 = \tangent$	Unit tangent	Direction of motion
$\frenet_2 = \normal$	Principal normal	Direction the curve is bending toward
normal to the osculating plane

The Frenet–Serret Equations

The power of the moving frame lies in how it evolves along the curve. The derivatives of the frame vectors with respect to arc length are expressed entirely in terms of the frame itself and a set of scalar functions called generalized curvatures.

Theorem 2 Frenet–Serret Equations

The Frenet–Serret frame satisfies \[\begin{equation}\label{eq:frenet_serret} \frac{\dd}{\ds} \begin{pmatrix} \frenet_1 \\ \frenet_2 \\ \frenet_3 \\ \vdots \\ \frenet_p \end{pmatrix} = \begin{pmatrix} 0 & \kappa_1 & 0 & \cdots & 0 \\ -\kappa_1 & 0 & \kappa_2 & \cdots & 0 \\ 0 & -\kappa_2 & 0 & \cdots & 0 \\ \vdots & & \ddots & \ddots & \kappa_{p-1} \\ 0 & \cdots & 0 & -\kappa_{p-1} & 0 \end{pmatrix} \begin{pmatrix} \frenet_1 \\ \frenet_2 \\ \frenet_3 \\ \vdots \\ \frenet_p \end{pmatrix}, \end{equation}\] where $\kappa_1(s) = \kappa(s) > 0$ is the curvature, $\kappa_2(s)$ is the torsion (often denoted $\tau$), and $\kappa_3, \ldots, \kappa_{p-1}$ are the higher-order curvatures. The matrix is skew-symmetric, which guarantees that the frame remains orthonormal as it evolves.

The skew-symmetry of this matrix is not a coincidence—it is the hallmark of a rotation. As the frame moves along the curve, it rotates without stretching. The curvatures $\kappa_1, \ldots, \kappa_{p-1}$ are the “angular velocities” of this rotation projected onto the various planes of the frame.

Example 4 Frenet–Serret for a 3D Helix

The helix $\traj(t) = (a\cos t,\; a\sin t,\; bt)$ with speed $v = \sqrt{a^2 + b^2}$ has arc-length parameter $s = vt$. The Frenet–Serret quantities are: \[\begin{align} \tangent &= \frac{1}{v}(-a\sin t,\; a\cos t,\; b), \\ \normal &= (-\cos t,\; -\sin t,\; 0), \\ \binormal &= \frac{1}{v}(b\sin t,\; -b\cos t,\; a), \\ \kappa &= \frac{a}{a^2 + b^2}, \qquad \tau = \frac{b}{a^2 + b^2}. \end{align}\] Both curvature and torsion are constant, which makes the helix a natural reference trajectory for testing tracking controllers—analogous to the role of step inputs in classical control. The helix is to trajectory control what the step function is to setpoint control.

The Moving Frame in State Space

For control applications, the state space typically has dimension $n = 2N$ where $N$ is the number of generalized coordinates (positions and velocities). A golf swing model with 4~DOF has $n = 8$. The Frenet–Serret frame in $\Reals^8$ has up to 7 generalized curvatures, $\kappa_1$ through $\kappa_7$.

Not all of these are physically meaningful for every system. In practice, the effective dimensionality of the curve—the number of linearly independent derivatives—is determined by the system dynamics:

For a controlled mechanical system with $N$ DOF and $m$ independent actuators, a generic trajectory has at most $p = 2m$ linearly independent arc-length derivatives. The Frenet–Serret frame therefore has at most $2m$ intrinsically meaningful vectors, and the curve has at most $2m - 1$ independent curvatures.

For a fully actuated system ($m = N$), the trajectory can bend freely in all $2N$ state-space directions. For an underactuated system ($m < N$), the trajectory is geometrically constrained: it lives on a submanifold, and the curvatures in the “forbidden” directions are determined by the passive dynamics.

Curvature and Torsion in the Language of Control

We now connect the geometric quantities to the dynamics and control of nonlinear systems.

Curvature as a Demand on the Controller

Consider the system $\dot{\state} = \bm{f}(\state, \control)$ tracking a reference trajectory $\traj^*(s)$ at instantaneous speed $v(t) = \dot{s}(t)$. Using the chain rule: \[\begin{equation} \dot{\state} = v\, \tangent, \qquad \ddot{\state} = \dot{v}\, \tangent + v^2\, \kappa\, \normal. \end{equation}\] The acceleration decomposes into: - A tangential component $\dot{v}\, \tangent$ that changes the speed along the curve. - A normal component $v^2\kappa\, \normal$ that “steers” the system around the bend.

Along a reference trajectory $\traj^*(s)$ traversed at speed $v$, the instantaneous control demand decomposes as \[\begin{equation}\label{eq:control_demand} \control_{\text{demand}} = \underbrace{\control_\parallel}_{\text{tangential}} + \underbrace{\control_\perp}_{\text{normal}}, \end{equation}\] where: \[\begin{align} \control_\parallel &\propto \dot{v} && \text{(accelerate/decelerate along path)}, \\ \control_\perp &\propto v^2\, \kappa && \text{(centripetal force to follow curvature)}. \end{align}\] The normal demand $\control_\perp$ grows quadratically with speed and linearly with curvature. This is the curvature cost of the trajectory.

Example 5 Curvature Cost in the Golf Downswing

In the transition from backswing to downswing, a professional golfer reverses the direction of the club in roughly 0.15 seconds. Let us estimate the curvature cost.

At the top of the backswing, the clubhead speed is approximately zero. At impact (0.3,s later), it reaches $\sim\!50$,m/s. The path through configuration space has a tight reversal—a region of high curvature $\kappa$.

Using $a_\perp = \kappa v^2$, and noting that mid-downswing $v \approx 25$,m/s with a conceptual reversal curvature $\kappa \approx 2$,rad/m (estimated as a representative value from elite motion capture data (Lephart et al. 2007)), the centripetal acceleration demand is \[\begin{equation} a_\perp \approx 2 \times 25^2 = 1250\;\text{m/s}^2 \approx 127\;g. \end{equation}\] (Note that these specific kinematics are subject to large variance and act here as an illustrative upper bound). This enormous demand is met not by muscular torque alone, but by the passive coupling in the kinematic chain (Neumann 2010)—the very underactuation that the setpoint framework treats as a constraint. The geometry of the trajectory creates the inter-segmental forces needed to execute it.

Torsion as Out-of-Plane Complexity

If curvature measures how a trajectory bends within its osculating plane, torsion measures how the osculating plane itself rotates along the curve. A planar curve has zero torsion everywhere. A helix has constant nonzero torsion. A golf swing—which involves simultaneous rotation in the transverse, sagittal, and frontal planes—has torsion that varies dramatically through the motion.

For a curve in $\Reals^n$ with $n \geq 3$, the torsion $\tau(s) = \kappa_2(s)$ is the second generalized curvature in the Frenet–Serret equations: \[\begin{equation} \frac{\dd\normal}{\ds} = -\kappa\, \tangent + \tau\, \binormal. \end{equation}\] Torsion measures the rate at which the osculating plane rotates about the tangent direction, per unit arc length.

In high-dimensional state spaces ($n \gg 3$), the higher curvatures $\kappa_3, \ldots, \kappa_{p-1}$ measure increasingly subtle geometric twisting. For control design, the first two curvatures ($\kappa$ and $\tau$) capture the dominant geometric effects, with higher curvatures becoming relevant primarily in highly articulated systems.

Remark

The Curvature Signature

The ordered set of generalized curvatures $(\kappa_1(s), \kappa_2(s), \ldots, \kappa_{p-1}(s))$ as functions of arc length constitutes the curvature signature of the trajectory. By the fundamental theorem of curves, this signature (up to rigid motions) uniquely determines the trajectory. Two motions with identical curvature signatures are geometrically congruent—they are the same shape, possibly translated and rotated.

This has a striking implication for motor control: if a golfer can reproduce the curvature signature of their swing, the spatial path is automatically reproduced, regardless of timing.

Reparameterization and Timing Laws

We now formalize the separation between the shape of a trajectory and the timing of its execution.

Given a reference trajectory $\traj^*(s)$ parameterized by arc length, a timing law is a smooth, monotonically increasing function $s: [0, T] \to [0, L]$ with $s(0) = 0$ and $s(T) = L$. The time-parameterized trajectory is \[\begin{equation} \state^*(t) = \traj^*(s(t)). \end{equation}\] The timing law is equivalently specified by the speed profile $v(t) = \dot{s}(t)$ or the phase velocity $v(s) = \dd s / \dd t$ expressed as a function of arc length.

Different timing laws produce different motions through the same geometric path:

[Figure: See PDF edition for TikZ diagram.]

The Path–Timing Decomposition of Control

The reparameterization framework leads to a clean decomposition of the control problem:

The trajectory control problem decomposes into two subproblems: 1. Path design: Choose the geometric curve $\traj^*(s)$ in state space that achieves the task (e.g., delivers the clubhead to the ball with the correct velocity vector). 2. Timing design: Choose the timing law $s(t)$ that balances speed against tracking difficulty, control effort, and robustness.

Example 6 Timing the Downswing

Suppose the downswing path has a curvature profile $\kappa(s)$ with a peak of $\kappa_{\max}$ at the transition point. The maximum centripetal acceleration the controller can provide is $a_{\max}$. Then the speed at the transition is bounded by \[\begin{equation} v_{\text{transition}} \leq \sqrt{\frac{a_{\max}}{\kappa_{\max}}}. \end{equation}\] This is a geometric speed limit: no matter how powerful the actuators, the system cannot traverse a tight bend faster than its control authority allows. The timing law must respect this limit, typically by slowing down through high-curvature regions and accelerating through straight sections.

This is precisely what golfers do instinctively. The “pause at the top”—the brief deceleration between backswing and downswing—is not aesthetic affectation. It is the timing law respecting the curvature speed limit at the point of maximum path curvature.

The Osculating Objects

The Frenet–Serret frame defines a sequence of “best-fit” geometric objects at each point of the curve. These osculating objects provide increasingly refined local approximations that are useful for controller design.

At a point $\traj^*(s_0)$ on a curve in $\Reals^n$: 1. The osculating line is the tangent line: the best linear approximation. 2. The osculating circle lies in the $(\tangent, \normal)$ plane, has radius $R = 1/\kappa$, and center at $\traj^*(s_0) + R\,\normal$. It is the best second-order (circular) approximation. 3. The osculating plane is the plane spanned by $\tangent$ and $\normal$. Torsion measures how fast the curve leaves this plane. 4. The osculating sphere is the best third-order approximation and accounts for both curvature and torsion.

For trajectory control, the osculating circle is the most immediately useful. It tells us: if the system were constrained to follow a circle at this point, what circle would it be? The radius of that circle is the radius of curvature $R = 1/\kappa$, and the center indicates the direction toward which the trajectory bends.

The Geometry of Trajectory Deviations

We now use the Frenet–Serret frame to analyze what happens when the system deviates from the reference trajectory. This provides the geometric foundation for the transverse linearization of Chapter 4.

Frenet–Serret Coordinates

Given a reference trajectory $\traj^*(s)$ and the associated Frenet–Serret frame $\{\frenet_1(s), \ldots, \frenet_p(s)\}$, any state $\state$ near the trajectory can be decomposed as: \[\begin{equation}\label{eq:frenet_coords} \state = \traj^*(s^*) + \sum_{k=2}^{n} \xi_k\, \frenet_k(s^*), \end{equation}\] where $s^* = s^*(\state)$ is the projection phase (the arc-length parameter of the nearest point on the reference), and $\xi_2, \ldots, \xi_n$ are the transverse coordinates measuring deviation in each normal direction.

The Frenet–Serret coordinate representation of a state $\state$ relative to a reference trajectory $\traj^*$ is the tuple \[\begin{equation} (s^*,\; \xi_2,\; \xi_3,\; \ldots,\; \xi_n), \end{equation}\] where $s^*$ is the tangential (along-trajectory) coordinate and $\bm{\xi} = (\xi_2, \ldots, \xi_n)$ are the transverse (cross-trajectory) coordinates. The transverse distance from Chapter 1 is \[\begin{equation} d_\perp = \norm{\bm{\xi}} = \sqrt{\xi_2^2 + \cdots + \xi_n^2}. \end{equation}\]

These coordinates have a powerful property: the reference trajectory itself corresponds to $\bm{\xi} = \bm{0}$, and the trajectory-tracking problem becomes the problem of regulating $\bm{\xi}$ to zero while $s^*$ advances}. This converts the tracking problem into a regulation problem, but in the moving frame rather than a fixed frame.

Curvature-Induced Coupling

There is a subtlety: the Frenet–Serret coordinates are not inertial. Because the frame rotates as it moves along the curve (governed by the Frenet–Serret equations $\eqref{eq:frenet_serret}$), fictitious forces appear in the transverse dynamics. The dominant effect is a curvature-induced coupling:

Proposition 1 In Frenet–Serret coordinates, the linearized transverse dynamics about the reference trajectory have the form \[\begin{equation}\label{eq:transverse_lin} \dot{\bm{\xi}} = \bigl[\bm{A}(s) - v^2 \bm{K}(s)\bigr]\, \bm{\xi} + \bm{B}(s)\, \control_{\text{fb}}, \end{equation}\] where $\bm{A}(s)$ comes from linearizing the original dynamics, and $\bm{K}(s)$ is a matrix that depends on the generalized curvatures $\kappa_1(s), \ldots, \kappa_{p-1}(s)$. The term $v^2\bm{K}(s)$ represents the geometric coupling between the curve’s shape and the transverse stability.

This result has a profound implication: the stability of trajectory tracking depends on the geometry of the trajectory. A trajectory with low curvature is inherently easier to stabilize than one with high curvature, even if the underlying system dynamics are identical. The curvature modifies the effective dynamics transverse to the path.

Fundamental Theorem and Uniqueness

We close this chapter with a theorem that is central to the trajectory-first philosophy.

Theorem 3 Fundamental Theorem of Curves

Let $\kappa_1(s), \ldots, \kappa_{p-1}(s)$ be smooth functions on $[0, L]$ with $\kappa_k(s) > 0$ for $k = 1, \ldots, p-2$. Then there exists a curve $\traj: [0, L] \to \Reals^n$, parameterized by arc length, whose generalized curvatures are exactly $\kappa_1, \ldots, \kappa_{p-1}$. Moreover, this curve is unique up to rigid motions (translations and rotations) of $\Reals^n$.

This theorem says: the curvature signature is the trajectory. Two trajectories with the same curvature functions are identical in shape. This has a practical corollary for control:

Corollary

To specify a reference trajectory, it suffices to specify the generalized curvatures as functions of arc length, together with an initial position and orientation. This representation is: 1. Compact: $p - 1$ scalar functions instead of $n$ coordinate functions. 2. Intrinsic: independent of coordinate choice. 3. Timing-independent: the trajectory shape is fully determined without specifying when each point is reached.

Example 7 Curvature Signature of a Golf Swing

A simplified 2-DOF model of the golf swing (arm + club) has a 4-dimensional state space ($\theta_{\text{arm}}, \theta_{\text{club}}, \dot{\theta}_{\text{arm}}, \dot{\theta}_{\text{club}}$). The trajectory through this space has up to 3 generalized curvatures: $\kappa_1(s)$, $\kappa_2(s)$, and $\kappa_3(s)$.

The curvature signature reveals the structure of the swing: - $\kappa_1(s)$ peaks at the transition (the “pause at the top”). - $\kappa_2(s)$ peaks during the release, where the swing plane rotates. - $\kappa_3(s)$ is small for a well-executed swing (the motion is approximately confined to a 3D subspace of $\Reals^4$).

A coach’s instruction to “flatten your swing plane” is, in geometric terms, a request to reduce $\kappa_2(s)$ during a specific phase of the motion—a precise modification to the curvature signature.

Summary

This chapter has established the geometric language we will use throughout the book:

Concept	Control significance
from timing
$a_\perp = \kappa v^2$
multi-joint coordination
decomposing deviations into tangential and transverse components
of timing and coordinates
temporal speed planning

The key takeaway is that a trajectory is not just “a function of time.” It is a geometric object with intrinsic structure, and this structure directly determines the difficulty and requirements of controlling it. In Chapter 3, we will see that the state spaces of mechanical systems are not flat $\Reals^n$—they are curved manifolds with their own geometry. The interplay between the geometry of the curve and the geometry of the space will produce the richest and most practically important results of this book.

Exercises

(Computation.) Compute the curvature and torsion of the curve $\traj(t) = (t, t^2, t^3)$ at $t = 0$ and $t = 1$. At which point is the curve “harder to track” at constant speed?
(Arc length.) The logarithmic spiral $\traj(t) = e^{at}(\cos t,\; \sin t)$ with $a > 0$ is a model for certain biological growth patterns.
(Frenet–Serret.) For the curve $\traj(s) = (\cos(s/\sqrt{2}),\; \sin(s/\sqrt{2}),\; s/\sqrt{2})$ (a helix parameterized by arc length):
Verify that $\norm{\traj'(s)} = 1$.
Compute the Frenet–Serret frame $\{\tangent, \normal, \binormal\}$.
Compute $\kappa$ and $\tau$ and verify the Frenet–Serret equations.
(Path–timing separation.) A robot end-effector must trace the path $\bm{p}(s) = (s,\; \sin(\pi s),\; 0)$ for $s \in [0, 2]$ (one full sine wave).
1. Compute $\kappa(s)$ and identify the points of maximum curvature.
2. If the maximum centripetal acceleration is $a_{\max} = 10\;\text{m/s}^2$, find the speed limit $v_{\max}(s)$ along the path.
3. Design a timing law $s(t)$ that traverses the path in minimum time while respecting the speed limit. Sketch $v(s)$ and $s(t)$.
(Conceptual.) Consider two trajectories in $\Reals^4$ with identical curvature signatures $(\kappa_1(s), \kappa_2(s), \kappa_3(s))$ but different initial conditions.
1. By the fundamental theorem, how are these trajectories related?
2. If you designed a tracking controller for one, would it work for the other? Under what conditions?
3. Relate this to the idea that a well-learned golf swing can be started from slightly different address positions.
(Numerical project.) Using motion capture data (or simulated data from a double pendulum), compute the curvature signature of a swing-like motion:
1. Numerically estimate $s(t)$ by integrating $\norm{\dot{\state}(t)}$.
2. Compute $\kappa_1(s)$ using Eq.~$\eqref{eq:curvature_general_n}$.
3. Identify the phases of the motion (backswing, transition, downswing, impact) from features of the curvature profile.
4. Plot the curvature cost $\kappa(s)\, v(s)^2$ and identify the most demanding phase of the motion.

The Gimbal Lock Catastrophe

If we represent orientation using three Euler angles $(\phi, \theta, \psi) \in \Reals^3$, we encounter gimbal lock: configurations where the representation degenerates and loses a degree of freedom. At $\theta = \pm\pi/2$, the axes for $\phi$ and $\psi$ align, making the system appear to have only 2~DOF when it actually has 3.

This is not a hardware problem—it is a coordinate singularity, an artifact of forcing a non-Euclidean space ($\SOthree$) into Euclidean coordinates ($\Reals^3$). The Apollo guidance system’s three-gimbal inertial platform had a built-in gimbal-lock caution region that crews were trained to avoid; this was a known design constraint of the IMU, not a specific Apollo~13 operational event. Every modern spacecraft avoids Euler angles for precisely this reason.

The lesson for control: if you use the wrong coordinates, your controller will have singularities that the physical system does not.

The configuration space of a mechanical system is a smooth manifold $\configspace$—a space that is locally like $\Reals^n$ but may have nontrivial global topology. Control theory built on $\Reals^n$ must be replaced by control theory built on $\configspace$ whenever: 1. Joints allow full rotation (wrapping around $\Sone$ or $\SOthree$). 2. The trajectory traverses a significant fraction of the configuration space. 3. Singularity-free representation is required for robustness.

For the golf swing—which involves large rotations in multiple joints simultaneously—all three conditions are met.

Smooth Manifolds: The Minimum You Need

We develop only as much manifold theory as the control engineer needs. The reader seeking a comprehensive treatment should consult Lee~(2012) or Abraham & Marsden~(1978).

A smooth manifold $\manifold$ of dimension $n$ is a set together with a collection of coordinate charts $\{(U_\alpha, \varphi_\alpha)\}$ such that: 1. Each $U_\alpha \subseteq \manifold$ is an open set, and the $U_\alpha$ cover $\manifold$. 2. Each $\varphi_\alpha: U_\alpha \to \Reals^n$ is a homeomorphism onto an open subset of $\Reals^n$. 3. Where two charts overlap, the transition maps $\varphi_\beta \circ \varphi_\alpha^{-1}$ are smooth ($C^\infty$) functions from $\Reals^n$ to $\Reals^n$.

The key intuition is: a manifold is a space where you can locally use coordinates, but no single coordinate system works globally. You need an atlas of overlapping charts, like a road atlas needs multiple pages to cover a country.

Example 8 The Circle $\Sone$

The circle $\Sone = \{(\cos\theta, \sin\theta) : \theta \in \Reals\}$ is a 1-dimensional manifold. It cannot be covered by a single coordinate chart: any angle parameterization $\theta \in [0, 2\pi)$ has a discontinuity at $\theta = 0 = 2\pi$. We need at least two charts: \[\begin{align} U_1 &= \Sone \setminus \{(-1, 0)\}, & \varphi_1(p) &= \theta \in (-\pi, \pi), \\ U_2 &= \Sone \setminus \{(1, 0)\}, & \varphi_2(p) &= \theta \in (0, 2\pi). \end{align}\] On the overlap $U_1 \cap U_2$, the transition map is either the identity or a shift by $2\pi$—both smooth.

For a hinge joint, this means: no single angle variable $\theta$ can represent all configurations without a discontinuity. If the joint rotates past the discontinuity, any controller using that angle will experience a “jump”—a coordinate artifact, not a physical event.

Tangent Spaces and Tangent Vectors

On a manifold, there is no global notion of “direction.” Velocity is a local concept, defined at each point.

The tangent space $\tangentspace{p}$ at a point $p \in \manifold$ is the vector space of all possible velocity vectors at $p$. If $\manifold$ has dimension $n$, then $\tangentspace{p} \cong \Reals^n$ as a vector space, but each point has its own tangent space.

Given a curve $\gamma: (-\varepsilon, \varepsilon) \to \manifold$ with $\gamma(0) = p$, the tangent vector $\dot{\gamma}(0)$ is an element of $\tangentspace{p}$.

The tangent bundle $T\!\manifold = \bigsqcup_{p \in \manifold} \tangentspace{p}$ is the collection of all tangent spaces, one for each point. It is itself a smooth manifold of dimension $2n$. A point in $T\!\manifold$ is a pair $(p, v)$ where $p \in \manifold$ and $v \in \tangentspace{p}$.

For a mechanical system with configuration manifold $\configspace$, the state space is the tangent bundle $\statespace = \tangentbundle$. A state is a pair $(\bm{q}, \dot{\bm{q}})$—a configuration and a velocity.

[Figure: See PDF edition for TikZ diagram.]

The Configuration Manifolds of Joints

Every mechanical joint constrains relative motion between two rigid bodies. The set of allowed relative configurations forms a manifold whose topology depends on the joint type.

Joint type	DOF	Manifold	Topology
Revolute (hinge)	1	$\Sone$	Circle
Prismatic (slider)	1	$\Reals$	Line (or interval)
Universal	2	$\Stwo$	2-sphere
Ball-and-socket	3	$\SOthree$	Rotation group
Planar	3	$\SEthree\|_{\text{2D}} = \Reals^2 \times \Sone$	Plane + rotation
Free body	6	$\SEthree = \Reals^3 \rtimes \SOthree$	Rigid motions of 3-space

The configuration space of a multi-body system is the product of individual joint manifolds (modulo kinematic constraints):

For a serial chain of $k$ joints with individual configuration manifolds $\manifold_1, \ldots, \manifold_k$, the total configuration space is \[\begin{equation}\label{eq:product_config} \configspace = \manifold_1 \times \manifold_2 \times \cdots \times \manifold_k. \end{equation}\] For example, a robot arm with three revolute joints has $\configspace = \Sone \times \Sone \times \Sone = \Torus^3$, the 3-dimensional torus.

Example 9 The Human Shoulder

The glenohumeral (shoulder) joint is approximately a ball-and-socket joint with configuration manifold $\SOthree$. Combined with the 1-DOF hinge at the elbow ($\Sone$) and the 1-DOF pronation/supination of the forearm ($\Sone$), the arm has configuration space \[\begin{equation} \configspace_{\text{arm}} = \SOthree \times \Sone \times \Sone. \end{equation}\] This is a 5-dimensional manifold with the topology of $\SOthree \times \Torus^2$. No set of 5 angle variables can parameterize this space without singularities.

The Rotation Group $\SOthree$

The rotation group $\SOthree$ appears wherever a joint allows full 3D rotation. It is the single most important non-Euclidean manifold in mechanical control, and its properties drive many of the challenges we will encounter.

$\SOthree$ is the group of $3 \times 3$ real orthogonal matrices with determinant $+1$: \[\begin{equation} \SOthree = \{\bm{R} \in \Reals^{3 \times 3} : \bm{R}^\top\!\bm{R} = \bm{I},\; \det(\bm{R}) = 1\}. \end{equation}\] It is a 3-dimensional compact Lie group. As a manifold, it is diffeomorphic to the real projective space $\Reals P^3$.

Why Euler Angles Fail

The most common parameterization of $\SOthree$ uses three Euler angles $(\phi, \theta, \psi)$, defining the rotation matrix as a product of three elemental rotations. This parameterization has two fundamental problems:

Singularities. Every 3-parameter representation of $\SOthree$ has at least one singularity. For the ZYX convention, gimbal lock occurs at $\theta = \pm\pi/2$. This is a topological necessity, not a flaw in the choice of convention: $\SOthree$ is not homeomorphic to any open subset of $\Reals^3$.
Non-uniqueness. Euler angles are periodic: $(\phi, \theta, \psi)$ and $(\phi + 2\pi, \theta, \psi)$ represent the same rotation. Near the singularity, drastically different angle triples can represent nearly identical orientations, creating numerical chaos in controllers.

For trajectory control on $\SOthree$, use one of: 1. Rotation matrices $\bm{R} \in \Reals^{3 \times 3}$: 9 parameters with 6 orthogonality constraints. Singularity-free but redundant. 2. Unit quaternions $\bm{q} \in S^3 \subset \Reals^4$: 4 parameters with 1 unit-norm constraint. Singularity-free, minimal redundancy, excellent for interpolation and computation. 3. Exponential coordinates (axis-angle or Lie algebra): 3 parameters with a singularity at $\norm{\bm{\omega}} = \pi$, but natural for small rotations and linearization.

Never use Euler angles for control of large-rotation trajectories.

The Lie Algebra $\sothree$

The tangent space of $\SOthree$ at the identity is the Lie algebra $\sothree$—the space of $3 \times 3$ skew-symmetric matrices: \[\begin{equation} \sothree = \{\bm{\Omega} \in \Reals^{3\times 3} : \bm{\Omega}^\top = -\bm{\Omega}\} = \left\{ \skewmat{\bm{\omega}} := \begin{pmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{pmatrix} : \bm{\omega} \in \Reals^3 \right\}. \end{equation}\] The “hat map” $(\cdot)^\wedge: \Reals^3 \to \sothree$ and its inverse “vee map” $(\cdot)^\vee: \sothree \to \Reals^3$ identify the Lie algebra with $\Reals^3$ (the space of angular velocity vectors).

The exponential map $\exp: \sothree \to \SOthree$ connects the Lie algebra to the Lie group: \[\begin{equation}\label{eq:exp_map} \bm{R} = \exp(\skewmat{\bm{\omega}}) = \bm{I} + \frac{\sin\theta}{\theta}\skewmat{\bm{\omega}} + \frac{1 - \cos\theta}{\theta^2}\skewmat{\bm{\omega}}^2, \end{equation}\] where $\theta = \norm{\bm{\omega}}$. This is the Rodrigues formula. Geometrically, $\bm{R}$ is a rotation by angle $\theta$ about the axis $\bm{\omega}/\theta$.

The inverse map $\log: \SOthree \to \sothree$ is well-defined for rotations with angle $\theta < \pi$ and gives the rotation vector representation.

The exponential map is the bridge between the linear world (the Lie algebra, where we can add and scale) and the nonlinear world (the Lie group, where the actual dynamics live). This bridge is the central tool for linearization on manifolds.

The Golf Swing Configuration Space

We now apply the manifold framework to our running example. The result will show exactly why Euclidean control theory is insufficient for the golf swing.

A Simplified Kinematic Model

Consider a 4-segment model of the golf swing: 1. Torso: rotates relative to ground via the spine. Modeled as a 3-DOF ball joint: $\manifold_1 = \SOthree$. 2. Lead arm: rotates relative to the torso via the shoulder. Modeled as a 3-DOF ball joint: $\manifold_2 = \SOthree$. 3. Forearm/hands: rotates relative to the upper arm via the elbow (1~DOF) and wrist flexion (1~DOF): $\manifold_3 = \Sone \times \Sone$. 4. Club: rotates relative to the hands via wrist cock/uncock (1~DOF): $\manifold_4 = \Sone$.

The total configuration space is: \[\begin{equation}\label{eq:golf_config} \configspace_{\text{golf}} = \SOthree \times \SOthree \times \Sone \times \Sone \times \Sone. \end{equation}\] This is a 9-dimensional manifold. Its topology is $\SOthree \times \SOthree \times \Torus^3$—decidedly not $\Reals^9$.

[Figure: See PDF edition for TikZ diagram.]

Why This Matters for Control

The state space (tangent bundle) has dimension $2 \times 9 = 18$. A trajectory through this space is a curve on an 18-dimensional manifold. The consequences for control are immediate:

No global linearization. You cannot write $\state = \state^* + \delta\state$ with $\delta\state \in \Reals^{18}$, because the “$+$” operation is not defined globally on the manifold. Linearization must be done in the tangent space, using the exponential map.
Error is not a vector. The “error” between two configurations $\bm{q}_1, \bm{q}_2 \in \configspace$ is not $\bm{q}_2 - \bm{q}_1$ (subtraction is undefined on manifolds). The correct notion of error is the logarithmic map: $\bm{e} = \log(\bm{q}_1^{-1} \bm{q}_2)$, which lives in the Lie algebra.
Geodesics replace straight lines. The “shortest path” between two configurations is not a straight line in any coordinate system—it is a geodesic on the manifold, determined by the Riemannian metric. In mechanical coordinates, the kinetic-energy metric is represented by the mass-inertia matrix after that modeling choice has been made.
Parallel transport replaces vector addition. To compare tangent vectors (velocities, errors) at different points on the trajectory, we must transport them along the manifold using the connection. Simply subtracting velocity vectors at different times is mathematically meaningless.

Euler Angle Catastrophe in Golf Modeling

Many biomechanics studies parameterize the golf swing using Euler angles for each joint. For the shoulder joint ($\SOthree$), this means three angles $(\phi_s, \theta_s, \psi_s)$. But during the backswing, the shoulder passes through configurations near gimbal lock ($\theta_s \approx \pm\pi/2$).

In these regions: - The Jacobian mapping angular velocities to Euler angle rates becomes singular. - Small physical rotations produce large jumps in angle coordinates. - Any controller computing torques from angle errors will produce infinite commanded torques at the singularity. The fix is not to “choose a better Euler angle convention” (all conventions have singularities somewhere). The fix is to work on $\SOthree$ directly, using rotation matrices or quaternions.

Riemannian Metrics and the Mass Matrix

A manifold by itself has no notion of distance, angle, or length. To measure these, we need a Riemannian metric—a smooth choice of inner product on each tangent space.

A Riemannian metric on a manifold $\manifold$ is a smooth assignment of an inner product $\inner{\cdot}{\cdot}_p$ on each tangent space $\tangentspace{p}$. In local coordinates $\bm{q} = (q_1, \ldots, q_n)$, the metric is represented by a positive-definite matrix $\bm{G}(\bm{q})$: \[\begin{equation} \inner{\bm{v}}{\bm{w}}_{\bm{q}} = \bm{v}^\top \bm{G}(\bm{q})\, \bm{w}, \qquad \bm{v}, \bm{w} \in T_{\bm{q}}\manifold. \end{equation}\]

For mechanical systems, nature provides a canonical Riemannian metric: the kinetic energy metric.

For a mechanical system with generalized coordinates $\bm{q}$ and mass-inertia matrix $\bm{M}(\bm{q})$, the kinetic energy \[\begin{equation} T = \frac{1}{2}\dot{\bm{q}}^\top \bm{M}(\bm{q})\, \dot{\bm{q}} \end{equation}\] defines a Riemannian metric $\bm{G} = \bm{M}(\bm{q})$ on $\configspace$. This metric has deep physical significance: 1. Distance between configurations is measured in “units of kinetic energy”—configurations connected by high-inertia motions are “far apart.” 2. Geodesics (shortest paths) under this metric are the trajectories of the unforced system—free motions with no applied torques. 3. Arc length from Chapter 2, computed with this metric, naturally weights each DOF by its effective inertia.

Example 10 Inertia-Weighted Distance in the Swing

Consider two configurations that differ by either: 1. A $10^\circ$ rotation of the torso (high inertia, $I \approx 12\;\text{kg}\cdot\text{m}^2$), or 2. A $10^\circ$ rotation of the wrist (low inertia, $I \approx 0.03\;\text{kg}\cdot\text{m}^2$).

In Euclidean coordinates, both represent “$10^\circ$ of change.” In the kinetic energy metric, the torso rotation is $\sqrt{12/0.03} = 20$ times “farther” than the wrist rotation. This correctly reflects the physical reality: rotating the torso requires 400 times more energy than rotating the wrist by the same angle.

The curvature of a trajectory measured in this metric therefore naturally captures the physical difficulty of the motion, not merely the angular change.

Geodesics as Natural Trajectories

A geodesic on a Riemannian manifold $(\manifold, \bm{G})$ is a curve that locally minimizes arc length. Geodesics satisfy the geodesic equation: \[\begin{equation}\label{eq:geodesic} \ddot{q}^i + \sum_{j,k} \Gamma^i_{jk}(\bm{q})\, \dot{q}^j\, \dot{q}^k = 0, \end{equation}\] where $\Gamma^i_{jk}$ are the Christoffel symbols of the metric $\bm{G}$.

The remarkable fact is that the geodesic equation $\eqref{eq:geodesic}$ is exactly the equation of motion for the unforced mechanical system $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} = \bm{0}$. The Coriolis and centrifugal terms are precisely the Christoffel symbol terms. Free motions of mechanical systems are geodesics on the configuration manifold equipped with the kinetic energy metric.

For trajectory design, geodesics represent the “easiest” paths—the motions the system would naturally follow with no applied forces. A well-designed trajectory stays close to geodesics where possible, deviating only where task requirements (e.g., impact conditions) demand it.

In the golf swing, the follow-through is nearly geodesic: after impact, the golfer relaxes and the body follows its natural, inertia-weighted path through configuration space. The downswing deviates significantly from the geodesic because the task (delivering maximum clubhead speed at impact) requires active torques to redirect the motion.

Curves on Manifolds Revisited

With the manifold framework in place, we can extend every concept from Chapter 2 to non-Euclidean configuration spaces.

Covariant Differentiation

On $\Reals^n$, we differentiate vector fields by taking component-wise derivatives. On a manifold, this does not work—the tangent spaces at different points are different vector spaces, so we cannot simply subtract vectors at different points.

The covariant derivative (or Levi-Civita connection) $\nabla$ is the unique way to differentiate vector fields along curves on a Riemannian manifold that: 1. Is compatible with the metric: $\frac{\dd}{\dd t}\inner{\bm{V}}{\bm{W}} = \inner{\nabla_{\dot{\gamma}} \bm{V}}{\bm{W}} + \inner{\bm{V}}{\nabla_{\dot{\gamma}} \bm{W}}$. 2. Is torsion-free: $\nabla_{\bm{V}}\bm{W} - \nabla_{\bm{W}}\bm{V} = [\bm{V}, \bm{W}]$.

In local coordinates, the covariant derivative of a vector field $\bm{V}$ along a curve $\gamma(t)$ is \[\begin{equation} (\nabla_{\dot{\gamma}} \bm{V})^i = \dot{V}^i + \sum_{j,k} \Gamma^i_{jk}\, \dot{\gamma}^j\, V^k. \end{equation}\]

The covariant derivative replaces the ordinary derivative in all of our Chapter 2 formulas. The generalized curvatures, the Frenet–Serret frame, and the transverse decomposition all carry over, with $\dd/\dd s$ replaced by $\nabla_{\dot{\gamma}}$.

Manifold Curvature of Trajectories

The geodesic curvature of a curve $\gamma(s)$ on a Riemannian manifold, parameterized by arc length, is \[\begin{equation} \kappa_g(s) = \norm{\nabla_{\dot{\gamma}} \dot{\gamma}}. \end{equation}\] This measures how much the curve deviates from a geodesic. A geodesic has $\kappa_g = 0$ everywhere.

Geodesic curvature is the manifold generalization of the curvature from Chapter 2, and it has the same control interpretation: geodesic curvature measures the force per unit mass needed to follow the curve, beyond what the natural dynamics would provide for free.

The control force required to track a trajectory $\gamma^*(s)$ at speed $v$ on a configuration manifold with metric $\bm{M}(\bm{q})$ has magnitude \[\begin{equation} \norm{\bm{\tau}_{\perp}} = \kappa_g(s)\, v^2, \end{equation}\] where $\norm{\cdot}$ is measured in the dual (force) metric. Comparing with the Euclidean formula $a_\perp = \kappa v^2$ from Chapter 2: the Euclidean curvature $\kappa$ is replaced by the geodesic curvature $\kappa_g$, which accounts for the curvature of the manifold itself.

This is a deeper version of the Curvature–Authority Principle. The control demand is reduced along directions where the manifold’s own curvature “helps”—i.e., where the natural dynamics carry the system in the desired direction. The control demand increases where the trajectory fights the natural dynamics.

Example 11 Geodesic Curvature of the Downswing

During the downswing, the trajectory has two sources of curvature: 1. Euclidean curvature: the path bends in coordinate space (the club changes direction). 2. Manifold curvature: the Christoffel symbol terms ($\Gamma^i_{jk}\dot{q}^j\dot{q}^k$) represent Coriolis and centrifugal effects that curve the natural trajectories.

The geodesic curvature $\kappa_g$ is the difference. When the Coriolis/centrifugal terms push the system in the direction the trajectory wants to go (as happens during the wrist release, where centrifugal acceleration “unhinges” the club), $\kappa_g$ is less than the Euclidean $\kappa$. The passive dynamics are doing work for free.

This is the geometric explanation for the “free speed” of the golf swing: a well-designed trajectory aligns with the manifold’s geodesics during the speed-critical phase, exploiting the natural dynamics rather than fighting them.

Error Metrics on Manifolds

We now address the critical question: how do we measure tracking error when the state space is a manifold?

The Logarithmic Error

For two configurations $\bm{q}_1, \bm{q}_2 \in \configspace$ where $\configspace$ is a Lie group, the error is \[\begin{equation}\label{eq:log_error} \bm{e} = \log(\bm{q}_1^{-1}\, \bm{q}_2) \in \text{Lie algebra}. \end{equation}\] For the rotation group $\SOthree$, this gives \[\begin{equation} \bm{e}_R = \log(\bm{R}_1^\top \bm{R}_2)^\vee \in \Reals^3, \end{equation}\] which is the rotation vector needed to go from $\bm{R}_1$ to $\bm{R}_2$. This error is zero if and only if $\bm{q}_1 = \bm{q}_2$, and it varies smoothly with both arguments (away from the cut locus at $\norm{\bm{e}} = \pi$).

Remark

Why Not Just Subtract Quaternions?

Given two unit quaternions $\bm{q}_1$ and $\bm{q}_2$, a tempting error metric is $\bm{e} = \bm{q}_2 - \bm{q}_1$. But this lives in $\Reals^4$, not in the tangent space of $S^3$. Worse, it violates the double-cover property: $\bm{q}$ and $-\bm{q}$ represent the same rotation, so the error $\bm{q}_2 - \bm{q}_1$ and $-\bm{q}_2 - \bm{q}_1$ should be “the same,” but they point in opposite directions.

The logarithmic error correctly handles the group structure and produces an error vector in the Lie algebra that has a clear physical interpretation (the angular velocity needed to correct the error in unit time).

The Trajectory Tube on a Manifold

The trajectory tube from Chapter 1 now generalizes naturally:

Let $\traj^*: [0, T] \to \configspace$ be a reference trajectory on a Riemannian manifold $(\configspace, \bm{G})$ and let $\varepsilon: [0, T] \to \Reals_{>0}$ be a tube radius function. The manifold trajectory tube is \[\begin{equation} \mathcal{T}(t) = \left\{\, \bm{q} \in \configspace : d_{\bm{G}}\!\left(\bm{q},\, \traj^*(t)\right) \leq \varepsilon(t) \,\right\}, \end{equation}\] where $d_{\bm{G}}$ is the geodesic distance—the length of the shortest geodesic connecting $\bm{q}$ and $\traj^*(t)$.

The geodesic distance correctly accounts for the topology of the configuration space. On $\SOthree$, the maximum distance between any two orientations is $\pi$ (a half-rotation), and the tube wraps properly around the group.

Equations of Motion on Manifolds

The Euler–Lagrange equations on a manifold take a coordinate-free form that reveals the geometric structure:

Theorem 4 Lagrangian Mechanics on Manifolds

For a mechanical system on a configuration manifold $(\configspace, \bm{M})$ with potential energy $V: \configspace \to \Reals$ and generalized forces $\bm{\tau}$, the equations of motion are \[\begin{equation}\label{eq:euler_lagrange_manifold} \bm{M}(\bm{q})\, \nabla_{\dot{\bm{q}}} \dot{\bm{q}} = -\mathrm{grad}\, V + \bm{B}\, \control, \end{equation}\] where $\nabla_{\dot{\bm{q}}} \dot{\bm{q}}$ is the covariant acceleration and $\mathrm{grad}\, V$ is the Riemannian gradient of the potential energy.

In local coordinates, this reduces to the familiar $\bm{M}\ddot{\bm{q}} + \bm{C}\dot{\bm{q}} + \bm{g} = \bm{B}\control$, but the coordinate-free form $\eqref{eq:euler_lagrange_manifold}$ makes clear that: - The Coriolis matrix $\bm{C}$ is not a separate “term”—it is the Christoffel symbol contribution to the covariant derivative. - The equation has the same form on any manifold, regardless of coordinate choice. - The “centrifugal and Coriolis forces” are geometric artifacts of the manifold’s curvature, not real forces. On a flat manifold ($\Reals^n$), they vanish.

Practical Implications for Controller Implementation

We conclude with concrete guidance for implementing trajectory controllers on manifold-valued configuration spaces.

A trajectory tracking controller for a system on $\configspace$ must: 1. Compute errors using the logarithmic map, not coordinate subtraction: \[\begin{equation} \bm{e}_q = \log\!\left((\bm{q}^*)^{-1}\, \bm{q}\right), \qquad \bm{e}_v = \dot{\bm{q}} - \mathrm{PT}_{q^* \to q}(\dot{\bm{q}}^*), \end{equation}\] where $\mathrm{PT}$ denotes parallel transport of the reference velocity to the current configuration. 2. Compute feedforward using the covariant derivative, not ordinary differentiation of coordinates. 3. Design feedback gains in the tangent space, then map the resulting control back to the manifold via the exponential map. 4. Monitor for cut-locus proximity: when the error approaches $\pi$ (for $\SOthree$), the logarithmic map becomes ill-conditioned. Switch to a recovery strategy.

Example 12 PD Control on $\SOthree$

The manifold-correct PD controller for tracking a reference orientation $\bm{R}^*(t)$ with a rigid body (moment of inertia $\bm{J}$, angular velocity $\bm{\omega}$) is: \[\begin{equation}\label{eq:pd_so3} \bm{\tau} = -k_p\, \log(\bm{R}^{*\top}\bm{R})^\vee - k_d\, (\bm{\omega} - \bm{R}^{*\top}\bm{R}\,\bm{\omega}^*) + \bm{J}\dot{\bm{\omega}}^* + \bm{\omega} \times \bm{J}\bm{\omega}, \end{equation}\] where the first term is the manifold-correct proportional error (logarithmic map), the second is the velocity error (parallel-transported), and the last two terms are the feedforward (covariant derivative of the reference).

Compare with the na"{}ve Euler-angle PD controller: $\tau = -k_p(\bm{\theta} - \bm{\theta}^*) - k_d(\dot{\bm{\theta}} - \dot{\bm{\theta}}^*)$. The na"{}ve controller has singularities; Eq.~$\eqref{eq:pd_so3}$ does not. The na"{}ve controller produces torques proportional to angle differences (which can be misleading near $\pm\pi$); Eq.~$\eqref{eq:pd_so3}$ produces torques proportional to the true rotation needed to correct the error.

Summary

Euclidean assumption	Manifold replacement
$\configspace = \Reals^n$	$\configspace = \SOthree^k \times \Torus^m \times \cdots$
Error: $\bm{e} = \bm{q} - \bm{q}^*$	Error: $\bm{e} = \log((\bm{q}^*)^{-1}\bm{q})$
Derivative: $\ddot{\bm{q}}$	Covariant derivative: $\nabla_{\dot{q}}\dot{\bm{q}}$
Shortest path: straight line	Shortest path: geodesic
Distance: $\norm{\bm{q}_2 - \bm{q}_1}$	Geodesic distance: $d_{\bm{G}}(\bm{q}_1, \bm{q}_2)$
Curvature: $\kappa$ (Ch.~2)	Geodesic curvature: $\kappa_g$
Trajectory tube: Euclidean ball	Geodesic ball
Coriolis terms: “extra forces”	Christoffel symbols: manifold geometry
PD: $-k_p\bm{e} - k_d\dot{\bm{e}}$	$-k_p\log(\cdot)^\vee - k_d(\text{parallel transport})$

The central message of this chapter is that the configuration spaces of mechanical systems have geometry, and this geometry matters for control. Working in $\Reals^n$ when the true space is $\SOthree \times \Torus^k$ introduces artificial singularities, incorrect error metrics, and controllers that fail when the system moves far from a nominal configuration.

The trajectory-first philosophy of this book demands that we take the geometry seriously from the start. The curves we designed in Chapter 2 live on manifolds, and the stability theory we will develop in Chapter 4 must respect the manifold structure. The payoff is controllers that work globally, without singularities, and that correctly exploit the natural dynamics encoded in the manifold’s Riemannian geometry.

Exercises

(Topology.) Determine the configuration manifold and its dimension for each system:
(Euler angle singularity.) For the ZYX Euler angle representation $\bm{R} = R_z(\psi)\, R_y(\theta)\, R_x(\phi)$ (yaw $\psi$ about Z, pitch $\theta$ about Y, roll $\phi$ about X, per NOTATION.md):
Compute the Jacobian $\bm{J}$ relating angular velocity $\bm{\omega}$ to Euler angle rates $(\dot{\phi}, \dot{\theta}, \dot{\psi})$.
Show that $\bm{J}$ is singular at $\theta = \pm\pi/2$.
Find an orientation trajectory $\bm{R}(t)$ that passes through the singularity and compute the Euler angle rates. What happens?
(Logarithmic error.) For two rotations $\bm{R}_1 = \bm{I}$ and $\bm{R}_2 = R_z(\alpha)$ (rotation by $\alpha$ about $z$):
1. Compute $\log(\bm{R}_1^\top\bm{R}_2)^\vee$ for $\alpha = \pi/6, \pi/2, \pi, 5\pi/3$.
2. Compare with the “error” $\alpha - 0 = \alpha$ as a function of $\alpha$. At what point do they differ significantly?
3. What happens at $\alpha = \pi$? Explain geometrically.
(Geodesics.) For a double pendulum with masses $m_1, m_2$ and lengths $l_1, l_2$:
1. Write the mass matrix $\bm{M}(\theta_1, \theta_2)$.
2. Compute the Christoffel symbols (or use the Coriolis matrix formula $C_{ijk} = \frac{1}{2}(\partial M_{ij}/\partial q_k + \partial M_{ik}/\partial q_j - \partial M_{jk}/\partial q_i)$).
3. Numerically integrate the geodesic equation from initial condition $(\theta_1, \theta_2) = (0, 0)$ with $(\dot{\theta}_1, \dot{\theta}_2) = (1, 0)$. Plot the geodesic on $\Torus^2$.
(Manifold PD control.) Implement and compare:
1. An Euler-angle PD controller for a rigid body tracking a reference orientation $\bm{R}^*(t) = R_z(\omega t)$ (constant-speed rotation).
2. The manifold PD controller from Eq.~$\eqref{eq:pd_so3}$ for the same task.
3. Run both controllers with the reference trajectory passing through $\theta = \pi/2$ (gimbal lock for ZYX convention). Compare the tracking errors and commanded torques.
(Golf swing configuration space.) For the 9-DOF golf swing model of Eq.~$\eqref{eq:golf_config}$:
1. How many Euler angles would be needed to parameterize the full configuration space? How many singularity surfaces exist?
2. Propose a quaternion-based parameterization for the two $\SOthree$ joints. What is the total number of parameters? What constraints must be maintained?
3. Discuss the trade-offs between the Euler angle and quaternion representations for numerical simulation of the golf swing.

The sphere cannot be flattened without tearing. The rotation group cannot be charted without singularity. Do not fight the topology—ride it. The manifold is not an obstacle. It is the landscape through which every motion flows.

Orbital Stability and Transverse Linearization

Stability is not the absence of motion.\ It is the faithfulness of motion.

The Problem With Lyapunov

The classical Lyapunov framework asks a simple question: does the state converge to an equilibrium? We demonstrated in Chapter 1 that this is the wrong question for systems whose purpose is to move. But the problem goes deeper than philosophical framing—Lyapunov stability is structurally incapable of analyzing trajectory-tracking systems.

Consider a system perfectly tracking a reference trajectory $\traj^*(t)$. The state $\state(t)$ matches $\traj^*(t)$ at every instant. Now apply a small timing perturbation: the system executes the same motion but shifted by $\varepsilon$ seconds. The perturbed state is $\tilde{\state}(t) = \traj^*(t - \varepsilon)$, and the time-indexed error is \[\begin{equation}\label{eq:timing_error} \bm{e}(t) = \tilde{\state}(t) - \traj^*(t) = \traj^*(t - \varepsilon) - \traj^*(t) \approx -\varepsilon\, \dot{\traj}^*(t). \end{equation}\] This error does not decay to zero—it persists indefinitely, oscillating with a magnitude proportional to $\varepsilon\, v(t)$ where $v$ is the speed. By Lyapunov’s definition, the trajectory is unstable. But the system is executing the correct motion perfectly! The only “error” is that it is slightly ahead of or behind schedule.

Lyapunov stability conflates two independent phenomena: 1. Transverse deviations: the system drifts away from the trajectory path. This is a genuine control failure. 2. Tangential deviations: the system is ahead of or behind schedule along the trajectory path. This is a timing difference, not a tracking failure.

Lyapunov stability rejects both. Orbital stability accepts tangential deviations and rejects only transverse ones. For systems whose purpose is motion, orbital stability is the correct notion.

[Figure: See PDF edition for TikZ diagram.]

Orbital Stability: Formal Definitions

We now define orbital stability rigorously. The key object is the orbit—the image of the trajectory as a set, stripped of all timing information.

Given a trajectory $\traj^*: [0, T] \to \statespace$, the orbit is the set \[\begin{equation} \orbit = \{\traj^*(t) : t \in [0, T]\} \subset \statespace. \end{equation}\] The orbit is a one-dimensional submanifold of $\statespace$ (a curve without parameterization). For periodic trajectories with period $T$, $\traj^*(0) = \traj^*(T)$ and $\orbit$ is a closed curve (a loop).

A trajectory $\traj^*(t)$ (or its orbit $\orbit$) is orbitally stable if for every $\varepsilon > 0$ there exists $\delta > 0$ such that \[\begin{equation}\label{eq:orbital_stability} d(\state(0), \orbit) < \delta \implies d(\state(t), \orbit) < \varepsilon \quad \forall\, t \geq 0, \end{equation}\] where $d(\state, \orbit) = \inf_{p \in \orbit} \norm{\state - p}$ is the distance from the state to the orbit.

It is asymptotically orbitally stable if additionally $d(\state(t), \orbit) \to 0$ as $t \to \infty$.

Critically, orbital stability does not require $\norm{\state(t) - \traj^*(t)} \to 0$. The system may drift along the orbit (ahead or behind schedule) while still satisfying the orbital-stability condition. Only deviations away from the orbit matter.

A trajectory is orbitally stable with asymptotic phase if it is asymptotically orbitally stable and additionally there exists a phase shift $\theta^*$ such that \[\begin{equation} \norm{\state(t) - \traj^*(t + \theta^*)} \to 0 \quad \text{as } t \to \infty. \end{equation}\] This means the system not only returns to the orbit but eventually “locks in” to a definite timing, shifted by $\theta^*$ from the nominal schedule. This is the strongest practical form of trajectory stability.

Example 13 Limit Cycles: The Prototype

The Van der Pol oscillator $\ddot{x} - \mu(1 - x^2)\dot{x} + x = 0$ possesses a limit cycle—a periodic orbit that is asymptotically orbitally stable with asymptotic phase. All nearby trajectories spiral toward the limit cycle and eventually synchronize to a definite phase.

The limit cycle is the simplest example of a system whose purpose is to move: the “target” is not a point but an orbit, and stability means the orbit attracts nearby trajectories. Walking, running, and heartbeats are all limit cycles. The golf swing is a transient trajectory (not periodic), but the same stability concepts apply over the finite interval $[0, T]$.

The Transverse–Tangential Decomposition

To analyze orbital stability, we must decompose the dynamics into components along and across the orbit. This is the transverse–tangential decomposition, the central technical tool of this chapter.

Projection Onto the Orbit

Given a state $\state$ sufficiently close to the orbit $\orbit$, the nearest-point projection is \[\begin{equation}\label{eq:projection} \pi(\state) = \arg\min_{p \in \orbit} \norm{\state - p}, \end{equation}\] and the associated projected phase is the unique $s^*(\state)$ satisfying $\pi(\state) = \traj^*(s^*)$. The projection is well-defined and smooth in a tubular neighborhood of $\orbit$ (as long as $\state$ is closer to $\orbit$ than the minimum radius of curvature of the orbit).

The Moving Hyperplane Decomposition

At each point $\traj^*(s)$ on the orbit, the state space $\Reals^n$ decomposes into a one-dimensional tangential direction and an $(n{-}1)$-dimensional transverse hyperplane:

At the point $\traj^*(s)$ on the orbit, define: \[\begin{align} \text{Tangential direction:} &\quad \bm{t}(s) = \frac{\dot{\traj}^*(s)}{\norm{\dot{\traj}^*(s)}}, \\ \text{Transverse hyperplane:} &\quad \Sigma(s) = \{\bm{v} \in \Reals^n : \bm{v}^\top \bm{t}(s) = 0\}. \end{align}\] A state $\state$ near $\traj^*(s)$ decomposes as \[\begin{equation}\label{eq:decomposition} \state = \traj^*(s^*) + \bm{\xi}, \qquad \bm{\xi} \in \Sigma(s^*), \end{equation}\] where $s^*$ is the projected phase and $\bm{\xi}$ is the transverse deviation—the component of the error perpendicular to the orbit. Orbital stability is equivalent to the stability of the origin $\bm{\xi} = \bm{0}$ in the transverse dynamics.

[Figure: See PDF edition for TikZ diagram.]

Transverse Linearization

The power of the transverse decomposition is that it reduces orbital stability to the stability of a time-varying linear system—the transverse linearization.

Deriving the Transverse Dynamics

Consider the nonlinear system $\dot{\state} = \bm{f}(\state, \control)$ with a reference trajectory $\traj^*(t)$ satisfying $\dot{\traj}^* = \bm{f}(\traj^*, \control^*(t))$. Using the decomposition $\state = \traj^*(s^*) + \bm{\xi}$ and linearizing about $\bm{\xi} = \bm{0}$:

Theorem 5 Transverse Linearization (Hauser and Hindman 1995; Shiriaev et al. 2010; Manchester and Slotine 2017)

The linearized transverse dynamics about the orbit $\orbit$ are \[\begin{equation}\label{eq:transverse_linear} \dot{\bm{\xi}} = \Amat_\transverse(s)\, \bm{\xi} + \Bmat_\transverse(s)\, \delta\control, \end{equation}\] where $\delta\control = \control - \control^*$ is the feedback perturbation, and \[\begin{align} \Amat_\transverse(s) &= \bm{P}(s) \left[ \frac{\partial \bm{f}}{\partial \state}\bigg|_{\traj^*, \control^*} - \frac{\dot{\traj}^* \otimes (\nabla_{\state} s^*)^\top} {\norm{\dot{\traj}^*}^2} \cdot \frac{\partial \bm{f}}{\partial \state}\bigg|_{\traj^*, \control^*} \right] \bm{P}(s), \label{eq:A_transverse} \\[6pt] \Bmat_\transverse(s) &= \bm{P}(s)\, \frac{\partial \bm{f}}{\partial \control}\bigg|_{\traj^*, \control^*}, \label{eq:B_transverse} \end{align}\] and $\bm{P}(s) = \bm{I} - \bm{t}(s)\bm{t}(s)^\top$ is the orthogonal projector onto $\Sigma(s)$.

Remark

Understanding the Transverse $\Amat_\transverse$

Equation $\eqref{eq:A_transverse}$ has a clear structure: - The first term $\bm{P}\, \frac{\partial \bm{f}}{\partial \state}\, \bm{P}$ is the standard Jacobian projected onto the transverse space. This captures how the system dynamics affect cross-trajectory deviations. - The second term subtracts the component of the Jacobian that acts along the tangent direction—this is exactly the tangential dynamics that orbital stability “forgets.” The resulting $\Amat_\transverse(s)$ has dimension $(n{-}1) \times (n{-}1)$, one less than the full state Jacobian. The removed eigenvalue corresponds to motion along the orbit—the tangential direction in which we are not demanding stability.

The reference trajectory $\traj^*(t)$ is asymptotically orbitally stable if and only if the origin of the transverse linearization $\eqref{eq:transverse_linear}$ (with $\delta\control = \bm{0}$) is asymptotically stable. This reduces the orbital stability question from a nonlinear problem on the full $n$-dimensional state space to a linear problem on the $(n{-}1)$-dimensional transverse space.

For controller design: if we can stabilize the linear system $\eqref{eq:transverse_linear}$, we achieve orbital stability of the nonlinear system. All the tools of linear time-varying (LTV) control theory apply.

The Transverse Jacobian: A Practical Formula

For implementation, the key quantities are computed as follows. Let $\bm{J}(t) = \frac{\partial \bm{f}}{\partial \state}\big|_{\traj^*, \control^*}$ be the Jacobian along the reference. Choose any orthonormal basis $\{\bm{n}_1(s), \ldots, \bm{n}_{n-1}(s)\}$ for $\Sigma(s)$—the Frenet–Serret frame from Chapter 2 provides a natural choice. Collect these into the matrix $\bm{N}(s) = [\bm{n}_1 \mid \cdots \mid \bm{n}_{n-1}] \in \Reals^{n \times (n-1)}$. Then:

\[\begin{equation}\label{eq:transverse_practical} \Amat_\transverse(s) = \bm{N}(s)^\top \left[\bm{J}(s) - \frac{\bm{f}^* \cdot \bm{J}(s)^\top \bm{t}(s)}{\norm{\bm{f}^*}^2}\, \bm{f}^*\, \bm{t}(s)^\top - \dot{\bm{N}}(s)\, \bm{N}(s)^\top \right] \bm{N}(s), \end{equation}\] where $\bm{f}^* = \bm{f}(\traj^*, \control^*)$ and $\dot{\bm{N}}$ accounts for the rotation of the transverse frame along the orbit (the Frenet–Serret equations).

Example 14 Transverse Linearization of a Planar System

Consider the system $\dot{x}_1 = x_2$, $\dot{x}_2 = -x_1 + u$ tracking the circular orbit $\traj^*(t) = (\cos t,\, -\sin t)$ with $u^* = 0$.

The Jacobian is $\bm{J} = \bigl(\begin{smallmatrix} 0 & 1 \\ -1 & 0 \end{smallmatrix}\bigr)$, the tangent vector is $\bm{t} = (-\sin t,\, -\cos t)^\top / 1 = (-\sin t,\, -\cos t)^\top$, and the normal is $\bm{n} = (-\cos t,\, \sin t)^\top$.

Computing $\Amat_\transverse = \bm{n}^\top[\bm{J} - \text{tangential terms}]\bm{n}$, we find: \[\begin{equation} \Amat_\transverse(t) = 0, \qquad \Bmat_\transverse(t) = \bm{n}^\top \begin{pmatrix} 0 \\ 1 \end{pmatrix} = \sin t. \end{equation}\] The open-loop transverse dynamics are neutrally stable ($\Amat_\transverse = 0$). Feedback $\delta u = -k\, \xi$ gives $\dot{\xi} = -k\sin^2\!t\, \xi$ (after averaging), which is stable for $k > 0$. The orbit is stabilizable.

Floquet Theory: Stability of Periodic Orbits

For periodic trajectories (limit cycles, walking gaits, repetitive manufacturing), the transverse linearization is a periodic linear system. Floquet theory provides the definitive stability analysis tool.

The State Transition Matrix

For the linear time-varying system $\dot{\bm{\xi}} = \Amat_\transverse(t)\, \bm{\xi}$, the state transition matrix $\PhiMat(t, t_0)$ satisfies \[\begin{equation} \dot{\Phi}(t, t_0) = \Amat_\transverse(t)\, \PhiMat(t, t_0), \qquad \PhiMat(t_0, t_0) = \bm{I}. \end{equation}\] The solution is $\bm{\xi}(t) = \PhiMat(t, t_0)\, \bm{\xi}(t_0)$.

The Monodromy Matrix

For a periodic orbit with period $T$, the monodromy matrix is the state transition matrix over one complete period: \[\begin{equation} \monodromy = \PhiMat(T, 0). \end{equation}\] The eigenvalues $\mu_1, \ldots, \mu_{n-1}$ of $\monodromy$ are the Floquet multipliers (also called characteristic multipliers).

Theorem 6 Floquet Stability Criterion

A periodic orbit is: 1. Orbitally stable if all Floquet multipliers satisfy $|\mu_k| \leq 1$, with multipliers on the unit circle being semisimple. 2. Asymptotically orbitally stable if all Floquet multipliers satisfy $|\mu_k| < 1$ (all strictly inside the unit circle). 3. Orbitally unstable if any Floquet multiplier satisfies $|\mu_k| > 1$.

Remark

The Missing Multiplier

If we had computed the monodromy matrix for the full $n$-dimensional system (not the transverse reduction), there would be $n$ Floquet multipliers, with one of them exactly equal to $+1$. This “trivial” multiplier corresponds to the tangential direction—perturbations along the orbit neither grow nor decay. The transverse reduction removes this trivial multiplier, leaving only the $n - 1$ multipliers that matter for orbital stability.

This is why the transverse reduction is essential: without it, the monodromy matrix always has an eigenvalue at $+1$, making the orbit appear only marginally stable even when it is asymptotically orbitally stable.

Example 15 Floquet Analysis of a Walking Gait

Consider a simplified compass-gait biped with 4-dimensional state space $(\theta_1, \theta_2, \dot{\theta}_1, \dot{\theta}_2)$. The walking gait is a periodic orbit with period $T$ (one stride). The transverse linearization has dimension $n - 1 = 3$.

After numerically integrating $\PhiMat(T, 0)$ for the transverse dynamics, suppose we find the monodromy matrix has eigenvalues $\mu_1 = 0.7$, $\mu_2 = 0.4$, $\mu_3 = -0.3$. Since all $|\mu_k| < 1$, the gait is asymptotically orbitally stable.

The physical interpretation: $\mu_1 = 0.7$ means that after one stride, transverse deviations in the first mode are reduced to 70% of their initial value. After 10 strides, they are reduced to $0.7^{10} \approx 3\%$. The negative multiplier $\mu_3 = -0.3$ indicates an alternating (flip-flop) convergence pattern in the third mode.

For the golf swing (a non-periodic motion), we cannot use the monodromy matrix directly, but the state transition matrix $\PhiMat(T, 0)$ plays an analogous role—its singular values measure how much transverse deviations amplify or attenuate over the course of the motion.

Moving Poincar'e Sections

A complementary tool for analyzing orbital stability is the Poincar'e section—a codimension-1 surface that the orbit crosses transversally. While Floquet theory gives continuous-time information, Poincar'e sections give discrete-time (strobed) information.

A Poincar'e section for an orbit $\orbit$ is a smooth $(n{-}1)$-dimensional surface $\poincare \subset \statespace$ such that: 1. $\orbit$ intersects $\poincare$ transversally (not tangentially). 2. In a neighborhood of the intersection, every orbit near $\orbit$ also crosses $\poincare$.

The Poincar'e return map $\bm{P}: \poincare \to \poincare$ sends each point on $\poincare$ to the next intersection with $\poincare$ under the flow.

For periodic orbits, the orbit crosses $\poincare$ once per period, and the Poincar'e return map has a fixed point at the intersection $\state^* = \orbit \cap \poincare$. Orbital stability of $\orbit$ is equivalent to stability of this fixed point under the return map.

Theorem 7 Poincar'e–Floquet Connection

The Jacobian of the Poincar'e return map at the fixed point equals the transverse monodromy matrix: \[\begin{equation} D\bm{P}(\state^*) = \monodromy_\transverse. \end{equation}\] The Floquet multipliers and the eigenvalues of the Poincar'e map are identical.

Moving Poincar'e Sections for Non-Periodic Trajectories

The golf swing is not periodic—it is a finite-time trajectory. The classical Poincar'e section (which relies on periodicity) does not directly apply. However, we can generalize to moving Poincar'e sections: a continuous family of transverse hyperplanes, one at each point of the orbit.

A moving Poincar'e section along a trajectory $\traj^*(s)$, $s \in [0, L]$, is the family of hyperplanes \[\begin{equation} \poincare(s) = \traj^*(s) + \Sigma(s), \end{equation}\] where $\Sigma(s)$ is the transverse hyperplane at $s$. The section-to-section map from $\poincare(s_1)$ to $\poincare(s_2)$ is defined by following the flow from $s_1$ to $s_2$: \[\begin{equation} \bm{P}_{s_1 \to s_2}: \poincare(s_1) \to \poincare(s_2), \qquad \bm{\xi}(s_1) \mapsto \bm{\xi}(s_2). \end{equation}\] The linearized section-to-section map is the state transition matrix $\PhiMat_\transverse(s_2, s_1)$ of the transverse dynamics.

For the golf swing, this gives us a powerful analysis tool:

For a finite-time trajectory $\traj^*: [0, T] \to \statespace$, define the transverse amplification ratio from phase $s_1$ to $s_2$: \[\begin{equation}\label{eq:amplification} \rho(s_1, s_2) = \norm{\PhiMat_\transverse(s_2, s_1)}_2 = \sigma_{\max}\!\left(\PhiMat_\transverse(s_2, s_1)\right), \end{equation}\] where $\sigma_{\max}$ is the largest singular value. If $\rho(s_1, s_2) < 1$, then transverse deviations shrink between $s_1$ and $s_2$. If $\rho(s_1, s_2) > 1$, they grow.

For trajectory design, we require: \[\begin{equation} \rho(0, s_{\text{impact}}) \ll 1 \qquad \text{(deviations shrink toward impact)}. \end{equation}\] This is a precise formulation of the funnel concept from Chapter 1: the tube narrows because the transverse dynamics are contracting.

Example 16 Amplification Profile of the Golf Swing

Consider the 8-dimensional state space of a 4-DOF golf swing model. The transverse dynamics have dimension 7. The transverse state transition matrix $\PhiMat_\transverse(s, 0)$ maps initial deviations to deviations at phase $s$.

Hypothetical numerical computation on representative swing models reveals the amplification ratio $\rho(0, s)$ as a function of theoretical phase $s$: - $s \in [0, 0.3]$ (backswing): $\rho$ is approximately 1—deviations neither grow nor shrink. The motion is kinematically slow and nearly linear. - $s \in [0.3, 0.5]$ (transition): $\rho$ increases past 1. This is the unstable phase—the high curvature and speed change amplify deviations. Without feedback, errors grow here. - $s \in [0.5, 0.7]$ (late downswing): $\rho$ decreases. In some simplified release models, the transverse dynamics contract over this phase interval. Interpreted carefully, the wrist-release geometry narrows the admissible error tube for that model; it is not proof that every real swing is open-loop self-stabilizing. - $s = 0.75$ (impact): $\rho(0, 0.75) < 1$. The net effect over the entire swing is transverse contraction—deviations are smaller at impact than at address.

This profile shows one way skilled golfers could remain accurate despite the extreme nonlinearity of the swing: a well-posed model may exhibit passive transverse contraction near impact, and feedback can exploit that contraction. The result is a model-level funnel, not a blanket claim that errors vanish without sensing or control.

Controller Design via Transverse Stabilization

We now use the transverse linearization to design tracking controllers. The approach is systematic: stabilize the transverse linear system $\eqref{eq:transverse_linear}$ using standard LTV techniques, then map the result back to the nonlinear system.

Time-Varying LQR for Transverse Stabilization

The trajectory tracking problem reduces to solving the differential Riccati equation for the transverse dynamics: \[\begin{equation}\label{eq:riccati} -\dot{\bm{S}}(t) = \Amat_\transverse^\top \bm{S} + \bm{S}\, \Amat_\transverse - \bm{S}\, \Bmat_\transverse \bm{R}^{-1} \Bmat_\transverse^\top \bm{S} + \bm{Q}(t), \end{equation}\] with terminal condition $\bm{S}(T) = \bm{S}_T$. The optimal transverse feedback gain is \[\begin{equation} \bm{K}_\transverse(t) = \bm{R}^{-1}\, \Bmat_\transverse(t)^\top\, \bm{S}(t). \end{equation}\] The full control law is \[\begin{equation}\label{eq:full_control_law} \control(t) = \underbrace{\control^*(t)}_{\text{feedforward}} + \underbrace{\bm{N}(s^*)\, \bm{K}_\transverse(t)\, \bm{N}(s^*)^\top (\state - \traj^*(s^*))}_{\text{transverse feedback}}, \end{equation}\] where $\bm{N}(s^*)$ maps between the transverse coordinates $\bm{\xi}$ and the full state coordinates.

The weighting matrices $\bm{Q}(t)$ and $\bm{R}$ have clear physical interpretations: - $\bm{Q}(t)$ penalizes transverse deviation. Making $\bm{Q}$ large near impact tightens the funnel where precision matters. - $\bm{R}$ penalizes control effort. Making $\bm{R}$ small allows aggressive correction but may demand unrealistic actuator authority. - The phase-dependent nature of $\bm{Q}(t)$ is essential: we want tight tracking near impact and loose tracking during the backswing. This is the time-varying tube from Chapter 1, now realized through the Riccati equation.

Phase-Varying Gains and the Funnel Connection

The terminal-value Riccati equation $\eqref{eq:riccati}$ naturally produces gains that tighten as the terminal time approaches. This is exactly the funnel behavior we designed geometrically in Chapter 1:

Theorem 8 Riccati Funnel

Let $\bm{S}(t)$ be the solution of $\eqref{eq:riccati}$ with terminal penalty $\bm{S}_T \succ 0$. The ellipsoidal set \[\begin{equation} \mathcal{E}(t) = \left\{\bm{\xi} \in \Sigma(s) : \bm{\xi}^\top \bm{S}(t)\, \bm{\xi} \leq c \right\} \end{equation}\] defines a time-varying tube whose width (in the direction of eigenvector $\bm{v}_k$ of $\bm{S}$) is proportional to $1/\sqrt{\lambda_k(t)}$, where $\lambda_k(t)$ is the corresponding eigenvalue of $\bm{S}(t)$.

As $t \to T$ and $\bm{S}(t) \to \bm{S}_T$, the tube tightens. The closed-loop transverse dynamics ensure that any trajectory starting inside the tube at $t = 0$ remains inside for all $t \in [0, T]$, provided the control authority is sufficient.

This is the formal connection between the geometric funnel of Chapter 1 and the algebraic Riccati equation: the funnel is the level set of the Riccati cost-to-go.

The Complete Tracking Controller Architecture

Assembling all the pieces, the trajectory tracking controller has three layers:

[Figure: See PDF edition for TikZ diagram.]

Feedforward $\control^*(t)$: computed offline from inverse dynamics, provides $\sim90$% of the total control effort.
Projection: measures the current state $\state(t)$, projects onto the orbit to find $s^*$, and computes the transverse deviation $\bm{\xi}$.
Transverse feedback $\delta\control = \bm{K}_\transverse(s^*)\, \bm{\xi}$: stabilizes the transverse dynamics using phase-varying gains from the Riccati equation.

Remark

Why Not Just Use Standard LQR?

One could linearize the full dynamics about $\traj^*(t)$ and apply standard time-varying LQR to the $n$-dimensional error $\bm{e}(t) = \state(t) - \traj^*(t)$. This works, but: 1. It “wastes” control effort trying to correct tangential deviations (timing errors), which are benign. 2. It fights the inherent marginal stability in the tangential direction, leading to unnecessarily high gains. 3. It does not produce the correct funnel shape—it penalizes all errors equally, including timing errors.

The transverse approach avoids all three problems. It is geometrically correct, computationally smaller (dimension $n - 1$ vs. $n$), and physically meaningful.

Robustness of Orbital Stability

A practical controller must be robust to model uncertainty and external disturbances. The transverse framework provides natural robustness margins.

For the closed-loop transverse system $\dot{\bm{\xi}} = [\Amat_\transverse - \Bmat_\transverse \bm{K}_\transverse]\, \bm{\xi}$, the transverse gain margin and transverse phase margin are the gain and phase margins of the transverse loop transfer function. These are the trajectory-tracking analogues of the classical setpoint stability margins.

Proposition 2 Robustness of Transverse LQR

If the transverse LQR is designed with $\bm{R} = r\bm{I}$, then the closed-loop transverse system has guaranteed gain margin $[1/2, \infty)$ and phase margin $\pm 60^\circ$, inheriting the classical LQR robustness guarantees in each transverse channel.

For the golf swing, robustness translates directly to repeatability: a swing with high transverse robustness margins produces consistent results even when the golfer’s motor commands are noisy.

Summary

Concept	Significance
only cross-orbit deviations matter
LTV stability; removes the tangential “nuisance” direction
determine stability of periodic orbits
trajectories; connect to the state transition matrix
quantifies transverse error growth or contraction
optimal funnel controller
geometric and algebraic views

This chapter has provided the mathematical backbone for everything that follows. The transverse linearization converts the nonlinear trajectory-tracking problem into a linear, lower-dimensional problem that inherits all the powerful tools of LTV control theory. The Floquet/Poincar'e framework provides both analytical insight and numerical algorithms for assessing stability.

The key insight for the golf swing is that orbital stability—not Lyapunov stability—is the correct framework. The swing is designed so that its passive transverse dynamics create a natural funnel toward impact, and the transverse LQR controller tightens this funnel using phase-varying gains that match the demands of each phase of the motion.

In Chapter 5, we will see how underactuation constrains the transverse dynamics, creating geometric structure that can be exploited rather than merely tolerated.

Exercises

(Conceptual.) Explain why a pendulum swinging on its limit cycle (pumped by a periodic torque) is orbitally stable but not Lyapunov stable. Sketch the phase portrait and identify the tangential and transverse directions.
(Computation.) For the Van der Pol oscillator $\ddot{x} - \mu(1 - x^2)\dot{x} + x = 0$ with $\mu = 1$:
(Transverse linearization.) For the system $\dot{x}_1 = x_2 + u$, $\dot{x}_2 = -x_1^3$, consider the trajectory $\traj^*(t) = (\cos t, -\sin t)$ with appropriate $u^*$.
Find $u^*(t)$ such that $\traj^*$ is a solution.
Compute the full Jacobian $\bm{J}(t)$ along $\traj^*$.
Compute the transverse Jacobian $\Amat_\transverse(t)$.
Is the orbit stable open-loop? Design stabilizing transverse feedback.
(Numerical project: Transverse LQR.) For the double pendulum from Exercise 3.4:
1. Compute a reference swing trajectory using direct collocation (or load from a data file).
2. Compute the transverse linearization along the trajectory.
3. Solve the differential Riccati equation with $\bm{Q}(t)$ that increases 10-fold near the terminal phase (“impact”).
4. Simulate the closed-loop system with random initial perturbations and plot the transverse deviation $\norm{\bm{\xi}(t)}$ over time.
5. Compute the amplification ratio profile $\rho(0, s)$ and identify the “natural funnel” phase.
(Design.) A walking robot with period-1 gait has Floquet multipliers $\mu_1 = 0.9$, $\mu_2 = 1.1$, $\mu_3 = 0.5$.
1. Is the gait orbitally stable? Which mode is problematic?
2. Design a transverse feedback controller that places the unstable multiplier $\mu_2$ at $0.6$. What gain is required?
3. After stabilization, what is the worst-case convergence rate per stride?
4. How many strides does it take to reduce a 10% transverse deviation to 1%?
(Philosophical.) Compare the transverse LQR approach (this chapter) with the standard trajectory-tracking LQR (linearize about $\traj^*(t)$, regulate $\bm{e} = \state - \traj^*(t)$ to zero).
1. Under what conditions do the two approaches give identical feedback gains? (Hint: consider straight-line trajectories.)
2. Construct a numerical example where standard LQR uses significantly more control effort than transverse LQR.
3. Discuss which approach is preferable for the golf swing and why.

The pendulum does not care if you are watching at noon or midnight. Its orbit knows no clock. Stability is not arrival. It is the promise that the motion, once begun, will keep its shape— even when the world pushes back.

Underactuation and Passive Dynamics

Do not force the motion. Let the physics do the work.

The Loss of Control Authority

In classical robotics, the goal is often full actuation: one motor for every degree of freedom. If a robot has six joints, it has six motors. This fully actuated paradigm allows the system to follow essentially any smooth trajectory in the configuration space, subject only to actuator saturation limits. The control problem is algebraically simple because the mapping from torques to joint accelerations is invertible.

But nature does not build fully actuated systems, and neither do athletes.

When a gymnast swings on a high bar, their center of mass rotates around the bar, but they have no motor at the bar—that “joint” is a passive pin constraint. When a human walks, the foot acting as the pivot is unactuated during the single-support phase; we cannot create torque from a point contact. And crucially for our running example, when a golfer releases the club during the downswing, the wrist joint acts as a free hinge. The club rapidly accelerates to over 100 miles per hour, driven entirely by the passive physics of the arms pulling the grip, not by muscular torque applied at the wrist.

Consider a mechanical system with $n$ degrees of freedom (DOF) and configuration coordinates $\bm{q} \in \configspace \subseteq \Reals^n$. The equations of motion take the standard manipulator form: \[\begin{equation} \Mmat(\bm{q})\ddot{\bm{q}} + \Cmat(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \gvec(\bm{q}) = \Bmat(\bm{q}) \control \end{equation}\] where $\Mmat$ is the inertia matrix, $\Cmat$ contains Coriolis and centrifugal terms, $\gvec$ is the gravity vector, $\control \in \Reals^m$ is the vector of control inputs, and $\Bmat(\bm{q}) \in \Reals^{n \times m}$ maps inputs into generalized forces.

The system is underactuated if $m < n$, meaning there are fewer actuators than degrees of freedom. In this case, the matrix $\Bmat$ is not full rank (it maps a lower-dimensional control space into the $n$-dimensional configuration space).

If we lack the authority to command arbitrary accelerations, the classical setpoint perspective sees this as a profound failing. A setpoint controller wants to cancel the nonlinearities, feedback-linearize the system, and dictate the exact motion. Underactuation prevents this brute-force cancellation.

However, from the trajectory-first perspective developed in Chapter 1, underactuation is not merely a restriction—it is a geometric feature. The passive dynamics (the unactuated portions of the dynamics) define a manifold of feasible trajectories. We don’t fight the physics; we ride it.

The Double Pendulum: A Window Into the Golf Swing

To understand how passive dynamics can be exploited, we study the simplest model that captures the physics of the golf downswing: the planar underactuated double pendulum.

Let the first link (the arms) have angle $\theta_1$, length $l_1$, mass $m_1$, and inertia $I_1$. Let the second link (the club) have angle $\theta_2$, length $l_2$, mass $m_2$, and inertia $I_2$. The configuration is $\bm{q} = (\theta_1, \theta_2)^\top$.

Crucially, we assume there is a motor at the shoulder (controlling torque $u_1 = \tau_{\text{shoulder}}$) but no motor at the wrist ($\tau_{\text{wrist}} = 0$).

The input matrix is therefore: \[\begin{equation} \Bmat = \begin{pmatrix} 1 \\ 0 \end{pmatrix}, \qquad \control = [u_1]. \end{equation}\]

The dynamics expand into two coupled equations: \[\begin{align} M_{11}\ddot{\theta}_1 + M_{12}\ddot{\theta}_2 + C_1 + g_1 &= u_1, \label{eq:pend_actuated} \\ M_{21}\ddot{\theta}_1 + M_{22}\ddot{\theta}_2 + C_2 + g_2 &= 0. \label{eq:pend_unactuated} \end{align}\]

Equation $\eqref{eq:pend_unactuated}$ is the unactuated dynamics or passive constraint. It dictates the motion of the club. Notice that we cannot directly command $\ddot{\theta}_2$. However, we can influence it through the coupling terms: 1. Inertial coupling ($M_{21}\ddot{\theta}_1$): Accelerating or decelerating the arms immediately exerts a force on the club. 2. Velocity coupling ($C_2$): This term contains the centrifugal forces. As the arms swing rapidly, centrifugal force naturally pulls the club outward.

In a golf swing, a simplified underactuated model can generate large clubhead speed at impact by exploiting the $C_2$ and $M_{21}$ terms. Early in the downswing, the golfer maintains an acute angle between the arms and the club ($\theta_2 - \theta_1 \approx 90^\circ$). The arms accelerate ($\ddot{\theta}_1 > 0$).

As the hands approach the bottom of the swing arc, the arms decelerate ($\ddot{\theta}_1 < 0$). This negative acceleration, transferred through the $M_{21}$ coupling, can accelerate the club forward. Simultaneously, the large arm velocity ($\dot{\theta}_1$) makes the centrifugal contribution in $C_2$ large enough to push the club toward alignment with the arms ($\theta_2 \to \theta_1$) in the model. This supports a passive-release hypothesis for the simplified system; it does not show that measured wrist torque is identically zero or that real golfers never add muscular wrist action.

Let’s look at the unactuated row algebraically to see this whipping action: \[\begin{equation} \ddot{\theta}_2 = -M_{22}^{-1} \bigl( M_{21}\ddot{\theta}_1 + C_2(\bm{q}, \dot{\bm{q}}) + g_2(\bm{q}) \bigr). \end{equation}\] Since $M_{22}$ is scalar and positive, a large negative $\ddot{\theta}_1$ (braking the arms) can produce a large positive $\ddot{\theta}_2$ (accelerating the club). This is one mechanism by which the model can accelerate the club within the moving-target control problem. Whether it is efficient or dominant in measured swings depends on grip mechanics, shaft compliance, actuator limits, and timing.

[Figure: See PDF edition for TikZ diagram.]

The Annihilator and the Null Space

The double pendulum example shows that the passive dynamics constrain the allowable states and accelerations. We generalize this using the mathematics of the null space.

Recall the dynamics: \[\begin{equation} \Mmat(\bm{q})\ddot{\bm{q}} + \Cmat(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \gvec(\bm{q}) = \Bmat(\bm{q}) \control. \end{equation}\]

We define a full-rank matrix $\Bmat^\perp(\bm{q}) \in \Reals^{(n-m) \times n}$, called the left annihilator of $\Bmat$, such that: \[\begin{equation} \Bmat^\perp(\bm{q}) \Bmat(\bm{q}) = \bm{0} \quad \text{for all } \bm{q}. \end{equation}\] The rows of $\Bmat^\perp$ span the left null space of $\Bmat$.

In our double pendulum where $\Bmat = [1, 0]^\top$, the annihilator is trivially $\Bmat^\perp = [0, 1]$.

By multiplying the equations of motion by the annihilator $\Bmat^\perp$, we eliminate the control input $\control$, revealing the intrinsic constraints on the system’s acceleration:

\[\begin{equation}\label{eq:unactuated_dynamics} \Bmat^\perp \Mmat \ddot{\bm{q}} + \Bmat^\perp (\Cmat\dot{\bm{q}} + \gvec) = \bm{0}. \end{equation}\]

A smooth curve $\traj^*(t)$ is a dynamically feasible trajectory for the underactuated system if and only if it satisfies the unactuated dynamics: \[\begin{equation} \Bmat^\perp(\traj^*) \Bigl[ \Mmat(\traj^*)\ddot{\traj}^* + \Cmat(\traj^*, \dot{\traj}^*)\dot{\traj}^* + \gvec(\traj^*) \Bigr] = \bm{0} \end{equation}\] for all $t \in [0, T]$. If a curve satisfies this condition, one can always find the necessary control input $\control^*(t)$ by projecting the dynamics onto the range of $\Bmat$.

For control design, this means we cannot arbitrarily select a path in $\statespace$ and expect a controller to track it. The reference trajectory must live in the restricted subspace defined by $\eqref{eq:unactuated_dynamics}$. The trajectory optimization algorithms in Chapter 6 will explicitly enforce this constraint.

Partial Feedback Linearization

While we cannot feedback-linearize the entire underactuated system, we can feedback-linearize the actuated degrees of freedom. This technique is called partial feedback linearization or collocated feedback linearization.

By cleverly substituting coordinates, we can partition the system into a linear, fully actuated subsystem (say, the golfer’s arms) and a nonlinear, passive subsystem (the club) that is driven by the state of the first.

Partition the coordinates as $\bm{q} = (\bm{q}_a, \bm{q}_u)$, representing actuated and unactuated DOFs. We can rewrite the dynamics and choose $\control$ to exactly cancel the nonlinearities of the actuated part: \[\begin{equation} \ddot{\bm{q}}_a = \bm{v}_a \end{equation}\] where $\bm{v}_a$ is our new synthetic control input. The unactuated dynamics become: \[\begin{equation}\label{eq:zero_dynamics} \Mmat_{uu}(\bm{q})\ddot{\bm{q}}_u + \Mmat_{ua}(\bm{q})\bm{v}_a + \text{nonlinear terms} = 0. \end{equation}\]

If we force the actuated coordinates to track a reference trajectory (e.g., using a high-gain PD controller, $\bm{v}_a = \ddot{\bm{q}}_a^* - K_d \dot{\tilde{\bm{q}}}_a - K_p \tilde{\bm{q}}_a$), the unactuated coordinates are left to evolve according to $\eqref{eq:zero_dynamics}$. The resulting behavior depends on whether this forced subsystem remains bounded or convergent for the chosen reference, parameters, and feedback gains. This leads us to the concept of the zero dynamics.

Zero Dynamics and the Golf Swing Funnel

If we perfectly stabilize the actuated tracking errors to zero ($\bm{q}_a \equiv \bm{q}_a^*$), the remaining dynamics governing $\bm{q}_u$ are called the zero dynamics.

For the golf swing model, the zero dynamics describe how the club moves when the arm trajectory is prescribed and the wrist DOF remains unactuated in the reduced system. The practical question is not whether “the golf swing” is globally stable, but whether a specified phase interval of a specified model has locally attracting transverse behavior.

Revisiting the concept of Transverse Linearization (Chapter 4), if we linearize the unactuated dynamics around the trajectory $\traj^*(t)$, we analyze the transverse divergence of the club from its optimal path.

We obtain a phase-dependent picture rather than a universal law: - During the early downswing, the club’s zero dynamics are unstable. If the club gets slightly out of position, the centrifugal force tends to pull it further off the optimal plane. - During the late downswing (the release phase), the club’s zero dynamics may become strongly attracting in the simplified model. Large club acceleration can act like a restoring term, but the degree of alignment depends on the selected parameters, grip model, and disturbance assumptions.

This is the model analogue of the time-varying tube from Chapter 1. In favorable parameter regimes, passive coupling can make the admissible-error set wide early in the motion and narrower near impact. The result is not a literal singularity and not a coaching rule that wrist torque is unnecessary; it is a statement about a specified underactuated model whose phase, grip, shaft, and actuator assumptions must be checked.

Remark

Why Passive Release Can Feel Effortless

The common coaching phrase “let the club release” is compatible with an underactuated model, but it should not be read as a measured neuromuscular fact. A skilled swing may use the arms and torso to set boundary conditions for passive release while still applying grip and wrist torques. Whether that strategy improves speed or consistency must be tested against measured kinematics, force data, and the assumed wrist model.

Summary

Concept	Significance
Underactuation	Fewer actuators ($m$) than coordinates ($n$). Cannot arbitrarily command accelerations in configuration space.
Passive Dynamics	The natural, unforced motion of the unactuated DOFs.
Coupling	Inertial ($M_{21}$) and velocity ($C_2$) terms transmit energy from actuated to unactuated DOFs.
Left Annihilator	A matrix $\Bmat^\perp$ that strips away the control input to reveal the intrinsic geometric constraints of the motion.
Partial Feedback Lin.	Cancelling nonlinearities only on the actuated joints to simplify control.
Zero Dynamics	The residual dynamics of the unactuated subsystem when the actuated subsystem perfectly tracks its reference.

Underactuation forces a shift from “forcing” a system to an equilibrium, to “orchestrating” the natural physics of the system. The trajectory is a choreography between the motors and gravity/inertia. In Chapter 6, we will see how numerical trajectory optimization can synthesize these choreographies automatically by solving optimal control problems that explicitly respect the $\Bmat^\perp$ constraint.

Exercises

(Annihilator construction.) A cart-pole system has state $\bm{q} = (x, \theta)^\top$, where $x$ is the cart position and $\theta$ is the pole angle. The motor applies horizontal force to the cart.
(Passive pumping.) In a playground swing, the person swinging cannot exert an external torque on the swing setup (there is no motor at the top pivot). By what mechanism (which dynamics term) does the swinger add energy to the system by leaning back and forth? Relate this to the golf wrist release.
(Zero Dynamics Stability.) Consider a generic system partitioned into $\ddot{q}_a = v_a$ and $\ddot{q}_u + c q_u = q_a$. Let the reference trajectory be $q_a^*(t) = \sin(t)$.
1. Assume we enforce $q_a = q_a^*$ perfectly. Write the zero dynamics for $q_u$.
2. Under what conditions on the parameter $c$ are the zero dynamics stable? Is it possible to have a trajectory that passes through both stable and unstable regimes of zero dynamics?

Control is not dominion over nature; it is an intelligent negotiation with it.

Trajectory Optimization

We do not merely seek a path that obeys the laws of physics.\ We seek the path that bends them to our advantage.

The Moving Target Problem

In Chapter 1, we fundamentally reframed our control objective from “reaching a destination” to “tracking a trajectory.” Up to this point, we have assumed that this optimal reference trajectory $\traj^*(t)$ is given to us by an oracle. This chapter answers the obvious next question: Where does this reference come from?

For systems whose purpose is to move—like our running example of the golf swing—the optimal trajectory is not arbitrary. We are not just trying to go from point A to point B while minimizing energy. We are trying to fulfill what we term the Moving Target Objective: optimizing the kinematics (velocity, pose, acceleration) of a system precisely at the moment it intersects a target manifold (e.g., the golf ball).

Crucially, the exact clock time that impact occurs does not matter. A golfer has the freedom to start their backswing slowly and pause at the top. The optimization is entirely concerned with the shape of the motion and its terminal kinematics. Time is a resource, not a constraint.

Find the state trajectory $\state(t)$, control input $\control(t)$, and terminal time $t_f$ that minimize the cost functional: \[\begin{equation}\label{eq:ocp_cost} J = \phi(\state(t_f)) + \int_0^{t_f} L(\state(t), \control(t)) \dt \end{equation}\] subject to the dynamics and constraints: \[\begin{align} \dot{\state}(t) &= \bm{f}(\state(t), \control(t)) \qquad &\text{(Dynamics)}\\ \bm{h}(\state(t), \control(t)) &\leq \bm{0} \qquad &\text{(Path constraints)}\\ \psi(\state(t_f)) &= \bm{0} \qquad &\text{(Terminal manifold/Moving goal)}\\ \state(0) &= \state_0 \qquad &\text{(Initial state)} \end{align}\]

In the golf downswing, the terminal cost $\phi(\state(t_f))$ might be negative clubhead speed (to maximize speed), the integral cost $L$ might penalize excessive joint torques to ensure repeatability, and the terminal manifold constraint $\psi(\state(t_f)) = \bm{0}$ forces the clubhead to be geometrically located at the ball with a squared face. Notice that $t_f$ is a free parameter in the optimization.

Transcribing the Continuous Problem

The OCP described above occurs in infinite-dimensional function space. To solve it on a computer, we must transcribe it into a finite-dimensional Nonlinear Programming problem (NLP): \[\begin{align*} \min_{\bm{w}} \quad & f(\bm{w}) \\ \text{subject to} \quad & \bm{c}(\bm{w}) \leq \bm{0}, \end{align*}\] where $\bm{w}$ is a large vector of decision variables. How we discretize the integrals and differential equations defines the transcription method. We will focus on two modern approaches: Direct Shooting and Direct Collocation.

Direct Multi-Shooting

In direct multi-shooting, we break the time interval $[0, t_f]$ into $N$ segments. The decision variables are the control parameters $\control_k$ on each interval and the state values $\state_k$ at the beginning of each interval.

The dynamic constraint $\dot{\state} = \bm{f}(\state, \control)$ is enforced by numerical integration (e.g., Runge-Kutta). We require that the integrated state at the end of interval $k$ exactly matches the assumed starting state of interval $k+1$: \[\begin{equation} \state_{k+1} - \bm{\Phi}(\state_k, \control_k, \Delta t) = \bm{0} \qquad \text{(Defect constraint)} \end{equation}\] where $\bm{\Phi}$ represents the numerical integrator step. Multi-shooting is excellent for highly nonlinear dynamics because it keeps the integration intervals short, preventing the massive nonlinear sensitivities that plague single-shooting methods.

Direct Collocation

Direct Collocation takes this a step further. We represent both the state trajectory and the control signal as piecewise polynomials (e.g., cubic splines for state, linear splines for control). The decision variables are the coefficients of these polynomials, typically located at discrete collocation points.

Instead of using a numerical integrator, we enforce the differential equation algebraicaly at the collocation points: \[\begin{equation} \dot{\tilde{\state}}(t_c) - \bm{f}(\tilde{\state}(t_c), \tilde{\control}(t_c)) = \bm{0} \qquad \text{(Collocation constraint)} \end{equation}\] where $\tilde{\state}$ and $\tilde{\control}$ are the polynomial approximations.

Direct collocation results in a large, sparse NLP. Because the solver has access to the state at every grid point simultaneously, it can impose dynamics and path constraints across the whole trajectory rather than advancing one simulated step at a time. Collocation is a standard method for generating complex dynamic motions such as walking, jumping, and swinging, subject to model fidelity and solver convergence.

Optimizing Underactuated Geometries

Recall from Chapter 5 that the underactuated dynamics enforce a strict algebraic constraint on the system: \[\begin{equation} \Bmat^\perp(\state) \Bigl[ \Mmat(\state)\ddot{\state} + \Cmat(\state, \dot{\state})\dot{\state} + \gvec(\state) \Bigr] = \bm{0}. \end{equation}\]

When optimizing for a golf swing, direct collocation handles this underactuation organically. We do not need to explicitly partition the variables into actuated and unactuated sets for the solver. The collocation constraint $\dot{\state} - \bm{f} = \bm{0}$ already implicitly contains the passive dynamic constraints.

Example 17 The Golf Swing OCP

Consider our planar double pendulum model from Chapter 5. The solver is given the task to maximize $\dot{\theta}_2(t_f)$ (club speed).

Initial Guess: If we seed the NLP solver with a naive guess (e.g., constant shoulder torque), the club speed will be pitifully low.

The optimized result in this model: The solver may select a positive $u_1$ phase to accelerate the arms and then reverse $u_1$ before $t_f$ if that sequence improves the terminal objective under the imposed constraints. In that case, the solution is exploiting inertial coupling through terms such as $M_{21}\ddot{\theta}_1$. This is a model result, not evidence that every human swing uses the same torque pattern.

Repeatability: The Stochastic OCP

The previous sections frame a deterministic problem: “Find the swing that maximizes terminal speed.”

Biological systems and athletes are not well described by peak deterministic performance alone. If an NLP solver maximizes golf club speed without a noise or robustness term, it can select trajectories near constraint boundaries. The resulting motion may be highly sensitive to timing or state errors; the size of that sensitivity must be computed for the chosen model rather than asserted generically.

A deterministic optimum can be poor if its local geometry amplifies small execution errors into large terminal errors. A movement objective intended to describe repeatable biological performance should include some treatment of variance, uncertainty, or robustness.

We modify our deterministic cost $J = \phi(\state(t_f)) + \int L$ into a stochastic expectation: \[\begin{equation} \min_{\traj^*} \quad \mathbb{E}\left[ \phi(\state(t_f, \bm{w})) \right] + \lambda \cdot \mathrm{Var}\left[ \psi(\state(t_f, \bm{w})) \right] \end{equation}\] where $\bm{w} \sim \mathcal{N}(0, \Sigma_w)$ represents signal-dependent motor noise (a known property of human muscle actuation, where noise scales proportionally to the applied torque).

To solve this in a collocation framework without massive Monte Carlo simulations, we use Covariance Steering or Linear Covariance Analysis. We augment the state vector $\state(t)$ with its local covariance matrix $\bm{\Sigma}(t)$. The dynamics of $\bm{\Sigma}(t)$ are propagated alongside the nominal trajectory using the linearized Jacobian matrices along the path.

The optimizer can then trade peak performance against sensitivity to noise. In a favorable model, this may select a trajectory with better transverse robustness near the moving goal. Whether the resulting trajectory is repeatable for a human depends on the accuracy of the noise model, actuator limits, and validation data.

Summary

Concept	Significance
Moving Target OCP	Finding a trajectory that optimizes terminal kinematics when intersecting a spatial manifold, without prescribing the clock time.
Multi-Shooting	Discretizing the time domain into segments, using numerical integrators to enforce dynamics constraints between nodes.
Direct Collocation	Representing the entire trajectory as splines and enforcing dynamics algebraically at discrete points arrayed along the trajectory.
Passive Exploitation	Solvers naturally optimize underactuated topologies by weaponizing coupling matrices ($M_{21}, C_2$).
Stochastic Robustness	Augmenting the OCP with expected variance to ensure the resulting trajectory can be consistently executed by noisy nonlinear plants.

Determining the shape of the reference trajectory is central to Control Is Motion. With a passive geometry model and a candidate reference trajectory in hand, the next step is to estimate a control tube around that trajectory and state the assumptions under which disturbances remain inside it. We turn to funnel synthesis in Chapter 7.

Exercises

(Free Terminal Time Formulation.) Explain how one sets up an OCP where final time $t_f$ is a decision variable in a fixed-grid Direct Collocation solver. (Hint: consider scaling time $t = \tau \cdot t_f$, where $\tau \in [0, 1]$). Show how the dynamic constraint $\dot{\state} = \bm{f}$ changes under this substitution.
(Kinematic Goals vs Setpoints.) You are optimizing a baseball bat swing. Your state is $(\theta, \dot{\theta})$. Assume the ball arrives at the plate between $t=0.5$ and $t=0.6$ seconds. Formulate the path constraint and the terminal manifold constraint $\psi$ assuming you want maximum $\dot{\theta}$ when $\theta = \pi/2$, but the exact time of impact within that window does not matter.
(Signal-Dependent Noise Penalty.) Human muscle torque output $\tau$ has variance $\sigma^2 = k |\tau|^2$. Write a modified integral cost function $L(\state, \tau)$ that penalizes the introduction of variance into the system. Interpret what this does to the optimal trajectory shape.

The perfect motion is rarely the fastest. It is the one that survives reality.

Funnel Synthesis

You cannot control the exact trajectory a stone will take down a mountain.\ But you can build a trough to ensure it reaches the bottom.

Beyond LQR: The Need for Guaranteed Invariance

In Chapter 4, we showed that applying time-varying Linear Quadratic Regulation (LQR) to the transverse dynamics creates a contracting envelope of stability. The Riccati equation naturally shapes this envelope into a “funnel” that narrows as the terminal constraint approaches.

However, LQR guarantees are inherently local. They rely on the linear approximation of the transverse dynamics $\dot{\bm{\xi}} = \bm{A}_\perp(t)\bm{\xi} + \bm{B}_\perp(t)\delta\control$. For highly nonlinear physical systems, a disturbance large enough to leave the local neighborhood can invalidate the linear approximation. The consequence may be large terminal error, but the threshold depends on the reference trajectory, controller, and nonlinear model.

We need a mathematically explicit way to define the boundary of a funnel. For a stated nonlinear model and controller, the goal is to certify that states starting inside the candidate set remain inside it for the modeled time interval.

Given a reference trajectory $\traj^*(t)$, a feedback controller $\control = \bm{k}(\state, t)$, and a scalar boundary function $V(\state, t)$, the set: \[\begin{equation} \mathcal{F}(t) = \{ \state \in \statespace \mid V(\state, t) \leq 1 \} \end{equation}\] constitutes a valid dynamic funnel if the set is positively invariant under the closed-loop dynamics. That is, if $\state(0) \in \mathcal{F}(0)$, then $\state(t) \in \mathcal{F}(t)$ for all $t \in [0, t_f]$.

Time-Varying Lyapunov Functions for Trajectories

To verify invariance, we use the machinery of Lyapunov theory, adapted for trajectories rather than equilibria. We require our boundary function $V(\state, t)$ to act as a time-varying Lyapunov function.

Theorem 9 Funnel Invariance

The set $\mathcal{F}(t) = \{ \state \mid V(\state, t) \leq 1 \}$ is invariant if, on the boundary where $V(\state, t) = 1$, the function is strictly decreasing along the system trajectories: \[\begin{equation} \dot{V}(\state, t) = \frac{\partial V}{\partial t} + \nabla_{\state} V(\state, t) \cdot \bm{f}(\state, \bm{k}(\state, t)) < 0 \quad \text{for all } \state \text{ s.t. } V(\state, t) = 1. \end{equation}\]

If the derivative of the boundary function points inward everywhere on the boundary, trajectories may circulate inside the funnel, but they can never escape it. For a moving target problem, we want the spatial volume of $\mathcal{F}(t)$ to shrink as $t \to t_f$, terminating in a tight bounded region at impact.

A computed funnel is an analysis tool for repeatability. Under the stated model, controller, and disturbance assumptions, it defines a certified set of initial conditions. It does not by itself prove successful ball striking in an unmodeled human swing, and it should not be identified with the full reachable set of the nonlinear system.

Sums-of-Squares (SOS) Programming

Proving that $\dot{V} < 0$ on the boundary $V=1$ for a general nonlinear function $\bm{f}$ is famously difficult. We have an infinite number of states on the boundary to check! We circumvent this by restricting our dynamics $\bm{f}$ algorithms to polynomials (via Taylor expansion or substitution) and leveraging Sums-of-Squares (SOS) Programming (Boyd et al. 1994; Majumdar and Tedrake 2017).

A polynomial $p(x)$ is a Sum of Squares if it can be written as $p(x) = \sum_{i} q_i(x)^2$ for some polynomials $q_i(x)$. If a polynomial is SOS, it is globally non-negative. Testing if a polynomial is SOS turns out to be equivalent to solving a Semidefinite Program (SDP)—a convex optimization problem that can be solved very efficiently.

We can reframe Theorem $\ref{thm:funnel_invariance}$ using the S-procedure. To guarantee that $\dot{V}(\state, t) < 0$ whenever $V(\state, t) = 1$, we require: \[\begin{equation} -\dot{V}(\state, t) - L(\state, t)(1 - V(\state, t)) \quad \text{is SOS} \end{equation}\] where $L(\state, t)$ is a strictly positive polynomial multiplier. If $V(\state, t) = 1$, the second term vanishes, and the SOS condition forces $-\dot{V} > 0$, achieving our invariance proof!

Computing the Funnel for the Golf Swing

Let us return to the double pendulum of the golf swing. Our reference trajectory $\traj^*(t)$ from Chapter 6 gives us the nominal kinematics. Our transverse LQR from Chapter 4 gives us a feedback matrix $\bm{K}(t)$ designed to stabilize deviations. We wish to compute the largest possible funnel around this sequence for which the closed-loop system satisfies the stated invariance certificate.

We define our candidate Lyapunov function as a time-varying quadratic form: \[\begin{equation} V(\state, t) = (\state - \traj^*(t))^\top \bm{P}(t) (\state - \traj^*(t)) \end{equation}\] where $\bm{P}(t)$ is a positive definite matrix that changes continuously along the trajectory. The funnel boundary is the time-varying ellipsoid where $V=1$. By altering $\bm{P}(t)$, we alter the shape and orientation of the funnel.

Example 18 Volume Maximization via SDP

We transcribe the continuous time $t$ into discrete nodes $t_k$, just as we did in direct collocation. At each node, we set up an SDP that seeks to maximize the volume of the ellipsoid defined by $\bm{P}(t_k)$, subject to the SOS invariance constraints connecting it to adjacent nodes.

The optimizer estimates structural limits for the modeled passive dynamics. In one model, early-downswing funnels may be wider than late-downswing funnels, and SOS constraints may elongate $\bm{P}(t_k)$ along selected unactuated directions to maintain the invariance certificate. The result is a numerical certificate for that model, not a direct measurement of athletic consistency.

In Chapter 8, we will explore how to make these funnels robust to temporal variation by migrating out of the time domain entirely and into the phase domain.

Phase-Variable Control

Time is a river that carries us forward. Phase is the boat we build to navigate it.

Spatial Paths vs. Temporal Execution

One common limitation of direct time-tracking architectures, when applied to biomechanics or advanced robotics, is their rigid adherence to the clock. If you construct a time-varying trajectory $\traj^*(t)$ for a robotic arm and command a tracking controller to follow it, the controller evaluates the error as $\bm{e}(t) = \state(t) - \traj^*(t)$.

If the robot hits an unmodeled friction patch and slows down, the reference trajectory $\traj^*(t)$ continues advancing. The controller can calculate a large position error not because the robot has left the intended path, but because it is behind schedule. Depending on gains, actuator limits, and saturation handling, this can produce excessive catch-up commands.

A system executing a motion can often be controlled more safely when progress is measured by state-dependent phase rather than by clock time alone. If the system is impeded, the reference can slow down. The mathematical task is to decouple the spatial geometry of the motion from its temporal execution by replacing chronological time $t$ with a locally monotone phase variable $s$.

Instead of parameterizing our optimal trajectory as $\traj^*(t)$, we parameterize it as a function of the phase: $\traj^*(s)$. The phase $s \in [0, 1]$ represents the percentage of motion completed. For a walking robot, $s$ could be the forward position of the hip. For a golf swing, $s$ could be the angular position of the shoulder relative to the ball. The reference trajectory is now a spatial curve with a separately modeled timing law.

This construction is publication-safe only after the phase variable’s domain, monotonicity condition, and reset behavior are specified. This is the same practical issue that appears in maneuver regulation, transverse linearization, and hybrid zero dynamics (Hauser and Hindman 1995; Westervelt et al. 2007; Shiriaev et al. 2010).

A phase variable $s(\state)$ is a continuously differentiable scalar function of the state $\state$ such that its time derivative is strictly positive along all feasible trajectories inside the nominal funnel: \[\begin{equation} \dot{s}(\state) > 0 \quad \text{for all } \state \in \mathcal{F}. \end{equation}\]

Virtual Constraints

If the goal is no longer to match a time sequence but rather to conform to a spatial path defined by $s(\state)$, we can express the control objective as a set of holonomic constraints on the state. However, because these constraints are not mechanical linkages but rather enforced by software, they are termed Virtual Constraints.

Let our control objective be to drive the state $\state$ to the invariant manifold $\mathcal{Z}$ defined by: \[\begin{equation} \mathcal{Z} = \{ \state \mid \state - \traj^*(s(\state)) = \bm{0} \}. \end{equation}\]

By using partial feedback linearization (Chapter 5), we can design a synthetic input $\bm{v}_a$ that drives the actuated degrees of freedom $\bm{q}_a$ onto their nominal surfaces proportional to the phase: \[\begin{equation} y = \bm{q}_a - \traj^*_a(s(\state)) \to 0. \end{equation}\] As $y \to 0$, the system converges toward the virtual constraints. Crucially, the rate at which it progresses along the path is entirely governed by $\dot{s}$, which is itself determined by the current momentum and state of the system, not a stopwatch.

Hybrid Zero Dynamics

When the virtual constraints are perfectly satisfied ($y = 0$), the entire complexity of the $n$-dimensional state space collapses down into a highly reduced dynamical system that evolves strictly along the manifold $\mathcal{Z}$. This is the Zero Dynamics of the system.

For systems that undergo discrete impacts (a foot hitting the ground, or a golf club striking a ball), the system evolves via continuous differential equations until an impact surface is triggered, followed by an instantaneous jump map (a reset) dictated by conservation of momentum.

A control architecture achieves Hybrid Zero Dynamics if the invariant manifold $\mathcal{Z}$ (the zero dynamics surface) is invariant not just under the continuous flow of the differential equations, but also invariant under the discrete impact map. That is, an impact starting on the manifold $\mathcal{Z}$ must immediately map to a new state that is also on $\mathcal{Z}$.

By enforcing HZD, we can design rhythmic walking gaits that are astonishingly robust. If the robot trips, its phase slows down, but the virtual constraints remain active, keeping the limbs on the correct spatial path until the next footfall resets the sequence. The temporal sequence arises naturally from the interplay of gravity and momentum.

Application: The Golf Downswing

While the golf swing is not a periodic walking gait, a phase-variable approach can still be useful if the selected marker remains monotone through the interval being modeled. A candidate phase might be shoulder angle, hand path progress, or another measured progress coordinate; none is automatically valid outside the stated model. The target event is impact, not a universal manifold $s=1.0$ independent of the coordinate choice.

By structuring a model through virtual constraints, the analysis can separate spatial deviations from timing delays. A transverse feedback law (Chapter 4) can correct selected spatial errors, while the phase variable schedules arm actuation ($u_1$) relative to measured progress rather than clock time. This can improve timing robustness, but it does not guarantee perfect passive impact dynamics; impact quality still depends on state estimation, actuator limits, contact modeling, and the validity of the chosen phase coordinate.

Stochastic Trajectories & Motor Variability

Perfection is inherently fragile. Biological optimization seeks the strongest balance between intent and reality.

Signal-Dependent Noise

When a roboticist specifies a torque of $50\text{ Nm}$, the motor delivers $50\text{ Nm}$ with a tiny, constant variance. Biological actuators do not work this way. Human muscles generate force by recruiting discrete motor units. As higher forces are required, larger, more fast-twitch oriented motor units are recruited. This physiological reality manifests as a profound control challenge: motor noise is proportional to the command signal.

A control input $\control(t)$ applied by a biological system is actually realized as a stochastic variable $\tilde{\control}(t)$: \[\begin{equation} \tilde{\control}(t) = \control(t) + \bm{W} \control(t) \cdot \bm{\epsilon}(t) \end{equation}\] where $\bm{W}$ is a scaling matrix representing the coefficient of variation, and $\bm{\epsilon}(t) \sim \mathcal{N}(0, I)$ is white noise. That is, the variance of the applied force grows squarely with the magnitude of the ordered force: $\text{Var}(\tilde{u}) = c u^2$.

This complicates deterministic optimal control. If a computed trajectory requires very large muscle commands, signal-dependent noise can increase endpoint variance. The magnitude of that effect depends on the noise coefficient, the controller, and the sensitivity of the trajectory.

Minimum Variance Theory of Human Movement

Why do humans move in smooth, bell-shaped velocity profiles rather than jerky “bang-bang” optimal control solutions when reaching for coffee?

One family of models explains smooth reaching with “minimum jerk” objectives, while another uses signal-dependent noise and endpoint variance. The trajectory-first perspective used here is closest to the minimum variance theory of Harris and Wolpert, but it should be read as one modeling account among several.

In minimum-variance models, the nervous system is represented as selecting movements that reduce expected endpoint variance across repeated trials.

Under signal-dependent noise, reducing sudden spikes in actuator force can reduce endpoint variance because larger commands inject more variability into the dynamics. Smooth trajectories often have lower peak torque demands than abrupt acceleration profiles, although the result depends on task constraints and the model being optimized. In this view, smoothness can emerge from accuracy under noisy actuation rather than from an explicitly imposed smoothness objective.

The Speed-Accuracy Tradeoff in the Golf Swing

Fitts’s Law captures a speed-accuracy tradeoff in targeted movement. In a golf model, a deterministic optimal-control solution may maximize clubhead speed by requesting large input torques.

However, if we introduce the Stochastic OCP (via Covariance Steering) from Chapter 6 and apply the realistic premise of SDN, the optimizer faces a profound tradeoff.

Applying maximum $u_1$ torque during the downswing can inject substantial variance into the kinematic state if the noise model scales with command magnitude. The unactuated club ($\theta_2$) may still gain speed through zero dynamics, but its phase progression can become more uncertain.

In such a model, a stochastic optimizer may reduce peak requested torque if the variance penalty outweighs the speed benefit. The exact tradeoff, including any clubhead-speed loss, must be computed from the model; it does not prove that a biological swing is universally sub-maximal.

Learning to Move

The first swing establishes the manifold. The ten-thousandth swing perfects the funnel.

Iterative Learning Control (ILC)

Trajectory optimization (Chapter 6) computes a reference motion strictly from a mathematical model. However, dynamic models of humans—and even robots—are invariably flawed. Frictional forces are unmodeled, inertias are estimated, and actuators possess hidden dynamics. If you execute a computed reference $\traj^*(t)$ open-loop, the physical system will likely fail to strike the moving target due to these discrepancies.

Iterative Learning Control (ILC) resolves this by leveraging the repetitive nature of tasks like a golf swing. ILC exploits the premise that while models are inaccurate, the unmodeled dynamics are highly deterministic across repeated trials, provided that initial states are repeatable and disturbances are bounded. (Standard ILC convergence conditions require stable, repetitive exogenous disturbances (Sastry 1999) and fail if parameters drift arbitrarily).

If a robot under-swings by $5$ cm on the first attempt, the controller records this terminal error and updates the feedforward command $\control_{\text{ff}}$ for the second attempt. Over repeated trials, the system learns the inverse dynamics of the true physical plant without ever explicitly identifying the model parameters, acting as a robust convergence mechanism.

Let $j$ denote the trial index. The feedforward command for trial $j+1$ is updated based on the command and the transverse error $\bm{\xi}$ from trial $j$: \[\begin{equation} \control_j(t) = \control_{\text{ff}, j}(t) + \bm{K}_\perp(t) \bm{\xi}_j(t) \end{equation}\] \[\begin{equation} \control_{\text{ff}, j+1}(t) = \control_{\text{ff}, j}(t) + \bm{L}(t) \bm{\xi}_j(t) \end{equation}\] where $\bm{L}(t)$ is a learning filter. This updates the nominal trajectory to iteratively cancel out deterministic disturbances.

Policy Search and Reinforcement Learning

While ILC updates the feedforward commands to zero-out deterministic tracking errors, Policy Search algorithms optimize the shape of the funnel itself.

Instead of computing the transverse LQR gains $\bm{K}_\perp(t)$ from an analytical Riccati equation (Chapter 4), we parameterize the controller as a policy $\pi_{\bm{\theta}}(\state)$ and search the parameter space $\bm{\theta}$ to minimize the variance at the moving goal under realistic noise.

Reinforcement learning (RL), including policy-gradient methods, can sample many simulated swings and update policy parameters to maximize expected reward terms such as clubhead speed and face orientation at impact. If the simulator, reward, and constraints match the chapter assumptions, a trained policy may recover behaviors similar to the earlier control arguments: 1. It abandons rigid time-tracking and autonomously discovers phase-variable mapping. 2. It exploits the underactuated double-pendulum whipping motion to gain speed. 3. It artificially widens its “funnel” during the transition phase, realizing that overly tight feedback control here merely amplifies motor noise and spoils the impact constraints.

Trajectory Libraries

A singular optimal trajectory $\traj^*$ and its corresponding funnel $\mathcal{F}$ are highly specific to a single objective. What if the golfer wishes to fade the ball, hit a draw, or execute a punch shot out of the rough?

Instead of re-running a large NLP optimization for every scenario, engineered controllers can maintain Trajectory Libraries. A library consists of several distinct, pre-computed trajectories and their corresponding local funnels. The analogy to biological movement planning is suggestive rather than established by this chapter.

When faced with a new task (e.g., striking the ball on an uneven lie), the system searches its library for the closest matching funnel. By interpolating between established funnels using techniques like LQR-Trees (Tedrake 2009) or simple spatial blending, the system can instantly generate a stable, feasible motion without any online optimization.

Case Study: The Complete Golf Swing

The physics are invariant. What separates the professional is their mastery over the geometry of the resultant funnel.

Unifying the “Control Is Motion” Framework

Throughout this text, we have reframed control around trajectory geometry and local transverse correction. We conclude with a hypothetical 15-DOF musculoskeletal golf-swing model that illustrates how the framework could be assembled into a reproducible analysis.

The objective is to maximize clubhead speed at the spatial intersection with the golf ball while controlling face orientation and respecting biological torque limits. The terminal time may be treated as a decision variable, but the formulation must still state admissible timing, contact, and phase-coordinate assumptions. Robustness is included through an explicit signal-dependent-noise model.

Phase 1: Defining the Geometry and Optimization

The system is heavily underactuated in this example. The wrists are treated as passive pin joints, which is a simplifying assumption used to expose inertial coupling rather than a complete anatomical model.

We formulate a Stochastic Moving Target Optimal Control Problem (Chapter 6). Direct collocation optimizes the trajectory $\traj^*(s)$ over a spatial phase variable $s$ representing shoulder angle, provided that this phase coordinate remains monotone over the modeled interval (Chapter 8). The integral cost penalizes signal variance through local covariance propagation (Chapter 9).

In the illustrative model, the collocation solver satisfies the $\Bmat^\perp$ annihilator constraint (Chapter 5) and may produce a smooth reference motion. A torque reversal before impact can exploit passive wrist dynamics, but claims about face squaring require explicit contact geometry, grip modeling, and validation against measured swings.

Phase 2: Funnel Synthesis and Robustness

With $\traj^*(s)$ computed, we lock down the trajectory using Transverse Linearization (Chapter 4). We ignore errors along the tangent of the path (since chronological timing is irrelevant) and construct a time-varying LQR feedback matrix to regulate only the transverse deviations.

Finally, we compute candidate bounds for this stability region using Sums-of-Squares Programming (Chapter 7). The SDP supplies a boundary function $V(\state, t) = 1$ that certifies invariance under the polynomial model, controller, and disturbance assumptions used in the computation.

The final computed funnel describes the model’s certified region of attraction. During the backswing, the funnel may be wider if the modeled dynamics tolerate larger transverse deviations without violating the invariant boundary.

As the swing approaches the transition and downswing, the funnel may tighten because the model becomes more sensitive to timing, phase, and actuation errors. Any claim about a final $0.15$ seconds, near-zero funnel diameter, or passive absorption of muscular noise must be supported by the actual SDP solution and sensitivity analysis.

The case study illustrates the intended use of the framework: define a motion objective, state the model assumptions, compute a trajectory, and certify the local control region. It does not replace empirical validation, and it should be read as a reproducible modeling program rather than a proof of a universal control language.

References

Boyd, S., L. El Ghaoui, E. Feron, and V. Balakrishnan. 1994. Linear Matrix Inequalities in System and Control Theory. SIAM.

Bullo, F., and A. D. Lewis. 2004. Geometric Control of Mechanical Systems. Springer.

Carmo, M. P. do. 1992. Riemannian Geometry. Birkhäuser.

Flash, T., and N. Hogan. 1985. “The Coordination of Arm Movements: An Experimentally Confirmed Mathematical Model.” Journal of Neuroscience 5 (7): 1688–703. https://doi.org/10.1523/JNEUROSCI.05-07-01688.1985.

Hauser, John, and Rick Hindman. 1995. “Maneuver Regulation from Trajectory Tracking: Feedback Linearizable Systems.” IFAC Proceedings Volumes 28 (14): 595–600. https://doi.org/10.1016/S1474-6670(17)46893-5.

Lephart, Scott M., James M. Smoliga, John B. Myers, Timothy C. Sell, and Yu-Shian Tsai. 2007. “An Electromyographic Analysis of the Efficacy of Incorporating Neuromuscular Training to Improve Lower Extremity Mechanics During the Golf Swing.” Journal of Strength and Conditioning Research 21 (2): 521–30.

Majumdar, A., and R. Tedrake. 2017. “Funnel Libraries for Real-Time Robust Feedback Motion Planning.” The International Journal of Robotics Research 36 (8): 947–82. https://doi.org/10.1177/0278364917712421.

Manchester, I. R., and J.-J. E. Slotine. 2017. “Control Contraction Metrics: Convex and Intrinsic Criteria for Nonlinear Feedback Design.” IEEE Transactions on Automatic Control 62 (6): 3046–53. https://doi.org/10.1109/TAC.2017.2668380.

Neumann, Donald A. 2010. Kinesiology of the Musculoskeletal System: Foundations for Rehabilitation. 2nd ed. Mosby/Elsevier.

Sastry, S. 1999. Nonlinear Systems: Analysis, Stability, and Control. Springer.

Shiriaev, A. S., L. B. Freidovich, and S. V. Gusev. 2010. “Transverse Linearization for Controlled Mechanical Systems with Several Passive Degrees of Freedom.” IEEE Transactions on Automatic Control 55 (4): 893–906. https://doi.org/10.1109/TAC.2010.2042000.

Tedrake, R. 2009. “LQR-Trees: Feedback Motion Planning on Sparse Randomized Trees.” Robotics: Science and Systems. https://www.roboticsproceedings.org/rss05/p3.pdf.

Todorov, E., and M. I. Jordan. 2002. “Optimal Feedback Control as a Theory of Motor Coordination.” Nature Neuroscience 5 (11): 1226–35. https://doi.org/10.1038/nn963.

Westervelt, E. R., J. W. Grizzle, C. Chevallereau, J. H. Choi, and B. Morris. 2007. Feedback Control of Dynamic Bipedal Robot Locomotion. CRC Press. https://doi.org/10.1201/9781420053739.

Winter, David A. 2009. Biomechanics and Motor Control of Human Movement. 4th ed. John Wiley & Sons.

Euclidean assumption	Manifold replacement
\(\configspace = \Reals^n\)	\(\configspace = \SOthree^k \times \Torus^m \times \cdots\)
Error: \(\bm{e} = \bm{q} - \bm{q}^*\)	Error: \(\bm{e} = \log((\bm{q}^*)^{-1}\bm{q})\)
Derivative: \(\ddot{\bm{q}}\)	Covariant derivative: \(\nabla_{\dot{q}}\dot{\bm{q}}\)
Shortest path: straight line	Shortest path: geodesic
Distance: \(\norm{\bm{q}_2 - \bm{q}_1}\)	Geodesic distance: \(d_{\bm{G}}(\bm{q}_1, \bm{q}_2)\)
Curvature: \(\kappa\) (Ch.~2)	Geodesic curvature: \(\kappa_g\)
Trajectory tube: Euclidean ball	Geodesic ball
Coriolis terms: “extra forces”	Christoffel symbols: manifold geometry
PD: \(-k_p\bm{e} - k_d\dot{\bm{e}}\)	\(-k_p\log(\cdot)^\vee - k_d(\text{parallel transport})\)

Throwing Away the Target

The Wrong Question

Why Setpoints Are a Special Case

Anatomy of a Motion: The Golf Swing as Prototype

The Mathematical Landscape

Phase Variables and Time Reparameterization

Transverse Dynamics and Orbital Stability

Funnel Control and Time-Varying Tubes

What Changes When Control Is Motion

Stability Is Not Convergence—It Is Confinement

The Cost Function Is a Functional, Not a Function

Feedforward Is Not Optional—It Is Primary

Underactuation Creates Geometry, Not Just Constraints

A Roadmap for the Book

A New Language for Control

Exercises

Curves in State Space

Why Geometry Comes First

Parameterized Curves

Velocity, Speed, and the Tangent Vector

Arc Length: The Natural Parameter

Curvature: How Curves Bend

Curvature in Arbitrary Parameterization

Why Curvature Matters for Control

The Frenet–Serret Frame

Construction in \(\Reals^n\)

The Frenet–Serret Equations

The Moving Frame in State Space

Curvature and Torsion in the Language of Control

Curvature as a Demand on the Controller

Torsion as Out-of-Plane Complexity

Reparameterization and Timing Laws

The Path–Timing Decomposition of Control

The Osculating Objects

The Geometry of Trajectory Deviations

Frenet–Serret Coordinates

Curvature-Induced Coupling

Fundamental Theorem and Uniqueness

Summary

Exercises

Smooth Manifolds: The Minimum You Need

Tangent Spaces and Tangent Vectors

The Configuration Manifolds of Joints

The Rotation Group \(\SOthree\)

Why Euler Angles Fail

The Lie Algebra \(\sothree\)

The Golf Swing Configuration Space

A Simplified Kinematic Model

Why This Matters for Control

Riemannian Metrics and the Mass Matrix

Geodesics as Natural Trajectories

Curves on Manifolds Revisited

Covariant Differentiation

Manifold Curvature of Trajectories

Error Metrics on Manifolds

The Logarithmic Error

The Trajectory Tube on a Manifold

Equations of Motion on Manifolds

Practical Implications for Controller Implementation

Summary

Exercises

Orbital Stability and Transverse Linearization

The Problem With Lyapunov

Orbital Stability: Formal Definitions

The Transverse–Tangential Decomposition

Projection Onto the Orbit

The Moving Hyperplane Decomposition

Transverse Linearization

Deriving the Transverse Dynamics

The Transverse Jacobian: A Practical Formula

Floquet Theory: Stability of Periodic Orbits

The State Transition Matrix

The Monodromy Matrix

Moving Poincar'e Sections

Moving Poincar'e Sections for Non-Periodic Trajectories

Controller Design via Transverse Stabilization

Time-Varying LQR for Transverse Stabilization

Phase-Varying Gains and the Funnel Connection

The Complete Tracking Controller Architecture

Robustness of Orbital Stability