Hybrid Tangent Spaces
Beyond Smooth Dynamics: Impacts, Switches, and Discontinuities
Part I: Motivation and Theory
1 Chapter 1: Why Smoothness Fails
1.1 The C¹ Assumption and Its Limits
The entire Tangent Hyperplane framework rests on a single critical assumption:
The vector field f: \mathbb{R}^n \times \mathbb{R}^m \to \mathbb{R}^n is continuously differentiable (C^1), meaning:
\dot{x} = f(x, u), \quad f \in C^1 \tag{1}
Under the smoothness assumption, this gives:
- The Jacobian A(x,u) = \frac{\partial f}{\partial x} exists and is continuous
- Tangent spaces T_x \mathcal{M} are well-defined at every state x
- Linearized dynamics \delta\dot{x} = A\delta x + B\delta u are exact in the infinitesimal limit
This assumption is physically reasonable for most classical mechanical systems derived from Lagrangian or Hamiltonian mechanics. Pendulums, robotic arms, spacecraft, and chemical reactors all produce smooth vector fields.
But consider the following scenarios:
Example 1: Humanoid Foot Strike
A bipedal robot’s foot collides with the ground. Before contact: v_{foot} < 0 (downward velocity). After contact: v_{foot} = 0 (instantaneous stop). The velocity experiences a discontinuous jump \Delta v \neq 0 over zero time.
At the instant of impact, the derivative \dot{x} is undefined. The tangent space does not exist.
Example 2: Golf Ball Impact
A golf club head traveling at v_{club} = 50 m/s strikes a stationary ball. The collision lasts \Delta t \approx 0.5 ms. During this interval, contact forces F \sim 10^4 N produce accelerations \ddot{x} \sim 10^6 m/s². The force model F(x, \dot{x}) is discontinuous at the contact surface.
The Jacobian \frac{\partial f}{\partial x} has a singularity at x_{contact}.
Example 3: Coulomb Friction
The Coulomb friction model is:
f_{friction} = \begin{cases} -\mu_s F_N \cdot \text{sgn}(\dot{x}) & \text{if } |\dot{x}| > 0 \\ -F_{applied} & \text{if } |\dot{x}| = 0, |F_{applied}| < \mu_s F_N \\ -\mu_k F_N \cdot \text{sgn}(\dot{x}) & \text{if sliding} \end{cases} \tag{2}
The signum function \text{sgn}(\dot{x}) is discontinuous at \dot{x} = 0. The derivative \frac{\partial f}{\partial \dot{x}} does not exist at stiction transitions.
Example 4: Bang-Bang Control
Fuel-optimal control for thrust-limited spacecraft often produces discontinuous control policies:
u^*(x) = \begin{cases} u_{max} & \text{if } \lambda^T B > 0 \\ 0 & \text{if } \lambda^T B = 0 \\ -u_{max} & \text{if } \lambda^T B < 0 \end{cases} \tag{3}
where \lambda is the costate from Pontryagin’s Maximum Principle. The control u(t) switches discontinuously, making f(x, u(x)) non-smooth even if f itself is smooth in its arguments.
These are not pathological edge cases—they are core phenomena in robotics, aerospace, and mechanical engineering. A framework that cannot handle them is incomplete.
1.2 The Failure of Naïve Linearization
What happens if we blindly apply the tangent space framework to a system with discontinuities? Consider a simple 1D impact model:
\dot{v} = -g, \quad v^+ = -e \cdot v^- \quad \text{when } x = 0 \tag{4}
where e \in [0,1] is the coefficient of restitution. A ball falls under gravity until it hits the ground (x = 0), at which point the velocity reverses with energy loss.
Attempt 1: Ignore the discontinuity
If we linearize around a trajectory that includes an impact, computing \delta\dot{x} = A\delta x at the impact instant gives:
A_{impact} = \frac{\partial}{\partial v}\left(-e \cdot v^-\right) = \text{undefined}
The derivative does not exist because the velocity is a function of the left-limit v^-, not the state itself. The Jacobian is singular.
Attempt 2: Treat impact as a stiff force
We might approximate the impact as a very stiff spring:
F_{contact}(x) = \begin{cases} 0 & x > 0 \\ -k x - c \dot{x} & x \leq 0 \end{cases}, \quad k \to \infty
This makes f continuous but introduces numerical stiffness. Explicit integrators require timesteps \Delta t \sim 1/\sqrt{k}, which becomes prohibitively small. Implicit integrators work but lose the analytical structure we seek.
Attempt 3: Smooth via mollification
Replace the discontinuous jump with a smooth transition:
v^+ = -e \cdot v^- \cdot \sigma_\epsilon(x)
where \sigma_\epsilon(x) = \frac{1}{2}(1 + \tanh(x/\epsilon)) is a sigmoid. This makes f \in C^\infty, but:
- Accuracy depends on \epsilon: Too large and physics is wrong, too small and we’re back to stiffness
- Loss of structure: The sharp transition at x = 0 has physical meaning (guard surface), which is obscured
- Parameter tuning: Every discontinuity requires choosing \epsilon, with no principled method
1.3 The Need for Hybrid Formalism
None of these workarounds is satisfactory. What we need is a mathematically rigorous framework that:
- Accepts discontinuities as first-class features, not approximations
- Defines tangent spaces on either side of jumps, even if not at the jump itself
- Maps perturbations across discontinuities via a well-defined linear operator
- Integrates into optimization, preserving the computational advantages of DDP/iLQR
This framework already exists: hybrid dynamical systems. The goal of this article is to extend the tangent space perspective to hybrid systems, developing the geometric tools needed to handle impacts, switches, and discontinuities rigorously.
1.4 Chapter Summary
The C^1 smoothness assumption is not a minor technicality—it is central to the Tangent Hyperplane framework. Without it:
- Jacobians may not exist
- Tangent spaces are undefined
- Linearization fails
Physically important systems (impacts, friction, mode switches) violate this assumption. Naïve workarounds (ignoring, stiffening, mollifying) all have fatal flaws.
The solution is to extend the framework to hybrid systems, where discontinuities are handled explicitly via guard surfaces and jump maps. This is the subject of the remainder of this article.
2 Chapter 2: Hybrid Automata Primer
3 What Is a Hybrid System?
A hybrid dynamical system is one that exhibits both continuous evolution and discrete jumps. Mathematically, it consists of:
A hybrid automaton \mathcal{H} is a tuple:
\mathcal{H} = (Q, \mathcal{X}, \mathcal{U}, F, G, R, \Sigma) \tag{5}
where:
- Q = \{q_1, q_2, \ldots, q_m\} is a finite set of discrete modes (also called locations or charts)
- \mathcal{X} = \mathbb{R}^n is the continuous state space
- \mathcal{U} = \mathbb{R}^p is the control input space
- F = \{f_1, f_2, \ldots, f_m\} is a family of vector fields, one per mode: \dot{x} = f_q(x, u), \quad q \in Q \tag{6}
- G = \{G_{ij} \subseteq \mathcal{X} \mid i, j \in Q\} is a family of guard sets (switching surfaces)
- R = \{R_{ij}: G_{ij} \to \mathcal{X} \mid i, j \in Q\} is a family of reset maps (jump maps)
- \Sigma = \{e_{ij} \mid i, j \in Q\} is the set of discrete events (mode transitions)
Intuition: A hybrid system is like a finite state machine where each state has its own continuous dynamics. While in mode q, the state evolves according to \dot{x} = f_q(x,u). When x enters a guard set G_{ij}, an event e_{ij} is triggered, the state jumps via the reset map x^+ = R_{ij}(x^-), and the mode switches from q_i to q_j.
4 Example: Bouncing Ball
The bouncing ball from Equation 4 can be formalized as a hybrid automaton:
Discrete modes: Q = \{\text{flight}\} (Single mode—ball is always in flight when not at boundary)
Continuous state: x = \begin{bmatrix} h \\ v \end{bmatrix}, \quad h = \text{height}, \; v = \text{velocity}
Vector field (flight mode): f_{\text{flight}}(x) = \begin{bmatrix} v \\ -g \end{bmatrix} \tag{7}
Guard set: G = \{x \in \mathbb{R}^2 \mid h = 0, \; v < 0\} \tag{8}
The ball triggers the guard when it reaches the ground (h = 0) while moving downward (v < 0).
Reset map: R(x^-) = \begin{bmatrix} 0 \\ -e \cdot v^- \end{bmatrix} \tag{9}
The height remains zero, but the velocity reverses and scales by the restitution coefficient e.
Execution: The system evolves as:
- Integrate \dot{x} = f_{\text{flight}}(x) continuously
- Monitor guard condition h = 0, v < 0
- When guard is triggered:
- Compute pre-impact state x^-
- Apply reset: x^+ = R(x^-)
- Reinitialize continuous evolution from x^+
- Repeat
This formalism makes the discontinuity explicit and localized to the guard surface.
5 Multi-Mode Example: Walking Robot
A more realistic example is a simplified walking robot with stance and swing phases:
Discrete modes: Q = \{\text{stance}, \text{swing}\}
Continuous state: x = \begin{bmatrix} \theta \\ \phi \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} where \theta = torso angle, \phi = leg angle.
Vector fields:
Stance mode (foot on ground, single support): M_s(x)\ddot{q} + C_s(x, \dot{q})\dot{q} + G_s(x) = B_s u + J_s^T \lambda \tag{10}
where \lambda is the contact force (computed via constraint).
Swing mode (foot in air, no contact): M_{sw}(x)\ddot{q} + C_{sw}(x, \dot{q})\dot{q} + G_{sw}(x) = B_{sw} u \tag{11}
Guards:
G_{\text{stance} \to \text{swing}}: Toe-off condition, e.g., \lambda_N = 0 (normal force vanishes)
G_{\text{swing} \to \text{stance}}: Heel-strike condition, e.g., h_{\text{foot}} = 0 (foot contacts ground)
Reset maps:
At heel strike, the foot velocity must become zero (instantaneous impact): R_{\text{swing} \to \text{stance}}(x^-) = \begin{bmatrix} q^- \\ \Phi(q^-) \dot{q}^- \end{bmatrix} \tag{12}
where \Phi(q) is the impact map derived from collision equations (see Section 11).
At toe-off, state is continuous: R_{\text{stance} \to \text{swing}}(x^-) = x^- \tag{13}
6 Filippov Solutions at Discontinuities
What happens if the guard surface is not a single isolated event but a region where the system “slides” along a discontinuity? This occurs in:
- Coulomb friction at \dot{x} = 0 (stiction)
- Saturation limits where u = u_{max} for extended time
- Complementarity constraints in contact mechanics
The mathematical tool for defining solutions is the Filippov convex hull:
At a discontinuity surface S = \{x \mid h(x) = 0\} where the vector field is multi-valued, define the Filippov set-valued map:
F_{\text{Filippov}}(x) = \bigcap_{\delta > 0} \bigcap_{\mu(N) = 0} \overline{\text{conv}} \, f(B(x, \delta) \setminus N) \tag{14}
In words: The Filippov convex hull is the convex closure of all limit values of f in arbitrarily small neighborhoods, ignoring sets of measure zero.
For piecewise smooth systems with two modes separated by h(x) = 0:
F_{\text{Filippov}}(x) = \begin{cases} \{f_1(x)\} & h(x) > 0 \\ \{f_2(x)\} & h(x) < 0 \\ \text{conv}\{f_1(x), f_2(x)\} & h(x) = 0, \; \nabla h \cdot f_1 \cdot \nabla h \cdot f_2 < 0 \end{cases} \tag{15}
The last case (sliding mode) occurs when the vector fields on either side point toward the surface.
Example: Coulomb friction with \dot{x} = 0:
If the applied force is less than the maximum static friction, the system remains at \dot{x} = 0 via a Filippov solution. The friction force f \in [-\mu_s N, \mu_s N] takes whatever value is needed to maintain \ddot{x} = 0.
Relevance to tangent spaces: Filippov solutions are the measure-theoretic generalization of classical solutions. They allow us to define trajectories through discontinuities in a way compatible with Lebesgue integration. This is essential for variational analysis, as we’ll see in Section 17.
7 Measure-Theoretic Interpretation
Why do we need measure theory? Because derivatives are defined via limits, and limits require a notion of “almost everywhere” convergence.
In a hybrid system, the trajectory x(t) is:
- Absolutely continuous within each mode (standard ODE solution)
- Discontinuous at guard crossings (jumps in x)
- Piecewise smooth overall
The derivative \dot{x}(t) exists almost everywhere (i.e., everywhere except the finite or countable set of jump times). This is sufficient for:
- Calculus of variations (action integrals \int L \, dt are well-defined)
- Necessary conditions for optimality (Pontryagin’s Maximum Principle)
- Numerical integration (Lebesgue integral equals Riemann integral a.e.)
Key insight: We do not need \dot{x} to exist everywhere—only almost everywhere (a.e.). Jumps have measure zero, so they contribute zero to integrals:
\int_0^T L(x(t), u(t)) \, dt = \sum_{i=1}^{N-1} \int_{t_i}^{t_{i+1}} L(x(t), u(t)) \, dt \tag{16}
where t_1, \ldots, t_{N-1} are the jump times (finite set).
8 Chapter Summary
Hybrid systems formalize discontinuous dynamics via:
- Modes (Q): Discrete states with their own continuous dynamics
- Guards (G): Surfaces triggering transitions
- Resets (R): Jump maps applied at transitions
Examples include bouncing balls (single mode, impulsive reset) and walking robots (multi-mode with foot strikes).
Filippov solutions extend classical ODE solutions to discontinuous vector fields via convex hulls, enabling sliding modes.
Measure theory ensures derivatives exist almost everywhere, making variational calculus and optimization well-defined despite jumps.
Next, we develop the geometry of tangent spaces at jumps, enabling linearization across discontinuities.
9 Chapter 3: Tangent Spaces at Jumps
9.1 Left and Right Tangent Spaces
At a jump, the state x(t) is discontinuous. Limits from the left and right differ:
x^- = \lim_{t \to t_j^-} x(t), \quad x^+ = \lim_{t \to t_j^+} x(t), \quad x^+ = R(x^-) \tag{17}
The critical question: How do perturbations \delta x behave across the jump?
In the smooth case, perturbations evolve via the state transition:
\delta x(t) = \Phi(t, t_0) \delta x(t_0) \tag{18}
where \Phi satisfies \frac{\partial}{\partial t}\Phi(t, t_0) = A(t)\Phi(t, t_0).
In the hybrid case, we need a generalized state transition that accounts for jumps. Define:
At a guard crossing at time t_j:
Left tangent space: T_{x^-}\mathcal{M}^- = \mathbb{R}^n (standard tangent space before jump)
Right tangent space: T_{x^+}\mathcal{M}^+ = \mathbb{R}^n (standard tangent space after jump)
Jump map on tangent spaces: The reset map R: G_{ij} \to \mathcal{X} induces a linear map on tangent spaces:
\boxed{D R(x^-): T_{x^-}G_{ij} \to T_{x^+}\mathcal{X}} \tag{19}
This is the Jacobian of the reset map, denoted:
P_j = \frac{\partial R}{\partial x}\bigg|_{x^-} \tag{20}
Perturbations jump according to:
\delta x^+ = P_j \, \delta x^- \tag{21}
Geometric interpretation: The reset map R is a smooth function from the guard surface G_{ij} to the state space \mathcal{X}. Its derivative DR is a linear map between tangent spaces. Even though the trajectory itself jumps discontinuously, perturbations evolve linearly according to P_j.
9.2 Jump Map Jacobians
Let’s compute P_j for our examples.
9.2.1 Example 1: Bouncing Ball
The reset map is: R\begin{pmatrix} h^- \\ v^- \end{pmatrix} = \begin{pmatrix} 0 \\ -e v^- \end{pmatrix}
Taking the Jacobian: P_j = \frac{\partial R}{\partial x} = \begin{bmatrix} \frac{\partial h^+}{\partial h^-} & \frac{\partial h^+}{\partial v^-} \\ \frac{\partial v^+}{\partial h^-} & \frac{\partial v^+}{\partial v^-} \end{bmatrix} = \begin{bmatrix} 0 & 0 \\ 0 & -e \end{bmatrix} \tag{22}
Interpretation:
- A perturbation in height \delta h^- does not affect the post-impact state (first column is zero)
- A perturbation in velocity \delta v^- scales by -e (second column)
- The matrix is rank-deficient (rank 1), reflecting the constraint h^+ = 0
9.2.2 Example 2: Rigid Body Impact (Poisson’s Hypothesis)
For a rigid body colliding with a surface, conservation of momentum during impact gives:
M v^+ = M v^- + J^T \Lambda \tag{23}
where J is the contact Jacobian and \Lambda is the impulsive force. Using the restitution law:
J v^+ = -e J v^-
Solving for v^+:
v^+ = v^- - (1 + e) M^{-1} J^T (J M^{-1} J^T)^{-1} J v^- \tag{24}
The Jacobian is:
P_j = I - (1 + e) M^{-1} J^T (J M^{-1} J^T)^{-1} J \tag{25}
This is the impact map Jacobian, a projection matrix that removes and reverses the normal component of velocity.
9.3 Saltation Matrices (Anitescu & Potra)
The jump map Jacobian P_j is part of a more general structure called the saltation matrix, which accounts for not just the jump in state but also the variation in jump time.
Consider a trajectory that crosses a guard surface S = \{x \mid h(x) = 0\} at time t_j. If we perturb the initial condition x_0 \to x_0 + \delta x_0, both the jump time and the jump state vary:
t_j \to t_j + \delta t_j, \quad x^- \to x^- + \delta x^-
The saltation matrix S_j maps the pre-jump perturbation (including time variation) to the post-jump perturbation:
\delta x^+ = S_j \, \delta x^- - f^+ \delta t_j \tag{26}
where:
\boxed{S_j = P_j + \frac{(f^+ - P_j f^-) \nabla h^T}{\nabla h^T f^-}} \tag{27}
and:
- P_j = \frac{\partial R}{\partial x}\big|_{x^-} is the reset Jacobian
- f^- = f(x^-, u^-), f^+ = f(x^+, u^+) are the vector fields before/after
- \nabla h is the gradient of the guard function
- \delta t_j = -\dfrac{\nabla h^T \delta x^-}{\nabla h^T f^-} is the variation in crossing time
Derivation (sketch):
The guard crossing condition is h(x(t_j)) = 0. Under a perturbation:
h(x^- + \delta x^-) = 0 \implies \nabla h^T \delta x^- + O(\delta x^2) = 0
But \delta x^- includes evolution up to time t_j + \delta t_j:
\delta x^-(t_j + \delta t_j) = \Phi(t_j, t_0)\delta x_0 + \Phi(t_j, t_0) f^- \delta t_j
The crossing time variation is:
\nabla h^T (\delta x^- + f^- \delta t_j) = 0 \implies \delta t_j = -\frac{\nabla h^T \delta x^-}{\nabla h^T f^-}
After the jump:
\delta x^+ = P_j \delta x^- + \text{(correction for time variation)}
The correction term comes from the fact that we reinitialize at x^+ at time t_j + \delta t_j, not t_j. Combining these:
\delta x^+ = P_j \delta x^- + (f^+ - P_j f^-) \delta t_j
Substituting \delta t_j and simplifying yields Equation 27.
Physical interpretation:
The saltation matrix has two contributions:
- P_j: The direct effect of the reset map on the state
- Correction term: Accounts for the fact that perturbed trajectories cross the guard at different times, experiencing different amounts of pre- and post-impact flow
9.3.1 Example: Bouncing Ball Saltation Matrix
For the bouncing ball:
- h(x) = h (height), so \nabla h = \begin{bmatrix} 1 \\ 0 \end{bmatrix}
- f^- = \begin{bmatrix} v^- \\ -g \end{bmatrix}, f^+ = \begin{bmatrix} -ev^- \\ -g \end{bmatrix}
- P_j = \begin{bmatrix} 0 & 0 \\ 0 & -e \end{bmatrix}
Computing the correction:
f^+ - P_j f^- = \begin{bmatrix} -ev^- \\ -g \end{bmatrix} - \begin{bmatrix} 0 \\ -ev^- - eg \end{bmatrix} = \begin{bmatrix} -ev^- \\ (1-e)(-g) + ev^- \end{bmatrix}
Correcting the calculation:
P_j f^- = \begin{bmatrix} 0 & 0 \\ 0 & -e \end{bmatrix} \begin{bmatrix} v^- \\ -g \end{bmatrix} = \begin{bmatrix} 0 \\ eg \end{bmatrix}
f^+ - P_j f^- = \begin{bmatrix} -ev^- \\ -g \end{bmatrix} - \begin{bmatrix} 0 \\ eg \end{bmatrix} = \begin{bmatrix} -ev^- \\ -g(1+e) \end{bmatrix}
\nabla h^T f^- = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} v^- \\ -g \end{bmatrix} = v^- < 0
The saltation matrix is:
S_j = \begin{bmatrix} 0 & 0 \\ 0 & -e \end{bmatrix} + \frac{1}{v^-} \begin{bmatrix} -ev^- \\ -g(1+e) \end{bmatrix} \begin{bmatrix} 1 & 0 \end{bmatrix}
= \begin{bmatrix} 0 & 0 \\ 0 & -e \end{bmatrix} + \begin{bmatrix} -e & 0 \\ -g(1+e)/v^- & 0 \end{bmatrix}
\boxed{S_j = \begin{bmatrix} -e & 0 \\ -\frac{g(1+e)}{v^-} & -e \end{bmatrix}} \tag{28}
Interpretation:
- First row: Height perturbation \delta h^+ depends on pre-impact velocity perturbation \delta v^- via time-of-crossing variation
- Second row: Velocity perturbation jumps by -e, with additional dependence on height perturbation due to gravity during the time correction
9.4 State Transition Through Multiple Jumps
For a hybrid trajectory with multiple jumps at times t_1, \ldots, t_N, the state transition operator is:
\boxed{\Phi(t, t_0) = \Phi_c(t, t_N) S_N \Phi_c(t_N, t_{N-1}) S_{N-1} \cdots S_1 \Phi_c(t_1, t_0)} \tag{29}
where:
- \Phi_c(t_j, t_{j-1}) is the continuous state transition within mode j (standard smooth case)
- S_j is the saltation matrix at jump j
This product structure allows us to propagate perturbations through arbitrarily complex hybrid trajectories, alternating between continuous flow and discrete jumps.
9.5 Chapter Summary
At guard crossings, states jump discontinuously, but perturbations evolve linearly via:
- Reset Jacobian P_j = \frac{\partial R}{\partial x}: Direct effect of jump map
- Saltation Matrix S_j: Includes correction for variation in crossing time
- Hybrid state transition \Phi: Product of continuous flows and saltation matrices
The key insight: Even for non-smooth trajectories, the tangent space dynamics remain linear. This extends the core Tangent Hyperplane principle to hybrid systems.
Next, we develop the geometric interpretation of these jumps as transformations of manifolds.
Part II: Geometric Framework
10 Chapter 4: Manifolds With Boundaries
10.1 Guard Surfaces as Codimension-1 Manifolds
In smooth dynamics, the state space \mathcal{X} = \mathbb{R}^n is an n-dimensional manifold without boundary. Trajectories flow smoothly through this space, never encountering edges or discontinuities.
In hybrid dynamics, guard surfaces partition the state space into regions. Each guard G_{ij} is a codimension-1 submanifold, meaning it has dimension n-1 and locally divides \mathbb{R}^n into two half-spaces.
A guard surface G is the zero level set of a smooth function h: \mathbb{R}^n \to \mathbb{R}:
G = \{x \in \mathbb{R}^n \mid h(x) = 0\} \tag{30}
Assume h is C^1 and regular, meaning \nabla h(x) \neq 0 for all x \in G. By the Implicit Function Theorem, G is a smooth (n-1)-dimensional embedded submanifold.
Tangent space of G:
At x \in G, the tangent space T_x G is the null space of the gradient:
T_x G = \{\delta x \in \mathbb{R}^n \mid \nabla h(x)^T \delta x = 0\} \tag{31}
This is an (n-1)-dimensional linear subspace orthogonal to \nabla h(x).
Normal space of G:
The normal space N_x G is the one-dimensional space spanned by \nabla h(x):
N_x G = \text{span}\{\nabla h(x)\} \tag{32}
Every vector v \in \mathbb{R}^n decomposes uniquely:
v = v_{\parallel} + v_{\perp}, \quad v_{\parallel} \in T_x G, \; v_{\perp} \in N_x G \tag{33}
10.1.1 Example: Bouncing Ball
The guard is h(x) = h = 0. The gradient is:
\nabla h = \begin{bmatrix} 1 \\ 0 \end{bmatrix}
The tangent space of G (at any point on the ground) is:
T_x G = \left\{ \begin{bmatrix} 0 \\ \delta v \end{bmatrix} \mid \delta v \in \mathbb{R} \right\}
Tangent perturbations are purely in velocity, not height. The normal space is:
N_x G = \left\{ \begin{bmatrix} \delta h \\ 0 \end{bmatrix} \mid \delta h \in \mathbb{R} \right\}
Normal perturbations are purely in height.
10.1.2 Example: Humanoid Foot Strike
For a humanoid robot, the guard is the foot height:
h(q) = \text{forward_kinematics}_{\text{foot}}(q) \cdot \hat{z}
This is a nonlinear function of the joint angles q. The gradient is:
\nabla h(q) = J_{\text{foot}}^T(q) \hat{z}
where J_{\text{foot}}(q) is the foot Jacobian. The tangent space T_q G has dimension n-1 (one constraint), and contains all joint angle perturbations that keep the foot at ground level (to first order).
10.2 Tangent Cones vs. Tangent Spaces
At a boundary point, the tangent cone is the set of velocity directions that keep the trajectory inside the domain.
For a domain \mathcal{D} = \{x \mid h(x) \geq 0\}, the tangent cone at a boundary point x \in \partial \mathcal{D} (where h(x) = 0) is:
T_x \mathcal{D} = \{v \in \mathbb{R}^n \mid \nabla h(x)^T v \geq 0\} \tag{34}
This is a half-space, not a linear subspace. It includes:
- Vectors pointing into the domain (\nabla h^T v > 0)
- Vectors tangent to the boundary (\nabla h^T v = 0)
- But excludes vectors pointing out (\nabla h^T v < 0)
Why does this matter?
In optimization with state constraints (e.g., h(x) \geq 0), the tangent cone determines the feasible perturbation directions. The KKT conditions for constrained optimization involve projections onto the tangent cone, not the full tangent space.
For hybrid systems, the tangent cone at x^- (pre-impact) is:
T_{x^-} \mathcal{D}^- = \{v \mid \nabla h^T v \leq 0\}
(pointing toward or tangent to the guard, allowing impact). At x^+ (post-impact):
T_{x^+} \mathcal{D}^+ = \{v \mid \nabla h^T v \geq 0\}
(pointing away from guard, leaving the surface).
10.3 Normal/Tangent Decomposition at Impact
The velocity v at a guard crossing decomposes as:
v = v_\parallel + v_\perp \tag{35}
where:
v_\perp = \frac{\nabla h \cdot v}{\|\nabla h\|^2} \nabla h, \quad v_\parallel = v - v_\perp \tag{36}
For a frictionless impact, the tangent component is preserved, and the normal component reverses:
v^+ = v_\parallel - e \, v_\perp \tag{37}
In matrix form:
v^+ = (I - (1+e) P_N) v^- \tag{38}
where P_N = \frac{\nabla h \nabla h^T}{\nabla h^T \nabla h} is the orthogonal projection onto the normal space.
With friction (Coulomb model):
The tangent component may also change due to friction:
v^+_\parallel = v^-_\parallel - \mu \frac{\|v_\perp^-\|}{\|v_\parallel^-\|} v_\parallel^- \tag{39}
This is a nonlinear function of the velocity components, requiring iterative solution for the Jacobian.
10.4 Riemannian Metric and Energy
On a manifold with a metric g, the kinetic energy is:
T = \frac{1}{2} \dot{q}^T M(q) \dot{q} \tag{40}
where M(q) is the mass matrix (inertia tensor). At impact, kinetic energy is lost:
T^+ = \frac{1}{2} (v^+)^T M v^+ < \frac{1}{2} (v^-)^T M v^- = T^- \tag{41}
The energy loss is:
\Delta T = T^+ - T^- = -\frac{1}{2}(1 - e^2) (v_\perp^-)^T M v_\perp^- \tag{42}
For elastic collisions (e = 1), energy is conserved. For perfectly inelastic (e = 0), the normal kinetic energy is completely dissipated.
Geometric interpretation:
The mass matrix M(q) defines a Riemannian metric on the configuration manifold. Impact maps are not isometries—they change distances and angles in tangent space. The saltation matrix S_j can be viewed as a metric-preserving map followed by a dissipative projection.
10.5 Chapter Summary
Guard surfaces are codimension-1 manifolds with:
- Tangent space T_x G: (n-1)-dimensional, parallel to surface
- Normal space N_x G: 1-dimensional, perpendicular to surface
- Tangent cones: Half-spaces defining feasible directions
Velocities decompose into tangent and normal components. Impacts act differently on each:
- Normal: reverses and scales (-e v_\perp)
- Tangent: preserved (frictionless) or reduced (friction)
Energy is dissipated in proportion to (1-e^2) and the normal velocity. This geometric structure underpins the saltation matrix and variational dynamics.
11 Chapter 5: Impact Maps as Tangent Space Rotations
12 Restitution Coefficients Geometrically
The coefficient of restitution e is often introduced phenomenologically: “A ball bounces to height e^2 h_0.” But what is e geometrically?
The coefficient of restitution e \in [0, 1] is the ratio of post- to pre-impact normal velocities:
e = -\frac{v_{\perp}^+}{v_{\perp}^-} \tag{43}
In terms of the angle \theta between the velocity and the guard surface:
v_\perp = \|v\| \cos\theta
After impact:
\|v^+\| \cos\theta^+ = -e \|v^-\| \cos\theta^- \tag{44}
For frictionless impact with v_\parallel unchanged:
\|v^+\|^2 = \|v_\parallel\|^2 + e^2 \|v_\perp^-\|^2 \tag{45}
The velocity magnitude decreases by a factor depending on the impact angle.
12.1 Example: Oblique Impact
A ball strikes a wall at angle \theta to the normal:
- Pre-impact: v^- = v_0(\cos\theta, \sin\theta)
- Post-impact normal: v_\perp^+ = -e v_0 \cos\theta
- Post-impact tangent: v_\parallel^+ = v_0 \sin\theta
- Post-impact angle: \tan\theta^+ = \frac{v_0 \sin\theta}{e v_0 \cos\theta} = \frac{\tan\theta}{e}
The rebound angle is steeper than the impact angle (unless e = 1). This is the familiar “shallow bounce” phenomenon—oblique impacts produce more horizontal bounces.
13 Energy Dissipation and Metric Change
The energy loss (Equation 42) can be written using the metric:
\Delta T = -\frac{1}{2}(1 - e^2) \|v_\perp^-\|_M^2 \tag{46}
where \|v\|_M^2 = v^T M v is the kinetic energy norm.
Geometric interpretation:
The impact map shrinks the tangent space in the normal direction by a factor e, while preserving the tangent direction. This is a squeeze mapping:
S_{\text{squeeze}} = I - (1 - e^2) \frac{\nabla h \nabla h^T}{\|\nabla h\|^2} \tag{47}
The actual map is:
v^+ = v_\parallel - e v_\perp = v - v_\perp - e v_\perp = v - (1 + e) v_\perp
= v - (1 + e) P_N v = (I - (1+e) P_N) v
So the impact map matrix is:
\boxed{R_v = I - (1 + e) P_N} \tag{48}
where P_N = \frac{\nabla h \nabla h^T}{\nabla h^T \nabla h}.
This is an oblique reflection (for e = 1, it’s a pure reflection; for e = 0, it’s a projection onto the tangent space).
14 Examples
14.1 Example 1: Golf Club-Ball Collision
A golf ball at rest is struck by a club head moving at v_c = 50 m/s. The impact is highly inelastic in the center-of-mass frame, with effective e \approx 0.78.
Setup:
- Club mass: m_c = 200 g
- Ball mass: m_b = 46 g
- Mass ratio: \mu = m_b / m_c \approx 0.23
Center-of-mass velocity:
v_{cm} = \frac{m_c v_c}{m_c + m_b} \approx 0.81 v_c
Relative velocity before impact:
v_{rel}^- = v_c - 0 = v_c
Relative velocity after impact:
v_{rel}^+ = -e v_{rel}^- = -0.78 v_c
Ball velocity after impact. In the CM frame the ball reverses its approach speed scaled by e; transforming back to the lab frame,
v_b^+ = v_{cm} + \frac{m_c}{m_c + m_b}\,e\,v_c = \frac{(1 + e) m_c}{m_c + m_b}\,v_c \approx 1.45\,v_c
which is the standard inelastic-collision result:
v_b^+ = \frac{(1 + e) m_c v_c}{m_c + m_b} = \frac{1.78 \times 200 \times 50}{246} \approx 72.4 \text{ m/s} \approx 1.45\,v_c
The ball leaves at approximately 1.45 times the club speed, which matches empirical data for golf (smash factor).
Energy loss:
T^- = \frac{1}{2} m_c v_c^2 = 250 \text{ J}
T^+ = \frac{1}{2} m_c v_c^{'+2} + \frac{1}{2} m_b v_b^{'+2}
Using momentum conservation to find v_c^{'+}:
m_c v_c = m_c v_c^{'} + m_b v_b^{'}
v_c^{'} = v_c - \frac{m_b}{m_c} v_b^{'} \approx 50 - 0.23 \times 72.4 \approx 33.3 \text{ m/s}
T^+ \approx \frac{1}{2}(0.2)(33.3)^2 + \frac{1}{2}(0.046)(72.4)^2 \approx 111 + 121 = 232 \text{ J}
Energy dissipated: \Delta T \approx 18 J, about 7% loss. This is consistent with e = 0.78:
\frac{\Delta T}{T^-} = 1 - \frac{T^+}{T^-} \approx 1 - \frac{(1 + e)^2}{1 + m_b/m_c} \cdot \frac{m_b}{m_c}
(This formula is approximate; exact calculation involves solving the collision equations.)
14.2 Example 2: Spacecraft Docking
A spacecraft approaches a docking port at v = 0.1 m/s. The docking mechanism is a soft capture with e = 0.3 (highly dissipative to avoid rebound).
Before docking:
x^- = \begin{bmatrix} 0.01 \text{ m} \\ 0.1 \text{ m/s} \end{bmatrix}
After capture:
x^+ = \begin{bmatrix} 0 \\ -0.3 \times 0.1 \end{bmatrix} = \begin{bmatrix} 0 \\ -0.03 \text{ m/s} \end{bmatrix}
The spacecraft rebounds slightly (3 cm/s) before the latches engage. In practice, active damping (thrusters or mechanical dampers) further reduces this.
Saltation matrix:
For this 1D impact:
S = \begin{bmatrix} -e & 0 \\ -g(1+e)/v^- & -e \end{bmatrix}
With g = 0 (no gravity in space) and v^- = 0.1:
S = \begin{bmatrix} -0.3 & 0 \\ 0 & -0.3 \end{bmatrix}
Perturbations are scaled by -0.3 in both position and velocity.
15 Tangent Space Interpretation
The impact map acts on the tangent bundle T\mathcal{M} = \mathcal{M} \times \mathbb{R}^n (position and velocity together):
R: (q, v) \mapsto (q, R_v(v))
Position is continuous (q^+ = q^- at the guard), but velocity jumps. The linearization is:
\frac{\partial R}{\partial (q, v)} = \begin{bmatrix} I & 0 \\ \frac{\partial R_v}{\partial q} & R_v \end{bmatrix} \tag{49}
For configuration-independent restitution (e constant):
R_v = I - (1+e) P_N(q)
\frac{\partial R_v}{\partial q} = -(1+e) \frac{\partial P_N}{\partial q}
This derivative captures how the impact direction varies with configuration (e.g., foot angle changes during heel strike).
16 Chapter Summary
Impact maps are geometric transformations of tangent spaces:
- Restitution e: Ratio of normal velocities, causes oblique reflections
- Energy dissipation: Proportional to (1 - e^2) \|v_\perp\|^2
- Tangent space action: Squeeze/reflection in normal direction, preserve tangent direction
Examples (golf, docking) show realistic calculations. The saltation matrix linearizes these transformations, enabling optimization through impacts.
17 Chapter 6: Variational Dynamics Through Impacts
17.1 Jump in δx Across Guard Surface
We now ask: How do perturbations \delta x(t) evolve through a hybrid trajectory?
Within each mode, perturbations satisfy the standard variational equation:
\frac{d}{dt}\delta x = A(t) \delta x + B(t) \delta u \tag{50}
with solution:
\delta x(t) = \Phi_c(t, t_0) \delta x(t_0) + \int_{t_0}^t \Phi_c(t, s) B(s) \delta u(s) \, ds \tag{51}
At a jump (time t_j, guard G), the state jumps via x^+ = R(x^-). The perturbation jumps via the saltation matrix:
\boxed{\delta x^+ = S_j \delta x^-} \tag{52}
where S_j is given by Equation 27.
Full hybrid trajectory:
For a trajectory with jumps at t_1, \ldots, t_N, the perturbation evolves as:
\begin{aligned} \delta x(t) &= \Phi_c(t, t_N) S_N \Phi_c(t_N, t_{N-1}) \cdots S_1 \Phi_c(t_1, t_0) \delta x(t_0) \\ &\quad + \sum_{k=0}^{N-1} \Phi_c(t, t_{k+1}) S_{k+1} \cdots S_1 \int_{t_k}^{t_{k+1}} \Phi_c(\tau, t_k) B(\tau) \delta u(\tau) \, d\tau \end{aligned} \tag{53}
This is the hybrid state transition, generalizing Equation 18 to include jumps.
17.2 Modified State Transition Operator Φ
Define the hybrid state transition matrix as:
\Phi_{\text{hybrid}}(t, t_0) = \Phi_c(t, t_N) \prod_{j=1}^{N} S_j \Phi_c(t_j, t_{j-1}) \tag{54}
where the product runs backward (right-to-left) from the initial time to the final time.
Properties:
Composition: \Phi_{\text{hybrid}}(t_3, t_1) = \Phi_{\text{hybrid}}(t_3, t_2) \Phi_{\text{hybrid}}(t_2, t_1) if no jumps occur between t_2 and t_3; otherwise the jump must be included.
Inverse: If no guard is crossed, \Phi_{\text{hybrid}}^{-1} = \Phi_c^{-1}. If jumps occur, the inverse involves reversing the jumps (which may not be well-defined if S_j is singular).
Determinant: \det(\Phi_{\text{hybrid}}) = \det(\Phi_c) \prod_j \det(S_j). For impacts, \det(S_j) < 1 generically (volume contraction due to dissipation).
17.3 Accumulation of Variations in Hybrid Systems
A key question: Do perturbations grow or shrink through impacts?
Spectral radius of S_j:
The eigenvalues of the saltation matrix determine stability of perturbations. For the bouncing ball:
S = \begin{bmatrix} -e & 0 \\ -g(1+e)/v^- & -e \end{bmatrix}
Eigenvalues: \lambda_1 = \lambda_2 = -e. All eigenvalues have magnitude |e| < 1 for e < 1.
Conclusion: Perturbations decay through impacts. Each bounce stabilizes the trajectory (perturbations are damped).
Geometric interpretation:
Dissipative impacts contract the tangent space, creating a form of discrete-time stability. Even if the continuous dynamics are unstable (e.g., inverted pendulum), the impacts can stabilize the overall hybrid system.
17.3.1 Example: Bouncing Ball With Exponential Decay
Consider a ball dropped from height h_0 with e = 0.8. After each bounce:
v^+ = -e v^- = -e \sqrt{2gh}
The next bounce occurs at:
h_{n+1} = \frac{(v^+)^2}{2g} = e^2 h_n
The sequence of bounce heights is:
h_n = e^{2n} h_0
Perturbations in initial height also decay as e^{2n}:
\delta h_n = e^{2n} \delta h_0 \to 0
The system is asymptotically stable to the equilibrium (h, v) = (0, 0) (rest on ground), despite the continuous phase being neutral (constant velocity in flight).
17.4 Zeno Behavior and Measure Zero Jump Sets
An important pathology: Zeno behavior, where infinitely many jumps occur in finite time.
Example: Bouncing ball with e < 1:
The time between bounces decreases geometrically:
T_n = \frac{2v_n}{g} = \frac{2e^n v_0}{g}
The total time to rest is:
T_{\text{total}} = \sum_{n=0}^\infty T_n = \frac{2v_0}{g} \sum_{n=0}^\infty e^n = \frac{2v_0}{g(1 - e)} < \infty
The ball executes infinitely many bounces in finite time, accumulating at t = T_{\text{total}}.
What happens to variational dynamics?
The state transition through infinitely many jumps is:
\Phi_{\text{Zeno}} = \lim_{N \to \infty} \prod_{j=1}^N S_j
If \|S_j\| < 1 uniformly, this product converges to zero:
\Phi_{\text{Zeno}} = 0
Perturbations are completely damped by the infinite sequence of dissipative impacts.
Measure-theoretic resolution:
The set of jump times \{t_j\} has measure zero. The trajectory x(t) is:
- Càdlàg: Right-continuous with left limits (French: continu à droite, limites à gauche)
- Bounded variation: Total variation \sum_j \|x^+ - x^-\| < \infty
- Absolutely continuous on each mode: Standard ODE solution between jumps
The derivative \dot{x}(t) exists almost everywhere (Lebesgue), and integrals are well-defined:
\int_0^T L(x(t), u(t)) \, dt = \int_0^T L(x(t), u(t)) \, dt \quad (\text{Lebesgue})
Jump times contribute zero measure, so they can be ignored in cost functionals.
17.5 Chapter Summary
Variational dynamics through hybrid systems involve:
- Smooth propagation within modes: \delta\dot{x} = A\delta x + B\delta u
- Jumps at guards: \delta x^+ = S_j \delta x^-
- Hybrid state transition: Product \Phi_c \cdot S_j \cdot \Phi_c \cdots
Impacts can stabilize or destabilize depending on \|S_j\|:
- \|S_j\| < 1: Perturbations decay (e.g., dissipative collisions)
- \|S_j\| > 1: Perturbations grow (e.g., energy-adding mechanisms)
Zeno behavior (infinitely many jumps) is measure-theoretically well-behaved: jumps have measure zero, integrals are finite, and perturbations may converge to zero.
Part III: Optimization
18 Chapter 7: Mode-Aware DDP
18.1 Handling Known Guard Crossings
Standard DDP assumes smooth dynamics. To handle hybrid systems, we must account for jumps in the backward pass.
Setup:
- Nominal trajectory \bar{x}(t), \bar{u}(t) with known jump times t_1, \ldots, t_N
- Cost functional: J = \Phi(x(T)) + \sum_{k=0}^{N-1} \int_{t_k}^{t_{k+1}} L(x, u) \, dt {#hs-eq-hybrid-cost-ddp}
- Goal: Compute feedback gains K(t) and feedforward k(t) to minimize J under perturbations
Key modification: At each jump, the value function must account for the discontinuity.
18.2 Backward Pass With Saltation Matrices
The DDP backward pass computes the cost-to-go:
V(x, t) = \min_{u(\cdot)} \left\{ \int_t^T L(x(\tau), u(\tau)) \, d\tau + \Phi(x(T)) \right\} {#hs-eq-value-function}
In the smooth case, V satisfies the Hamilton-Jacobi-Bellman (HJB) equation:
-\frac{\partial V}{\partial t} = \min_u \left\{ L(x, u) + \frac{\partial V}{\partial x}^T f(x, u) \right\} \tag{55}
At a jump (guard crossing at time t_j), the value function is discontinuous:
V(x^-, t_j^-) = V(R(x^-), t_j^+) + 0 \tag{56}
(We assume no instantaneous cost at jumps; this can be generalized.)
The gradient of V jumps according to:
\boxed{\frac{\partial V}{\partial x}\bigg|_{t_j^-} = S_j^T \frac{\partial V}{\partial x}\bigg|_{t_j^+}} \tag{57}
This is the adjoint jump condition: the costate (gradient of value) is pulled back through the transpose of the saltation matrix.
18.2.1 Derivation
By the chain rule:
\frac{\partial V}{\partial x}\bigg|_{x^-} = \frac{\partial V}{\partial x^+} \frac{\partial x^+}{\partial x^-} = \frac{\partial V}{\partial x^+} S_j
This gives \frac{\partial V}{\partial x}|_{x^-} = \frac{\partial V}{\partial x}|_{x^+} S_j, which is \lambda^- = \lambda^+ S_j.
Transposing: (\lambda^-)^T = S_j^T (\lambda^+)^T, or as column vectors:
\lambda^- = S_j^T \lambda^+
So Equation 57 is correct.
18.3 Backward Pass Algorithm
Inputs:
- Nominal trajectory \bar{x}(t), \bar{u}(t) for t \in [0, T]
- Jump times t_1, \ldots, t_N and saltation matrices S_1, \ldots, S_N
- Cost function L(x, u), terminal cost \Phi(x_T)
Initialization:
At t = T:
V_x(T) = \frac{\partial \Phi}{\partial x}\bigg|_{x_T}, \quad V_{xx}(T) = \frac{\partial^2 \Phi}{\partial x^2}\bigg|_{x_T}
Backward integration (starting from t = T):
For k = N, N-1, \ldots, 0:
Integrate backward from t_{k+1} to t_k (within mode k):
Solve the Riccati-like equations: \begin{aligned} \dot{V}_x &= -L_x - A^T V_x \\ \dot{V}_{xx} &= -L_{xx} - A^T V_{xx} - V_{xx} A - Q_{xu}^T Q_{uu}^{-1} Q_{ux} \end{aligned} \tag{58}
where Q_{uu}, Q_{ux}, Q_{xu} come from the Hamiltonian second derivatives (standard DDP).
At jump time t_k (if k > 0):
Apply the adjoint jump: V_x(t_k^-) = S_k^T V_x(t_k^+) \tag{59}
V_{xx}(t_k^-) = S_k^T V_{xx}(t_k^+) S_k \tag{60}
Output:
- Feedback gains K(t) = -Q_{uu}^{-1} Q_{ux}
- Feedforward k(t) = -Q_{uu}^{-1} Q_u
These are defined piecewise on each interval [t_k, t_{k+1}], with discontinuities at t_k.
Interpretation:
The backward pass “threads” through the jumps by pulling back the value gradient via S_j^T. This ensures that the feedback gains correctly account for how perturbations propagate through impacts.
18.4 Convergence Theory for Hybrid Systems
Question: Does hybrid DDP converge to a local optimum?
Answer (under regularity conditions): Yes, with quadratic convergence near the optimum.
Assume:
- The nominal trajectory \bar{x}(t) is feasible (satisfies guard crossing conditions)
- The dynamics f_q(x, u) are C^2 within each mode
- The reset maps R_j(x) are C^2
- The guard functions h_j(x) are C^2 with \nabla h_j \neq 0 (transversal crossing)
- The cost Hessian Q_{uu} \succ 0 (positive definite) along the trajectory
- The saltation matrices S_j are non-singular (invertible)
Then:
- Local convergence: If initialized sufficiently close to the optimum, hybrid DDP converges
- Quadratic rate: Near the optimum, the error decreases as \|x^{(k+1)} - x^*\| = O(\|x^{(k)} - x^*\|^2)
- Necessity: At convergence, the first-order necessary conditions (hybrid Pontryagin) are satisfied
Caveat: If S_j is singular (e.g., impacts into a lower-dimensional manifold), convergence may be slower or fail.
Sketch of proof:
The hybrid cost functional J(x, u) is C^2 piecewise. The DDP update is a Newton-like method on the first-order necessary conditions. Near the optimum, the Hessian is well-approximated by V_{xx}, and the update is quadratic.
The saltation matrices S_j act as linear “connectors” between modes. If S_j is invertible, perturbations can propagate freely backward and forward, preserving the Newton structure.
If S_j is singular, some perturbation directions are “lost” at the jump, breaking the Newton step. Regularization (e.g., adding \mu I to S_j) can restore convergence.
18.5 Example: Hopping Robot
A simple hopper (one leg, mass m, spring k) has two modes:
- Flight: \ddot{z} = -g
- Contact: \ddot{z} = (k/m)(L_0 - z) - g, where L_0 is rest length
Guards:
- Flight \to Contact: z = L_0
- Contact \to Flight: F_{\text{spring}} = 0 (toe-off)
Reset: At touchdown, velocity is continuous (no impact for soft landing). At takeoff, also continuous.
DDP setup:
- State: x = [z, \dot{z}]^T
- Control: u = L_0 (virtual leg length, controls takeoff)
- Cost: Minimize energy while tracking desired height
Saltation matrices:
At touchdown (z = L_0):
S_{\text{touchdown}} = I + \frac{(f^+ - f^-) \nabla h^T}{\nabla h^T f^-}
Since velocity is continuous, f^+ \approx f^- (no jump), so S \approx I (small correction for time variation).
At takeoff, similar analysis.
Result: Hybrid DDP converges in 5-10 iterations to a periodic hopping gait with desired apex height. The backward pass correctly handles the mode switches, producing stabilizing feedback gains.
19 Chapter 8: Contact-Implicit Trajectory Optimization
20 Complementarity Constraints
An alternative to explicit guards is contact-implicit optimization, where contact forces are decision variables subject to complementarity constraints.
For a single contact point with gap \phi(q) \geq 0 and normal force \lambda \geq 0:
\boxed{0 \leq \phi(q) \perp \lambda \geq 0} {#hs-eq-complementarity}
This means:
- Either \phi = 0 (contact) or \lambda = 0 (no force)
- Both cannot be nonzero simultaneously
The complementarity condition is written as:
\phi(q) \lambda = 0, \quad \phi(q) \geq 0, \quad \lambda \geq 0 \tag{61}
This is a nonlinear, non-smooth constraint (product equals zero).
Dynamics with contacts:
The equations of motion become:
M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = B u + J_c^T(q) \lambda \tag{62}
where J_c(q) is the contact Jacobian (maps joint velocities to contact point velocity).
The contact force \lambda is an algebraic variable (determined by constraints, not integrated).
21 Smoothing Methods vs. Exact Jumps
To make complementarity constraints tractable in optimization, several approaches exist:
21.1 1. Smoothing via Sigmoid
Replace the hard constraint \phi \lambda = 0 with a smooth approximation:
\phi \lambda \leq \epsilon \tag{63}
or use a barrier function:
\mu \sum_i \log(\phi_i) + \log(\lambda_i) \tag{64}
As \mu \to 0, this approaches the complementarity condition.
Pros: Smooth optimization (gradient-based solvers work)
Cons: Requires tuning \epsilon or \mu; approximate solution
21.2 2. Exact Jumps via Mode Enumeration
Explicitly enumerate contact modes (e.g., {contact, flight}) and solve a trajectory optimization for each sequence of modes.
Pros: Exact representation of hybrid dynamics
Cons: Combinatorial explosion (2^N mode sequences for N potential contacts)
21.3 3. Contact-Implicit (Posa, Kuindersma, Tedrake)
Discretize time and enforce complementarity at each timestep via:
\phi_k \geq 0, \quad \lambda_k \geq 0, \quad \phi_k \lambda_k \leq \epsilon \tag{65}
Use nonlinear programming (NLP) solvers with constraint relaxation.
Pros: Handles multiple contacts, does not require mode enumeration
Cons: Non-convex (multiple local minima), requires warm-starting
22 Trade-Offs and Robustness
Contact-implicit advantages:
- Discovers contact timings automatically (no manual guard placement)
- Handles simultaneous contacts (e.g., four-legged walking)
- Smooth objective (no discrete jumps in cost)
Contact-implicit challenges:
- Non-convex: Many local minima (e.g., step over vs. step on obstacle)
- Initialization-sensitive: Poor guesses may converge to infeasible solutions
- Slack variables: Complementarity slack \phi \lambda \leq \epsilon introduces error
Hybrid DDP advantages:
- Quadratic convergence near optimum (Newton-like)
- Explicit guard handling: Physics is directly represented
- Geometric insight: Saltation matrices have clear interpretation
Hybrid DDP challenges:
- Requires known guard crossings: Must detect/predict contact times
- Mode enumeration: Need to know contact sequence a priori
- Guard smoothness: Assumes h(x) is C^2 and regular
Recommendation:
Use contact-implicit for initial guess generation (exploratory phase), then refine with hybrid DDP for final policy (exploitation phase).
23 Example: Block Pushing
A robot pushes a block across a table. Contact modes:
- Free: Block slides freely (friction < push force)
- Stuck: Block sticks (friction \geq push force)
Contact-implicit formulation:
Decision variables: (q(t), \dot{q}(t), u(t), \lambda(t)) for t \in [0, T]
Constraints:
- Dynamics: M\ddot{q} = Bu + J_c^T \lambda
- Complementarity: \phi(q) \lambda = 0, \lambda \in [0, \mu N] (friction cone)
- Boundary: q(0) = q_0, q(T) = q_{\text{goal}}
Cost: Minimize effort \int u^T u \, dt
Solution:
The optimizer automatically discovers:
- Phase 1: Ramp up push force until \lambda = \mu N (break static friction)
- Phase 2: Slide at constant velocity (kinetic friction)
- Phase 3: Ramp down, block sticks again
No manual mode switching needed.
24 Chapter 9: Applications
24.1 Humanoid Locomotion (Stance/Swing Phases)
System: 5-link biped (torso + 2 legs, point feet)
State: x = [q, \dot{q}]^T \in \mathbb{R}^{10}, where q = [\theta_{\text{torso}}, \theta_{\text{hip1}}, \theta_{\text{knee1}}, \theta_{\text{hip2}}, \theta_{\text{knee2}}]
Modes:
- Single support (stance): One foot on ground, contact force \lambda
- Double support: Both feet on ground (brief transition)
- Flight: Both feet in air (running)
Guards:
- Heel strike: h_{\text{foot}}(q) = 0 (foot height)
- Toe-off: \lambda = 0 (contact force vanishes)
Reset map at heel strike:
Angular momentum conservation about new contact point:
L^+ = L^- \quad \Rightarrow \quad I^+ \dot{q}^+ = I^- \dot{q}^-
where I is the inertia about the contact. Solving for \dot{q}^+:
\dot{q}^+ = (I^+)^{-1} I^- \dot{q}^-
The saltation matrix is:
S_{\text{heel}} = \begin{bmatrix} I & 0 \\ 0 & (I^+)^{-1} I^- \end{bmatrix} + \text{(time variation correction)} \tag{66}
DDP formulation:
Cost: \int (q - q_{\text{ref}})^T Q (q - q_{\text{ref}}) + u^T R u \, dt + \text{terminal cost}
Backward pass:
- Integrate V_x, V_{xx} backward through swing phase
- Apply V_x^- = S_{\text{heel}}^T V_x^+ at heel strike
- Integrate backward through stance phase
- Repeat for multiple steps
Result:
Hybrid DDP generates a stabilizing walking gait in 10-15 iterations. The feedback gains K(t) vary through the gait cycle, increasing during stance (when contact provides control authority) and decreasing during swing (when only hip/knee torques are available).
Comparison to contact-implicit:
Contact-implicit can discover novel gaits (e.g., skipping, galloping) but struggles with local minima. In the examples discussed here, Hybrid DDP converges faster once a feasible gait pattern is supplied and makes the local stability discussion more explicit.
24.2 Golf Swing (Club-Ball Impact)
System: Club (rigid body) + ball (point mass)
State: x = [q_{\text{club}}, \dot{q}_{\text{club}}, x_{\text{ball}}, v_{\text{ball}}]^T
Phases:
- Backswing: Club rotates, ball stationary
- Downswing: Club accelerates toward ball
- Impact: Club and ball collide (duration \sim 0.5 ms)
- Follow-through: Club decelerates, ball in flight
Impact model:
Coefficient of restitution: e = 0.78
Momentum conservation:
m_c v_c^- + m_b v_b^- = m_c v_c^+ + m_b v_b^+
Restitution:
v_b^+ - v_c^+ = -e(v_b^- - v_c^-)
Solving:
v_b^+ = \frac{m_c(1 + e)}{m_c + m_b} v_c^- + \frac{m_b - e m_c}{m_c + m_b} v_b^- \tag{67}
For stationary ball (v_b^- = 0):
v_b^+ = \frac{m_c(1 + e)}{m_c + m_b} v_c^- \approx 1.48 v_c^- \tag{68}
Optimization goal:
Maximize ball distance d = \frac{v_b^{'+2} \sin(2\alpha)}{g} subject to:
- Joint torque limits
- Timing constraints (swing duration)
- Impact accuracy (club face angle \alpha)
Saltation matrix:
The impact Jacobian is:
P_{\text{impact}} = \begin{bmatrix} I & 0 & 0 & 0 \\ 0 & \frac{\partial v_c^+}{\partial v_c^-} & 0 & 0 \\ 0 & 0 & I & 0 \\ 0 & \frac{\partial v_b^+}{\partial v_c^-} & 0 & \frac{\partial v_b^+}{\partial v_b^-} \end{bmatrix} \tag{69}
where:
\frac{\partial v_b^+}{\partial v_c^-} = \frac{m_c(1+e)}{m_c + m_b}, \quad \frac{\partial v_b^+}{\partial v_b^-} = \frac{m_b - e m_c}{m_c + m_b}
Illustrative optimization result:
In the worked example, Hybrid DDP converges in 8-12 iterations for the chosen initialization and model settings. A safer interpretation is that the optimizer reduces active torque before impact to let the modeled release unfold, rather than that the clubhead should decelerate to maximize energy transfer. Any stronger golf claim would require the objective definition, parameter values, and validation plots.
24.3 Manipulation With Contact Switches
System: Robot arm + object on table
Task: Pick and place with contact-rich interaction
Modes:
- Pre-grasp: Arm approaches object
- Contact: Fingertip touches object (force increases)
- Grasp: Fingers close, object lifts
- Transport: Arm moves object to target
- Release: Fingers open, object settles
Contact model:
At each finger-object contact:
- Gap function: \phi_i(q) = \text{dist}(\text{finger}_i, \text{object})
- Normal force: \lambda_i \geq 0
- Friction cone: \|\lambda_{i,t}\| \leq \mu \lambda_{i,n}
Complementarity:
\phi_i \lambda_{i,n} = 0, \quad \phi_i \geq 0, \quad \lambda_{i,n} \geq 0 \tag{70}
Hybrid formulation:
Explicit modes:
- Mode 1: No contact (\phi_i > 0, \lambda_i = 0)
- Mode 2: Contact established (\phi_i = 0, \lambda_i > 0)
Guards:
- G_{1 \to 2}: \phi_i = 0, \dot{\phi}_i \leq 0 (approach)
- G_{2 \to 1}: \lambda_i = 0 (release)
DDP optimization:
Cost: Minimize time + effort + impact forces
Backward pass accounts for:
- Saltation at contact establishment
- Saltation at release
- Force constraints during contact
Result:
Generates smooth pick-place trajectories with minimal impact transients. The hybrid formulation ensures forces remain within friction cone limits.
24.4 Chapter Summary
Applications demonstrate hybrid tangent space framework in:
- Locomotion: Heel strikes stabilized by saltation matrix feedback
- Golf swing: Impact modeled rigorously, counter-intuitive optimal strategy discovered
- Manipulation: Contact modes handled explicitly, smooth force profiles achieved
The recurring theme: Explicit treatment of discontinuities via guards, resets, and saltation matrices produces better results than smoothing or ignoring.
Part IV: Implementation and Extensions
25 Chapter 10: Practical Algorithms
25.1 Guard Detection and Event Handling
Problem: Given a trajectory integration from t_0 to t_f, detect when a guard h(x) = 0 is crossed.
Naïve approach:
Check h(x_k) at each timestep t_k. If h(x_k) < 0 and h(x_{k+1}) > 0, a crossing occurred.
Issue: The crossing time t^* is between t_k and t_{k+1}. Using t_{k+1} introduces O(\Delta t) error.
Better approach: Event detection via root-finding
- Detect bracketing: Find t_k, t_{k+1} such that h(x(t_k)) \cdot h(x(t_{k+1})) < 0
- Refine crossing time:
- Integrate from t_k to t_m = (t_k + t_{k+1})/2
- Evaluate h(x(t_m))
- If h(x(t_k)) \cdot h(x(t_m)) < 0, set t_{k+1} \gets t_m, else set t_k \gets t_m
- Repeat until |h(x(t_m))| < \epsilon
- Apply reset: x^+ = R(x(t_m))
- Continue integration from t_m
Computational cost: Each bisection requires one integration step. Typically 5-10 bisections achieve \epsilon \sim 10^{-8}.
Alternative: Rootfinding with Newton
If \nabla h is available, use Newton’s method on h(x(t)) = 0:
t_{k+1} = t_k - \frac{h(x(t_k))}{\frac{d}{dt}h(x(t_k))}
where:
\frac{d}{dt}h(x(t)) = \nabla h^T \dot{x} = \nabla h^T f(x, u)
This converges quadratically (2-3 iterations).
25.2 JAX Implementation Considerations
JAX (Google’s autodiff framework) is ideal for implementing hybrid DDP due to its:
- Automatic differentiation: Computes Jacobians A, B and Hessians automatically
- JIT compilation: Speeds up integration and optimization loops
- GPU/TPU support: Parallelizes mode sequences
Challenge: Discontinuities and autodiff
JAX’s autodiff assumes smooth functions. At a jump, the derivative is undefined. How do we handle this?
Solution: Custom VJP (vector-Jacobian product)
Define the forward pass (primal):
def forward_with_jump(x0, u, t_jump, S):
# Integrate to t_jump
x_minus = integrate(x0, u, 0, t_jump)
# Apply reset
x_plus = jnp.dot(S, x_minus)
# Continue to final time
x_final = integrate(x_plus, u, t_jump, T)
return x_finalDefine the backward pass (tangent):
def backward_with_jump(residuals, g):
# residuals = (x_minus, x_plus, S)
# g = gradient from downstream
# Backprop through second integration
g_plus = integrate_backward(g, ...)
# Pull back through saltation: g_minus = S^T g_plus
g_minus = jnp.dot(S.T, g_plus)
# Backprop through first integration
g_x0 = integrate_backward(g_minus, ...)
return g_x0Register as custom VJP:
from jax import custom_vjp
@custom_vjp
def hybrid_rollout(x0, u, jumps):
return forward_with_jump(x0, u, jumps)
hybrid_rollout.defvjp(forward_with_jump, backward_with_jump)This allows JAX to differentiate through the jump correctly, using S^T in the backward pass.
25.3 Debugging Hybrid Optimizations
Common failure modes:
Bisection fails to converge: Guard function h(x) may have multiple zeros in the interval (e.g., oscillations). Use narrower timesteps or filter h before root-finding.
Saltation matrix is singular: Reset map R collapses dimensionality (e.g., all points on guard map to fixed set). Solution: Regularize S \gets S + \epsilon I or reformulate guard.
DDP diverges after jump: Q_{uu} may lose positive definiteness due to S_j. Solution: Add regularization Q_{uu} \gets Q_{uu} + \mu I at jumps.
Zeno behavior detected: Too many jumps in short time (e.g., chattering contact). Solution: Use time-stepping with collision detection (semi-implicit Euler) or increase dissipation.
Debugging tools:
- Plot h(t): Visualize guard crossings, check for spurious zero-crossings
- Plot \det(S_j): Check if saltation is volume-preserving (det = 1) or contractive (det < 1)
- Plot eigenvalues of V_{xx}: Ensure Hessian remains positive definite
- Animate trajectory: Visual inspection often reveals mode sequencing errors
25.4 Example Code: Bouncing Ball in JAX
import jax.numpy as jnp
from jax import jit, grad
from jax.experimental.ode import odeint
# Dynamics: free fall
def f(x, t, u):
h, v = x
return jnp.array([v, -9.81])
# Guard function
def guard(x):
return x[0] # h = 0
# Reset map
def reset(x, e=0.8):
h, v = x
return jnp.array([0.0, -e * v])
# Saltation matrix
def saltation_matrix(x_minus, e=0.8):
h, v = x_minus
# From equation (eq-ball-saltation)
S = jnp.array([
[-e, 0],
[-9.81 * (1 + e) / v, -e]
])
return S
# Event detection (simplified: assume known t_jump)
def simulate_bounce(x0, T, t_jump, e=0.8):
# Phase 1: fall until guard
t1 = jnp.linspace(0, t_jump, 100)
x_minus = odeint(f, x0, t1, 0.0)[-1]
# Apply reset
x_plus = reset(x_minus, e)
# Phase 2: continue after bounce
t2 = jnp.linspace(t_jump, T, 100)
x_final = odeint(f, x_plus, t2, 0.0)
return x_final
# Usage
x0 = jnp.array([1.0, 0.0]) # Drop from h=1m
trajectory = simulate_bounce(x0, T=2.0, t_jump=0.45)Note: This example assumes known t_jump. A full implementation would use bisection or Newton’s method to find it.
25.5 Chapter Summary
Implementing hybrid trajectory optimization requires:
- Event detection: Bisection or Newton rootfinding to locate guard crossings accurately
- Autodiff handling: Custom VJP in JAX to differentiate through jumps using S^T
- Debugging: Check guard functions, saltation determinants, Hessian conditioning
Code examples (bouncing ball in JAX) demonstrate practical implementation patterns.
26 Conclusion and Extensions
27 What We’ve Achieved
This article extends the Tangent Hyperplane framework from smooth C^1 dynamics to hybrid systems with:
- Impacts: Velocity discontinuities at collisions (bouncing balls, foot strikes)
- Mode switches: Discrete transitions between dynamical regimes (stance/swing, contact/flight)
- Complementarity: Force constraints with inequality conditions (friction cones)
The key geometric insight: Tangent spaces remain well-defined on either side of jumps, and perturbations map linearly via the saltation matrix S_j. This preserves the core principle of the framework—linearization is exact infinitesimally—even when trajectories themselves are non-smooth.
28 Relationship to Main Thesis
The main Tangent Hyperplane thesis states that linearization is the exact local structure of smooth dynamics. This article clarifies the scope of that claim:
- Within each mode: The statement holds exactly (smoothness assumed)
- At jumps: The statement must be refined to “left and right tangent spaces” with a linear connector S_j
- Overall: Hybrid systems admit piecewise linear tangent space structure
This addresses Critique 2 from the critical review: we now explicitly state where smoothness is required and provide rigorous tools for where it fails.
29 Open Questions and Future Work
- Optimal mode sequencing: How to choose which guards to cross? (Combinatorial optimization + DDP)
- Stochastic hybrid systems: How do perturbations propagate when jump times are random?
- Measure-theoretic gradients: Can we define Sobolev derivatives for hybrid trajectories?
- Infinite-dimensional hybrid systems: PDEs with moving boundaries (Stefan problems)
30 Further Reading
Hybrid systems theory:
- Goebel, Sanfelice, Teel (2012): Hybrid Dynamical Systems
- Lygeros et al. (2003): “Controllers for reachability specifications for hybrid systems”
Contact mechanics:
- Stewart & Trinkle (1996): “An implicit time-stepping scheme for rigid body dynamics with inelastic collisions”
- Anitescu & Potra (1997): “Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems”
Trajectory optimization:
- Posa, Kuindersma, Tedrake (2016): “Optimization and stabilization of trajectories for constrained dynamical systems”
- Manchester & Kuindersma (2017): “Variational contact-implicit trajectory optimization”
Saltation matrices:
- Burden, Sastry, et al. (2015): “The role of saltation matrices in hybrid system analysis”
<div class="laymans-terms-inner">
<p class="laymans-terms-intro">
This article tackles the math of "sudden changes"—like a ball bouncing or a foot hitting the ground—where standard physics equations break down because things happen instantly.
</p>
<div class="laymans-item">
<h3>The Smooth vs. The Snappy (Hybrid Systems)</h3>
<p>
Most engineering math assumes things change smoothly, like a dimmer switch. But the real world has "clicks" and "bangs"—hybrid systems mix smooth motion with sudden jumps. We need special tools to handle the "thud" of an impact differently than the "glide" of flight.
</p>
<div class="analogy"></div>
<div class="laymans-item">
<h3>The Magic Lens (Saltation Matrices)</h3>
<p>
When a system jumps, small errors in speed or timing get scrambled. We use a "saltation matrix" to predict exactly how those errors survive the crash, allowing us to control the system even through impacts.
</p>
<div class="analogy">