Control and Dynamics

This appendix collects the control and dynamics models used throughout this book. Their guarantees apply only within a declared state, actuator, disturbance, and timing envelope.

How to Use This Appendix

Consult this appendix when encountering specific physical systems bottlenecks, control interface boundaries, or design sizing decisions across chapters in this book:

When you encounter this systems symptom	Consult	For this quantitative tool
A learned policy commands unsafe actions near physical obstacles or joint limits	section 1.2	Control Barrier Function (CBF-QP) safety filters and kinetic stopping insets
Mode switching (policy handover, human intervention, or fallback) causes torque spikes	section 1.3	\(C^2\) quintic splines, \(C^1\) cubic Hermite interpolation, and bumpless transfer bounds
Sizing the validity horizon, spatial lease, or watchdog timeout for an action chunk	section 1.4	Triangular, trapezoidal, and jerk-limited kinematic reachability envelopes
An articulated manipulator stalls, exceeds joint velocity limits, or loses control authority	section 1.4.1	Manipulator Jacobian singularities and Yoshikawa manipulability index
Spatial sensor offsets or IMU-camera lever arms corrupt state estimation under rotation	section 1.4.3	Spatial coordinate transforms (\(SE(3)\)) and lever-arm acceleration propagation
Discrete sampling and transport latency erode control loop stability margins	section 1.3.5	Discrete zero-order hold (ZOH) phase lag and latency margin penalties
Physics simulation diverges, impacts exceed limits, or wheels spin out during maneuvers	section 1.5	Contact complementarity, elastodynamic impact, and wheel slip boundaries
Sensor occlusions or unobservable states cause state estimation uncertainty to diverge	section 1.6	Continuous Lyapunov covariance propagation and Lie derivative observability rank
Teleoperation or compliant contact chatters due to transport delay	section 1.8	Passivity conditions, wave variables, and energy-bounded interaction
Underactuated legged or humanoid robots balance dynamic momentum across multi-contact points	section 1.7	Centroidal momentum equations and hierarchical Whole-Body Control QP

Safety and Control Barrier Functions

An independent enforcer can check a learned policy proposal against a control barrier constraint before actuation. A Control Barrier Function (CBF) describes a candidate invariant set; its protection depends on observed state, feasible input, plant model, and sampled response time. Torque ceilings and obstacle clearance may require separate constraints.

Let the system state be \(x \in \mathcal{X} \subset \mathbb{R}^n\) and the control input be \(u \in \mathcal{U} \subset \mathbb{R}^m\). Suppose we define a safe operating set \(\mathcal{S}\) as the superlevel set of a continuously differentiable scalar function \(h(x)\): \[ \mathcal{S} = \{ x \in \mathcal{X} \mid h(x) \ge 0 \}, \quad \partial \mathcal{S} = \{ x \in \mathcal{X} \mid h(x) = 0 \} \]

\(h(x) > 0\) when the system is strictly inside the safe interior.
\(h(x) = 0\) on the boundary of the safe set.
\(h(x) < 0\) when the system violates safety.

Lie derivatives and forward invariance

Forward invariance means a trajectory starting in \(\mathcal{S}\) remains there for the modeled time interval. This claim requires an initial state in \(\mathcal{S}\) and an admissible control at every reachable state.

For a continuously differentiable relative-degree-one barrier under continuous-time control, one sufficient condition is:¹ \[ \dot{h}(x, u) \ge -\alpha h(x) \] where \(\alpha > 0\) is a chosen gain in \(\text{s}^{-1}\), not a measured braking bandwidth.

When \(h(x)\) is large, the condition permits some decrease. At \(h(x)=0\), it requires \(\dot h\ge0\) in the continuous model. The selected barrier must first encode the plant’s momentum and available braking; raw distance cannot be regulated directly by torque in a second-order plant.

For a control-affine dynamical system \(\dot{x} = f(x) + g(x)u\), the time derivative \(\dot{h}\) is evaluated via chain-rule directional derivatives (Lie derivatives): \[ \dot{h}(x, u) = \frac{\partial h}{\partial x} \dot{x} = \frac{\partial h}{\partial x} f(x) + \frac{\partial h}{\partial x} g(x) u = L_f h(x) + L_g h(x) u \] where \(L_f h(x) \in \mathbb{R}\) represents the unforced drift of clearance (momentum, gravity), and \(L_g h(x) \in \mathbb{R}^{1 \times m}\) represents the directional control effectiveness matrix.

Nagumo’s viability theorem and contingent cones

The geometric foundation of set invariance for dynamic systems is Nagumo’s Viability Theorem (Blanchini 1999). Let \(\mathcal{S} \subset \mathbb{R}^n\) be a non-empty closed set. The Bouligand contingent cone (or tangent cone) \(T_{\mathcal{S}}(x)\) to \(\mathcal{S}\) at a point \(x \in \mathcal{S}\) is defined as: \[ T_{\mathcal{S}}(x) = \left\{ v \in \mathbb{R}^n \;\middle|\; \liminf_{\tau \to 0^+} \frac{\text{dist}(x + \tau v, \mathcal{S})}{\tau} = 0 \right\} \] where \(\text{dist}(y, \mathcal{S}) = \inf_{z \in \mathcal{S}} \|y - z\|\). Nagumo’s theorem proves that for an autonomous system \(\dot{x} = f(x)\) with locally Lipschitz vector field \(f\), the set \(\mathcal{S}\) is forward-invariant if and only if the velocity vector satisfies the subtangentiality condition: \[ f(x) \in T_{\mathcal{S}}(x) \quad \forall x \in \mathcal{S} \] In the interior \(\text{int}(\mathcal{S})\), \(T_{\mathcal{S}}(x) = \mathbb{R}^n\), so the condition is trivially satisfied. On the boundary \(\partial \mathcal{S}\) of a smooth set defined by the zero-superlevel set of \(h(x)\) (\(\mathcal{S} = \{x \mid h(x) \ge 0\}\) with \(\nabla h(x) \neq \mathbf{0}\)), the inward normal is \(\nabla h(x)\). The contingent cone becomes the closed half-space pointing into or along the tangent plane of the safe set: \[ T_{\mathcal{S}}(x) = \left\{ v \in \mathbb{R}^n \;\middle|\; \nabla h(x)^\top v \ge 0 \right\} \quad \forall x \in \partial \mathcal{S} \] Consequently, the system cannot exit the set if and only if \(\dot{h}(x) = \nabla h(x)^\top f(x) \ge 0\) whenever \(h(x) = 0\).

Blanchini, Franco. 1999. “Set Invariance in Control.” Automatica 35 (11): 1747–67.

Ames, Aaron D, Samuel Coogan, Magnus Egerstedt, Gennaro Notomista, Koushil Sreenath, and Paulo Tabuada. 2019. “Control Barrier Functions: Theory and Applications.” European Control Conference (ECC), 3420–31.

Control Barrier Functions (Ames et al. 2019) extend Nagumo’s subtangentiality to controlled affine systems \(\dot{x} = f(x) + g(x)u\). For safety, there must exist an admissible control \(u \in \mathcal{U}\) such that \(f(x) + g(x)u \in T_{\mathcal{S}}(x)\). By introducing an extended class \(\mathcal{K}_\infty\) decay rate \(\alpha(h(x))\) satisfying \(\alpha(0) = 0\), the CBF inequality: \[ \dot{h}(x, u) = L_f h(x) + L_g h(x)u \ge -\alpha(h(x)) \] ensures that as the state approaches the boundary (\(h(x) \to 0\)), the admissible decay rate contracts to zero (\(-\alpha(h(x)) \to 0\)). At the boundary \(h(x) = 0\), it strictly enforces \(\dot{h}(x, u) \ge 0\), directly preserving Nagumo’s subtangentiality invariant and preventing trajectory exit into the unsafe set.

Relative degree and kinetic stopping insets

The relative degree \(r\) of a barrier function \(h(x)\) is the number of times it must be differentiated with respect to time before the control input \(u\) explicitly appears: \[ L_g L_f^k h(x) = 0 \quad \forall k < r - 1, \quad \text{and} \quad L_g L_f^{r-1} h(x) \neq 0 \] For a one-dimensional approach to a stationary obstacle, let raw clearance be \(h_r=D\), approach speed \(v>0\), and commanded approach acceleration \(a_{\rm cmd}\). Then \(\dot D=-v\) and \(\ddot D=-a_{\rm cmd}\). Acceleration enters raw clearance only at the second derivative, so \(h_r\) has relative degree two with respect to this input. One approach uses a kinetic inset. In a one-dimensional approach to a stationary obstacle, let \(D\) be measured clearance, \(v>0\) approach speed, and \(a_{\text{brake}}>0\) a validated lower bound on available braking deceleration for the operating state. Then: \[ h_k(D,v)=D-\frac{v^2}{2a_{\text{brake}}},\qquad \dot D=-v,\quad \dot v=a_{\rm cmd}. \] For immediate commanded acceleration, \(\dot h_k=-v-(v/a_{\text{brake}})a_{\rm cmd}\). Thus \(\dot h_k+\gamma h_k\ge0\) gives the affine constraint \(a_{\rm cmd}\le(a_{\text{brake}}/v)(\gamma h_k-v)\) for \(v>0\). At standstill, use a separate start/clearance rule. State error, sampling, and brake-onset delay need additional clearance reserves; the continuous calculation alone does not cover them.

Minimal-intervention quadratic program

For a barrier \(h_b\) whose first derivative actually depends on \(u\), the enforcer can choose a command near the nominal proposal while respecting the feasible input set: \[ \begin{align*} u^* &= \arg\min_{u \in \mathcal{U}} \frac{1}{2} \| u - u_{\text{nom}} \|^2 \\ &\text{subject to:} \quad L_f h_b(x) + L_g h_b(x) u \ge -\alpha h_b(x), \quad u_{\min} \le u \le u_{\max} \end{align*} \] Writing its half-space as \(a^\top u\le b\), with \(a=-L_g h_b(x)^\top\) and \(b=L_f h_b(x)+\alpha h_b(x)\), gives this projection for one constraint when \(a\ne0\) and no box bound is active: \[ u^* = u_{\text{nom}} - \lambda^* a, \quad \text{where } \lambda^* = \max\left( 0, \frac{a^\top u_{\text{nom}} - b}{\|a\|^2} \right) \] If the feasible set is nonempty and convex, the strictly convex objective has a unique minimizer. Solver response time and total dispatch delay must be measured for the selected dimension and hardware.

Systems Perspective 1.1: Check feasibility before relying on invariance

The projection formula does not solve the box-constrained problem. Clamping its result afterward can violate \(a^\top u\le b\). If no command satisfies both the barrier and actuator limits, the modeled invariant cannot be maintained by this filter. A separately validated fallback may still limit harm; it does not restore the lost forward-invariance claim. Admission must reserve stopping authority before the state reaches this boundary.

Smooth Trajectories and Bumpless Transfer

Physical systems possess inertia. A velocity step is an ideal acceleration impulse; an acceleration step is a finite torque step under a simple inertia model. Whether a commanded seam causes resonance or damage depends on the plant, load, and controller response.

Analytical quintic splines

A quintic (5th-degree) polynomial trajectory segment \(\theta(t)\) over time interval \(t \in [0, T]\) is defined as: \[ \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \] The six coefficients \([a_0, \dots, a_5]\) are uniquely determined by matching six boundary conditions at \(t = 0\) and \(t = T\): \[ \theta(0) = q_0, \quad \dot{\theta}(0) = \dot{q}_0, \quad \ddot{\theta}(0) = \ddot{q}_0 \] \[ \theta(T) = q_1, \quad \dot{\theta}(T) = \dot{q}_1, \quad \ddot{\theta}(T) = \ddot{q}_1 \] Solving the resulting linear system yields the exact analytical formulas: \[ \begin{aligned} a_0 &= q_0 \\ a_1 &= \dot{q}_0 \\ a_2 &= \frac{1}{2}\ddot{q}_0 \\ a_3 &= \frac{10(q_1 - q_0) - (6\dot{q}_0 + 4\dot{q}_1)T - (3\ddot{q}_0 - \ddot{q}_1)\frac{T^2}{2}}{T^3} \\ a_4 &= \frac{-15(q_1 - q_0) + (8\dot{q}_0 + 7\dot{q}_1)T + (3\ddot{q}_0 - 2\ddot{q}_1)\frac{T^2}{2}}{T^4} \\ a_5 &= \frac{6(q_1 - q_0) - 3(\dot{q}_0 + \dot{q}_1)T - (\ddot{q}_0 - \ddot{q}_1)\frac{T^2}{2}}{T^5} \end{aligned} \] Differentiating three times gives the segment’s jerk profile, a quadratic in time that stays finite over a finite blend: \[ \dddot{\theta}(t)=6a_3+24a_4t+60a_5t^2 \] Matching position, velocity, and acceleration at the endpoints gives a \(C^2\) join to adjacent segments with those same endpoint values and removes the commanded acceleration step at the join. The endpoint conditions say nothing about the segment’s interior, so its jerk and torque extrema, and the joint’s resonant response, still require a check against the machine’s measured limits. Behavior at the Seam places this bridge at the seam between two action chunks.

Boundary value matrix formulation

The polynomial coefficients \(\mathbf{a} = [a_0, a_1, a_2, a_3, a_4, a_5]^\top\) are determined by evaluating the trajectory and its first two derivatives at the temporal endpoints \(t = 0\) and \(t = T\). This yields the \(6 \times 6\) confluent Vandermonde linear system \(\mathbf{M} \mathbf{a} = \mathbf{b}\): \[ \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 2 & 0 & 0 & 0 \\ 1 & T & T^2 & T^3 & T^4 & T^5 \\ 0 & 1 & 2T & 3T^2 & 4T^3 & 5T^4 \\ 0 & 0 & 2 & 6T & 12T^2 & 20T^3 \end{bmatrix} \begin{bmatrix} a_0 \\ a_1 \\ a_2 \\ a_3 \\ a_4 \\ a_5 \end{bmatrix} = \begin{bmatrix} q_0 \\ \dot{q}_0 \\ \ddot{q}_0 \\ q_1 \\ \dot{q}_1 \\ \ddot{q}_1 \end{bmatrix} \] The top three rows trivially yield the initial boundary terms: \(a_0 = q_0\), \(a_1 = \dot{q}_0\), and \(a_2 = \frac{1}{2}\ddot{q}_0\). Substituting these values into the bottom three rows isolates the higher-order coefficients \([a_3, a_4, a_5]^\top\) under the reduced \(3 \times 3\) system: \[ \begin{bmatrix} T^3 & T^4 & T^5 \\ 3T^2 & 4T^3 & 5T^4 \\ 6T & 12T^2 & 20T^3 \end{bmatrix} \begin{bmatrix} a_3 \\ a_4 \\ a_5 \end{bmatrix} = \begin{bmatrix} q_1 - q_0 - \dot{q}_0 T - \frac{1}{2}\ddot{q}_0 T^2 \\ \dot{q}_1 - \dot{q}_0 - \ddot{q}_0 T \\ \ddot{q}_1 - \ddot{q}_0 \end{bmatrix} \] The determinant of this \(3 \times 3\) matrix is \(\det(\mathbf{M}_{33}) = 2 T^9 > 0\) for all strictly positive blending horizons \(T > 0\), guaranteeing non-singularity and uniqueness. The analytic matrix inverse is: \[ \begin{bmatrix} T^3 & T^4 & T^5 \\ 3T^2 & 4T^3 & 5T^4 \\ 6T & 12T^2 & 20T^3 \end{bmatrix}^{-1} = \begin{bmatrix} \frac{10}{T^3} & -\frac{4}{T^2} & \frac{1}{2T} \\ -\frac{15}{T^4} & \frac{7}{T^3} & -\frac{1}{T^2} \\ \frac{6}{T^5} & -\frac{3}{T^4} & \frac{1}{2T^3} \end{bmatrix} \] Matrix multiplication \(\mathbf{a}_{3:5} = \mathbf{M}_{33}^{-1} \mathbf{b}_{\text{red}}\) directly produces the closed-form coefficients \(a_3, a_4, a_5\) stated above.

Cubic hermite spline interpolation for multi-rate synchronization

When synchronizing physical sensor and state streams arriving at heterogeneous rates (such as 30 Hz camera frames against 1 kHz joint encoder trajectories), intermediate poses must be continuously interpolated while preserving velocity continuity. A cubic Hermite spline over interval \(t \in [t_0, t_1]\) with segment duration \(T = t_1 - t_0\) interpolates position and velocity between endpoints: \[ q(t_0) = q_0, \quad \dot{q}(t_0) = \dot{q}_0, \quad q(t_1) = q_1, \quad \dot{q}(t_1) = \dot{q}_1 \]

Introducing the normalized time coordinate \(s = (t - t_0) / T \in [0, 1]\), the trajectory is parameterized by the cubic Hermite basis polynomials: \[ q(s) = h_{00}(s) q_0 + h_{10}(s) T \dot{q}_0 + h_{01}(s) q_1 + h_{11}(s) T \dot{q}_1 \] where the basis functions satisfy: \[ \begin{aligned} h_{00}(s) &= 2s^3 - 3s^2 + 1 \\ h_{10}(s) &= s^3 - 2s^2 + s \\ h_{01}(s) &= -2s^3 + 3s^2 \\ h_{11}(s) &= s^3 - s^2 \end{aligned} \] Differentiating with respect to physical time \(t\) yields the instantaneous velocity: \[ \dot{q}(t) = \frac{1}{T} \frac{dq}{ds} = \frac{1}{T} \left( h'_{00}(s) q_0 + h_{01}'(s) q_1 \right) + h'_{10}(s) \dot{q}_0 + h'_{11}(s) \dot{q}_1 \] with derivative basis functions: \[ \begin{aligned} h'_{00}(s) &= 6s^2 - 6s \\ h'_{10}(s) &= 3s^2 - 4s + 1 \\ h'_{01}(s) &= -6s^2 + 6s \\ h'_{11}(s) &= 3s^2 - 2s \end{aligned} \] Because the basis polynomials satisfy \(h_{00}(0)=1, h_{00}(1)=0, h'_{10}(0)=1, h'_{10}(1)=0\), this yields a \(C^1\)-continuous trajectory matching position and velocity exactly at sample boundaries without the computational overhead of solving a 6-boundary-condition quintic system. However, the resulting acceleration \(\ddot{q}(t)\) is piecewise linear and discontinuous across sample boundaries (\(C^1\) but not \(C^2\)), producing finite torque steps \(\Delta \tau = M(q) \Delta \ddot{q}\) at knot points that must remain within actuator torque-slew limits.

Bumpless transfer and smoothstep blending

For bumpless transfer (Transfer Without Discontinuity), let \(s=(t-t_0)/\tau_{\text{blend}}\in[0,1]\) and use the \(C^2\) smoothstep \(\alpha(s)\) to interpolate two commands: \[ \alpha(s)=6s^5-15s^4+10s^3,\qquad u(t)=(1-\alpha(s))u_{\rm prior}(t)+\alpha(s)u_{\rm new}(t). \] The endpoint first and second derivatives of \(\alpha\) vanish. If \(u\) is commanded acceleration, its time derivative is commanded-acceleration jerk: \[ j_{\rm cmd}(t)=\frac{\alpha'(s)}{\tau_{\text{blend}}}(u_{\rm new}-u_{\rm prior})+(1-\alpha(s))\dot u_{\rm prior}+\alpha(s)\dot u_{\rm new}. \] Here \(\alpha'(s)=30s^2(1-s)^2\) differentiates with respect to normalized time and peaks at \(1.875\). Only for two fixed, latched endpoint accelerations (\(\dot u_{\rm prior}=\dot u_{\rm new}=0\)) does the blend contribution satisfy \[ \max|j_{\rm cmd}|=\frac{1.875|\Delta u|}{\tau_{\text{blend}}},\qquad \tau_{\text{blend}}\ge\frac{1.875|\Delta u|}{j_{\rm allowable}}. \] This is a command-rate condition. Measured plant jerk, tracking error, and available stopping clearance remain separate admission checks.

Flexible joint dynamics and singular perturbations

Mechanical transmissions such as harmonic strain-wave drives, planetary reducers, and cable linkages introduce torsional elasticity between actuator motor rotors and robot links. Following Mark W. Spong’s canonical formulation (Spong 1987), an \(n\)-degree-of-freedom manipulator with elastic joints is governed by two coupled second-order vector differential equations: \[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{K} (\boldsymbol{\theta} - \mathbf{q}) \] \[ \mathbf{J}_m \ddot{\boldsymbol{\theta}} + \mathbf{B}_m \dot{\boldsymbol{\theta}} + \mathbf{K} (\boldsymbol{\theta} - \mathbf{q}) = \boldsymbol{\tau} \] where:

Spong, Mark W. 1987. “Modeling and Control of Elastic Joint Robots.” Journal of Dynamic Systems, Measurement, and Control 109 (4): 310–19.

\(\mathbf{q} \in \mathbb{R}^n\) represents the physical link coordinates.
\(\boldsymbol{\theta} \in \mathbb{R}^n\) represents the motor rotor coordinates reflected through gear reduction \(N\) to the link side.
\(\mathbf{M}(\mathbf{q}) \in \mathbb{R}^{n \times n}\) is the symmetric, positive-definite link inertia matrix.
\(\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} \in \mathbb{R}^n\) collects Coriolis and centripetal terms.
\(\mathbf{g}(\mathbf{q}) \in \mathbb{R}^n\) is the gravitational torque vector.
\(\mathbf{J}_m = \operatorname{diag}(J_{m,1}, \dots, J_{m,n})\) is the diagonal matrix of reflected rotor inertias (\(J_{m,i} = N_i^2 J_{\text{rotor},i}\)).
\(\mathbf{B}_m = \operatorname{diag}(B_{m,1}, \dots, B_{m,n})\) collects viscous motor damping.
\(\mathbf{K} = \operatorname{diag}(k_1, \dots, k_n)\) is the diagonal joint torsional stiffness matrix (\(k_i > 0\)).
\(\boldsymbol{\tau} \in \mathbb{R}^n\) is the vector of electromagnetic actuator torques delivered to the rotor.

Singular perturbation formulation and time-scale separation

When joint stiffness is high relative to link inertia (\(k_i \gg 1\)), the system exhibits a two-time-scale separation between slow rigid-body dynamics and fast torsional oscillations. Define the scalar perturbation parameter \(\epsilon = 1 / \sqrt{k_{\min}}\), where \(k_{\min} = \min_i(k_i)\), and express stiffness as \(\mathbf{K} = (1/\epsilon^2) \tilde{\mathbf{K}}\) with \(\tilde{\mathbf{K}} = \mathcal{O}(1)\).

Define the elastic torque fast state variable \(\mathbf{z} \in \mathbb{R}^n\): \[ \mathbf{z} = \frac{1}{\epsilon^2} \tilde{\mathbf{K}} (\boldsymbol{\theta} - \mathbf{q}) = \mathbf{K} (\boldsymbol{\theta} - \mathbf{q}) \] Differentiating \(\mathbf{z}\) twice with respect to continuous time \(t\) gives: \[ \ddot{\mathbf{z}} = \mathbf{K} (\ddot{\boldsymbol{\theta}} - \ddot{\mathbf{q}}) = \mathbf{K} \left[ \mathbf{J}_m^{-1} (\boldsymbol{\tau} - \mathbf{B}_m \dot{\boldsymbol{\theta}} - \mathbf{z}) - \mathbf{M}(\mathbf{q})^{-1} (\mathbf{z} - \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})) \right] \] Multiplying through by \(\epsilon^2\) reveals the singularly perturbed boundary layer: \[ \epsilon^2 \ddot{\mathbf{z}} = \tilde{\mathbf{K}} \left[ \mathbf{J}_m^{-1} \boldsymbol{\tau} - (\mathbf{J}_m^{-1} + \mathbf{M}(\mathbf{q})^{-1}) \mathbf{z} + \mathbf{M}(\mathbf{q})^{-1} (\mathbf{C} \dot{\mathbf{q}} + \mathbf{g}) - \mathbf{J}_m^{-1} \mathbf{B}_m \dot{\boldsymbol{\theta}} \right] \] Introducing the stretched fast time variable \(\tau_{\text{fast}} = t / \epsilon\) transforms the elastic dynamics into: \[ \frac{d^2 \mathbf{z}}{d\tau_{\text{fast}}^2} + \tilde{\mathbf{K}} (\mathbf{J}_m^{-1} + \mathbf{M}(\mathbf{q})^{-1}) \mathbf{z} = \tilde{\mathbf{K}} \mathbf{J}_m^{-1} \boldsymbol{\tau} + \tilde{\mathbf{K}} \mathbf{M}(\mathbf{q})^{-1} (\mathbf{C} \dot{\mathbf{q}} + \mathbf{g}) \]

The rigid manifold and boundary-layer resonance

In the singular limit as joint stiffness approaches infinity (\(\epsilon \to 0\)), the fast elastic variable \(\mathbf{z}\) collapses algebraically onto the quasi-steady manifold \(\mathbf{z}_0\): \[ \mathbf{z}_0 = (\mathbf{J}_m^{-1} + \mathbf{M}(\mathbf{q})^{-1})^{-1} \left[ \mathbf{J}_m^{-1} \boldsymbol{\tau} + \mathbf{M}(\mathbf{q})^{-1} (\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q})) \right] \] Substituting \(\mathbf{z}_0\) into the link dynamics recovers the classical rigid-body equations with aggregated inertia \(\mathbf{M}(\mathbf{q}) + \mathbf{J}_m\): \[ (\mathbf{M}(\mathbf{q}) + \mathbf{J}_m) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) + \mathbf{B}_m \dot{\mathbf{q}} = \boldsymbol{\tau} \] For finite physical stiffness (\(\epsilon > 0\)), any high-frequency excitation in commanded torque \(\boldsymbol{\tau}\)—such as discontinuous torque steps produced across discrete inference intervals by learned neural policies—acts as an impulsive forcing function on the boundary-layer oscillator \(\frac{d^2 \mathbf{z}}{d\tau_{\text{fast}}^2} + \boldsymbol{\Omega}^2(\mathbf{q}) \mathbf{z} = \mathbf{0}\), where the natural structural resonance frequency matrix is: \[ \boldsymbol{\Omega}(\mathbf{q}) = \sqrt{\mathbf{K} (\mathbf{J}_m^{-1} + \mathbf{M}(\mathbf{q})^{-1})} \] Because transmission mechanical damping is minimal (\(\zeta < 0.05\)), unsmoothed step commands excite persistent limit-cycle chatter, accelerate gear-tooth micro-fatigue, and invalidate rigid-link assumptions. Enforcing \(C^2\) quintic spline interpolation and limiting torque slew rate (\(d\boldsymbol{\tau}/dt\)) are necessary to prevent energy injection into this fast boundary-layer manifold.

Discrete-time sampling and zero-order hold phase lag

Real-time digital controllers execute periodically at sampling period \(T_s = 1 / f_s\). Actuator drivers hold commanded setpoints constant between clock ticks using a Zero-Order Hold (ZOH).

At frequencies well below the sampling frequency, the zero-order hold’s phase is equivalent to about half a sample period of delay: \[ \tau_{\text{ZOH}} = \frac{T_s}{2} \] For a closed-loop control system operating at crossover frequency \(\omega_c\), this transport delay erodes the system’s phase margin \(\Phi_m\) by:² \[ \Delta \Phi = \omega_c \frac{T_s}{2} \quad \text{(radians)} \approx 28.65^\circ \cdot \left(\frac{\omega_c}{f_s}\right) \]

For fixed modeled compute and bus delays, the added phase lag near crossover is approximately \(\Delta\Phi_{\rm total}=\omega_c(\tau_{\rm comp}+\tau_{\rm bus}+T_s/2)\). The controller must validate phase margin over the measured latency and operating envelope; the formula alone does not predict a particular contact response.

Kinematic Reachability and Intent Envelopes

Kinematic reachability characterizes the spatial set of configurations or end-effector poses an embodied system can reach over a given planning or intent horizon \(\tau\).

Forward kinematics and singularities

For an \(n\)-joint arm, the forward-kinematics map \(f(q)\) sends joint coordinates \(q\in\mathbb R^n\) to an end-effector pose \(T_{WE}\in SE(3)\): \[ T_{WE}=f(q). \] In a declared spatial or body twist convention, the manipulator Jacobian \(J(q)\in\mathbb R^{6\times n}\) relates joint rates to the six-vector end-effector twist \(V_E\): \[ V_E=J(q)\dot q. \] For a six-dimensional task and \(n\ge6\), rank loss (\(\det(JJ^\top)=0\)) marks a kinematic singularity. Tracking a nonzero twist along a lost direction would require unbounded joint rates in the ideal inverse model; real actuators saturate first. A task-specific design may monitor \(w(q)=\sqrt{\det(JJ^\top)}\) against a validated threshold.³

Trajectory transit time and reachable distance

For a one-dimensional rest-to-rest move with symmetric available acceleration \(a_{\max}\) and speed ceiling \(v_{\max}\), the shortest nominal transit has two regimes:

Triangular profile (acceleration-limited): For short horizons where peak velocity \(v_{\max}\) is not attained (\(d \le d_{\text{crit}} = v_{\max}^2 / a_{\max}\)): \[ t_{\min} = 2 \sqrt{\frac{d}{a_{\max}}}, \quad d_{\text{reach}}(\tau) = \frac{1}{4} a_{\max} \tau^2 \]
Trapezoidal profile (velocity-limited): For larger horizons (\(d > d_{\text{crit}}\)): \[ t_{\min} = \frac{d}{v_{\max}} + \frac{v_{\max}}{a_{\max}}, \quad d_{\text{reach}}(\tau) = v_{\max} \left( \tau - \frac{v_{\max}}{a_{\max}} \right) \]

Below \(d_{\text{crit}}\) the reachable distance grows with the square of the available time, and above it only linearly, because the mechanism spends the rest of the move at its speed ceiling.

With \(|\dot a|\le j_{\max}\), reaching full acceleration from zero takes at least \(a_{\max}/j_{\max}\), if the profile reaches it at all. The bound exists because an acceleration step is a torque step, which shocks gearbox backlash, excites structural modes, and drives motor current toward the inverter’s fault limits. The minimum-time trajectory may have four or seven jerk phases, so an unconditional additive offset to the trapezoidal time is invalid. Solve the constrained profile for the stated start/end velocity and acceleration, then include the stopping suffix and response delay when sizing a lease.

The idealized profiles still serve admission, because each added limit (jerk, torque, or thermal derating) can only shrink the dynamic reachable set \(\mathcal{R}(\tau)\), the states reachable from the measured state within the remaining time \(\tau=t_{\text{exp}}-t_{\text{now}}\). Their transit times are therefore necessary lower bounds, and a proposal whose idealized time already exceeds \(\tau\) can be rejected before a full path, torque, and stopping check. Admissible Goal Sets applies this rejection to the arm’s reach against the tracked horizon.

Spatial coordinate frames and lever-arm kinematics

Rigid-body pose in three-dimensional space is parameterized by the Special Euclidean group \(SE(3)\). A rigid transform from frame \(B\) to frame \(A\) is represented by the homogeneous transformation matrix \(\mathbf{T}_{A B} \in SE(3)\): \[ \mathbf{T}_{A B} = \begin{bmatrix} \mathbf{R}_{A B} & \mathbf{p}_{A B} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad \mathbf{R}_{A B} \in SO(3), \quad \mathbf{p}_{A B} \in \mathbb{R}^3 \] When sensors—such as IMUs or cameras—are displaced from the body center of rotation by lever arm \(\mathbf{r}_{P/O}\), rotational motion induces apparent velocities and accelerations at sensor location \(P\). The linear velocity \(\mathbf{v}_P\) and linear acceleration \(\mathbf{a}_P\) satisfy: \[ \mathbf{v}_P = \mathbf{v}_O + \boldsymbol{\omega} \times \mathbf{r}_{P/O} \] \[ \mathbf{a}_P = \mathbf{a}_O + \boldsymbol{\alpha} \times \mathbf{r}_{P/O} + \boldsymbol{\omega} \times (\boldsymbol{\omega} \times \mathbf{r}_{P/O}) \] where \(\boldsymbol{\omega}\) is the body angular velocity, \(\boldsymbol{\alpha} = \dot{\boldsymbol{\omega}}\) is the angular acceleration, \(\boldsymbol{\alpha} \times \mathbf{r}\) is the tangential acceleration, and \(\boldsymbol{\omega} \times (\boldsymbol{\omega} \times \mathbf{r})\) is the centripetal acceleration. State estimation and perception pipelines must compensate for these kinematic lever-arm terms when synchronizing multi-camera extrinsics and fusing IMU data.

Watchdog validity leases and teleoperation latency bounds

In distributed autonomous systems and supervisory teleoperation architectures, actuation authority is governed by bounded-duration validity leases and communications latency constraints.

Kinematic derivation of watchdog validity leases

Consider an autonomous mobile platform or manipulator moving at speed \(v\) toward an obstacle with measured physical clearance \(D_{\text{clear}}\). If the host processor or neural network fails to refresh the control setpoint, a hardware or firmware watchdog must intervene to prevent boundary violation.

The total elapsed time from the sensor observation until physical braking initiates comprises five sequential intervals: \[ \tau_{\text{delay}} = T_{\text{lease}} + T_{\text{detect}} + T_{\text{hold}} + t_{\text{age}} + T_{\text{act}} \] where:

\(T_{\text{lease}}\) is the granted duration during which the policy proposal remains valid for execution.
\(T_{\text{detect}}\) is the watchdog countdown detection latency before heartbeat loss is recognized.
\(T_{\text{hold}}\) is an optional transient hold or fault confirmation debounce window.
\(t_{\text{age}}\) is the sensor evidence age (exposure, readout, and bus transit to the controller clock).
\(T_{\text{act}}\) is the physical actuator onset delay before braking deceleration reaches the commanded floor.

Under constant cruising speed \(v\) prior to brake onset, a verified lower bound on mechanical braking deceleration \(a_{\text{brake}} > 0\), a localization bound \(\delta_{\text{loc}}\), and a fixed protective clearance \(\delta_{\text{margin}}\), the total stopping distance bounded by available clearance \(D_{\text{clear}}\) satisfies: \[ v\,\tau_{\text{delay}} + \frac{v^2}{2 a_{\text{brake}}} + \delta_{\text{loc}} + \delta_{\text{margin}} \le D_{\text{clear}} \] Solving directly for the maximum admissible validity lease \(T_{\text{lease}}\) yields: \[ T_{\text{lease}} \le \frac{D_{\text{clear}} - \delta_{\text{loc}} - \delta_{\text{margin}} - \frac{v^2}{2 a_{\text{brake}}}}{v} - \left( T_{\text{detect}} + T_{\text{hold}} + t_{\text{age}} + T_{\text{act}} \right) \] If \(T_{\text{lease}} \le 0\), the platform’s speed exceeds its stopping margin for the current clearance, and the real-time enforcer must immediately revoke motion permission and execute a verified emergency stop.

Teleoperation network round-trip time bounds

When a human supervisor remotely teleoperates the robot across an external wireless link, the control loop includes human perceptual reaction latency and network packet transport delays. Let:

\(T_{\text{video}}\) represent video camera exposure, encoding, network downlink, and operator display latency.
\(T_{\text{human}}\) denote human neuromuscular reaction time (\(T_{\text{human}} \approx 150\text{--}250\text{ ms}\)).
\(T_{\text{RTT}} = T_{\text{downlink}} + T_{\text{uplink}}\) represent network round-trip transport latency.
\(T_{\text{act}}\) denote actuator braking engagement lag.
\(d_{\text{brake}} = \frac{v^2}{2 a_{\text{brake}}}\) represent the purely mechanical braking distance.

Total available time before available clearance \(D_{\text{clear}}\) is exhausted is bounded by \(T_{\text{total}} = (D_{\text{clear}} - d_{\text{brake}}) / v\). The allowable round-trip network latency ceiling is: \[ T_{\text{RTT}} \le \frac{D_{\text{clear}} - d_{\text{brake}}}{v} - \left( T_{\text{video}} + T_{\text{human}} + T_{\text{act}} \right) \] If network jitter pushes \(T_{\text{RTT}}\) beyond this bound, the human operator cannot perceive an obstacle and deliver a corrective command before the vehicle exhausts its stopping distance. Consequently, the local real-time enforcer must evaluate physical invariants independently at \(1\text{ kHz}\), revoking operator command authority and executing autonomous braking before physical clearance is lost. Human Authority Over Actuation uses this bound to size a handover, where the operator’s round trip spends the same pre-brake budget.

Kinodynamic bounds versus wave-variable passivity

Transport delay in teleoperation creates two distinct failure modes: spatial boundary breach (kinematic collision) and high-frequency dynamic chatter (loss of closed-loop passivity). While wave variables (section 1.8) preserve port passivity and prevent energy accumulation under delay, passivity guarantees only that the teleoperator will not become an active energy source. It does not bound physical displacement. Safety requires satisfying both invariants simultaneously: wave variables bound interactive contact energy, while the kinodynamic latency inequality guarantees collision-free spatial clearance.

Rigid Body Contact Mechanics

Simulating and controlling physical interaction—such as locomotion, dexterous grasping, and tool assembly—requires modeling mechanical contact. The governing physical challenge of contact is its non-smooth complementarity: interaction forces arise strictly when bodies establish physical contact.

Let \(\phi(q)\) be the signed distance between two bodies:

\(\phi(q) > 0\): Bodies are separated.
\(\phi(q) = 0\): Bodies are touching.
\(\phi(q) < 0\): Bodies are penetrating (a geometric interpenetration that rigid mechanics strictly forbids).

Let \(f_n\) be the normal contact force pushing the bodies apart:

If \(\phi(q) > 0\), there is no contact, so \(f_n = 0\).
If bodies are touching, they can only exert repulsive contact force, so \(f_n \ge 0\).

Ideal unilateral contact is represented by complementarity conditions:

Non-penetration: \(\phi(q) \ge 0\)
Unilateral force: \(f_n \ge 0\)
Complementarity: \(\phi(q) f_n = 0\)

These conditions exclude tensile normal contact. A time-discretized rigid-contact problem becomes a linear complementarity problem only under suitable linearization and contact assumptions; frictional multi-contact models can be nonlinear or require other solvers. They do not establish sim-to-real fidelity by themselves.

Non-smooth collision manifolds and velocity jumps

For frictionless normal impact against a stationary rigid plane, let \(v_n^-<0\) be the pre-impact normal velocity and \(e\in[0,1]\) the restitution coefficient. The normal component obeys \(v_n^+=-e v_n^-\); tangential velocity and a moving boundary require additional equations. With effective normal mass \(m_{\rm eff}\), the normal impulse is \(P_n=m_{\rm eff}(v_n^+-v_n^-)\). In an ideal zero-duration impact, velocity jumps and acceleration is an impulse rather than an ordinary finite derivative:⁴ \[ \lim_{\Delta t \to 0} \frac{\| \dot{x}(t + \Delta t) - \dot{x}(t) \|}{\Delta t} = \infty \]

Smooth learned models and trajectory gradients may be inaccurate at contact-mode changes. A controller can use an explicit hybrid/contact model, while compliance, pads, or torque limiters can reduce force only within validated stroke, energy, wear, and load-path conditions. Neither modeling nor a passive element alone guarantees safe contact.

Elastodynamic impact force and compliance deformation

When an effective moving mass \(m_{\rm eff}\) strikes an obstacle or human tissue, rigid-body approximations break down because real materials deform elastically over a nonzero compression stroke \(\delta(t)\). Let \(k_{\rm eff}\) denote the effective contact stiffness across the interface. In series compliance between the manipulator structural chain (\(k_{\rm robot}\)) and human tissue or obstacle (\(k_{\rm tissue}\)): \[ \frac{1}{k_{\rm eff}} = \frac{1}{k_{\rm robot}} + \frac{1}{k_{\rm tissue}} \]

Unmitigated elastic impact force

For an unmitigated collision at impact velocity \(v_{\max}\), initial kinetic energy \(E_k = \frac{1}{2} m_{\rm eff} v_{\max}^2\) converts entirely into elastic strain energy \(U_{\rm elastic} = \frac{1}{2} k_{\rm eff} \delta_{\max}^2\) at the point of maximum compression (\(\dot{\delta}=0\)). Energy conservation yields the peak structural deflection: \[ \frac{1}{2} m_{\rm eff} v_{\max}^2 = \frac{1}{2} k_{\rm eff} \delta_{\max}^2 \implies \delta_{\max} = v_{\max} \sqrt{\frac{m_{\rm eff}}{k_{\rm eff}}} \] The resulting peak transient contact force \(F_{\rm peak} = k_{\rm eff} \delta_{\max}\) evaluates to: \[ F_{\rm peak} = v_{\max} \sqrt{k_{\rm eff} m_{\rm eff}} \] This relation establishes that peak impact force scales linearly with velocity and with the square root of both moving mass and contact stiffness. Even modest kinetic energies (e.g., \(17.5\text{ J}\)) striking stiff biological structures (\(k_{\rm tissue} \approx 15{,}000\text{ N/m}\)) generate peak forces exceeding \(700\text{ N}\)—well beyond collaborative safety limits (such as ISO/TS 15066 transient thresholds).

Coupled contact indentation with mechanical braking

When active mechanical brakes engage with constant deceleration \(a_{\rm brake}\) during deformation, the braking system performs mechanical work \(W_{\rm brake} = m_{\rm eff} a_{\rm brake} \delta\) across the indentation depth \(\delta\). The coupled energy balance becomes: \[ \frac{1}{2} m_{\rm eff} v_{\rm impact}^2 = \frac{1}{2} k_{\rm eff} \delta_{\max}^2 + m_{\rm eff} a_{\rm brake} \delta_{\max} \] Rearranging into quadratic form: \[ \delta_{\max}^2 + \frac{2 m_{\rm eff} a_{\rm brake}}{k_{\rm eff}} \delta_{\max} - \frac{m_{\rm eff} v_{\rm impact}^2}{k_{\rm eff}} = 0 \] Solving for the physically admissible positive deformation root: \[ \delta_{\max} = \sqrt{\left(\frac{m_{\rm eff} a_{\rm brake}}{k_{\rm eff}}\right)^2 + \frac{m_{\rm eff} v_{\rm impact}^2}{k_{\rm eff}}} - \frac{m_{\rm eff} a_{\rm brake}}{k_{\rm eff}} \] When \(a_{\rm brake} = 0\), this expression smoothly recovers the unmitigated deformation \(\delta_{\max} = v_{\rm impact}\sqrt{m_{\rm eff}/k_{\rm eff}}\). Because the quarter-cycle compression duration \(t_{\rm peak} = \frac{\pi}{2}\sqrt{m_{\rm eff}/k_{\rm eff}}\) is typically on the order of \(20\text{--}50\text{ ms}\), reactive braking triggered only after physical contact cannot attenuate the initial force peak if sensor detection and brake engagement latency exceed \(t_{\rm peak}\). Consequently, safety cases cannot substitute reactive post-impact braking for verified pre-impact separation clearance envelopes (\(d_{\rm sep} \ge d_{\rm stop}\)).

Wheel slip ratio and friction boundaries

For wheeled mobile robots and terrestrial platforms, tractive force transmission between the wheel contact patch and the ground plane is governed by non-linear slip dynamics. The longitudinal slip ratio \(\lambda\) quantifies the kinematic velocity mismatch between the wheel circumferential speed \(r\omega\) (wheel radius \(r\), angular rate \(\omega\)) and the vehicle forward longitudinal speed \(v_x\): \[ \lambda = \begin{cases} \frac{r\omega - v_x}{v_x}, & \text{under acceleration } (r\omega > v_x, \ v_x > 0) \\[6pt] \frac{r\omega - v_x}{r\omega}, & \text{under braking } (r\omega < v_x, \ r\omega > 0) \end{cases} \] A normalized form frequently implemented in digital traction control avoids denominator singularities via: \[ \lambda = \frac{r\omega - v_x}{\max(|v_x|, |r\omega|, \epsilon_{\text{reg}})} \in [-1, 1] \] where \(\lambda = 0\) corresponds to pure rolling without slip, \(\lambda = 1\) denotes complete drive-wheel spin-out (\(v_x = 0, \omega > 0\)), and \(\lambda = -1\) corresponds to complete wheel lock-up (\(r\omega = 0, v_x > 0\)).

Tractive force \(F_x\) scales with the normal contact load \(F_z\) through the longitudinal friction coefficient \(\mu_x(\lambda)\): \[ F_x(\lambda) = \mu_x(\lambda) F_z \] At low slip ratios (\(|\lambda| < \lambda_{\text{peak}} \approx 0.10\text{--}0.15\)), tire or tread rubber shears elastically, and tractive force is approximately linear in slip: \(F_x \approx C_x \lambda\), with longitudinal traction stiffness \(C_x\). Beyond \(\lambda_{\text{peak}}\), adhesion collapses into gross sliding friction: the slope \(d\mu_x / d\lambda\) turns negative, rendering open-loop torque control unstable. If an ungrounded policy commands drive motor torque exceeding peak traction (\(F_{\text{cmd}} > \mu_{\text{peak}} F_z\)), the wheel rapidly accelerates into the unstable slip regime (\(d\omega/dt = (\tau - r F_x)/I_{\text{wheel}} > 0\)). This causes spin-out, severe yaw rate disturbance, or loss of lateral authority—a cyber-physical disconnect that cannot be detected by directional agreement or cosine similarity metrics that evaluate only vector angles while ignoring control magnitude.

Continuous Covariance Propagation and Observability

When sensors suffer line-of-sight dropouts or environmental occlusion, internal state estimators must project the physical state forward in time. Quantifying how rapidly estimation confidence degrades across blind intervals requires analyzing continuous error covariance dynamics and systems observability.

Continuous lyapunov and riccati covariance dynamics

Consider a continuous-time linear dynamic system governed by: \[ \dot{\mathbf{x}}(t) = \mathbf{A} \mathbf{x}(t) + \mathbf{B} \mathbf{u}(t) + \mathbf{w}(t) \] \[ \mathbf{y}(t) = \mathbf{C} \mathbf{x}(t) + \mathbf{v}(t) \] For the continuous white-noise model, \(\mathbf{Q}\) and \(\mathbf{R}\) are process and measurement noise intensities, with covariance kernels \(E[\mathbf{w}(t)\mathbf{w}(s)^\top]=\mathbf{Q}\delta(t-s)\) and \(E[\mathbf{v}(t)\mathbf{v}(s)^\top]=\mathbf{R}\delta(t-s)\). The model assumes suitable independence and \(\mathbf{R}\succ0\).

Under an optimal continuous-time estimator (the Kalman-Bucy filter (Kalman 1960)), the error covariance matrix \(\mathbf{P}(t) = \mathbb{E}[(\mathbf{x} - \hat{\mathbf{x}})(\mathbf{x} - \hat{\mathbf{x}})^\top]\) evolves according to the continuous matrix differential Riccati equation:⁵ \[ \dot{\mathbf{P}}(t) = \mathbf{A} \mathbf{P}(t) + \mathbf{P}(t) \mathbf{A}^\top + \mathbf{Q} - \mathbf{P}(t) \mathbf{C}^\top \mathbf{R}^{-1} \mathbf{C} \mathbf{P}(t) \]

Kalman, Rudolph Emil. 1960. “A New Approach to Linear Filtering and Prediction Problems.” Journal of Basic Engineering 82 (1): 35–45.

When direct sensing is occluded or dropped (\(\mathbf{C} = \mathbf{0}\)), the measurement correction term vanishes. The covariance growth collapses to the open-loop continuous Lyapunov differential equation: \[ \dot{\mathbf{P}}(t) = \mathbf{A} \mathbf{P}(t) + \mathbf{P}(t) \mathbf{A}^\top + \mathbf{Q} \] The solution propagates initial uncertainty and accumulated \(\mathbf{Q}\) through \(e^{\mathbf{A}t}\). Unstable modes can grow exponentially; a random walk with \(\mathbf{A}=0\) grows linearly in covariance. A state-memory record should carry the calibrated growth model and expire tracking permission before its declared error bound crosses the physical tolerance. Covariance alone is not a hard error envelope unless its tail and model assumptions are justified.

Closed-form double-integrator covariance expansion

Consider a one-dimensional double-integrator kinematic system modeling position \(p(t)\) and velocity \(v(t)\) under unmeasured stochastic acceleration noise \(w(t)\): \[ \begin{bmatrix} \dot{p}(t) \\ \dot{v}(t) \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} p(t) \\ v(t) \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \end{bmatrix} w(t) \] where \(w(t)\) is zero-mean continuous white process noise with continuous spectral power intensity \(q_c > 0\), such that \(\mathbb{E}[w(t) w(s)] = q_c \delta(t - s)\). Here, \(\mathbf{A} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}\) and the continuous process noise matrix is \(\mathbf{Q} = \begin{bmatrix} 0 & 0 \\ 0 & q_c \end{bmatrix}\).

The continuous state transition matrix \(\mathbf{\Phi}(t) = e^{\mathbf{A}t}\) is nilpotent: \[ \mathbf{\Phi}(t) = \mathbf{I} + \mathbf{A}t = \begin{bmatrix} 1 & t \\ 0 & 1 \end{bmatrix} \] During a blind occlusion window \(t \in [0, \Delta t]\) with no measurement updates (\(\mathbf{C} = \mathbf{0}\)), the total state covariance \(\mathbf{P}(t)\) evolves as the sum of unforced homogeneous propagation and accumulated process diffusion: \[ \mathbf{P}(t) = \mathbf{\Phi}(t) \mathbf{P}_0 \mathbf{\Phi}(t)^\top + \int_0^t \mathbf{\Phi}(t - \tau) \mathbf{Q} \mathbf{\Phi}(t - \tau)^\top d\tau \] Let the initial state covariance at the instant of sensor loss (\(t = 0\)) be: \[ \mathbf{P}_0 = \begin{bmatrix} \sigma_{p,0}^2 & \sigma_{pv,0} \\ \sigma_{pv,0} & \sigma_{v,0}^2 \end{bmatrix} \] The homogeneous propagation term evaluates to: \[ \mathbf{\Phi}(t) \mathbf{P}_0 \mathbf{\Phi}(t)^\top = \begin{bmatrix} 1 & t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \sigma_{p,0}^2 & \sigma_{pv,0} \\ \sigma_{pv,0} & \sigma_{v,0}^2 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ t & 1 \end{bmatrix} = \begin{bmatrix} \sigma_{p,0}^2 + 2\sigma_{pv,0} t + \sigma_{v,0}^2 t^2 & \sigma_{pv,0} + \sigma_{v,0}^2 t \\ \sigma_{pv,0} + \sigma_{v,0}^2 t & \sigma_{v,0}^2 \end{bmatrix} \] The accumulated stochastic diffusion term integrates across elapsed time: \[ \mathbf{Q}_d(t) = \int_0^t \begin{bmatrix} 1 & t - \tau \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 & 0 \\ 0 & q_c \end{bmatrix} \begin{bmatrix} 1 & 0 \\ t - \tau & 1 \end{bmatrix} d\tau = \int_0^t \begin{bmatrix} q_c (t - \tau)^2 & q_c (t - \tau) \\ q_c (t - \tau) & q_c \end{bmatrix} d\tau = \begin{bmatrix} \frac{1}{3} q_c t^3 & \frac{1}{2} q_c t^2 \\ \frac{1}{2} q_c t^2 & q_c t \end{bmatrix} \] Summing the homogeneous and stochastic contributions yields the closed-form positional variance \(\sigma_p^2(t) = \mathbf{P}_{11}(t)\): \[ \sigma_p^2(t) = \sigma_{p,0}^2 + 2 \sigma_{pv,0} t + \sigma_{v,0}^2 t^2 + \frac{1}{3} q_c t^3 \] This derivation explains the distinct physical error regimes during sensor occlusion: initial velocity uncertainty \(\sigma_{v,0}^2\) induces quadratic error growth in time (\(t^2\)), while continuous stochastic disturbances \(q_c\) (such as air currents or unmodeled drivetrain chatter) inject cubic uncertainty growth (\(\frac{1}{3} q_c t^3\)). In long blind intervals, the cubic term dominates, rapidly exhausting safety margins unless motion is derated or halted.

Observability rank and Lie derivative extensions

A linear system is observable on \([0,T]\) if its initial state can be uniquely recovered from outputs and known inputs. For the linear time-invariant pair \((\mathbf{A},\mathbf{C})\), the observability matrix has full column rank \(n\) exactly when the state is observable: \[ \mathcal{O} = \begin{bmatrix} \mathbf{C} \\ \mathbf{C}\mathbf{A} \\ \vdots \\ \mathbf{C}\mathbf{A}^{n-1} \end{bmatrix}, \quad \text{rank}(\mathcal{O}) = n \] For a smooth autonomous nonlinear system \(\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x})\), \(\mathbf{y}=\mathbf{h}(\mathbf{x})\), a local rank test builds the codistribution from differentials of successive Lie derivatives \(dL_{\mathbf f}^k\mathbf h(\mathbf x)\) until its rank stops growing.⁶ Full local rank \(n\) is a sufficient local weak-observability condition under its regularity assumptions; a finite truncation at \(n-1\) is not universally sufficient.

If \(\operatorname{rank}(\mathcal O)<n\), \(\ker(\mathcal O)\) contains initial-state differences that produce identical ideal outputs. Measurement updates cannot resolve those directions from this sensor model, though a particular Kalman gain need not contain literal zero entries. The estimator should expose such unobservable components and maintain an independent growth bound or restrict the task.

Centroidal Dynamics and Floating-Base Control

Legged robots and dynamic humanoids differ fundamentally from fixed-base manipulators: they possess an underactuated 6-degree-of-freedom floating base that cannot be commanded directly by motors. The robot can only move its center of mass (CoM) and alter its spatial orientation by exchanging contact forces with the environment through its feet or hands.

Floating-base equations of motion

Let \(\mathbf{q} = [\mathbf{q}_b^\top, \mathbf{q}_j^\top]^\top \in SE(3) \times \mathbb{R}^n\) represent the generalized coordinates, where \(\mathbf{q}_b \in SE(3)\) is the floating-base pose and \(\mathbf{q}_j \in \mathbb{R}^n\) are the actuated joint angles. The equations of motion are (Siciliano et al. 2009): \[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{S}^\top \boldsymbol{\tau} + \sum_{k=1}^{N_c} \mathbf{J}_k(\mathbf{q})^\top \mathbf{f}_k \] where \(\mathbf{S} = [\mathbf{0}_{n \times 6}, \mathbf{I}_{n \times n}]\) is the actuation selection matrix separating the 6 unactuated floating-base coordinates from the \(n\) actuated joints, and \(\mathbf{J}_k(\mathbf{q}) = \partial \mathbf{p}_k / \partial \mathbf{q} \in \mathbb{R}^{3 \times (n+6)}\) is the contact point Jacobian for contact wrench \(\mathbf{f}_k\).

Centroidal momentum dynamics

Projecting the floating-base dynamics onto the robot’s center of mass \(\mathbf{c} \in \mathbb{R}^3\) yields the Newton-Euler centroidal momentum equations (Lynch and Park 2017): \[ \dot{\mathbf{h}}_{\text{lin}} = m \ddot{\mathbf{c}} = \sum_{k=1}^{N_c} \mathbf{f}_k + m \mathbf{g} \] \[ \dot{\mathbf{h}}_{\text{ang}} = \sum_{k=1}^{N_c}\left[(\mathbf{p}_k - \mathbf{c}) \times \mathbf{f}_k + \boldsymbol{\tau}_k\right] \] where \(\mathbf{h}_{\text{lin}} \in \mathbb{R}^3\) is linear momentum, \(\mathbf{h}_{\text{ang}} \in \mathbb{R}^3\) is angular momentum about the CoM, and \((\mathbf{p}_k, \mathbf{f}_k, \boldsymbol{\tau}_k)\) are the position, force, and moment at each active contact patch.⁷

Physical contact feasibility constraints

Because the environment cannot pull on the robot and contact surfaces exhibit finite friction, admissible contact wrenches must satisfy three physical invariants:

Unilateral Normal Force: Feet can only push against the ground: \[ f_{k,\perp} = \mathbf{f}_k^\top \hat{\mathbf{n}}_k \ge 0 \]
Coulomb Friction Cone: Tangential shear forces cannot exceed normal traction: \[ \| \mathbf{f}_{k,\parallel} \| = \| \mathbf{f}_k - f_{k,\perp} \hat{\mathbf{n}}_k \| \le \mu f_{k,\perp} \]
Support condition for coplanar contacts: Under flat-ground assumptions, the Zero Moment Point (ZMP) should remain inside the effective support polygon with a case-specific inset:⁸ \[ \mathbf{p}_{\text{ZMP}}\in\mathcal P_{\rm support},\qquad \mathcal P_{\rm support}=\operatorname{Conv}\!\left(\bigcup_k\mathcal A_k\right), \] where \(\mathcal A_k\) is each active contact patch projected onto that plane.

Hierarchical whole-body control quadratic program

One whole-body controller can track torso and foot tasks while constraining dynamics and contact forces. For an illustrative sampled implementation, a conservative polyhedral friction approximation makes the following convex quadratic program possible; the exact circular norm cone in the preceding equation instead gives a second-order cone constraint: \[ \begin{aligned} \min_{\ddot{\mathbf{q}}, \mathbf{f}, \boldsymbol{\tau}} \quad & \sum_{i} w_i \| \mathbf{J}_i \ddot{\mathbf{q}} + \dot{\mathbf{J}}_i \dot{\mathbf{q}} - \ddot{\mathbf{x}}_i^{\text{des}} \|^2 + w_\tau \| \boldsymbol{\tau} \|^2 + w_f \| \mathbf{f} \|^2 \\ \text{subject to:} \quad & \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{S}^\top \boldsymbol{\tau} + \sum_{k=1}^{N_c} \mathbf{J}_k^\top \mathbf{f}_k \\ & \mathbf F_k\mathbf f_k\le\mathbf 0,\quad f_{k,\perp}\ge0 \qquad\text{(conservative friction pyramid)} \\ & |\boldsymbol{\tau}| \le \boldsymbol{\tau}_{\max}, \quad \mathbf{q}_{j,\min} \le \mathbf{q}_j \le \mathbf{q}_{j,\max} \end{aligned} \] Here \(\mathbf F_k\) describes the chosen inner approximation to the measured friction cone. The displayed joint-position bounds concern the current configuration; a real controller must also constrain predicted motion between samples. A 1 ms solve is an example deadline to validate with the selected contact count, solver, processor, friction estimate, and fallback. A feasible numerical solution does not itself prove that a foothold will support the load.

Passivity and Energy-Bounded Interaction

During teleoperation or stiff contact, transport delay can reduce the damping margin of a feedback loop. Passivity is an energy-accounting condition useful for analyzing a specified interconnection; it does not remove the need to verify actuator saturation, contact geometry, sampling, and stored energy.

Energy storage and the passivity definition

Consider a continuous-time dynamical system with state \(\mathbf{x}(t)\), control input \(\mathbf{u}(t)\) (an effort variable, such as force or torque \(\mathbf{F}\)), and measured output \(\mathbf{y}(t)\) (a flow variable, such as velocity \(\mathbf{v}\)). The system is defined as passive if there exists a continuously differentiable, non-negative storage function \(V(\mathbf{x}) \ge 0\) (representing the total internal physical energy stored in mass momentum and structural compliance) such that: \[\dot{V}(\mathbf{x}) \le \mathbf{u}(t)^\top \mathbf{y}(t) - g(\mathbf{x}) \tag{1}\] where \(\mathbf{u}(t)^\top \mathbf{y}(t)\) represents instantaneous power injected into the system, and \(g(\mathbf{x}) \ge 0\) denotes internal energy dissipation (such as mechanical damping or electrical resistance). Integrating equation 1 over any operational duration \([0, T]\) yields the fundamental passivity energy inequality: \[\int_0^T \mathbf{u}(t)^\top \mathbf{y}(t) \, dt \ge V(\mathbf{x}(T)) - V(\mathbf{x}(0)) + \int_0^T g(\mathbf{x}(t))\,dt \ge -V(\mathbf{x}(0)) \tag{2}\] Under equation 2 and the declared port orientation, the net energy extractable from this modeled component is bounded by its initial storage. A loaded mechanism can still release enough stored energy to injure or damage; passivity is not a contact-force limit.

Negative-feedback interconnection of compatible passive components preserves a passive energy balance when ports and signs match. Bounded or asymptotic state stability requires additional storage and detectability conditions. A human, powered tool, or active environment cannot be assumed passive without evidence.

The transport delay destabilization hazard

An untreated effort/flow communication channel need not remain passive when delayed. A fixed pure delay contributes phase \(\Delta\phi=-\omega\tau_d\) at angular frequency \(\omega\), but no universal \(90^\circ\) threshold determines the sign of an arbitrary coupled mechanical impedance.

For a particular controller, late force feedback can oppose the operator’s current motion poorly and inject energy into the closed loop. Whether this causes chatter depends on the port signs, damping, delay distribution, and contact stiffness; measure those conditions rather than inferring failure from latency alone.

Passivity preservation via wave variables

For a delay channel with specified port orientation, Anderson and Spong (1989)’s wave variable transformation maps effort \(\mathbf F\) and flow \(\dot{\mathbf x}\) into incoming and outgoing waves with characteristic impedance \(b>0\): \[\mathbf{u}_m(t) = \frac{\mathbf{F}_m(t) + b \mathbf{\dot{x}}_m(t)}{\sqrt{2b}}, \quad \mathbf{w}_m(t) = \frac{\mathbf{F}_m(t) - b \mathbf{\dot{x}}_m(t)}{\sqrt{2b}} \tag{3}\] Computing instantaneous power from the wave transformation in equation 3 reveals: \[P_m(t) = \mathbf{F}_m(t)^\top \mathbf{\dot{x}}_m(t) = \frac{1}{2} \|\mathbf{u}_m(t)\|^2 - \frac{1}{2} \|\mathbf{w}_m(t)\|^2\] For an ideal fixed delay, a received wave is a delayed transmitted wave. The channel can hold energy in flight, so a same-window inequality between master and slave port powers needs a channel-storage term: \(E_{\rm ch}(T)-E_{\rm ch}(0)\le\int_0^T(P_m-P_s)\,dt\) with consistent signs. The scattering construction can preserve channel passivity under its assumptions; time-varying delay, packet loss, sampling, saturation, and powered endpoints require additional compensation or tests. It does not guarantee contact-force safety or convergence of the complete human-robot system.

Anderson, Robert J., and Mark W. Spong. 1989. “Bilateral Control of Teleoperators with Time Delay.” IEEE Transactions on Automatic Control 34 (5): 494–501. https://doi.org/10.1109/9.24201.

Footnotes

Boundary Viability: Nagumo’s tangent-cone condition characterizes invariance under regularity assumptions. The displayed CBF inequality is a sufficient design condition for the stated smooth control-affine model. A sampled implementation needs a separate bound for state error, computation, hold interval, and actuator response; failure of one candidate inequality does not prove contact within one integration step.↩︎
Phase Margin and Delay: A pure delay contributes phase \(-\omega\tau\). The half-sample approximation applies in the baseband; actual stability depends on the sampled plant, compensation, jitter, and measured total response. Consuming modeled phase margin predicts loss of the stated stability reserve, not a specific destructive failure mode.↩︎
Yoshikawa Manipulability Measure: The measure summarizes the velocity ellipsoid for a chosen task Jacobian. Near rank loss, inverse-kinematics commands can exceed joint-rate or torque limits. The actual drive response and fallback depend on the specified controller and hardware.↩︎
Moreau Time-Stepping and Non-Smooth Mechanics: Measure-differential models represent ideal impulses without resolving microscopic contact duration. Numerical contact solvers still require a validated geometry, friction law, and step-size study for the plant; a smooth optimizer that omits impact can miss a relevant force transient.↩︎
Continuous Matrix Riccati Equation: The measurement term reduces modeled covariance when valid measurements arrive. During occlusion, propagation follows the plant and process-noise model. Its growth may be bounded, linear, polynomial, or exponential depending on the dynamics and noise; the estimator must compare the resulting bound with the task tolerance.↩︎
Hermann-Krener Nonlinear Observability: Full rank of the observation codistribution is a local structural test, not a sensor-noise or finite-horizon accuracy bound. A missing state direction may require another sensor, an active maneuver with its own permission, or a restricted operating envelope.↩︎
Centroidal Momentum Matrix: Centroidal dynamics summarize the external wrench needed for a proposed CoM and angular-momentum change. A controller still needs feasible contacts and actuator torque; omitting angular momentum can mispredict the required contact moments.↩︎
Zero Moment Point (ZMP): ZMP is useful for a specified coplanar support model. Edge contact, friction loss, and multi-height supports require more general wrench feasibility checks; crossing a modeled polygon edge does not by itself predict a particular fall.↩︎