This appendix collects the models needed to reason from a robot command to the motion it can produce. It develops state-space dynamics, feedback stability, trajectory continuity, stopping and contact bounds, and state-estimation uncertainty. Each result identifies the assumptions under which it can support a tracking or containment decision in the main text.
How to Use This Appendix
Use the table to enter at the physical question being checked. For a first encounter with control theory, start with state-space models and PID before consulting stability or constrained control. The longer Riccati, Lie-derivative, and differential-game derivations supply optional depth; the local model, assumptions, and resulting bound are the reference tools needed by the main text.
When you encounter this systems symptom
Consult
For this quantitative tool
An inner-loop tracking controller drifts or loses asymptotic convergence
Centroidal momentum equations and hierarchical Whole-Body Control QP
Classical and Modern State-Space Control
A robot moving toward a target retains velocity when a new command arrives. A state-space model records that memory explicitly: its state contains enough information to predict subsequent motion from the current state and future inputs. A learned policy can propose an input, but the plant dynamics determine how that input changes position, velocity, and stored energy.
The physical body is the plant, the system a controller acts on (Actuator Transmission Limits). Its state \(\MechanicalPlantState(\TimeInstant)\) can include joint positions, velocities, winding currents, and temperatures. These quantities are physically constrained, but they are not all conserved. Actuators change their rates of change; they do not overwrite the state instantaneously. For a mass driven by force, for example, the state contains both position and velocity because force changes acceleration before it changes either of them.
Linear and nonlinear state-space representations
A dot denotes a time derivative, and bold symbols collect several coordinates into vectors or matrices. Matrix multiplication describes how one coordinate affects another. An eigenvalue describes the growth, decay, or oscillation of a linear mode; a negative real part means decay in continuous time, while discrete-time decay requires magnitude below one. A positive-definite matrix assigns a positive quadratic measure to every nonzero vector. These interpretations connect the algebra below to motion and stored energy.
A continuous-time Linear Time-Invariant (LTI) dynamic system is represented in canonical state-space form as: \[ \PlantStateRate(\TimeInstant) = \StateSpaceSystemMatrix \MechanicalPlantState(\TimeInstant) + \StateSpaceInputMatrix \ControlInput(\TimeInstant) \]\[ \MeasurementVector(\TimeInstant) = \MeasurementMatrix \MechanicalPlantState(\TimeInstant) + \FeedthroughMatrix \ControlInput(\TimeInstant) \] where \(\MechanicalPlantState(\TimeInstant) \in \mathbb{R}^\StateDimension\) denotes the internal state vector (e.g., joint angles and velocities, motor winding currents, or vehicle pose and velocity), \(\ControlInput(\TimeInstant) \in \mathbb{R}^\ControlInputDimension\) is the control input vector (e.g., commanded motor currents, voltages, or torques), and \(\MeasurementVector(\TimeInstant) \in \mathbb{R}^\OutputDimension\) represents the measured output vector produced by physical sensors. The system matrix \(\StateSpaceSystemMatrix \in \mathbb{R}^{\StateDimension \times \StateDimension}\) governs unforced internal system dynamics (the natural drift vector field), the input matrix \(\StateSpaceInputMatrix \in \mathbb{R}^{\StateDimension \times \ControlInputDimension}\) models control authority over state rates, the output matrix \(\MeasurementMatrix \in \mathbb{R}^{\OutputDimension \times \StateDimension}\) maps internal states to sensor measurements, and \(\FeedthroughMatrix \in \mathbb{R}^{\OutputDimension \times \ControlInputDimension}\) represents direct feedthrough (which vanishes in strictly proper physical plants, \(\FeedthroughMatrix = \ZeroMatrix\)).
When physical dynamics include kinematic nonlinearities, centrifugal couplings, or configuration-dependent gravity (section 1.6.1), the dynamics generalize to the control-affine nonlinear form: \[ \PlantStateRate(\TimeInstant) = \ControlAffineDriftField(\MechanicalPlantState(\TimeInstant)) + \ControlAffineInputField(\MechanicalPlantState(\TimeInstant)) \ControlInput(\TimeInstant) \] where \(\ControlAffineDriftField: \mathbb{R}^\StateDimension \to \mathbb{R}^\StateDimension\) is the unforced drift vector field and \(\ControlAffineInputField: \mathbb{R}^\StateDimension \to \mathbb{R}^{\StateDimension \times \ControlInputDimension}\) is the state-dependent control effectiveness matrix.
Discrete-time equivalent under zero-order hold
Digital microcontrollers and real-time operating systems execute periodic control loops at sampling frequency \(\SamplingRate = 1 / \SamplingPeriod\) (e.g., \(1\text{ kHz}\), \(\SamplingPeriod = 1\text{ ms}\); see Multi-Rate Cadences). When the actuator driver holds control inputs constant across each clock tick using a Zero-Order Hold (\(\ControlInput(\TimeInstant) = \ControlInput_k\) for \(\TimeInstant \in [\ControlSampleIndex \SamplingPeriod, (\ControlSampleIndex+1) \SamplingPeriod)\)), the continuous LTI dynamics integrate into the exact discrete-time recurrence: \[ \MechanicalPlantState_{k+1} = \DiscreteStateSpaceSystemMatrix \MechanicalPlantState_k + \DiscreteStateSpaceInputMatrix \ControlInput_k \] where the discrete transition operators evaluate via the continuous matrix exponential: \[ \DiscreteStateSpaceSystemMatrix = e^{\StateSpaceSystemMatrix \SamplingPeriod} = \IdentityMatrix + \StateSpaceSystemMatrix \SamplingPeriod + \frac{1}{2!} \StateSpaceSystemMatrix^2 \SamplingPeriod^2 + \dots \]\[ \DiscreteStateSpaceInputMatrix = \int_0^{\SamplingPeriod} e^{\StateSpaceSystemMatrix \IntegrationDummyTime} d\IntegrationDummyTime \, \StateSpaceInputMatrix = \left( \IdentityMatrix \SamplingPeriod + \frac{1}{2!} \StateSpaceSystemMatrix \SamplingPeriod^2 + \dots \right) \StateSpaceInputMatrix \] When \(\StateSpaceSystemMatrix\) is non-singular, the discrete input matrix simplifies to \(\DiscreteStateSpaceInputMatrix = \StateSpaceSystemMatrix^{-1}(e^{\StateSpaceSystemMatrix \SamplingPeriod} - \IdentityMatrix)\StateSpaceInputMatrix\). This recurrence is exact for the stated LTI model with a held input. A nonlinear plant generally requires numerical integration or a local linear model; its discretization error must enter the prediction bound.
To an ML engineer, a forward Euler discretization \(\MechanicalPlantState_{k+1} \approx (\IdentityMatrix + \StateSpaceSystemMatrix \SamplingPeriod)\MechanicalPlantState_k + \StateSpaceInputMatrix \SamplingPeriod \ControlInput_k = \MechanicalPlantState_k + \SamplingPeriod (\StateSpaceSystemMatrix\MechanicalPlantState_k + \StateSpaceInputMatrix\ControlInput_k)\) appears structurally identical to a Residual Network (ResNet) skip connection \(\MechanicalPlantState_{k+1} = \MechanicalPlantState_k + \ResidualFunction(\MechanicalPlantState_k)\). However, in physical dynamical systems, Euler truncation introduces spurious numerical energy that can artificially destabilize the model or ruin gradient propagation in backpropagation through time.
Consider an undamped harmonic physical oscillator \(\OscillatorAcceleration + \OscillatorNaturalFrequency^2 \OscillatorPosition = 0\) with state \(\MechanicalPlantState = [\OscillatorPosition, \OscillatorVelocity]^\top\) and continuous system matrix \(\StateSpaceSystemMatrix = \begin{bmatrix} 0 & 1 \\ -\OscillatorNaturalFrequency^2 & 0 \end{bmatrix}\). The continuous eigenvalues are purely imaginary: \(\MatrixEigenvalue = \pm \ImaginaryUnit\OscillatorNaturalFrequency\). Analyzing how common numerical discretization schemes map these eigenvalues to the complex \(\ZTransformVariable\)-plane illustrates their stability properties:
Exact Zero-Order Hold (Matrix Exponential): The discrete transition operator is \(\DiscreteStateSpaceSystemMatrix = e^{\StateSpaceSystemMatrix \SamplingPeriod}\). Its eigenvalues lie on the unit circle: \[ |\MatrixEigenvalue(\DiscreteStateSpaceSystemMatrix)| = |e^{\pm \ImaginaryUnit \OscillatorNaturalFrequency \SamplingPeriod}| = 1 \] This preserves the physical total energy \(\MechanicalEnergy = \frac{1}{2}\OscillatorVelocity^2 + \frac{1}{2}\OscillatorNaturalFrequency^2 \OscillatorPosition^2\) over infinite horizons without artificial growth or decay. Phase rotates at the true angular rate \(\OscillatorNaturalFrequency \SamplingPeriod\) per step.
Forward Euler Discretization: Approximating the transition as \(\ForwardEulerTransitionMatrix = \IdentityMatrix + \StateSpaceSystemMatrix \SamplingPeriod = \begin{bmatrix} 1 & \SamplingPeriod \\ -\OscillatorNaturalFrequency^2 \SamplingPeriod & 1 \end{bmatrix}\) yields eigenvalues \(\ForwardEulerEigenvalue = 1 \pm \ImaginaryUnit\OscillatorNaturalFrequency \SamplingPeriod\). Their complex modulus is: \[ |\ForwardEulerEigenvalue| = \sqrt{1 + \OscillatorNaturalFrequency^2 \SamplingPeriod^2} > 1 \quad \forall \SamplingPeriod > 0 \] Because the spectral radius strictly exceeds unity, forward Euler simulation injects artificial numerical energy at every step, converting an intrinsically conservative physical oscillator into an exponentially diverging explosion. In differentiable simulation and policy learning, this causes exploding gradients during backpropagation through time.
Backward (Implicit) Euler Discretization: Approximating transitions via \(\MechanicalPlantState_{k+1} = \MechanicalPlantState_k + \SamplingPeriod \StateSpaceSystemMatrix \MechanicalPlantState_{k+1}\) yields \(\BackwardEulerTransitionMatrix = (\IdentityMatrix - \StateSpaceSystemMatrix \SamplingPeriod)^{-1}\). The discrete eigenvalues evaluate to \(\BackwardEulerEigenvalue = \frac{1}{1 \mp \ImaginaryUnit\OscillatorNaturalFrequency \SamplingPeriod}\), with modulus: \[ |\BackwardEulerEigenvalue| = \frac{1}{\sqrt{1 + \OscillatorNaturalFrequency^2 \SamplingPeriod^2}} < 1 \quad \forall \SamplingPeriod > 0 \] While unconditionally stable, backward Euler introduces artificial numerical dissipation (numerical damping) that bleeds energy from the physical system. In unrolled optimization, that artificial contraction causes gradient vanishing over long horizons.
Bilinear (Tustin / Crank-Nicolson) Discretization: Employing the trapezoidal integration operator \(\LaplaceVariable \approx \frac{2}{\SamplingPeriod}\frac{\ZTransformVariable-1}{\ZTransformVariable+1}\) yields: \[ \TustinTransitionMatrix = \left(\IdentityMatrix - \frac{\SamplingPeriod}{2}\StateSpaceSystemMatrix\right)^{-1} \left(\IdentityMatrix + \frac{\SamplingPeriod}{2}\StateSpaceSystemMatrix\right) \] For purely imaginary continuous poles \(\LaplaceVariable = \pm \ImaginaryUnit\OscillatorNaturalFrequency\), the discrete eigenvalues evaluate to \(\TustinEigenvalue = \frac{1 \pm \ImaginaryUnit\OscillatorNaturalFrequency \SamplingPeriod / 2}{1 \mp \ImaginaryUnit\OscillatorNaturalFrequency \SamplingPeriod / 2}\). Their modulus is identically unity: \[ |\TustinEigenvalue| = \left| \frac{1 \pm \ImaginaryUnit\OscillatorNaturalFrequency \SamplingPeriod / 2}{1 \mp \ImaginaryUnit\OscillatorNaturalFrequency \SamplingPeriod / 2} \right| = \frac{\sqrt{1 + (\OscillatorNaturalFrequency \SamplingPeriod / 2)^2}}{\sqrt{1 + (\OscillatorNaturalFrequency \SamplingPeriod / 2)^2}} = 1 \quad \forall \SamplingPeriod > 0 \] The bilinear transform maps the continuous imaginary axis directly onto the discrete unit circle, preserving energy conservation without matrix exponentials. Higher frequencies undergo frequency warping, however, because \(\arctan(\OscillatorNaturalFrequency \SamplingPeriod / 2)\) compresses the phase advance per step.
Semi-Implicit (Symplectic) Euler Discretization: Staggering state updates by updating velocity with current acceleration and then updating position with the updated velocity: \[ \OscillatorStepVelocity_{k+1} = \OscillatorStepVelocity_k - \OscillatorNaturalFrequency^2 \SamplingPeriod \OscillatorPosition_k, \quad \OscillatorPosition_{k+1} = \OscillatorPosition_k + \SamplingPeriod \OscillatorStepVelocity_{k+1} = (1 - \OscillatorNaturalFrequency^2 \SamplingPeriod^2) \OscillatorPosition_k + \SamplingPeriod \OscillatorStepVelocity_k \] yields the discrete transition matrix: \[ \SymplecticEulerTransitionMatrix = \begin{bmatrix} 1 - \OscillatorNaturalFrequency^2 \SamplingPeriod^2 & \SamplingPeriod \\ -\OscillatorNaturalFrequency^2 \SamplingPeriod & 1 \end{bmatrix} \] The determinant evaluates to \(\det(\SymplecticEulerTransitionMatrix) = (1 - \OscillatorNaturalFrequency^2 \SamplingPeriod^2)(1) - (\SamplingPeriod)(-\OscillatorNaturalFrequency^2 \SamplingPeriod) = 1\) identically for all \(\SamplingPeriod > 0\). For this oscillator, the method is symplectic and preserves phase-space area. It conserves a modified quadratic energy rather than the original energy exactly; determinant one alone does not establish symplecticity in higher dimensions. For stable step sizes (\(\OscillatorNaturalFrequency \SamplingPeriod < 2\)), physical energy oscillates within bounded limits over infinite horizons rather than exploding or vanishing. The bounded energy error makes this method useful for conservative mechanical models, although contact, damping, and solver choices require separate analysis.
The integrator and step size must preserve the behavior needed by the application. Forward Euler can be adequate for sufficiently resolved dissipative dynamics; this undamped oscillator shows why a small step alone does not guarantee long-horizon energy fidelity. Sampling adds a separate phase effect (section 1.5.5).
Controllability and the Kalman rank condition
In deep learning, model capacity measures whether a neural architecture possesses the expressivity to represent a desired target function class. In physical systems, controllability addresses an even more fundamental structural question: does the physical actuator allocation \(\StateSpaceInputMatrix\) provide sufficient authority to steer the plant’s internal state \(\MechanicalPlantState\) to an arbitrary target configuration across the continuous dynamic transition graph \(\StateSpaceSystemMatrix\)?
An LTI system \((\StateSpaceSystemMatrix, \StateSpaceInputMatrix)\) is controllable if, for any initial state \(\MechanicalPlantState(0) = \MechanicalPlantState_0\) and any desired target state \(\TargetPlantState\), there exists a finite time \(\SteeringTransferTime > 0\) and an unconstrained input trajectory \(\ControlInput(\TimeInstant)\) that transfers the state from \(\MechanicalPlantState_0\) to \(\TargetPlantState\). This test does not impose actuator, state, or path limits. Under that unconstrained LTI model, the condition holds if and only if the controllability matrix\(\ControllabilityMatrix\) has full row rank: \[ \ControllabilityMatrix = \begin{bmatrix} \StateSpaceInputMatrix & \StateSpaceSystemMatrix\StateSpaceInputMatrix & \StateSpaceSystemMatrix^2\StateSpaceInputMatrix & \dots & \StateSpaceSystemMatrix^{n-1}\StateSpaceInputMatrix \end{bmatrix} \in \mathbb{R}^{\StateDimension \times \StateDimension \ControlInputDimension}, \quad \operatorname{rank}(\ControllabilityMatrix) = \StateDimension \] Each column block \(\StateSpaceSystemMatrix^k \StateSpaceInputMatrix\) represents the directional reach of the control input after \(\SystemMatrixPowerIndex\) steps of dynamic diffusion through the unforced system matrix \(\StateSpaceSystemMatrix\). The column space \(\operatorname{im}(\ControllabilityMatrix)\) defines the controllable subspace, mathematically equivalent to the \(\StateDimension\)-th order Krylov subspace \(\KrylovSubspace_n(\StateSpaceSystemMatrix, \operatorname{im}(\StateSpaceInputMatrix)) = \operatorname{span}\{\StateSpaceInputMatrix, \StateSpaceSystemMatrix\StateSpaceInputMatrix, \dots, \StateSpaceSystemMatrix^{n-1}\StateSpaceInputMatrix\}\).
The matrix sequence terminates at power \(\StateDimension-1\) because the Cayley-Hamilton theorem states that every square matrix \(\StateSpaceSystemMatrix \in \mathbb{R}^{\StateDimension \times \StateDimension}\) satisfies its own characteristic polynomial \(\CharacteristicPolynomial(\StateSpaceSystemMatrix)=\ZeroMatrix\), where: \[ \CharacteristicPolynomial(\MatrixEigenvalue) = \det(\MatrixEigenvalue \IdentityMatrix - \StateSpaceSystemMatrix) = \MatrixEigenvalue^\StateDimension + \CharacteristicPolynomialCoefficient_{n-1} \MatrixEigenvalue^{\StateDimension-1} + \dots + \CharacteristicPolynomialCoefficient_1 \MatrixEigenvalue + \CharacteristicPolynomialCoefficient_0 \] Substituting \(\StateSpaceSystemMatrix\) into its polynomial yields: \[ \StateSpaceSystemMatrix^n = -\CharacteristicPolynomialCoefficient_{n-1} \StateSpaceSystemMatrix^{n-1} - \CharacteristicPolynomialCoefficient_{n-2} \StateSpaceSystemMatrix^{n-2} - \dots - \CharacteristicPolynomialCoefficient_1 \StateSpaceSystemMatrix - \CharacteristicPolynomialCoefficient_0 \IdentityMatrix \] Multiplying repeatedly by \(\StateSpaceSystemMatrix\) demonstrates that every higher power \(\StateSpaceSystemMatrix^k\) for \(\SystemMatrixPowerIndex \ge \StateDimension\) can be expressed as a linear combination of the lower powers \(\{\IdentityMatrix, \StateSpaceSystemMatrix, \dots, \StateSpaceSystemMatrix^{n-1}\}\). Consequently, adding higher powers \(\StateSpaceSystemMatrix^k \StateSpaceInputMatrix\) for \(\SystemMatrixPowerIndex \ge \StateDimension\) cannot increase the dimension of the subspace spanned by the columns of \(\ControllabilityMatrix\).
If \(\operatorname{rank}(\ControllabilityMatrix) < \StateDimension\), the state space decomposes into a controllable subspace and an uncontrollable component in suitable coordinates. Notice the structural duality: the controllability matrix \(\ControllabilityMatrix\) evaluates whether control inputs can reach all \(\StateDimension\) state dimensions through powers of \(\StateSpaceSystemMatrix\), exactly dual to the observability matrix \(\ObservabilityMatrix\) (section 1.8), which evaluates whether sensor outputs can distinguish all \(\StateDimension\) state dimensions.
The Popov-Belevitch-Hautus (PBH) test provides an equivalent spectral condition: the pair \((\StateSpaceSystemMatrix, \StateSpaceInputMatrix)\) is controllable if and only if: \[ \operatorname{rank}\left( \begin{bmatrix} \MatrixEigenvalue \IdentityMatrix - \StateSpaceSystemMatrix & \StateSpaceInputMatrix \end{bmatrix} \right) = \StateDimension \quad \forall \MatrixEigenvalue \in \mathbb{C} \] If \(\operatorname{rank}([\MatrixEigenvalue \IdentityMatrix - \StateSpaceSystemMatrix, \StateSpaceInputMatrix]) < \StateDimension\) for some eigenvalue \(\MatrixEigenvalue\), the dynamic mode associated with \(\MatrixEigenvalue\) cannot be excited or influenced by any control input. If that uncontrollable mode is unstable (\(\operatorname{Re}(\MatrixEigenvalue) \ge 0\)), the physical plant is un-stabilizable, an unyielding physical limitation that no learned policy, foundation model, or reinforcement learning algorithm can overcome. Underactuation alone does not imply uncontrollability. Dynamic coupling can make an indirectly actuated state controllable; floating-base systems additionally depend on the available contacts (section 1.9).
Proportional-integral-derivative control as closed-loop error dissipation
When an arm link sags \(4\text{ mm}\) below its target height under a \(3\text{ kg}\) payload, position feedback pulls the joint toward the target, velocity feedback bleeds off overshoot, and accumulated-error feedback builds the extra torque needed to hold the load at zero steady-state offset. A joint-level Proportional-Integral-Derivative (PID) controller combines position error, accumulated error, and error rate to generate effort. For a simple mechanical plant, these terms act as a restoring spring, bias compensation, and damping. This interpretation explains their roles; PID is not generally an optimal controller, and positive gains alone do not guarantee stability.
In this subsection, let \(\MechanicalPlantState^*(\TimeInstant)\) and \(\MechanicalPlantState(\TimeInstant)\) denote desired and measured joint positions, rather than the full position-velocity state. Their difference defines the instantaneous tracking error: \[ \TrackingErrorVector(\TimeInstant) = \MechanicalPlantState^*(\TimeInstant) - \MechanicalPlantState(\TimeInstant) \] The continuous PID control law synthesizes commanded control effort \(\ControlInput(\TimeInstant)\) (e.g., motor drive current, electromagnetic torque, or hydraulic valve pressure) as: \[ \ControlInput(\TimeInstant) = \ProportionalGainMatrix \TrackingErrorVector(\TimeInstant) + \IntegralGainMatrix \int_0^\TimeInstant \TrackingErrorVector(\IntegrationDummyTime) \, d\IntegrationDummyTime + \DerivativeGainMatrix \TrackingErrorRate(\TimeInstant) \] where \(\ProportionalGainMatrix, \IntegralGainMatrix, \DerivativeGainMatrix \in \mathbb{R}^{\JointCount \times \JointCount}\) are symmetric, positive-definite gain matrices. For the energy argument below, assume a fixed setpoint, a constant scalar inertia \(\Mass>0\), and no saturation. Each term has a distinct role:
Proportional Term (\(\ProportionalGainMatrix \TrackingErrorVector\)): Conservative Restoring Force / Gradient Descent: Consider an artificial quadratic potential loss landscape \(\ProportionalPotentialEnergy(\TrackingErrorVector) = \frac{1}{2} \TrackingErrorVector^\top \ProportionalGainMatrix \TrackingErrorVector\). Because \(\TrackingErrorVector = \MechanicalPlantState^* - \MechanicalPlantState\), the restoring force is the negative gradient with respect to position, \(-\nabla_{\MechanicalPlantState}\ProportionalPotentialEnergy = \ProportionalGainMatrix\TrackingErrorVector\). Proportional feedback behaves as an elastic spring or continuous negative gradient step that pulls the physical coordinates toward the reference setpoint. However, on an undamped second-order plant (\(\Mass \PlantStateAcceleration = \ControlInput\)), proportional feedback alone generates an undamped harmonic oscillator \(\Mass \TrackingErrorAcceleration + \ProportionalGainMatrix \TrackingErrorVector = \ZeroMatrix\). The system conserves total mechanical energy \(\MechanicalEnergy = \frac{1}{2} \Mass \|\TrackingErrorRate\|^2 + \frac{1}{2} \TrackingErrorVector^\top \ProportionalGainMatrix \TrackingErrorVector\), orbiting closed level curves indefinitely with zero asymptotic convergence.
Derivative Term (\(\DerivativeGainMatrix \TrackingErrorRate\)): Rayleigh Dissipation / Momentum Damping: To force convergence to the origin, kinetic energy must be extracted from the closed loop. The derivative term introduces an artificial viscous damper or Rayleigh dissipation function \(\RayleighDissipationFunction(\TrackingErrorRate) = \frac{1}{2} \TrackingErrorRate^\top \DerivativeGainMatrix \TrackingErrorRate\). Differentiating the closed-loop mechanical energy \(\MechanicalEnergy(\TrackingErrorVector, \TrackingErrorRate)\) along plant trajectories under PD feedback yields: \[ \MechanicalEnergyDerivative = \Mass \TrackingErrorRate^\top \TrackingErrorAcceleration + \TrackingErrorRate^\top \ProportionalGainMatrix \TrackingErrorVector = \TrackingErrorRate^\top \left( -\ProportionalGainMatrix \TrackingErrorVector - \DerivativeGainMatrix \TrackingErrorRate \right) + \TrackingErrorRate^\top \ProportionalGainMatrix \TrackingErrorVector = -\TrackingErrorRate^\top \DerivativeGainMatrix \TrackingErrorRate \le 0 \] Because \(\DerivativeGainMatrix \succ \ZeroMatrix\), the derivative term converts tracking kinetic energy into continuous dissipation, damping oscillations and, for this plant, steering closed-loop poles into the open left-half complex plane. In machine learning optimization, this is analogous to the friction term in the continuous heavy-ball model used to stabilize gradient oscillations across steep ravines.
Integral Term (\(\IntegralGainMatrix \int_0^\TimeInstant \TrackingErrorVector(\IntegrationDummyTime) \, d\IntegrationDummyTime\)): Online Disturbance / Unmodeled Bias Estimation: In physical deployments, constant external loads \(\ExternalDisturbanceLoad\), such as gravitational sagging on robot arm links, cable friction, or payload offsets, induce a steady-state standoff. At rest (\(\TrackingErrorRate = \ZeroMatrix, \TrackingErrorAcceleration = \ZeroMatrix\)), a PD controller yields \(\ProportionalGainMatrix \SteadyStateTrackingError + \ExternalDisturbanceLoad = \ZeroMatrix\), leaving a persistent tracking error \(\SteadyStateTrackingError = -\ProportionalGainMatrix^{-1} \ExternalDisturbanceLoad\). The integral term introduces an internal dynamic state variable: \[ \IntegratorStateRate(\TimeInstant) = \TrackingErrorVector(\TimeInstant), \quad \IntegralControlEffort(\TimeInstant) = \IntegralGainMatrix \IntegratorState(\TimeInstant) \] Accumulating tracking error over time forces \(\IntegratorState(\TimeInstant)\) to integrate until \(\IntegralGainMatrix \SteadyStateIntegratorState = -\ExternalDisturbanceLoad\), counteracting the disturbance and driving steady-state position error to zero (\(\SteadyStateTrackingError \to \ZeroMatrix\)). In optimization terms, the integral term acts as an online running bias estimator that dynamically learns and cancels constant model mismatch.
For a scalar single-degree-of-freedom mechanical plant with mass \(\Mass\), the closed-loop error equation under PD control evaluates to: \[ \ScalarTrackingErrorAcceleration(\TimeInstant) + 2\DampingRatio \UndampedNaturalFrequency \ScalarTrackingErrorRate(\TimeInstant) + \UndampedNaturalFrequency^2 \ScalarTrackingError(\TimeInstant) = 0 \] where the undamped natural frequency is \(\UndampedNaturalFrequency = \sqrt{\PidProportionalGain / \Mass}\) and the dimensionless damping ratio is \(\DampingRatio = \frac{\PidDerivativeGain}{2 \sqrt{\Mass \PidProportionalGain}}\). The characteristic roots \(\LaplaceVariable_{1,2} = -\DampingRatio \UndampedNaturalFrequency \pm \UndampedNaturalFrequency \sqrt{\DampingRatio^2 - 1}\) partition the closed-loop tracking response into three fundamental physical regimes:
Underdamped (\(0 < \DampingRatio < 1\)): Complex conjugate poles; fast initial rise time but decaying ringing oscillations and overshoot that can damage mechanical gearboxes.
Critically Damped (\(\DampingRatio = 1\)): Repeated real negative root \(\LaplaceVariable = -\UndampedNaturalFrequency\); for fixed \(\UndampedNaturalFrequency\), gives the fastest nonoscillatory step response from rest in this second-order model.
Overdamped (\(\DampingRatio > 1\)): Distinct negative real roots; gives a slower nonoscillatory step response from rest. Other initial velocities can still produce a setpoint crossing.
Practical firmware implementation: Anti-windup and derivative filtering
While textbook PID assumes infinite actuator authority and continuous differentiation, deploying PID loops on physical robot joints exposes two critical cyber-physical pathologies:
Derivative Kick and Noise Amplification: In textbook PID, the derivative term evaluates error rate \(\DerivativeControlEffort = \DerivativeGainMatrix \TrackingErrorRate = \DerivativeGainMatrix (\JointVelocitySetpoint - \GeneralizedVelocities)\). When a learned policy or planner issues a step setpoint or hands over between action chunks (Transfer Without Discontinuity), the sudden change in reference setpoint \(\JointPositionSetpoint\) creates an impulsive derivative spike \(\JointVelocitySetpoint \approx \infty\) (derivative kick) that commands explosive actuator torques and can trigger current limiting or an inverter fault. Furthermore, discrete differentiation of quantized encoder signals amplifies high-frequency measurement noise as \(\mathcal{O}(\AngularFrequency)\). Production firmware resolves this by differentiating solely on the measured plant state (\(\DerivativeControlEffort = -\DerivativeGainMatrix \GeneralizedVelocities\)) and routing the velocity through a first-order low-pass filter: \[ \DerivativeControlEffort(\LaplaceVariable) = -\frac{\DerivativeGainMatrix \LaplaceVariable}{\DerivativeFilterTimeConstant \LaplaceVariable + 1} \GeneralizedCoordinates(\LaplaceVariable) \] where \(\DerivativeFilterTimeConstant > 0\) sets the derivative filter roll-off frequency, preserving damping in the control band while limiting the high-frequency gain that an ideal differentiator would amplify without bound.
Actuator Saturation and Integral Windup: Physical actuators are strictly bounded by torque and thermal current limits (\(\JointTorqueLowerLimit \le \JointTorque \le \JointTorqueUpperLimit\)). When a large disturbance or aggressive maneuver pushes commanded torque past physical limits, the physical plant cannot accelerate fast enough to eliminate tracking error. Meanwhile, the integral term continues accumulating error unhindered, winding up the internal dynamic state \(\IntegratorState(\TimeInstant)\) to massive values. Once the plant finally reaches the setpoint, the stored integrator charge forces the actuator to remain saturated in the forward direction, causing violent overshoot, severe limit-cycle hunting, and structural collisions. Real-time motor firmware implements anti-windup circuitry: clamping the integrator state whenever actuator limits are reached, or employing back-calculation: \[ \IntegratorStateRate(\TimeInstant) = \TrackingErrorVector(\TimeInstant) + \frac{1}{\AntiWindupTrackingTimeConstant} \IntegralGainMatrix^{-1} \left( \SaturatedJointTorque(\TimeInstant) - \PresaturationTorqueCommand(\TimeInstant) \right) \] where \(\ControlInput=\JointTorque\), the invertible integral gain converts the torque mismatch back to error-integral units, \(\AntiWindupTrackingTimeConstant > 0\) is a tracking time constant and \(\SaturatedJointTorque = \operatorname{clip}(\PresaturationTorqueCommand, \JointTorqueLowerLimit, \JointTorqueUpperLimit)\). When the actuator is unsaturated (\(\SaturatedJointTorque = \PresaturationTorqueCommand\)), standard integration proceeds; when saturated, the mismatch feeds back to discharge the integrator dynamically, reducing the excess stored integral command upon desaturation. With integral action, stability must be checked again: for the scalar mass model, positive gains must also satisfy \(\PidDerivativeGain \PidProportionalGain>\Mass \PidIntegralGain\).
Feedback linearization: Transforming nonlinear dynamics into decoupled double integrators
Swinging a manipulator’s elbow outward changes the shoulder’s effective inertia and injects centrifugal torque into the shoulder joint, so fixed linear gains tuned with the arm tucked become sluggish or oscillatory when the arm extends. Multi-body robotic plants are governed by coupled, configuration-dependent equations of motion (Actuator Transmission Limits): \[ \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \GeneralizedAccelerations + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) = \JointTorque \] where \(\GeneralizedInertiaMatrix(\GeneralizedCoordinates) \in \mathbb{R}^{\JointCount \times \JointCount}\) is the configuration-dependent mass matrix, \(\CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities)\GeneralizedVelocities\) collects velocity-dependent generalized forces, \(\GravitationalTorqueVector(\GeneralizedCoordinates)\) is gravity, and \(\JointTorque \in \mathbb{R}^\JointCount\) is the joint torque vector.
Feedback linearization, here implemented as computed torque control, uses a model of inertia, velocity coupling, and gravity to turn a requested joint acceleration into torque. The cancellation below assumes a fully actuated rigid-link model, valid joint coordinates, exact parameters, and sufficient actuator authority. It does not apply unchanged to a floating base or to saturated drives.
Because the physical inertia matrix \(\GeneralizedInertiaMatrix(\GeneralizedCoordinates)\) is symmetric and strictly positive definite everywhere on the configuration space (\(\GeneralizedInertiaMatrix(\GeneralizedCoordinates) = \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^\top \succ \ZeroMatrix\)), it is globally invertible. We define the nonlinear state-feedback control law: \[ \JointTorque = \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \VirtualAccelerationCommand + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) \] where \(\VirtualAccelerationCommand \in \mathbb{R}^\JointCount\) is a synthetic, virtual acceleration command. Substituting this control law directly into the physical equations of motion yields: \[ \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \GeneralizedAccelerations + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) = \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \VirtualAccelerationCommand + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) \] Subtracting the identical Coriolis and gravity vectors from both sides isolates: \[ \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \left( \GeneralizedAccelerations - \VirtualAccelerationCommand \right) = \ZeroMatrix \] Because \(\GeneralizedInertiaMatrix(\GeneralizedCoordinates)\) is non-singular, multiplying by \(\GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1}\) collapses the coupled nonlinear plant into \(\JointCount\) decoupled, linear double-integrator channels: \[ \GeneralizedAccelerations = \VirtualAccelerationCommand \iff \JointAcceleration_i = \JointAcceleration_i^* \quad \forall \JointIndex \in \{1, \dots, \JointCount\} \] Over these decoupled channels, linear tracking control laws synthesize virtual acceleration commands with global exponential stability: \[ \VirtualAccelerationCommand = \ReferenceJointAccelerations + \ProportionalGainMatrix (\ReferenceJointPositions - \GeneralizedCoordinates) + \DerivativeGainMatrix (\ReferenceJointVelocities - \GeneralizedVelocities) \] Substituting \(\VirtualAccelerationCommand\) back into the double-integrator dynamics reveals that the tracking error \(\TrackingErrorVector(\TimeInstant) = \ReferenceJointPositions(\TimeInstant) - \GeneralizedCoordinates(\TimeInstant)\) satisfies the linear error ODE: \[ \TrackingErrorAcceleration + \DerivativeGainMatrix \TrackingErrorRate + \ProportionalGainMatrix \TrackingErrorVector = \ZeroMatrix \] Choosing positive-diagonal gain matrices \(\ProportionalGainMatrix\) and \(\DerivativeGainMatrix\) guarantees that tracking errors in every joint coordinate decouple and decay to zero independently at the designer-specified rate. Each joint then behaves as an independent second-order oscillator whose natural frequency and damping ratio follow directly from its diagonal gain pair.
Lie-derivative formulation for general control-affine systems
For general control-affine nonlinear dynamical systems, whose state rate \(\PlantStateRate\) is a drift field \(\ControlAffineDriftField(\MechanicalPlantState)\) plus an input matrix field applied linearly to \(\ControlInput\), with output \(\MeasurementVector = \NonlinearOutputFunction(\MechanicalPlantState)\), feedback linearization differentiates each output channel \(\OutputChannel_{\OutputIndex}\) with respect to continuous time until the control input \(\ControlInput\) appears explicitly: \[ \OutputChannel_{\OutputIndex}^{(\OutputRelativeDegree_{\OutputIndex})} = \DriftLieDerivativeOperator^{\OutputRelativeDegree_{\OutputIndex}} \OutputComponentFunction_{\OutputIndex}(\MechanicalPlantState) + \InputLieDerivativeOperator \DriftLieDerivativeOperator^{\OutputRelativeDegree_{\OutputIndex}-1} \OutputComponentFunction_{\OutputIndex}(\MechanicalPlantState) \ControlInput \] where \(\OutputRelativeDegree_{\OutputIndex}\) is the relative degree of output \(\OutputIndex\). Collecting all output channels into vector form: \[ \MeasurementVector^{(\OutputRelativeDegree)} = \DecouplingDriftVector(\MechanicalPlantState) + \DecouplingMatrix(\MechanicalPlantState) \ControlInput \] where \(\DecouplingMatrix(\MechanicalPlantState) \in \mathbb{R}^{\OutputDimension \times \ControlInputDimension}\) is the decoupling matrix whose entries are Lie derivatives \(\DecouplingMatrixElement_{\OutputIndex, j}(\MechanicalPlantState) = \InputColumnLieDerivativeOperator \DriftLieDerivativeOperator^{\OutputRelativeDegree_{\OutputIndex}-1} \OutputComponentFunction_{\OutputIndex}(\MechanicalPlantState)\). If \(\DecouplingMatrix(\MechanicalPlantState)\) is square (\(\OutputDimension = \ControlInputDimension\)) and non-singular across the operating envelope, the state-feedback law: \[ \ControlInput = \DecouplingMatrix(\MechanicalPlantState)^{-1} \left( \LinearizedVirtualInput - \DecouplingDriftVector(\MechanicalPlantState) \right) \] cancels the modeled nonlinearities in the output equations, yielding \(\MeasurementVector^{(\OutputRelativeDegree)} = \LinearizedVirtualInput\). Here \(\DriftLieDerivativeOperator \OutputComponentFunction=\nabla \OutputComponentFunction^\top \ControlAffineDriftField\) is the rate of change of \(\OutputComponentFunction\) along \(\ControlAffineDriftField\), and repeated Lie derivatives differentiate that rate again. If \(\sum_{\OutputIndex} \OutputRelativeDegree_{\OutputIndex}\) is less than the dimension of \(\MechanicalPlantState\), internal states remain outside this input-output description; their zero dynamics must also be stable.
Unlike deep learning architectures where model inaccuracies are accommodated by statistical loss minimization, computed torque control assumes perfect knowledge of physical parameters (\(\GeneralizedInertiaMatrix, \CoriolisCentrifugalMatrix, \GravitationalTorqueVector\)). In real hardware, parametric inaccuracies (uncertain link masses, payload variations) leave residual nonlinear perturbations \(\ResidualDynamicsPerturbation = (\GeneralizedInertiaMatrix - \EstimatedInertiaMatrix)\GeneralizedAccelerations + (\CoriolisCentrifugalMatrix - \EstimatedCoriolisMatrix)\GeneralizedVelocities + (\GravitationalTorqueVector - \EstimatedGravitationalTorque)\). Physical AI architectures resolve this by pairing feedback linearization with robust inner loops (sliding-mode control or \(\Glyph{H_\infty}\) disturbance observers) or online adaptive parameter estimators.
In modern physical AI architectures, computed torque control also provides an analytical, differentiable physics inductive bias for learned policies. Rather than forcing a high-capacity neural network to learn the entire multi-body dynamics from scratch, the system splits torque synthesis into a nominal analytical dynamics model and a learned residual policy: \[ \JointTorque = \EstimatedInertiaMatrix(\GeneralizedCoordinates) \VirtualAccelerationCommand + \EstimatedCoriolisMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \EstimatedGravitationalTorque(\GeneralizedCoordinates) + \ResidualPolicyTorque(\GeneralizedCoordinates, \GeneralizedVelocities, \VirtualAccelerationCommand) \] The baseline physics model cancels the dominant, known multi-body nonlinearities, while the neural network \(\ResidualPolicyTorque\) is trained via reinforcement learning or imitation learning to capture unmodeled physical effects: joint cable friction, transmission backlash, hydrodynamic drag, and payload variations. This hybrid formulation can reduce the dynamics a policy must learn, but does not preserve baseline stability automatically. The residual needs an amplitude or energy bound compatible with the nominal controller, and the combined command must satisfy actuator limits.
Linear quadratic regulator and algebraic Riccati equations
In reinforcement learning (The latch task as a reward problem), policy optimization searches for an action policy \(\Policy(\RobotAction \mid \DecisionProcessState)\) that maximizes expected cumulative reward via trial-and-error sampling or learned critic networks \(\CriticReturnEstimate(\DecisionProcessState)\) / \(\CriticActionReturnEstimate(\DecisionProcessState, \RobotAction)\), frequently struggling with high gradient variance, non-convex loss landscapes, and empirical stability failures. In contrast, for linear-quadratic physical dynamics, the Linear Quadratic Regulator (LQR) provides the exact, analytical solution to the continuous-state Bellman optimality equation over an infinite time horizon, as summarized against reinforcement learning below:
Parameterized deep neural network \(\CriticReturnEstimate(\DecisionProcessState)\) or \(\CriticActionReturnEstimate(\DecisionProcessState, \RobotAction)\)
Direct algebraic factorizations (Schur decomposition, ARE solvers)
Stability Guarantees
Not supplied by empirical reward optimization alone
Stable under the stated model and rank conditions; ideal full-state margins below
Sample Complexity
Requires task- and algorithm-dependent training data
No policy-training samples once \((\StateSpaceSystemMatrix, \StateSpaceInputMatrix, \LqrStateWeightMatrix, \LqrControlWeightMatrix)\) are specified; identifying the model still needs evidence
Given the controllable continuous LTI system \(\PlantStateRate = \StateSpaceSystemMatrix\MechanicalPlantState + \StateSpaceInputMatrix\ControlInput\), the infinite-horizon regulator minimizes the quadratic performance objective: \[ \LqrQuadraticObjective = \int_0^\infty \left( \MechanicalPlantState(\TimeInstant)^\top \LqrStateWeightMatrix \MechanicalPlantState(\TimeInstant) + \ControlInput(\TimeInstant)^\top \LqrControlWeightMatrix \ControlInput(\TimeInstant) \right) d\TimeInstant \] where:
\(\LqrStateWeightMatrix \in \mathbb{R}^{\StateDimension \times \StateDimension}\) is a symmetric positive semi-definite state penalty matrix (\(\LqrStateWeightMatrix \succeq \ZeroMatrix\)), penalizing deviations from the target state \(\MechanicalPlantState^* = \ZeroMatrix\).
\(\LqrControlWeightMatrix \in \mathbb{R}^{\ControlInputDimension \times \ControlInputDimension}\) is a symmetric positive definite control penalty matrix (\(\LqrControlWeightMatrix \succ \ZeroMatrix\)), penalizing control effort.
The weights express a trade-off, not hard physical limits. Increasing \(\LqrControlWeightMatrix\) discourages effort, while increasing \(\LqrStateWeightMatrix\) discourages state error. Neither guarantees a current ceiling, a torque slew rate, or a position bound. Those requirements need explicit constraints, such as the MPC formulation below, or a separate admissibility check (Actuator Transmission Limits).
Dynamic programming derivation of continuous LQR
In continuous time, Bellman’s principle of optimality (Bellman 1957; Åström and Murray 2021) states that the optimal cost-to-go from state \(\MechanicalPlantState(\TimeInstant)\) across an infinitesimal time step \(\FiniteChange \TimeInstant \to 0\) satisfies: \[ \OptimalCostToGoFunction^*(\MechanicalPlantState(\TimeInstant)) = \min_{\ControlInputWindow} \left\{ \int_t^{\TimeInstant+\FiniteChange \TimeInstant} \left( \MechanicalPlantState(\IntegrationDummyTime)^\top \LqrStateWeightMatrix \MechanicalPlantState(\IntegrationDummyTime) + \ControlInput(\IntegrationDummyTime)^\top \LqrControlWeightMatrix \ControlInput(\IntegrationDummyTime) \right) d\IntegrationDummyTime + \OptimalCostToGoFunction^*(\MechanicalPlantState(\TimeInstant+\FiniteChange \TimeInstant)) \right\} \] Expanding the integral to first order in \(\FiniteChange \TimeInstant\) and applying a first-order Taylor expansion to the terminal cost gives: \[ \OptimalCostToGoFunction^*(\MechanicalPlantState(\TimeInstant+\FiniteChange \TimeInstant)) = \OptimalCostToGoFunction^*(\MechanicalPlantState(\TimeInstant)) + \nabla \OptimalCostToGoFunction^*(\MechanicalPlantState(\TimeInstant))^\top \PlantStateRate(\TimeInstant) \FiniteChange \TimeInstant + \mathcal{O}(\FiniteChange \TimeInstant^2) \] Substituting the plant dynamics \(\PlantStateRate = \StateSpaceSystemMatrix\MechanicalPlantState + \StateSpaceInputMatrix\ControlInput\), subtracting \(\OptimalCostToGoFunction^*(\MechanicalPlantState(\TimeInstant))\) from both sides, dividing by \(\FiniteChange \TimeInstant\), and taking the limit \(\FiniteChange \TimeInstant \to 0\) yields the continuous-time Hamilton-Jacobi-Bellman (HJB) equation: \[ 0 = \min_{\ControlInput} \left[ \MechanicalPlantState^\top \LqrStateWeightMatrix \MechanicalPlantState + \ControlInput^\top \LqrControlWeightMatrix \ControlInput + \nabla \OptimalCostToGoFunction^*(\MechanicalPlantState)^\top (\StateSpaceSystemMatrix\MechanicalPlantState + \StateSpaceInputMatrix\ControlInput) \right] \] Postulating a quadratic value function ansatz (the optimal continuous critic) \(\OptimalCostToGoFunction^*(\MechanicalPlantState) = \MechanicalPlantState^\top \LqrCostToGoMatrix \MechanicalPlantState\) with symmetric matrix \(\LqrCostToGoMatrix=\LqrCostToGoMatrix^\top\) (positive definite under the controllability and observability assumptions stated below), its spatial gradient evaluates to: \[ \nabla \OptimalCostToGoFunction^*(\MechanicalPlantState) = 2 \LqrCostToGoMatrix \MechanicalPlantState \] Differentiating the expression inside the bracket with respect to control input \(\ControlInput\) and setting the derivative to zero yields the stationarity condition: \[ 2 \LqrControlWeightMatrix \ControlInput + 2 \StateSpaceInputMatrix^\top \LqrCostToGoMatrix \MechanicalPlantState = \ZeroMatrix \implies \ControlInput^*(\TimeInstant) = -\LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix \MechanicalPlantState(\TimeInstant) = -\LqrFeedbackGain \MechanicalPlantState(\TimeInstant) \] The optimal state-feedback control law (the optimal actor policy) is strictly linear in the state, with constant optimal gain matrix \(\LqrFeedbackGain = \LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix\). Substituting this optimal control law \(\ControlInput^*\) back into the HJB equation eliminates the minimization: \[ \MechanicalPlantState^\top \LqrStateWeightMatrix \MechanicalPlantState + \MechanicalPlantState^\top \LqrCostToGoMatrix \StateSpaceInputMatrix \LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix \MechanicalPlantState + 2 \MechanicalPlantState^\top \LqrCostToGoMatrix \StateSpaceSystemMatrix \MechanicalPlantState - 2 \MechanicalPlantState^\top \LqrCostToGoMatrix \StateSpaceInputMatrix \LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix \MechanicalPlantState = 0 \] Combining quadratic terms and using the matrix transpose identity \(2 \MechanicalPlantState^\top \LqrCostToGoMatrix \StateSpaceSystemMatrix \MechanicalPlantState = \MechanicalPlantState^\top (\StateSpaceSystemMatrix^\top \LqrCostToGoMatrix + \LqrCostToGoMatrix \StateSpaceSystemMatrix) \MechanicalPlantState\) reveals: \[ \MechanicalPlantState^\top \left( \StateSpaceSystemMatrix^\top \LqrCostToGoMatrix + \LqrCostToGoMatrix \StateSpaceSystemMatrix - \LqrCostToGoMatrix \StateSpaceInputMatrix \LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix + \LqrStateWeightMatrix \right) \MechanicalPlantState = 0 \quad \forall \MechanicalPlantState \in \mathbb{R}^\StateDimension \] Because this equality must hold identically for all states \(\MechanicalPlantState\), the bracketed matrix equation must vanish, yielding the canonical Continuous Algebraic Riccati Equation (ARE): \[ \StateSpaceSystemMatrix^\top \LqrCostToGoMatrix + \LqrCostToGoMatrix \StateSpaceSystemMatrix - \LqrCostToGoMatrix \StateSpaceInputMatrix \LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix + \LqrStateWeightMatrix = \ZeroMatrix \] Substituting the control law into the plant dynamics yields the closed-loop system \(\PlantStateRate = (\StateSpaceSystemMatrix - \StateSpaceInputMatrix\LqrFeedbackGain)\MechanicalPlantState\). When \((\StateSpaceSystemMatrix, \StateSpaceInputMatrix)\) is controllable and \((\StateSpaceSystemMatrix, \LqrStateWeightMatrix^{1/2})\) is observable, all closed-loop eigenvalues are strictly Hurwitz (\(\operatorname{Re}(\MatrixEigenvalue_i(\StateSpaceSystemMatrix - \StateSpaceInputMatrix\LqrFeedbackGain)) < 0\)), guaranteeing exponential convergence to the origin.
Bellman, Richard. 1957. Dynamic Programming. Princeton University Press.
LQR as an optimal closed-loop error dissipator
Beyond minimizing quadratic cost, the Riccati solution \(\LqrCostToGoMatrix \succ \ZeroMatrix\) provides a Lyapunov function for the nominal closed loop. With a positive-definite running cost it also supplies a strict decrease certificate. Differentiating \(\OptimalCostToGoFunction^*(\MechanicalPlantState) = \MechanicalPlantState^\top \LqrCostToGoMatrix \MechanicalPlantState\) along the closed-loop trajectories of \(\PlantStateRate = (\StateSpaceSystemMatrix - \StateSpaceInputMatrix\LqrFeedbackGain)\MechanicalPlantState\) gives: \[
\begin{aligned}
\OptimalCostToGoDerivative^*(\MechanicalPlantState) &= \PlantStateRate^\top \LqrCostToGoMatrix \MechanicalPlantState + \MechanicalPlantState^\top \LqrCostToGoMatrix \PlantStateRate = \MechanicalPlantState^\top \left[ (\StateSpaceSystemMatrix - \StateSpaceInputMatrix\LqrFeedbackGain)^\top \LqrCostToGoMatrix + \LqrCostToGoMatrix (\StateSpaceSystemMatrix - \StateSpaceInputMatrix\LqrFeedbackGain) \right] \MechanicalPlantState \\
&= \MechanicalPlantState^\top \left[ \StateSpaceSystemMatrix^\top \LqrCostToGoMatrix + \LqrCostToGoMatrix\StateSpaceSystemMatrix - 2\LqrCostToGoMatrix\StateSpaceInputMatrix\LqrControlWeightMatrix^{-1}\StateSpaceInputMatrix^\top\LqrCostToGoMatrix \right] \MechanicalPlantState
\end{aligned}
\] Substituting the Algebraic Riccati Equation \(\StateSpaceSystemMatrix^\top \LqrCostToGoMatrix + \LqrCostToGoMatrix\StateSpaceSystemMatrix = \LqrCostToGoMatrix\StateSpaceInputMatrix\LqrControlWeightMatrix^{-1}\StateSpaceInputMatrix^\top\LqrCostToGoMatrix - \LqrStateWeightMatrix\) into this expression yields: \[ \OptimalCostToGoDerivative^*(\MechanicalPlantState) = \MechanicalPlantState^\top \left[ -\LqrStateWeightMatrix - \LqrCostToGoMatrix\StateSpaceInputMatrix\LqrControlWeightMatrix^{-1}\StateSpaceInputMatrix^\top\LqrCostToGoMatrix \right] \MechanicalPlantState = -\MechanicalPlantState^\top \left[ \LqrStateWeightMatrix + \LqrFeedbackGain^\top \LqrControlWeightMatrix \LqrFeedbackGain \right] \MechanicalPlantState \le 0 \] The derivative is strictly negative when the running-cost matrix is positive definite; with semidefinite \(\LqrStateWeightMatrix\) it can vanish at a nonzero state, even though observability establishes asymptotic convergence. The equality identifies the dissipated quantity as cost, not necessarily physical energy. The value function decreases at a rate matching the instantaneous running cost \(\MechanicalPlantState^\top \LqrStateWeightMatrix \MechanicalPlantState + \ControlInput^\top \LqrControlWeightMatrix \ControlInput\).
Robustness margins of continuous LQR
Continuous-time full-state LQR has a useful robustness property (Åström and Murray 2021) under the ideal full-state assumptions, expressed by the frequency-domain Kalman return difference equality: \[ \left( \IdentityMatrix + \OpenLoopTransferMatrix(\ImaginaryUnit\AngularFrequency) \right)^* \LqrControlWeightMatrix \left( \IdentityMatrix + \OpenLoopTransferMatrix(\ImaginaryUnit\AngularFrequency) \right) = \LqrControlWeightMatrix + \StateSpaceInputMatrix^\top (-\ImaginaryUnit\AngularFrequency \IdentityMatrix - \StateSpaceSystemMatrix^\top)^{-1} \LqrStateWeightMatrix (\ImaginaryUnit\AngularFrequency \IdentityMatrix - \StateSpaceSystemMatrix)^{-1} \StateSpaceInputMatrix \] where \(^*\) denotes conjugate transpose and \(\OpenLoopTransferMatrix(\ImaginaryUnit\AngularFrequency) = \LqrFeedbackGain(\ImaginaryUnit\AngularFrequency\IdentityMatrix - \StateSpaceSystemMatrix)^{-1}\StateSpaceInputMatrix\) is the open-loop frequency response broken at the plant input. Because \(\LqrStateWeightMatrix \succeq \ZeroMatrix\), the second term on the right-hand side is positive semi-definite at frequencies where the resolvents exist. For a single-input system with scalar \(\LqrControlWeightMatrix > 0\), this establishes: \[ |1 + \OpenLoopTransferFunction(\ImaginaryUnit\AngularFrequency)| \ge 1 \quad \forall \AngularFrequency \in \mathbb{R} \] Geometrically on the Nyquist plane, the loop transfer locus \(\OpenLoopTransferFunction(\ImaginaryUnit\AngularFrequency)\) never enters the open unit disk centered at \(-1 + \ImaginaryUnit0\). This property confers two guaranteed stability margins:
Infinite Gain Margin: The closed loop remains asymptotically stable under scalar gain scaling \(\LoopGainScalingFactor \in (1/2, \infty)\) inserted into the control loop.
Phase Margin Guarantee: The distance from \(-1\) to the Nyquist curve is at least \(1\), which by elementary trigonometry guarantees at least \(60^\circ\) of phase margin for the single-input loop: \[ \PhaseMargin \ge 2 \arcsin(1/2) = 60^\circ \]
These are continuous-time, full-state, nominal-model margins at the specified loop break. They are not an unrestricted guarantee against actuator dynamics, structural resonances, or simultaneous multivariable perturbations. Digital sampling and transport add phase lag; section 1.5.5 gives the low-frequency delay approximation used to budget that loss.
The separation principle and linear quadratic Gaussian control
In physical deployments, full-state access is rarely available; controllers must estimate \(\StateEstimate\) from noisy sensor observations \(\MeasurementVector = \MeasurementMatrix\MechanicalPlantState + \MeasurementNoiseVector\). Under a linear model with independent Gaussian white process and measurement noises of intensities \(\ProcessNoiseIntensity\) and \(\ContinuousMeasurementNoiseIntensity\), the optimal continuous state estimator is the continuous-time Kalman-Bucy filter (section 1.8; the discrete filter the machine runs is in Estimating State from Dated Measurements): \[ \StateEstimateDerivative(\TimeInstant) = \StateSpaceSystemMatrix\StateEstimate(\TimeInstant) + \StateSpaceInputMatrix\ControlInput(\TimeInstant) + \KalmanBucyObserverGain\left(\MeasurementVector(\TimeInstant) - \MeasurementMatrix\StateEstimate(\TimeInstant)\right) \] where \(\MeasurementVector(\TimeInstant) - \MeasurementMatrix\StateEstimate(\TimeInstant)\) is the innovation (the measurement residual), and the optimal Kalman observer gain is: \[ \KalmanBucyObserverGain = \SteadyStateErrorCovariance\MeasurementMatrix^\top \ContinuousMeasurementNoiseIntensity^{-1} \] At steady state, \(\SteadyStateErrorCovariance = \mathbb{E}[(\MechanicalPlantState - \StateEstimate)(\MechanicalPlantState - \StateEstimate)^\top]\) is the state estimation error covariance matrix, which solves the dual filter algebraic Riccati equation under the usual stabilizability and detectability conditions: \[ \StateSpaceSystemMatrix \SteadyStateErrorCovariance + \SteadyStateErrorCovariance \StateSpaceSystemMatrix^\top - \SteadyStateErrorCovariance \MeasurementMatrix^\top \ContinuousMeasurementNoiseIntensity^{-1} \MeasurementMatrix \SteadyStateErrorCovariance + \ProcessNoiseIntensity = \ZeroMatrix \]
To an ML practitioner, the Kalman filter is the exact recursive Bayesian Minimum Mean Square Error (MMSE) estimator: it continuously updates the posterior state distribution \(\GaussianDistribution(\StateEstimate, \SteadyStateErrorCovariance)\) by balancing dynamic model propagation (growing uncertainty via process noise \(\ProcessNoiseIntensity\)) against empirical sensor evidence (contracting uncertainty via inverse continuous measurement-noise intensity \(\ContinuousMeasurementNoiseIntensity^{-1}\)). For discrete measurements, the analogous update uses the per-sample measurement covariance \(\MeasurementNoiseCovariance\), not the continuous noise intensity. With invertible prior and measurement covariances, it evaluates as: \[ \UpdatedErrorCovariance^{-1} = \PredictedErrorCovariance^{-1} + \MeasurementMatrix^\top \MeasurementNoiseCovariance^{-1} \MeasurementMatrix, \quad \UpdatedErrorCovariance^{-1} \UpdatedStateEstimate = \PredictedErrorCovariance^{-1} \PredictedStateEstimate + \MeasurementMatrix^\top \MeasurementNoiseCovariance^{-1} \MeasurementVector_k \] In information (precision) space, the posterior precision is simply the linear addition of prior dynamic precision \(\PredictedErrorCovariance^{-1}\) and measurement evidence precision \(\MeasurementMatrix^\top \MeasurementNoiseCovariance^{-1} \MeasurementMatrix\). This reveals the Kalman filter as the exact recursive, online counterpart of Bayesian linear regression and Gaussian process conditioning, and highlights the mathematical duality between optimal control (LQR) and optimal estimation (Kalman filter):
Feedback gain \(\LqrFeedbackGain = \LqrControlWeightMatrix^{-1} \StateSpaceInputMatrix^\top \LqrCostToGoMatrix\)
Observer gain \(\KalmanBucyObserverGain = \SteadyStateErrorCovariance \MeasurementMatrix^\top \ContinuousMeasurementNoiseIntensity^{-1}\)
Optimality Criterion
Minimizes quadratic state and action penalty
Minimizes trace of estimation error covariance \(\mathbb{E}[\lVert\MechanicalPlantState-\StateEstimate\rVert^2]\)
The Linear Quadratic Gaussian (LQG) controller cascades this optimal estimator with the optimal LQR regulator by applying feedback directly to the estimated state: \[ \ControlInput(\TimeInstant) = -\LqrFeedbackGain \StateEstimate(\TimeInstant) \] The Separation Principle(Åström and Murray 2021) proves that under linear-Gaussian assumptions, observer design and state-feedback controller design are completely decoupled. Specifically, the poles of the closed-loop LQG system are strictly the union of the controller poles \(\operatorname{eig}(\StateSpaceSystemMatrix - \StateSpaceInputMatrix\LqrFeedbackGain)\) and the observer poles \(\operatorname{eig}(\StateSpaceSystemMatrix - \KalmanBucyObserverGain\MeasurementMatrix)\).
Systems Perspective 1.1: The Doyle warning: LQG robustness margins
While full-state LQR provides guaranteed \(60^\circ\) phase margins and infinite gain margins, John Doyle’s classic 1978 paper demonstrated that output-feedback LQG controllers possess no guaranteed stability margins whatsoever(Åström and Murray 2021). When an observer is inserted into the loop, unmodeled plant parameter variations couple estimation error \(\StateEstimationError = \MechanicalPlantState - \StateEstimate\) directly back into the control actuation channel, so even infinitesimal parameter perturbations can push the closed-loop poles across the imaginary axis into the unstable right-half plane.
For physical AI systems, this delivers an essential warning: cascading a high-capacity neural perception system (e.g., an end-to-end vision model estimating 6D object poses or end-effector coordinates) with an optimal tracking controller does not inherit the controller’s nominal robustness. Dynamic perception lag, out-of-distribution observation errors, and high-frequency sensor noise degrade feedback phase margin rapidly. An implementation must evaluate the combined estimator-controller dynamics and its safety bounds; loop transfer recovery is one design technique, not a universal requirement (The Per-Tick Gatekeeper).
Stability Theory and Model Predictive Control
An arm controller that converges exponentially to a target grasp pose can still swing its elbow through a cage-door post along the way. Stability concerns whether trajectories remain near, or converge to, an equilibrium; set invariance concerns whether they stay inside a safe region at every instant. These are separate claims, each conditional on a model, an initial-state domain, and the implementation assumptions.
Lyapunov stability and invariant sets
Continuous gradient flow provides a useful analogy for Lyapunov analysis. It evolves parameter weights according to \(\ParameterVelocity = -\nabla_\ModelParameters \LossFunction(\ModelParameters)\). Along this parameter trajectory, the empirical loss decreases monotonically via the multivariable chain rule: \[ \LossTimeDerivative(\ModelParameters) = \nabla_\ModelParameters\LossFunction(\ModelParameters)^\top \ParameterVelocity = -\nabla_\ModelParameters\LossFunction(\ModelParameters)^\top \nabla_\ModelParameters\LossFunction(\ModelParameters) = -\|\nabla_\ModelParameters\LossFunction(\ModelParameters)\|^2 \le 0 \] In parameter optimization, the software program has unconstrained, direct write access to the parameter velocity vector \(\ParameterVelocity\), allowing the optimizer to force loss descent along the negative gradient direction. In physical control, the robot’s physical state \(\MechanicalPlantState(\TimeInstant) \in \mathbb{R}^\StateDimension\) evolves in physical state space according to continuous differential equations \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState)\). Unlike software network weights \(\ModelParameters\), physical coordinates \(\MechanicalPlantState\) cannot be updated by arbitrary gradient descent: actuators cannot command arbitrary velocity vectors \(\PlantStateRate\), because physical momentum, mass inertia, unforced drift, and kinematic constraints govern the system. By Newton’s second law, motor actuators modulate second-order accelerations (\(\GeneralizedAccelerations = \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1}(\JointTorque - \NonlinearBiasTorque)\)), which must be integrated through continuous time against centrifugal forces, Coriolis couplings, and gravitational fields.
Instead, a Lyapunov function\(\LyapunovFunction(\MechanicalPlantState)\) acts as an analytical, continuous state-space loss function that certifies the asymptotic convergence of the physical plant’s continuous trajectories without simulating every rollout to infinity. In mechanical systems, \(\LyapunovFunction(\MechanicalPlantState)\) often corresponds to the physical or synthetic Hamiltonian energy of the plant: kinetic energy stored in moving mass plus potential energy stored in gravitational elevation or virtual elastic springs.
Consider an autonomous nonlinear dynamical system: \[ \PlantStateRate(\TimeInstant) = \ControlAffineDriftField(\MechanicalPlantState(\TimeInstant)), \quad \ControlAffineDriftField(\ZeroMatrix) = \ZeroMatrix \] where \(\MechanicalPlantState^* = \ZeroMatrix\) is an isolated equilibrium point, and \(\ControlAffineDriftField: \LyapunovAnalysisDomain \to \mathbb{R}^\StateDimension\) is locally Lipschitz on an open domain \(\LyapunovAnalysisDomain \subset \mathbb{R}^\StateDimension\) containing the origin.
A continuously differentiable scalar function \(\LyapunovFunction: \LyapunovAnalysisDomain \to \mathbb{R}\) is a candidate Lyapunov function if it is positive definite on \(\LyapunovAnalysisDomain\): \[ \LyapunovFunction(\ZeroMatrix) = 0, \quad \LyapunovFunction(\MechanicalPlantState) > 0 \quad \forall \MechanicalPlantState \in \LyapunovAnalysisDomain \setminus \{\ZeroMatrix\} \] The directional derivative of \(\LyapunovFunction\) along the trajectories of \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState)\) evaluates via the multivariable chain rule: \[ \LyapunovFunctionDerivative(\MechanicalPlantState) = \nabla \LyapunovFunction(\MechanicalPlantState)^\top \PlantStateRate = \nabla \LyapunovFunction(\MechanicalPlantState)^\top \ControlAffineDriftField(\MechanicalPlantState) = \nabla \LyapunovFunction(\MechanicalPlantState) \cdot \ControlAffineDriftField(\MechanicalPlantState) \] Control theory traditionally denotes this inner product as the Lie derivative\(\DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) = \nabla \LyapunovFunction(\MechanicalPlantState) \cdot \ControlAffineDriftField(\MechanicalPlantState)\). In computational terms, this is simply the directional derivative or forward-mode Jacobian-vector product (JVP) of the scalar loss \(\LyapunovFunction\) evaluated in the direction of the physical drift vector field \(\ControlAffineDriftField(\MechanicalPlantState)\): \[ \operatorname{JVP}(\LyapunovFunction, \MechanicalPlantState, \ControlAffineDriftField(\MechanicalPlantState)) = \left. \frac{\DerivativeOperator}{\DerivativeOperator\DirectionalDerivativeStep} \LyapunovFunction(\MechanicalPlantState + \DirectionalDerivativeStep \ControlAffineDriftField(\MechanicalPlantState)) \right|_{\DirectionalDerivativeStep = 0} = \nabla \LyapunovFunction(\MechanicalPlantState)^\top \ControlAffineDriftField(\MechanicalPlantState) = \DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \] Automatic differentiation can evaluate this directional derivative without forming a full Jacobian. Computing it at sampled states, however, does not verify its sign everywhere in a region.
Alexander Lyapunov established three foundational stability tiers (Slotine and Li 1991):
Stability in the Sense of Lyapunov: If \(\LyapunovFunctionDerivative(\MechanicalPlantState) \le 0\) for all \(\MechanicalPlantState \in \LyapunovAnalysisDomain\), the origin is stable. Trajectories starting arbitrarily close to the origin remain trapped within bounded neighborhood contours for all \(\TimeInstant \ge 0\).
Asymptotic Stability: If \(\LyapunovFunctionDerivative(\MechanicalPlantState) < 0\) for all \(\MechanicalPlantState \in \LyapunovAnalysisDomain \setminus \{\ZeroMatrix\}\), the origin is asymptotically stable: \(\lim_{t \to \infty} \MechanicalPlantState(\TimeInstant) = \ZeroMatrix\). The system is guaranteed to descend the energy landscape toward the origin.
Exponential Stability: If there exist positive constants \(\ExponentialStabilityConstant_1, \ExponentialStabilityConstant_2, \ExponentialStabilityConstant_3 > 0\) such that \(\ExponentialStabilityConstant_1 \|\MechanicalPlantState\|^2 \le \LyapunovFunction(\MechanicalPlantState) \le \ExponentialStabilityConstant_2 \|\MechanicalPlantState\|^2\) and \(\LyapunovFunctionDerivative(\MechanicalPlantState) \le -\ExponentialStabilityConstant_3 \|\MechanicalPlantState\|^2\), trajectories converge exponentially: \[ \|\MechanicalPlantState(\TimeInstant)\| \le \sqrt{\frac{\ExponentialStabilityConstant_2}{\ExponentialStabilityConstant_1}} \|\MechanicalPlantState_0\| e^{-\frac{\ExponentialStabilityConstant_3}{2 \ExponentialStabilityConstant_2} \TimeInstant} \quad \forall \TimeInstant \ge 0 \]
For mechanical systems with friction and viscous damping, \(\LyapunovFunctionDerivative(\MechanicalPlantState)\) often vanishes on a subspace (for example, when velocities are zero while position errors remain nonzero: \(\LyapunovFunctionDerivative(\GeneralizedCoordinates, \GeneralizedVelocities) = -\GeneralizedVelocities^\top \ViscousDampingMatrix \GeneralizedVelocities = 0\) whenever \(\GeneralizedVelocities = \ZeroMatrix\)). Standard Lyapunov theorems only certify stability in the sense of Lyapunov, failing to prove asymptotic convergence. LaSalle’s Invariance Principle(Slotine and Li 1991) resolves this: if \(\LyapunovFunctionDerivative(\MechanicalPlantState) \le 0\) on a compact, positively invariant set \(\LaSalleInvariantSet\), all trajectories starting in \(\LaSalleInvariantSet\) asymptotically converge to the largest invariant set \(\LargestInvariantSet\) contained within \(\{ \MechanicalPlantState \in \LaSalleInvariantSet \mid \LyapunovFunctionDerivative(\MechanicalPlantState) = 0 \}\). Because any trajectory trapped on \(\LargestInvariantSet\) with \(\GeneralizedVelocities \equiv \ZeroMatrix\) must also satisfy zero acceleration (\(\GeneralizedAccelerations \equiv \ZeroMatrix\)), the equations of motion force position errors to vanish (\(\PidProportionalGainMatrix \JointTrackingError = \ZeroMatrix \implies \JointTrackingError = \ZeroMatrix\)). LaSalle’s principle proves that the only trajectory that can remain indefinitely in the zero-dissipation manifold is the origin itself, certifying asymptotic convergence.
A compact sublevel set \(\LyapunovSublevelSet = \{ \MechanicalPlantState \in \LyapunovAnalysisDomain \mid \LyapunovFunction(\MechanicalPlantState) \le \SublevelSetThreshold \}\) contained in \(\LyapunovAnalysisDomain\) is positively invariant when \(\LyapunovFunctionDerivative \le 0\) throughout it. It is an inner estimate of the domain of attraction (DOA) only when trajectories there also converge to the equilibrium, for example by strict descent or the LaSalle argument. Invariance alone does not imply convergence.
A neural network can parameterize a candidate Lyapunov function and search for a stabilizing policy. For example, \(\LyapunovFunction(\MechanicalPlantState) = \MechanicalPlantState^\top \NeuralLyapunovFactor(\MechanicalPlantState) \NeuralLyapunovFactor(\MechanicalPlantState)^\top \MechanicalPlantState + \LyapunovPositivityMargin\lVert\MechanicalPlantState\rVert^2\), with \(\LyapunovPositivityMargin>0\), guarantees positivity away from the origin. Convexity alone does not guarantee this property. Penalizing violations of \(\LyapunovFunctionDerivative<0\) during training produces a candidate; a certificate requires verifying the derivative condition throughout the claimed region, including the model and implementation uncertainty.
Control Lyapunov functions and stabilization
For a controlled system \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState) + \ControlAffineInputField(\MechanicalPlantState)\ControlInput\), the directional derivative along the closed-loop dynamics splits linearly into unforced drift and control effectiveness: \[ \LyapunovFunctionDerivative(\MechanicalPlantState, \ControlInput) = \nabla \LyapunovFunction(\MechanicalPlantState) \cdot (\ControlAffineDriftField(\MechanicalPlantState) + \ControlAffineInputField(\MechanicalPlantState)\ControlInput) = \DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) + \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \ControlInput \] where \(\DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) = \nabla \LyapunovFunction(\MechanicalPlantState) \cdot \ControlAffineDriftField(\MechanicalPlantState) \in \mathbb{R}\) is the unforced directional drift and \(\InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) = \nabla \LyapunovFunction(\MechanicalPlantState)^\top \ControlAffineInputField(\MechanicalPlantState) \in \mathbb{R}^{1 \times \ControlInputDimension}\) is the control effectiveness gradient. A candidate Lyapunov function \(\LyapunovFunction(\MechanicalPlantState)\) is a Control Lyapunov Function (CLF)(Ames et al. 2014; Slotine and Li 1991) if there exists an admissible control input \(\ControlInput \in \AdmissibleControlSet\) that forces \(\LyapunovFunction\) to decrease at every non-equilibrium state: \[ \inf_{\ControlInput \in \AdmissibleControlSet} \left[ \DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) + \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \ControlInput + \ClfClassKFunction(\LyapunovFunction(\MechanicalPlantState)) \right] \le 0 \quad \forall \MechanicalPlantState \in \LyapunovAnalysisDomain \setminus \{\ZeroMatrix\} \] where \(\ClfClassKFunction\) is an extended class \(\ClassKFunctionSet\) function (a strictly increasing function with \(\ClfClassKFunction(0)=0\)). For ML practitioners, \(\ClfClassKFunction(\LyapunovFunction(\MechanicalPlantState))\) acts as a state-dependent convergence rate floor: the larger the current error \(\LyapunovFunction(\MechanicalPlantState)\), the steeper the required rate of descent. Geometrically, the CLF inequality defines a closed affine half-space in control space \(\ClfDescentControlSet(\MechanicalPlantState) = \{ \ControlInput \in \AdmissibleControlSet \mid \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \ControlInput \le -\DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) - \ClfClassKFunction(\LyapunovFunction(\MechanicalPlantState)) \}\); any control input chosen inside this half-space guarantees monotonic descent of the continuous state-space loss.
Ames, Aaron D, Jessy W Grizzle, and Paulo Tabuada. 2014. “Control Barrier Function Based Quadratic Programs with Application to Adaptive Cruise Control.”53rd IEEE Conference on Decision and Control, 6271–78.
Sontag’s universal stabilizing formula
For an unconstrained control-affine system, a smooth CLF admits the following stabilizing feedback construction (Slotine and Li 1991): \[
\SontagStabilizingControl(\MechanicalPlantState) = \begin{cases}
-\left( \frac{\DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) + \sqrt{(\DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState))^2 + \|\InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState)\|^4}}{\|\InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState)\|^2} \right) \left( \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \right)^\top & \text{if } \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \neq \ZeroMatrix \\[8pt]
\ZeroMatrix & \text{if } \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) = \ZeroMatrix
\end{cases}
\] The formula makes \(\LyapunovFunctionDerivative<0\) away from the origin when the CLF condition holds. Continuity at the origin additionally requires the small control property: arbitrarily small states can be stabilized with arbitrarily small inputs. The condition \(\DriftLieDerivativeOperator \LyapunovFunction<0\) wherever \(\InputLieDerivativeOperator \LyapunovFunction=0\) is the CLF’s uncontrollable-direction condition, not the small control property itself. The formula does not impose actuator limits; clipping its output can destroy the decrease condition.
The duality between CLFs and CBFs
Comparing Control Lyapunov Functions with Control Barrier Functions (section 1.4) reveals the central duality of modern safety-critical control:
Dimension
Control Lyapunov Function (CLF)
Control Barrier Function (CBF)
Objective
Performance tracking / Goal convergence
Safety guarantee / Set invariance
Geometry
Positive-definite potential well (\(\LyapunovFunction(\MechanicalPlantState^*) = 0\))
Superlevel safe set \(\ForwardInvariantSafeSet = \{\MechanicalPlantState \mid \BarrierFunction(\MechanicalPlantState) \ge 0\}\)
Trajectory Dynamic
Pulls trajectories inward toward the origin (\(\LyapunovFunction \to 0\))
Prevents outward crossing; may permit approach to the boundary
Soft constraint (relaxed via slack \(\TrackingRelaxationSlack \ge 0\))
Hard constraint (strictly non-negotiable)
When combining performance tracking (such as following an uncertified trajectory proposed by a vision-language-action policy) with guaranteed collision avoidance, the enforcer solves a unified CLF-CBF Quadratic Program (CLF-CBF-QP): \[
\begin{align*}
\min_{\ControlInput \in \ControlInputSet, \TrackingRelaxationSlack \ge 0} \quad & \frac{1}{2} \|\ControlInput - \NominalControlCommand\|^2 + \TrackingSlackPenaltyWeight \TrackingRelaxationSlack^2 \\
\text{subject to:} \quad & \LieDerivativeAlongDrift(\MechanicalPlantState) + \LieDerivativeAlongControl(\MechanicalPlantState) \ControlInput \ge -\ClassKFunction(\BarrierFunction(\MechanicalPlantState)) \quad \text{(hard safety constraint)} \\
& \DriftLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) + \InputLieDerivativeOperator \LyapunovFunction(\MechanicalPlantState) \ControlInput \le -\ClfClassKFunction(\LyapunovFunction(\MechanicalPlantState)) + \TrackingRelaxationSlack \quad \text{(soft tracking constraint)}
\end{align*}
\] where \(\TrackingSlackPenaltyWeight \gg 1\) heavily penalizes the tracking relaxation slack \(\TrackingRelaxationSlack \ge 0\). The slack can relax tracking while leaving the modeled barrier condition hard. This only helps when at least one admissible input satisfies the barrier; tracking relaxation cannot repair an infeasible safety constraint.
Constrained model predictive control and recursive feasibility
While LQR and CLF feedback laws operate reactively on instantaneous state feedback, Model Predictive Control (MPC) optimizes trajectories over a predictive finite horizon \(\MpcPredictionHorizon\), explicitly enforcing hard inequality constraints on physical boundaries, motor voltages, and actuator torques. The following architectural comparison summarizes the complementary roles of numerical MPC and learned generative policies:
Attribute
Classical Numerical MPC
Learned Action Chunking (Diffusion, ACT)
Input Modality
Low-dimensional numerical states \(\MechanicalPlantState \in \mathbb{R}^\StateDimension\)
High-dimensional perception (RGB images, point clouds, language)
At discrete sampling tick \(\SetpointSampleIndex\), given current state measurement \(\MechanicalPlantState_k = \MechanicalPlantState(\TimeInstant_k)\), the MPC controller solves a constrained finite-horizon optimal control problem over horizon \(\MpcPredictionHorizon\): \[
\begin{align*}
\min_{\MpcControlSequence} \quad & \MechanicalPlantState_{\MpcPredictionHorizon}^\top \MpcTerminalCostMatrix \MechanicalPlantState_{\MpcPredictionHorizon} + \sum_{i=0}^{\MpcPredictionHorizon-1} \left( \MechanicalPlantState_i^\top \LqrStateWeightMatrix \MechanicalPlantState_i + \ControlInput_i^\top \LqrControlWeightMatrix \ControlInput_i \right) \\
\text{subject to:} \quad & \MechanicalPlantState_{i+1} = \DiscreteStateSpaceSystemMatrix \MechanicalPlantState_i + \DiscreteStateSpaceInputMatrix \ControlInput_i, \quad \MechanicalPlantState_0 = \MechanicalPlantState_k \\
& \MechanicalPlantState_i \in \SafeStateConstraintSet \subset \mathbb{R}^\StateDimension \quad \forall \PredictionStepIndex \in \{0, \dots, \MpcPredictionHorizon\} \\
& \ControlInput_i \in \AdmissibleControlSet \subset \mathbb{R}^\ControlInputDimension \quad \forall \PredictionStepIndex \in \{0, \dots, \MpcPredictionHorizon-1\} \\
& \MechanicalPlantState_{\MpcPredictionHorizon} \in \TerminalInvariantSet \subset \SafeStateConstraintSet
\end{align*}
\] where \(\SafeStateConstraintSet = \{ \MechanicalPlantState \mid \StateConstraintMatrix \MechanicalPlantState \le \StateConstraintVector \}\) represents state clearance constraints (joint limits, obstacle margins), \(\AdmissibleControlSet = \{ \ControlInput \mid \ControlInputLowerLimit \le \ControlInput \le \ControlInputUpperLimit \}\) bounds actuator torque authority, \(\TerminalInvariantSet\) is a terminal invariant set, and \(\MpcTerminalCostMatrix\) is the terminal cost matrix.
The receding-horizon principle and closed-loop stability
MPC executes in a receding-horizon closed loop:
Solve the constrained QP at step \(\SetpointSampleIndex\), obtaining optimal sequence \(\MpcControlSequence^* = [\ControlInput_0^*, \ControlInput_1^*, \dots, \ControlInput_{\MpcPredictionHorizon-1}^*]\).
Apply only the first control setpoint to physical hardware: \(\ControlInput(\TimeInstant_k) = \ControlInput_0^*\).
Advance the hardware clock by \(\SamplingPeriod\), discard the remaining trajectory \(\MpcControlSequenceTail^*\), obtain updated state \(\MechanicalPlantState_{k+1}\), and re-solve across the shifted horizon.
For the exact discrete model, an initially feasible state, and successful on-time solves, closed-loop stability can be established by choosing the terminal cost \(\MpcTerminalCostMatrix\) and terminal set \(\TerminalInvariantSet\) to satisfy the classical Rawlings-Mayne stability conditions (Åström and Murray 2021; Wabersich and Zeilinger 2021):
Terminal Invariance: There exists a local linear feedback law \(\ControlInput = -\TerminalFeedbackGain \MechanicalPlantState\) such that for all \(\MechanicalPlantState \in \TerminalInvariantSet\), \((\DiscreteStateSpaceSystemMatrix - \DiscreteStateSpaceInputMatrix \TerminalFeedbackGain)\MechanicalPlantState \in \TerminalInvariantSet\) and \(-\TerminalFeedbackGain \MechanicalPlantState \in \AdmissibleControlSet\).
Under these conditions, the optimal cost function \(\MpcHorizonObjective^*(\MechanicalPlantState_k)\) acts as a discrete Lyapunov function: \[ \MpcHorizonObjective^*(\MechanicalPlantState_{k+1}) - \MpcHorizonObjective^*(\MechanicalPlantState_k) \le -(\MechanicalPlantState_k^\top \LqrStateWeightMatrix \MechanicalPlantState_k + \ControlInput_k^{*\top} \LqrControlWeightMatrix \ControlInput_k^*) \le 0 \] Strict descent away from the origin follows from a positive-definite state cost. With semidefinite cost, convergence instead requires an appropriate detectability argument. Under those assumptions, the nominal closed loop converges while respecting the modeled constraints.
Recursive feasibility and avoidance of dynamic traps
A critical hazard in constrained physical control is dynamic trap entry: a naive controller drives the system into a state where, due to physical inertia, finite stopping distance, and actuator saturation, no control sequence can prevent constraint violation \(\MpcPredictionHorizon\) steps in the future, rendering the QP instantaneously infeasible. For example, a mobile robot cruising at high velocity toward a wall may have a feasible solution over horizon \(\MpcPredictionHorizon\), but if the horizon terminates before braking completes, step \(\SetpointSampleIndex+1\) encounters an unavoidable collision because available deceleration cannot overcome kinetic momentum within the remaining distance.
A controller possesses recursive feasibility if the existence of a feasible solution at step \(\SetpointSampleIndex\) guarantees that the optimization problem remains feasible at step \(\SetpointSampleIndex+1\) for all time. With the terminal invariant set \(\TerminalInvariantSet\) and exact nominal dynamics, the candidate shifted sequence: \[ \ShiftedMpcControlSequence = [\ControlInput_1^*, \ControlInput_2^*, \dots, \ControlInput_{\MpcPredictionHorizon-1}^*, -\TerminalFeedbackGain \MechanicalPlantState_{\MpcPredictionHorizon}] \] is guaranteed to satisfy all dynamics, state, input, and terminal constraints at step \(\SetpointSampleIndex+1\). The shifted sequence proves nominal recursive feasibility and can supply a warm start. Bounded disturbances alone do not preserve this proof: robust MPC must tighten constraints or propagate disturbance tubes and use a compatible robust terminal set. Solver failure and missed deadlines still require a reserved fallback.
Classical numerical MPC vs. learned action chunking
As framed in Choosing a Trajectory Generator, modern physical AI embodies a direct trade-off between numerical MPC and learned generative action chunking:
Model Accuracy vs. Semantic Ingestion: Classical MPC relies on analytical differential equations (\(\MechanicalPlantState_{k+1} = \PlantDynamicsFunction(\MechanicalPlantState_k, \ControlInput_k)\)). Its stated low-dimensional formulation requires an upstream perception system to convert images, point clouds, or language into state estimates and goals. Modeling contact-mode changes creates an additional difficulty (section 1.7). In contrast, generative action chunking policies (Diffusion Policy, Action Chunking with Transformers (ACT), Flow Matching) learn complex, multimodal behavioral distributions directly from human demonstrations.
Computational Scaling: Dense optimization can become expensive as the horizon and state dimension grow; sparse and structured solvers can exploit the trajectory structure. No universal cubic runtime or control frequency follows from the MPC label. Learned action chunking moves much of the optimization into training, but its inference deadline still depends on model size, decoding steps, and hardware.
The Physical AI Synthesis: Learned policies can propose action chunks while a model-based controller checks their physical feasibility. Determinism, recursive feasibility, and collision avoidance remain properties to establish for the selected model, constraints, solver, timing, and fallback; neither MPC nor a CBF-QP provides them merely by being placed below the proposal boundary (The Per-Tick Gatekeeper).
Safety and Control Barrier Functions
When a learned policy commands an arm toward a rack post at \(1.0\text{ m/s}\), a position check that triggers only at contact is too late because inertia carries the link into the post; an independent enforcer can instead check a control barrier constraint before actuation. In reliable software systems, safety properties are proven using inductive loop invariants: if an invariant holds initially and is preserved across every state transition, it holds for all executions. A Control Barrier Function (CBF) lifts this principle to continuous physical systems, framing safety as an inductive loop invariant over continuous state trajectories:
Let the system state be \(\MechanicalPlantState \in \PlantStateSpace \subset \mathbb{R}^\StateDimension\) and the control input be \(\ControlInput \in \ControlInputSet \subset \mathbb{R}^\ControlInputDimension\). Suppose we define a safe operating set \(\ForwardInvariantSafeSet\) as the superlevel set of a continuously differentiable scalar certificate function \(\BarrierFunction(\MechanicalPlantState)\), with a nonzero gradient on its zero-level boundary: \[ \ForwardInvariantSafeSet = \{ \MechanicalPlantState \in \PlantStateSpace \mid \BarrierFunction(\MechanicalPlantState) \ge 0 \}, \quad \partial \ForwardInvariantSafeSet = \{ \MechanicalPlantState \in \PlantStateSpace \mid \BarrierFunction(\MechanicalPlantState) = 0 \} \]
\(\BarrierFunction(\MechanicalPlantState) > 0\) when the system is strictly inside the safe interior.
\(\BarrierFunction(\MechanicalPlantState) = 0\) on the boundary of the safe set.
\(\BarrierFunction(\MechanicalPlantState) < 0\) when the system violates safety.
Invariant Concept
Formal Software Verification (Dafny, TLA+)
Safety-Critical Control (CBF)
State Space
Discrete variable states \(\ProgramState \in \ProgramStateSpace\)
Continuous state \(\MechanicalPlantState \in \PlantStateSpace \subset \mathbb{R}^\StateDimension\)
Base Case: The system initializes within the safe operating envelope: \(\BarrierFunction(\MechanicalPlantState_0) \ge 0\).
Inductive Step: Along all continuous transitions governed by plant dynamics and control inputs \(\ControlInput_t \in \ControlInputSet\), the instantaneous rate of change satisfies: \[ \BarrierFunctionRate(\MechanicalPlantState_t, \ControlInput_t) \ge -\ClassKFunction(\BarrierFunction(\MechanicalPlantState_t)) \] where \(\ClassKFunction(\cdot)\) is an extended class \(\ExtendedClassKInfinityFamily\) function (or linear decay \(\ClassKFunction(\BarrierFunction) = \LinearBarrierDecayRate \BarrierFunction\) with design bandwidth \(\LinearBarrierDecayRate > 0\)).
Continuous Induction via Comparison Lemma: For the linear choice \(\ClassKFunction(\BarrierFunction)=\LinearBarrierDecayRate \BarrierFunction\), consider the scalar comparison differential equation: \[ \ComparisonTrajectoryDerivative(\TimeInstant) = -\LinearBarrierDecayRate \ComparisonTrajectory(\TimeInstant), \quad \ComparisonTrajectory(0) = \BarrierFunction(\MechanicalPlantState_0) \ge 0 \] whose exact analytical solution is \(\ComparisonTrajectory(\TimeInstant) = \BarrierFunction(\MechanicalPlantState_0) e^{-\LinearBarrierDecayRate \TimeInstant}\). Because \(\BarrierFunction(\MechanicalPlantState_0) \ge 0\) and \(e^{-\LinearBarrierDecayRate \TimeInstant} > 0\) for all finite \(\TimeInstant \ge 0\), the comparison trajectory remains non-negative: \(\ComparisonTrajectory(\TimeInstant) \ge 0\). By the Grönwall-Bellman comparison lemma (Slotine and Li 1991), the condition \(\BarrierFunctionRate(\MechanicalPlantState_t, \ControlInput_t) \ge -\LinearBarrierDecayRate \BarrierFunction(\MechanicalPlantState_t)\) guarantees: \[ \BarrierFunction(\MechanicalPlantState_t) \ge \ComparisonTrajectory(\TimeInstant) = \BarrierFunction(\MechanicalPlantState_0) e^{-\LinearBarrierDecayRate \TimeInstant} \ge 0 \quad \forall \TimeInstant \ge 0 \]
Conclusion: By induction over continuous time, \(\BarrierFunction(\MechanicalPlantState_t) \ge 0\) for all \(\TimeInstant \ge 0\), guaranteeing forward invariance of the safe set \(\ForwardInvariantSafeSet\). As the state approaches the boundary (\(\BarrierFunction(\MechanicalPlantState_t) \to 0^+\)), the permissible rate of decrease \(-\ClassKFunction(\BarrierFunction)\) contracts to zero, enforcing \(\BarrierFunctionRate(\MechanicalPlantState_t, \ControlInput_t) \ge 0\). Consequently, \(\BarrierFunction(\MechanicalPlantState_t)\) can never cross zero with a negative slope.
Lie derivatives and forward invariance
Forward invariance means a trajectory starting in \(\ForwardInvariantSafeSet\) remains in \(\ForwardInvariantSafeSet\) for all future time. For a control-affine continuous dynamical system \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState) + \ControlAffineInputField(\MechanicalPlantState)\ControlInput\), the total time derivative \(\BarrierFunctionRate\) evaluates via the multivariate chain rule as directional derivatives along system dynamics:1\[ \BarrierFunctionRate(\MechanicalPlantState, \ControlInput) = \nabla \BarrierFunction(\MechanicalPlantState)^\top \PlantStateRate = \nabla \BarrierFunction(\MechanicalPlantState)^\top \ControlAffineDriftField(\MechanicalPlantState) + \left(\nabla \BarrierFunction(\MechanicalPlantState)^\top \ControlAffineInputField(\MechanicalPlantState)\right)\ControlInput = \LieDerivativeAlongDrift(\MechanicalPlantState) + \LieDerivativeAlongControl(\MechanicalPlantState) \ControlInput \] In modern computational terminology, the Lie derivatives \(\LieDerivativeAlongDrift(\MechanicalPlantState)\) and \(\LieDerivativeAlongControl(\MechanicalPlantState)\) are Jacobian-vector products (JVPs) of the scalar safety function \(\BarrierFunction\) evaluated along the drift and control vector fields: \[ \LieDerivativeAlongDrift(\MechanicalPlantState) = \nabla \BarrierFunction(\MechanicalPlantState) \cdot \ControlAffineDriftField(\MechanicalPlantState) \in \mathbb{R}, \quad \LieDerivativeAlongControl(\MechanicalPlantState) = \nabla \BarrierFunction(\MechanicalPlantState)^\top \ControlAffineInputField(\MechanicalPlantState) \in \mathbb{R}^{1 \times \ControlInputDimension} \] where \(\LieDerivativeAlongDrift(\MechanicalPlantState)\) captures the unforced directional drift of clearance (passive momentum, centrifugal forces, gravity), and \(\LieDerivativeAlongControl(\MechanicalPlantState)\) is the directional control effectiveness row vector mapping control inputs into instantaneous safety margin growth.
1Boundary 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.
Nagumo’s viability theorem and contingent cones
The geometric foundation of set invariance for dynamic systems is Nagumo’s Viability Theorem (Blanchini 1999). Let \(\ForwardInvariantSafeSet \subset \mathbb{R}^\StateDimension\) be a non-empty closed set. The Bouligand contingent cone (or tangent cone) \(\BouligandContingentCone(\MechanicalPlantState)\) to \(\ForwardInvariantSafeSet\) at a point \(\MechanicalPlantState \in \ForwardInvariantSafeSet\) is defined as: \[
\BouligandContingentCone(\MechanicalPlantState) = \left\{ \TangentDirection \in \mathbb{R}^\StateDimension \;\middle|\; \liminf_{\ConeLimitStepSize \to 0^+} \frac{\text{dist}(\MechanicalPlantState + \ConeLimitStepSize \TangentDirection, \ForwardInvariantSafeSet)}{\ConeLimitStepSize} = 0 \right\}
\] where \(\text{dist}(\DistanceQueryPoint, \ForwardInvariantSafeSet) = \inf_{\SetMemberPoint \in \ForwardInvariantSafeSet} \|\DistanceQueryPoint - \SetMemberPoint\|\). For trajectories that exist for the interval considered, Nagumo’s theorem proves that an autonomous system \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState)\) with locally Lipschitz vector field \(\ControlAffineDriftField\) keeps \(\ForwardInvariantSafeSet\) forward-invariant if and only if the velocity vector satisfies the subtangentiality condition: \[
\ControlAffineDriftField(\MechanicalPlantState) \in \BouligandContingentCone(\MechanicalPlantState) \quad \forall \MechanicalPlantState \in \ForwardInvariantSafeSet
\] In the interior \(\text{int}(\ForwardInvariantSafeSet)\), \(\BouligandContingentCone(\MechanicalPlantState) = \mathbb{R}^\StateDimension\), so the condition is trivially satisfied. On the boundary \(\partial \ForwardInvariantSafeSet\) of a smooth set defined by the zero-superlevel set of \(\BarrierFunction(\MechanicalPlantState)\) (\(\ForwardInvariantSafeSet = \{\MechanicalPlantState \mid \BarrierFunction(\MechanicalPlantState) \ge 0\}\) with \(\nabla \BarrierFunction(\MechanicalPlantState) \neq \ZeroMatrix\)), the inward normal is \(\nabla \BarrierFunction(\MechanicalPlantState)\). The contingent cone becomes the closed half-space of inward or tangent velocities: \[
\BouligandContingentCone(\MechanicalPlantState) = \left\{ \TangentDirection \in \mathbb{R}^\StateDimension \;\middle|\; \nabla \BarrierFunction(\MechanicalPlantState)^\top \TangentDirection \ge 0 \right\} \quad \forall \MechanicalPlantState \in \partial \ForwardInvariantSafeSet
\] Consequently, the system cannot exit the set if and only if the directional derivative satisfies \(\BarrierFunctionRate(\MechanicalPlantState) = \nabla \BarrierFunction(\MechanicalPlantState)^\top \ControlAffineDriftField(\MechanicalPlantState) \ge 0\) whenever \(\BarrierFunction(\MechanicalPlantState) = 0\). Control Barrier Functions (Ames et al. 2019) extend Nagumo’s subtangentiality to controlled affine systems \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState) + \ControlAffineInputField(\MechanicalPlantState)\ControlInput\). For safety, there must exist an admissible control \(\ControlInput \in \ControlInputSet\) such that \(\ControlAffineDriftField(\MechanicalPlantState) + \ControlAffineInputField(\MechanicalPlantState)\ControlInput \in \BouligandContingentCone(\MechanicalPlantState)\). By introducing an extended class \(\ExtendedClassKInfinityFamily\) decay rate \(\ClassKFunction(\BarrierFunction(\MechanicalPlantState))\) satisfying \(\ClassKFunction(0) = 0\), the CBF inequality: \[
\BarrierFunctionRate(\MechanicalPlantState, \ControlInput) = \LieDerivativeAlongDrift(\MechanicalPlantState) + \LieDerivativeAlongControl(\MechanicalPlantState)\ControlInput \ge -\ClassKFunction(\BarrierFunction(\MechanicalPlantState))
\] ensures that as the state approaches the boundary (\(\BarrierFunction(\MechanicalPlantState) \to 0\)), the admissible decay rate contracts to zero (\(-\ClassKFunction(\BarrierFunction(\MechanicalPlantState)) \to 0\)). At the boundary \(\BarrierFunction(\MechanicalPlantState) = 0\), it strictly enforces \(\BarrierFunctionRate(\MechanicalPlantState, \ControlInput) \ge 0\), directly preserving Nagumo’s subtangentiality invariant and preventing trajectory exit into the unsafe set.
Blanchini, Franco. 1999. “Set Invariance in Control.”Automatica 35 (11): 1747–67.
Relative degree and kinetic stopping insets
The relative degree\(\BarrierRelativeDegree\) of a barrier function \(\BarrierFunction(\MechanicalPlantState)\) is the number of times it must be differentiated with respect to continuous time before the control input \(\ControlInput\) explicitly appears: \[ \InputLieDerivativeOperator \DriftLieDerivativeOperator^\LieDerivativeOrder \BarrierFunction(\MechanicalPlantState) = \ZeroMatrix \quad \forall \LieDerivativeOrder < \BarrierRelativeDegree - 1, \quad \text{and} \quad \InputLieDerivativeOperator \DriftLieDerivativeOperator^{\BarrierRelativeDegree-1} \BarrierFunction(\MechanicalPlantState) \neq \ZeroMatrix \] For a one-dimensional approach to a stationary obstacle, let raw spatial clearance be \(\RawClearanceBarrierFunction = \ClearDistance\), approach speed \(\BodySpeed > 0\), and commanded approach acceleration \(\CommandedApproachAcceleration\). Differentiating reveals: \[ \ClearanceRate = -\BodySpeed, \quad \ClearanceAcceleration = -\CommandedApproachAcceleration \] Because acceleration enters raw clearance only at the second time derivative (\(\InputLieDerivativeOperator \RawClearanceBarrierFunction = 0\) and \(\InputLieDerivativeOperator \DriftLieDerivativeOperator \RawClearanceBarrierFunction \neq 0\)), raw clearance has relative degree two (\(\BarrierRelativeDegree = 2\)) with respect to force or acceleration inputs. An actuator torque command cannot instantaneously change position or velocity; commanding maximum braking at the boundary \(\ClearDistance = 0\) is futile because momentum carries the system across the boundary before velocity can reach zero.
For systems with general relative degree \(\BarrierRelativeDegree \ge 2\), High-Order Control Barrier Functions (HOCBFs) construct a cascaded sequence of barrier certificates: \[
\begin{aligned}
\CascadedBarrierCertificate_0(\MechanicalPlantState) &= \BarrierFunction(\MechanicalPlantState) \\
\CascadedBarrierCertificate_1(\MechanicalPlantState) &= \CascadedBarrierDerivative_0(\MechanicalPlantState) + \CascadedBarrierClassKFunction_1(\CascadedBarrierCertificate_0(\MechanicalPlantState)) \\
&\;\;\vdots \\
\CascadedBarrierCertificate_{\BarrierRelativeDegree}(\MechanicalPlantState) &= \CascadedBarrierDerivative_{\BarrierRelativeDegree-1}(\MechanicalPlantState) + \CascadedBarrierClassKFunction_{\BarrierRelativeDegree}(\CascadedBarrierCertificate_{\BarrierRelativeDegree-1}(\MechanicalPlantState)) = \DriftLieDerivativeOperator^\BarrierRelativeDegree \BarrierFunction(\MechanicalPlantState) + \InputLieDerivativeOperator \DriftLieDerivativeOperator^{\BarrierRelativeDegree-1} \BarrierFunction(\MechanicalPlantState) \ControlInput + \dots \ge 0
\end{aligned}
\] The high-order condition also requires safe initialization of all intermediate certificates \(\CascadedBarrierCertificate_0,\ldots,\CascadedBarrierCertificate_{\BarrierRelativeDegree-1}\) and a feasible control throughout their intersection. In physical robotics, rather than tuning abstract class \(\ClassKFunctionSet\) functions across multiple derivatives, we can augment the barrier coordinate with kinetic energy and available stopping capacity. In a one-dimensional approach to a stationary obstacle, let \(\ClearDistance\) be measured clearance, \(\BodySpeed > 0\) approach speed, and \(\CredibleBrakingDeceleration > 0\) a validated lower bound on available braking deceleration for the operating state. Defining the kinetic stopping inset: \[ \KineticInsetBarrierFunction(\ClearDistance, \BodySpeed) = \ClearDistance - \frac{\BodySpeed^2}{2\CredibleBrakingDeceleration}, \qquad \ClearanceRate = -\BodySpeed, \quad \BodyAcceleration = \CommandedApproachAcceleration \] Differentiating \(\KineticInsetBarrierFunction(\ClearDistance, \BodySpeed)\) along the system dynamics yields: \[ \KineticInsetBarrierRate = \ClearanceRate - \frac{\BodySpeed}{\CredibleBrakingDeceleration} \BodyAcceleration = -\BodySpeed - \frac{\BodySpeed}{\CredibleBrakingDeceleration} \CommandedApproachAcceleration \] Notice that commanded acceleration \(\CommandedApproachAcceleration\) now appears directly in the first time derivative (\(\BarrierRelativeDegree = 1\)). The CBF condition \(\KineticInsetBarrierRate + \LinearBarrierDecayRate \KineticInsetBarrierFunction \ge 0\) (with gain \(\LinearBarrierDecayRate > 0\)) enforces the linear affine inequality on commanded acceleration: \[ \CommandedApproachAcceleration \le \frac{\CredibleBrakingDeceleration}{\BodySpeed} \left( \LinearBarrierDecayRate \KineticInsetBarrierFunction - \BodySpeed \right) \quad \text{for } \BodySpeed > 0 \] At standstill (\(\BodySpeed \to 0\)), a separate static clearance rule must be evaluated. State estimation error, digital sampling, and brake-onset delay require additional clearance reserves; the continuous kinematic calculation alone does not cover them.
Minimal-intervention quadratic program
The Control Barrier Function Quadratic Program (CBF-QP) changes a nominal command only as much as needed to satisfy the modeled barrier and actuator constraints. At a fixed state, the barrier condition is affine in the control input, so the filter solves a convex projection problem: \[
\begin{align*}
\ControlInput^* &= \arg\min_{\ControlInput \in \ControlInputSet} \frac{1}{2} \| \ControlInput - \NominalControlCommand \|^2 \\
&\text{subject to:} \quad \LieDerivativeAlongDrift(\MechanicalPlantState) + \LieDerivativeAlongControl(\MechanicalPlantState) \ControlInput \ge -\ClassKFunction(\BarrierFunction(\MechanicalPlantState)), \quad \ControlInputLowerLimit \le \ControlInput \le \ControlInputUpperLimit
\end{align*}
\] To a machine learning engineer, the CBF-QP is the continuous physical counterpart of projection operators used in optimization:
In deep learning optimization, Projected Gradient Descent (PGD) enforces hard weight or adversarial perturbation constraints by projecting updated parameters back onto an \(\ProjectionBallRadius\)-ball: \(\ConstrainedParameterVector_{k+1} = \operatorname{proj}_{\ProjectionConstraintSet}(\ConstrainedParameterVector_k - \LearningRate \LossGradientVector)\). In multi-task gradient learning (such as PCGrad or MGDA), conflicting task gradients \(\TaskLossGradient_i\) are projected onto the normal hyperplane of \(\TaskLossGradient_j\) via \(\TaskLossGradient_i \leftarrow \TaskLossGradient_i - \frac{\TaskLossGradient_i^\top \TaskLossGradient_j}{\|\TaskLossGradient_j\|^2}\TaskLossGradient_j\) whenever their inner product is negative.
In physical AI systems, the CBF-QP projects uncertified action vectors \(\NominalControlCommand \in \mathbb{R}^\ControlInputDimension\) onto the convex polyhedron of forward-invariant safe actions \(\BarrierHalfSpaceControlSet(\MechanicalPlantState) = \{ \ControlInput \in \ControlInputSet \mid \LieDerivativeAlongDrift(\MechanicalPlantState) + \LieDerivativeAlongControl(\MechanicalPlantState)\ControlInput \ge -\ClassKFunction(\BarrierFunction(\MechanicalPlantState)) \}\).
Writing the affine CBF constraint in standard half-space form \(\BarrierConstraintNormal^\top \ControlInput \le \BarrierConstraintBound\), with normal vector \(\BarrierConstraintNormal = -\LieDerivativeAlongControl(\MechanicalPlantState)^\top \in \mathbb{R}^\ControlInputDimension\) and scalar threshold \(\BarrierConstraintBound = \LieDerivativeAlongDrift(\MechanicalPlantState) + \ClassKFunction(\BarrierFunction(\MechanicalPlantState))\), assume \(\BarrierConstraintNormal \ne \ZeroMatrix\) and consider the unconstrained actuator case (\(\ControlInput \in \mathbb{R}^\ControlInputDimension\)): \[
\min_{\ControlInput \in \mathbb{R}^\ControlInputDimension} \frac{1}{2} \|\ControlInput - \NominalControlCommand\|^2 \quad \text{subject to } \BarrierConstraintNormal^\top \ControlInput \le \BarrierConstraintBound
\] The Lagrangian function is: \[ \BarrierProjectionLagrangian(\ControlInput, \BarrierProjectionMultiplier) = \frac{1}{2} \|\ControlInput - \NominalControlCommand\|^2 + \BarrierProjectionMultiplier (\BarrierConstraintNormal^\top \ControlInput - \BarrierConstraintBound) \] The Karush-Kuhn-Tucker (KKT) optimality conditions are:
Evaluating these conditions reveals two distinct operational regimes:
Permissive Pass-Through: When the nominal neural proposal \(\NominalControlCommand\) satisfies the safety constraint (\(\BarrierConstraintNormal^\top \NominalControlCommand \le \BarrierConstraintBound\)), the optimal Lagrange multiplier is \(\BarrierProjectionMultiplier^* = 0\). The filter acts as an identity pass-through: \[ \ControlInput^* = \NominalControlCommand \] Zero control effort is distorted in this regime. Passing the command untouched preserves the full behavioral fidelity of the learned policy during normal task execution.
Active Minimal-Intervention Projection: When the neural policy proposes an unsafe command that attempts to penetrate the barrier (\(\BarrierConstraintNormal^\top \NominalControlCommand > \BarrierConstraintBound\)), the safety constraint must be active (\(\BarrierConstraintNormal^\top \ControlInput^* = \BarrierConstraintBound\)) and \(\BarrierProjectionMultiplier^* > 0\). Substituting the stationarity condition into the active constraint yields: \[ \BarrierConstraintNormal^\top \left( \NominalControlCommand - \BarrierProjectionMultiplier^* \BarrierConstraintNormal \right) = \BarrierConstraintBound \implies \BarrierConstraintNormal^\top \NominalControlCommand - \BarrierProjectionMultiplier^* \|\BarrierConstraintNormal\|^2 = \BarrierConstraintBound \] Solving for the optimal multiplier yields: \[ \BarrierProjectionMultiplier^* = \frac{\BarrierConstraintNormal^\top \NominalControlCommand - \BarrierConstraintBound}{\|\BarrierConstraintNormal\|^2} > 0 \] The filtered control command is the exact, closed-form Euclidean projection onto the safe half-space boundary: \[ \ControlInput^* = \NominalControlCommand - \left( \frac{\BarrierConstraintNormal^\top \NominalControlCommand - \BarrierConstraintBound}{\|\BarrierConstraintNormal\|^2} \right) \BarrierConstraintNormal \]
Unifying both cases into a single compact projection operator: \[ \ControlInput^* = \operatorname{proj}_{\BarrierHalfSpaceControlSet}(\NominalControlCommand) = \NominalControlCommand - \frac{\max\left(0, \BarrierConstraintNormal^\top \NominalControlCommand - \BarrierConstraintBound\right)}{\|\BarrierConstraintNormal\|^2} \BarrierConstraintNormal \] Geometrically, the vector \(\BarrierConstraintNormal = -\LieDerivativeAlongControl(\MechanicalPlantState)^\top\) points outward along the gradient of safety penetration. The term \(\max\left(0, \BarrierConstraintNormal^\top \NominalControlCommand - \BarrierConstraintBound\right)\) is precisely the hinge loss (or ReLU activation) of safety margin penetration: when the proposal is safe, the loss is zero and the output is untouched; when unsafe, the projection operator subtracts the exact scalar penetration depth along \(\BarrierConstraintNormal\), leaving all orthogonal control directions completely untouched. To an ML engineer, this is the exact continuous-control counterpart of an analytical, one-step subgradient projection onto a Support Vector Machine margin hyperplane.
If \(\BarrierConstraintNormal=\ZeroMatrix\), division by \(\lVert\BarrierConstraintNormal\rVert^2\) is invalid: all inputs satisfy this constraint when \(\BarrierConstraintBound\ge0\), and none do when \(\BarrierConstraintBound<0\). With actuator limits or multiple barriers, the nonempty admissible set is a convex polyhedron and the strictly convex objective has a unique projection. A numerical implementation must check feasibility, residual tolerance, and completion before its deadline. A solver name or nominal cadence supplies none of those bounds.
Systems Perspective 1.2: Check feasibility before relying on invariance
The unconstrained projection formula does not solve the box-constrained problem. Clamping its result post hoc can violate \(\BarrierConstraintNormal^\top \ControlInput \le \BarrierConstraintBound\). If no command satisfies both the barrier half-space and actuator limits simultaneously (\(\BarrierHalfSpaceControlSet \cap \AdmissibleControlSet = \emptyset\)), the modeled invariant cannot be maintained by this filter. A separately validated emergency fallback may still limit harm, but it cannot restore the lost forward-invariance claim. Admission must reserve stopping authority before the state reaches this boundary.
Smooth Trajectories and Bumpless Transfer
Switching directly between two action chunks that differ in velocity or acceleration commands an abrupt torque step across the gearbox. Because physical links and rotors possess inertia, a velocity step requests an ideal acceleration impulse, and an acceleration step requests an instantaneous torque jump under a rigid-inertia model. Whether a commanded seam excites drivetrain resonance or trips an inverter fault depends on the plant, load, and controller response.
Analytical quintic splines
A quintic (5th-degree) polynomial trajectory segment \(\JointPosition(\TimeInstant)\) over time interval \(\TimeInstant \in [0, \Time]\) is defined as: \[ \JointPosition(\TimeInstant) = \PolynomialTrajectoryCoefficient_0 + \PolynomialTrajectoryCoefficient_1 \TimeInstant + \PolynomialTrajectoryCoefficient_2 \TimeInstant^2 + \PolynomialTrajectoryCoefficient_3 \TimeInstant^3 + \PolynomialTrajectoryCoefficient_4 \TimeInstant^4 + \PolynomialTrajectoryCoefficient_5 \TimeInstant^5 \] The six coefficients \([\PolynomialTrajectoryCoefficient_0, \dots, \PolynomialTrajectoryCoefficient_5]\) are uniquely determined by matching six boundary conditions at \(\TimeInstant = 0\) and \(\TimeInstant = \Time\): \[ \JointPosition(0) = \JointPosition_0, \quad \JointVelocity(0) = \JointVelocity_0, \quad \JointAcceleration(0) = \JointAcceleration_0 \]\[ \JointPosition(\Time) = \JointPosition_1, \quad \JointVelocity(\Time) = \JointVelocity_1, \quad \JointAcceleration(\Time) = \JointAcceleration_1 \] Solving the resulting linear system yields the exact analytical formulas: \[
\begin{aligned}
\PolynomialTrajectoryCoefficient_0 &= \JointPosition_0 \\
\PolynomialTrajectoryCoefficient_1 &= \JointVelocity_0 \\
\PolynomialTrajectoryCoefficient_2 &= \frac{1}{2}\JointAcceleration_0 \\
\PolynomialTrajectoryCoefficient_3 &= \frac{10(\JointPosition_1 - \JointPosition_0) - (6\JointVelocity_0 + 4\JointVelocity_1)\Time - (3\JointAcceleration_0 - \JointAcceleration_1)\frac{\Time^2}{2}}{\Time^3} \\
\PolynomialTrajectoryCoefficient_4 &= \frac{-15(\JointPosition_1 - \JointPosition_0) + (8\JointVelocity_0 + 7\JointVelocity_1)\Time + (3\JointAcceleration_0 - 2\JointAcceleration_1)\frac{\Time^2}{2}}{\Time^4} \\
\PolynomialTrajectoryCoefficient_5 &= \frac{6(\JointPosition_1 - \JointPosition_0) - 3(\JointVelocity_0 + \JointVelocity_1)\Time - (\JointAcceleration_0 - \JointAcceleration_1)\frac{\Time^2}{2}}{\Time^5}
\end{aligned}
\] Differentiating three times gives the segment’s jerk profile, a quadratic in time that stays finite over a finite blend: \[ \dddot{\JointPosition}(\TimeInstant)=6\PolynomialTrajectoryCoefficient_3+24\PolynomialTrajectoryCoefficient_4\TimeInstant+60\PolynomialTrajectoryCoefficient_5\TimeInstant^2 \] Matching position, velocity, and acceleration at the endpoints gives a \(\DifferentiabilityClass^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 \(\PolynomialCoefficientVector = [\PolynomialTrajectoryCoefficient_0, \PolynomialTrajectoryCoefficient_1, \PolynomialTrajectoryCoefficient_2, \PolynomialTrajectoryCoefficient_3, \PolynomialTrajectoryCoefficient_4, \PolynomialTrajectoryCoefficient_5]^\top\) are determined by evaluating the trajectory and its first two derivatives at the temporal endpoints \(\TimeInstant = 0\) and \(\TimeInstant = \Time\). This yields the \(6 \times 6\) confluent Vandermonde linear system \(\ConfluentVandermondeMatrix \PolynomialCoefficientVector = \BoundaryConditionVector\): \[
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 2 & 0 & 0 & 0 \\
1 & \Time & \Time^2 & \Time^3 & \Time^4 & \Time^5 \\
0 & 1 & 2\Time & 3\Time^2 & 4\Time^3 & 5\Time^4 \\
0 & 0 & 2 & 6\Time & 12\Time^2 & 20\Time^3
\end{bmatrix}
\begin{bmatrix}
\PolynomialTrajectoryCoefficient_0 \\ \PolynomialTrajectoryCoefficient_1 \\ \PolynomialTrajectoryCoefficient_2 \\ \PolynomialTrajectoryCoefficient_3 \\ \PolynomialTrajectoryCoefficient_4 \\ \PolynomialTrajectoryCoefficient_5
\end{bmatrix}
=
\begin{bmatrix}
\JointPosition_0 \\ \JointVelocity_0 \\ \JointAcceleration_0 \\ \JointPosition_1 \\ \JointVelocity_1 \\ \JointAcceleration_1
\end{bmatrix}
\] The top three rows trivially yield the initial boundary terms: \(\PolynomialTrajectoryCoefficient_0 = \JointPosition_0\), \(\PolynomialTrajectoryCoefficient_1 = \JointVelocity_0\), and \(\PolynomialTrajectoryCoefficient_2 = \frac{1}{2}\JointAcceleration_0\). Substituting these values into the bottom three rows isolates the higher-order coefficients \([\PolynomialTrajectoryCoefficient_3, \PolynomialTrajectoryCoefficient_4, \PolynomialTrajectoryCoefficient_5]^\top\) under the reduced \(3 \times 3\) system: \[
\begin{bmatrix}
\Time^3 & \Time^4 & \Time^5 \\
3\Time^2 & 4\Time^3 & 5\Time^4 \\
6\Time & 12\Time^2 & 20\Time^3
\end{bmatrix}
\begin{bmatrix}
\PolynomialTrajectoryCoefficient_3 \\ \PolynomialTrajectoryCoefficient_4 \\ \PolynomialTrajectoryCoefficient_5
\end{bmatrix}
=
\begin{bmatrix}
\JointPosition_1 - \JointPosition_0 - \JointVelocity_0 \Time - \frac{1}{2}\JointAcceleration_0 \Time^2 \\
\JointVelocity_1 - \JointVelocity_0 - \JointAcceleration_0 \Time \\
\JointAcceleration_1 - \JointAcceleration_0
\end{bmatrix}
\] The determinant of this \(3 \times 3\) matrix is \(\det(\ConfluentVandermondeMatrix_{33}) = 2 \Time^9 > 0\) for all strictly positive blending horizons \(\Time > 0\), guaranteeing non-singularity and uniqueness. The analytic matrix inverse is: \[
\begin{bmatrix}
\Time^3 & \Time^4 & \Time^5 \\
3\Time^2 & 4\Time^3 & 5\Time^4 \\
6\Time & 12\Time^2 & 20\Time^3
\end{bmatrix}^{-1}
=
\begin{bmatrix}
\frac{10}{\Time^3} & -\frac{4}{\Time^2} & \frac{1}{2\Time} \\
-\frac{15}{\Time^4} & \frac{7}{\Time^3} & -\frac{1}{\Time^2} \\
\frac{6}{\Time^5} & -\frac{3}{\Time^4} & \frac{1}{2\Time^3}
\end{bmatrix}
\] Matrix multiplication \(\PolynomialCoefficientVector_{3:5} = \ConfluentVandermondeMatrix_{33}^{-1} \ReducedBoundaryConditionVector\) directly produces the closed-form coefficients \(\PolynomialTrajectoryCoefficient_3, \PolynomialTrajectoryCoefficient_4, \PolynomialTrajectoryCoefficient_5\) stated above. Because the inverse scales as \(\Time^{-5}\) in its highest-order row, shrinking the blend duration \(\Time\) rapidly amplifies the commanded jerk and torque peaks.
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 \(\TimeInstant \in [\TimeInstant_0, \TimeInstant_1]\) with segment duration \(\Time = \TimeInstant_1 - \TimeInstant_0\) interpolates position and velocity between endpoints: \[ \JointPosition(\TimeInstant_0) = \JointPosition_0, \quad \JointVelocity(\TimeInstant_0) = \JointVelocity_0, \quad \JointPosition(\TimeInstant_1) = \JointPosition_1, \quad \JointVelocity(\TimeInstant_1) = \JointVelocity_1 \]
Introducing the normalized time coordinate \(\HermiteSegmentProgress = (\TimeInstant - \TimeInstant_0) / \Time \in [0, 1]\), the trajectory is parameterized by the cubic Hermite basis polynomials: \[ \JointPosition(\HermiteSegmentProgress) = \HermiteBasisFunction_{00}(\HermiteSegmentProgress) \JointPosition_0 + \HermiteBasisFunction_{10}(\HermiteSegmentProgress) \Time \JointVelocity_0 + \HermiteBasisFunction_{01}(\HermiteSegmentProgress) \JointPosition_1 + \HermiteBasisFunction_{11}(\HermiteSegmentProgress) \Time \JointVelocity_1 \] where the basis functions satisfy: \[
\begin{aligned}
\HermiteBasisFunction_{00}(\HermiteSegmentProgress) &= 2\HermiteSegmentProgress^3 - 3\HermiteSegmentProgress^2 + 1 \\
\HermiteBasisFunction_{10}(\HermiteSegmentProgress) &= \HermiteSegmentProgress^3 - 2\HermiteSegmentProgress^2 + \HermiteSegmentProgress \\
\HermiteBasisFunction_{01}(\HermiteSegmentProgress) &= -2\HermiteSegmentProgress^3 + 3\HermiteSegmentProgress^2 \\
\HermiteBasisFunction_{11}(\HermiteSegmentProgress) &= \HermiteSegmentProgress^3 - \HermiteSegmentProgress^2
\end{aligned}
\] Differentiating with respect to physical time \(\TimeInstant\) yields the instantaneous velocity: \[ \JointVelocity(\TimeInstant) = \frac{1}{\Time} \frac{d\JointPosition}{d\HermiteSegmentProgress} = \frac{1}{\Time} \left( \HermiteBasisFunction'_{00}(\HermiteSegmentProgress) \JointPosition_0 + \HermiteBasisFunction_{01}'(\HermiteSegmentProgress) \JointPosition_1 \right) + \HermiteBasisFunction'_{10}(\HermiteSegmentProgress) \JointVelocity_0 + \HermiteBasisFunction'_{11}(\HermiteSegmentProgress) \JointVelocity_1 \] with derivative basis functions: \[
\begin{aligned}
\HermiteBasisFunction'_{00}(\HermiteSegmentProgress) &= 6\HermiteSegmentProgress^2 - 6\HermiteSegmentProgress \\
\HermiteBasisFunction'_{10}(\HermiteSegmentProgress) &= 3\HermiteSegmentProgress^2 - 4\HermiteSegmentProgress + 1 \\
\HermiteBasisFunction'_{01}(\HermiteSegmentProgress) &= -6\HermiteSegmentProgress^2 + 6\HermiteSegmentProgress \\
\HermiteBasisFunction'_{11}(\HermiteSegmentProgress) &= 3\HermiteSegmentProgress^2 - 2\HermiteSegmentProgress
\end{aligned}
\] Because the basis polynomials satisfy \(\HermiteBasisFunction_{00}(0)=1, \HermiteBasisFunction_{00}(1)=0, \HermiteBasisFunction'_{10}(0)=1, \HermiteBasisFunction'_{10}(1)=0\), this yields a \(\Glyph{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 \(\JointAcceleration(\TimeInstant)\) is piecewise linear and discontinuous across sample boundaries (\(\Glyph{C}^1\) but not \(\Glyph{C}^2\)), producing finite torque steps \(\FiniteChange \CommandedJointTorque = \SingleJointInertia(\JointPosition) \FiniteChange \JointAcceleration\) at knot points that require further smoothing when a finite torque-slew limit must be enforced.
Bumpless transfer and smoothstep blending
For bumpless transfer (Transfer Without Discontinuity), let \(\BumplessTransferProgress=(\TimeInstant-\BumplessTransferStartInstant)/\BlendingTransitionWindow\in[0,1]\) be the normalized progress, as in equation 14.6, and use the same \(\Glyph{C}^2\) smoothstep \(\BumplessTransferBlend(\BumplessTransferProgress)\) to interpolate two commands: \[ \BumplessTransferBlend(\BumplessTransferProgress)=6\BumplessTransferProgress^5-15\BumplessTransferProgress^4+10\BumplessTransferProgress^3,\qquad \ChannelCommand(\TimeInstant)=(1-\BumplessTransferBlend(\BumplessTransferProgress))\OutgoingChannelCommand(\TimeInstant)+\BumplessTransferBlend(\BumplessTransferProgress)\IncomingChannelCommand(\TimeInstant). \] The endpoint first and second derivatives of \(\BumplessTransferBlend\) vanish. If \(\ChannelCommand\) is commanded acceleration, its time derivative is commanded-acceleration jerk: \[ \BlendedCommandJerk(\TimeInstant)=\frac{\BumplessTransferBlendSlope(\BumplessTransferProgress)}{\BlendingTransitionWindow}(\IncomingChannelCommand-\OutgoingChannelCommand)+(1-\BumplessTransferBlend(\BumplessTransferProgress))\OutgoingCommandDerivative+\BumplessTransferBlend(\BumplessTransferProgress)\IncomingCommandDerivative. \] Here \(\BumplessTransferBlendSlope(\BumplessTransferProgress)=30\BumplessTransferProgress^2(1-\BumplessTransferProgress)^2\) differentiates with respect to normalized time and peaks at \(1.875\). Only for two fixed, latched endpoint accelerations (\(\OutgoingCommandDerivative=\IncomingCommandDerivative=0\)) does the blend contribution satisfy \[ \max|\BlendedCommandJerk|=\frac{1.875|\FiniteChange \ChannelCommand|}{\BlendingTransitionWindow},\qquad \BlendingTransitionWindow\ge\frac{1.875|\FiniteChange \ChannelCommand|}{\JerkLimit}. \] 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 \(\GeneralizedCoordinateCount\)-degree-of-freedom manipulator with elastic joints is governed by two coupled second-order vector differential equations: \[ \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \GeneralizedAccelerations + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) = \JointTorsionalStiffnessMatrix (\MotorRotorCoordinates - \GeneralizedCoordinates) \]\[ \ReflectedRotorInertiaMatrix \MotorRotorAccelerations + \MotorViscousDampingMatrix \MotorRotorVelocities + \JointTorsionalStiffnessMatrix (\MotorRotorCoordinates - \GeneralizedCoordinates) = \JointTorque \] where:
Spong, Mark W. 1987. “Modeling and Control of Elastic Joint Robots.”Journal of Dynamic Systems, Measurement, and Control 109 (4): 310–19.
\(\GeneralizedCoordinates \in \mathbb{R}^\GeneralizedCoordinateCount\) represents the physical link coordinates.
\(\MotorRotorCoordinates \in \mathbb{R}^\GeneralizedCoordinateCount\) represents the motor rotor coordinates reflected through gear reduction \(\GearReductionRatio\) to the link side.
\(\GeneralizedInertiaMatrix(\GeneralizedCoordinates) \in \mathbb{R}^{\GeneralizedCoordinateCount \times \GeneralizedCoordinateCount}\) is the symmetric, positive-definite link inertia matrix.
\(\CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities \in \mathbb{R}^\GeneralizedCoordinateCount\) collects Coriolis and centripetal terms.
\(\GravitationalTorqueVector(\GeneralizedCoordinates) \in \mathbb{R}^\GeneralizedCoordinateCount\) is the gravitational torque vector.
\(\ReflectedRotorInertiaMatrix = \operatorname{diag}({\JointReflectedRotorInertia}_1, \dots, {\JointReflectedRotorInertia}_n)\) is the diagonal matrix of reflected rotor inertias (\({\JointReflectedRotorInertia}_i = {\GearReductionRatio}_i^2 {\RotorInertia}_i\)).
\(\MotorViscousDampingMatrix = \operatorname{diag}({\MotorViscousDamping}_1, \dots, {\MotorViscousDamping}_n)\) collects viscous motor damping.
\(\JointTorsionalStiffnessMatrix = \operatorname{diag}({\TransmissionTorsionalStiffness}_1, \dots, {\TransmissionTorsionalStiffness}_n)\) is the diagonal joint torsional stiffness matrix (\({\TransmissionTorsionalStiffness}_i > 0\)).
\(\JointTorque \in \mathbb{R}^\GeneralizedCoordinateCount\) is the vector of actuator torques reflected to the link side, consistent with the reflected motor coordinates and inertias.
Singular perturbation formulation and time-scale separation
Time-scale separation requires the elastic modes to be fast relative to the intended rigid-body motion. After nondimensionalizing time, inertia, and stiffness by declared reference scales, write \(\JointTorsionalStiffnessMatrix = \ScaledJointStiffnessMatrix/\TimeScaleSeparationRatio^2\), where \(\TimeScaleSeparationRatio\) is a small dimensionless ratio of fast to slow time scales and \(\ScaledJointStiffnessMatrix = \mathcal{O}(1)\). A comparison such as stiffness greater than one has no physical meaning before this scaling.
Define the elastic torque fast state variable \(\ElasticJointTorque \in \mathbb{R}^\GeneralizedCoordinateCount\): \[ \ElasticJointTorque = \frac{1}{\TimeScaleSeparationRatio^2} \ScaledJointStiffnessMatrix (\MotorRotorCoordinates - \GeneralizedCoordinates) = \JointTorsionalStiffnessMatrix (\MotorRotorCoordinates - \GeneralizedCoordinates) \] Differentiating \(\ElasticJointTorque\) twice with respect to continuous time \(\TimeInstant\) gives: \[ \ElasticTorqueSecondDerivative = \JointTorsionalStiffnessMatrix (\MotorRotorAccelerations - \GeneralizedAccelerations) = \JointTorsionalStiffnessMatrix \left[ \ReflectedRotorInertiaMatrix^{-1} (\JointTorque - \MotorViscousDampingMatrix \MotorRotorVelocities - \ElasticJointTorque) - \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1} (\ElasticJointTorque - \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities - \GravitationalTorqueVector(\GeneralizedCoordinates)) \right] \] Multiplying through by \(\TimeScaleSeparationRatio^2\) reveals the singularly perturbed boundary layer: \[ \TimeScaleSeparationRatio^2 \ElasticTorqueSecondDerivative = \ScaledJointStiffnessMatrix \left[ \ReflectedRotorInertiaMatrix^{-1} \JointTorque - (\ReflectedRotorInertiaMatrix^{-1} + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1}) \ElasticJointTorque + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1} (\CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates)) - \ReflectedRotorInertiaMatrix^{-1} \MotorViscousDampingMatrix \MotorRotorVelocities \right] \] For the following undamped fast-mode calculation, set \(\MotorViscousDampingMatrix = \ZeroMatrix\) and freeze the slow configuration. Introducing the stretched fast time variable \(\BoundaryLayerTime = \TimeInstant / \TimeScaleSeparationRatio\) transforms the elastic dynamics into: \[ \frac{\DerivativeOperator^2 \ElasticJointTorque}{d\BoundaryLayerTime^2} + \ScaledJointStiffnessMatrix (\ReflectedRotorInertiaMatrix^{-1} + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1}) \ElasticJointTorque = \ScaledJointStiffnessMatrix \ReflectedRotorInertiaMatrix^{-1} \JointTorque + \ScaledJointStiffnessMatrix \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1} (\CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates)) \]
The rigid manifold and boundary-layer resonance
In the singular limit as joint stiffness approaches infinity (\(\TimeScaleSeparationRatio \to 0\)), the fast elastic variable \(\ElasticJointTorque\) has the formal quasi-steady manifold \(\ElasticJointTorque_0\): \[ \ElasticJointTorque_0 = (\ReflectedRotorInertiaMatrix^{-1} + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1})^{-1} \left[ \ReflectedRotorInertiaMatrix^{-1} \JointTorque + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1} (\CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates)) \right] \] The undamped expression does not prove that fast oscillations decay onto this manifold. Restoring damping and imposing the rigid constraint \(\MotorRotorCoordinates = \GeneralizedCoordinates\) in the original equations gives the classical rigid-body equations with aggregated inertia \(\GeneralizedInertiaMatrix(\GeneralizedCoordinates) + \ReflectedRotorInertiaMatrix\): \[ (\GeneralizedInertiaMatrix(\GeneralizedCoordinates) + \ReflectedRotorInertiaMatrix) \GeneralizedAccelerations + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) + \MotorViscousDampingMatrix \GeneralizedVelocities = \JointTorque \] For finite stiffness, a torque step excites the elastic modes; it is a step forcing, not an impulse. In physical time, their squared natural frequencies are the eigenvalues of the frozen-configuration matrix \(\JointTorsionalStiffnessMatrix(\ReflectedRotorInertiaMatrix^{-1} + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1})\). Equivalently, the modal frequency matrix is: \[ \ElasticModalFrequencyMatrix(\GeneralizedCoordinates) = \sqrt{\JointTorsionalStiffnessMatrix (\ReflectedRotorInertiaMatrix^{-1} + \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1})} \] The actual damping and resonant frequencies depend on the transmission and load. Smoothing commands and limiting torque slew can reduce excitation, but \(\Glyph{C}^2\) continuity alone does not establish a resonance or fatigue bound. The blend spectrum and measured closed-loop response must be checked against the flexible plant.
Discrete-time sampling and zero-order hold phase lag
Real-time digital controllers execute periodically at sampling period \(\SamplingPeriod = 1 / \SamplingRate\). 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: \[ \ZohTransportDelay = \frac{\SamplingPeriod}{2} \] For a closed-loop control system operating at crossover frequency \(\CrossoverFrequency\), this transport delay erodes the system’s phase margin \(\PhaseMargin\) by:2\[ \FiniteChange \ZohPhaseShift = \CrossoverFrequency \frac{\SamplingPeriod}{2} \quad \text{(radians)} \approx 28.65^\circ \cdot \left(\frac{\CrossoverFrequency}{\SamplingRate}\right) \]
2Phase Margin and Delay: A pure delay \(\PureTransportDelay\) contributes phase \(-\AngularFrequency \PureTransportDelay\). 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.
For fixed modeled compute and bus delays, the added phase lag near crossover is approximately \(\FiniteChange\TotalDelayPhaseShift = \CrossoverFrequency(\EnforcerComputeTime + \CommandTransportTime + \SamplingPeriod/2)\). The controller must validate phase margin over the measured latency and operating envelope; the formula alone does not predict a particular contact response.
Dynamic Reachability, Intent Envelopes, and Differential Games
A target tote \(0.60\text{ m}\) from the wrist is geometrically inside a \(0.855\text{ m}\) manipulator workspace, yet unreachable within a \(150\text{ ms}\) intent lease if the arm cannot accelerate, traverse \(0.60\text{ m}\), and decelerate to rest within its torque and jerk limits before the lease expires. Kinematic reachability characterizes the set of configurations or end-effector poses a mechanism can reach geometrically, whereas dynamic reachability restricts that set to states attainable within the remaining time horizon and actuator bounds.
Forward kinematics and singularities
When a two-link arm stretches straight out, a small Cartesian velocity request along the extended link axis demands huge joint velocities near the singular alignment. The Planning Handoff and the Trajectory Record introduces forward kinematics, the Jacobian, and the singular pose on a planar two-link abstraction of the machine’s arm, and Time parameterization under torque and jerk bounds turns a path into a timed trajectory under torque and jerk bounds; this section states the general \(\GeneralizedCoordinateCount\)-joint form those examples specialize.
For an \(\GeneralizedCoordinateCount\)-joint arm, the forward-kinematics map \(\ForwardKinematicsMap(\GeneralizedCoordinates)\) sends joint coordinates \(\GeneralizedCoordinates\in\mathbb R^\GeneralizedCoordinateCount\) to an end-effector pose \(\EndEffectorPose\in \SpecialEuclideanGroup\): \[ \EndEffectorPose=\ForwardKinematicsMap(\GeneralizedCoordinates). \] In a declared spatial or body twist convention, the manipulator Jacobian\(\ManipulatorJacobian(\GeneralizedCoordinates)\in\mathbb R^{6\times \GeneralizedCoordinateCount}\) relates joint rates to the six-vector end-effector twist \(\EndEffectorTwist\): \[ \EndEffectorTwist=\ManipulatorJacobian(\GeneralizedCoordinates)\GeneralizedVelocities. \] For a six-dimensional task and \(\GeneralizedCoordinateCount\ge6\), rank loss (\(\det(\ManipulatorJacobian\ManipulatorJacobian^\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 \(\YoshikawaManipulability(\GeneralizedCoordinates)=\sqrt{\det(\ManipulatorJacobian\ManipulatorJacobian^\top)}\) against a validated threshold.3
3Yoshikawa 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.
To an ML engineer, the geometry of a kinematic singularity is best understood through the Singular Value Decomposition (SVD) of the Jacobian: \[ \ManipulatorJacobian(\GeneralizedCoordinates) = \JacobianLeftSingularMatrix \JacobianSingularNumberMatrix \JacobianRightSingularMatrix^\top = \sum_{i=1}^6 \JacobianSingularNumber_i \JacobianLeftSingularVector_i \JacobianRightSingularVector_i^\top \] For the full-row-rank case, \(\JacobianSingularNumber_1 \ge \JacobianSingularNumber_2 \ge \dots \ge \JacobianSingularNumber_6 > 0\) are the singular values. The Yoshikawa manipulability index evaluates to the product of these singular values: \(\YoshikawaManipulability(\GeneralizedCoordinates) = \prod_{i=1}^6 \JacobianSingularNumber_i\). In unregularized differential inverse kinematics, solving for commanded joint rates yields: \[ \GeneralizedVelocities = \ManipulatorJacobian(\GeneralizedCoordinates)^\dagger \EndEffectorTwist = \sum_{i=1}^6 \left( \frac{\JacobianLeftSingularVector_i^\top \EndEffectorTwist}{\JacobianSingularNumber_i} \right) \JacobianRightSingularVector_i \] As the arm approaches a singularity, the smallest singular value vanishes (\(\JacobianSingularNumber_6 \to 0\)). Any commanded task twist with a nonzero projection onto the lost direction (\(\JacobianLeftSingularVector_6^\top \EndEffectorTwist \neq 0\)) causes the commanded joint rate \(\GeneralizedVelocities\) to explode as \(\mathcal{O}(1/\JacobianSingularNumber_6)\), potentially exceeding motor velocity limits; the drive response depends on its current and fault logic.
Roboticists resolve this through Damped Least Squares (DLS) inverse kinematics (Wampler 1986; Nakamura and Hanafusa 1986): \[ \DampedLeastSquaresPseudoinverse = \ManipulatorJacobian^\top \left( \ManipulatorJacobian \ManipulatorJacobian^\top + \InverseKinematicsDamping^2 \IdentityMatrix \right)^{-1} \] For a spatial task mixing translation and rotation, first declare a task scaling or metric so that the singular values and damping parameter are comparable; the velocity bound below uses that same scaled norm. For each singular mode, the effective inversion gain is regularized from \(1/\JacobianSingularNumber_i\) to \(\frac{\JacobianSingularNumber_i}{\JacobianSingularNumber_i^2 + \InverseKinematicsDamping^2} \le \frac{1}{2\InverseKinematicsDamping}\). This is mathematically identical to \(\ell_2\) Tikhonov regularization (ridge regression / weight decay) in machine learning: it deliberately introduces a bounded tracking residual in exchange for strictly bounded joint velocities (\(\|\GeneralizedVelocities\| \le \|\EndEffectorTwist\| / (2\InverseKinematicsDamping)\)).
Wampler, Charles W., II. 1986. “Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods.”IEEE Transactions on Systems, Man, and Cybernetics 16 (1): 93–101. https://doi.org/10.1109/TSMC.1986.289285.
Nakamura, Yoshihiko, and Hideo Hanafusa. 1986. “Inverse Kinematic Solutions with Singularity Robustness for Robot Manipulator Control.”Journal of Dynamic Systems, Measurement, and Control 108 (3): 163–71. https://doi.org/10.1115/1.3143764.
Trajectory transit time and reachable distance
For a one-dimensional rest-to-rest move with symmetric available acceleration \(\ProfileAccelerationBound\) and speed ceiling \(\SpeedBound\), the shortest nominal transit has two regimes:
Triangular profile (acceleration-limited): For short horizons where peak velocity \(\SpeedBound\) is not attained (\(\TargetTravelDistance \le \CriticalTransitDistance = \SpeedBound^2 / \ProfileAccelerationBound\)): \[ \MinimumTransitTime = 2 \sqrt{\frac{\TargetTravelDistance}{\ProfileAccelerationBound}}, \quad \ReachableDistance(\RemainingIntentTime) = \frac{1}{4} \ProfileAccelerationBound \RemainingIntentTime^2 \]
Below \(\CriticalTransitDistance\) 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. When jerk is also bounded by \(|\ProfileJerk|\le \JerkLimit\), reaching full acceleration from zero takes at least \(\ProfileAccelerationBound/\JerkLimit\), if the profile reaches it at all. That 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\(\DynamicReachableSet(\RemainingIntentTime)\), the states reachable from the measured state within the remaining time \(\RemainingIntentTime=\IntentLeaseExpiry-\CurrentTime\). Their transit times are therefore necessary lower bounds, and a proposal whose idealized time already exceeds \(\RemainingIntentTime\) 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.
Hamilton-Jacobi reachability and worst-case differential games
While kinematic reachability examines static geometry and idealized transit bounds, dynamic reachability under external disturbances and modeling errors requires analyzing system evolution as a continuous differential game (Mitchell et al. 2005). Consider a nonlinear dynamical system governed by: \[ \PlantStateRate(\TimeInstant) = \PlantDynamicsFunction(\MechanicalPlantState(\TimeInstant), \ControlInput(\TimeInstant), \AdversarialDisturbance(\TimeInstant)) \] where \(\MechanicalPlantState \in \mathbb{R}^\PlantStateDimension\) is the system state, \(\ControlInput \in \AdmissibleControlSet \subset \mathbb{R}^\ControlInputDimension\) is the control input (seeking to maintain safety), and \(\AdversarialDisturbance \in \AdmissibleDisturbanceSet \subset \mathbb{R}^\DisturbanceDimension\) is an adversarial disturbance (representing unmodeled friction, wind gusts, dynamic obstacles, or worst-case model discrepancies).
Let the failure or collision region be represented by the open zero-sublevel set of a Lipschitz continuous surface function \(\FailureMarginFunction(\MechanicalPlantState)\): \[ \FailureSet = \{ \MechanicalPlantState \in \mathbb{R}^\PlantStateDimension \mid \FailureMarginFunction(\MechanicalPlantState) < 0 \} \] The safe set is the superlevel set \(\{ \MechanicalPlantState \mid \FailureMarginFunction(\MechanicalPlantState) \ge 0 \}\). The Backward Reachable Tube (BRT)\(\BackwardReachableTube(\TimeInstant)\) is defined as the set of states from which the disturbance can force the system into collision at any time \(\ReachabilityHorizonInstant \in [\TimeInstant, \ReachabilityTerminalTime]\), even if the control executes optimal evasive action: \[ \BackwardReachableTube(\TimeInstant) = \left\{ \MechanicalPlantState \in \mathbb{R}^\PlantStateDimension \;\middle|\; \forall \ControlInput(\cdot) \in \AdmissibleControlSet, \; \exists \AdversarialDisturbance(\cdot) \in \AdmissibleDisturbanceSet, \; \exists \ReachabilityHorizonInstant \in [\TimeInstant, \ReachabilityTerminalTime] \text{ s.t. } \MechanicalPlantState(\ReachabilityHorizonInstant) \in \FailureSet \right\} \]
Formulating safety verification as a continuous two-player zero-sum differential game, the value function \(\ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant)\) represents the outcome of continuous-state minimax Bellman dynamic programming over trajectory time: \[ \ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant) = \sup_{\ControlInput \in \AdmissibleControlSet} \inf_{\AdversarialDisturbance \in \AdmissibleDisturbanceSet} \min_{\ReachabilityHorizonInstant \in [\TimeInstant, \ReachabilityTerminalTime]} \FailureMarginFunction(\MechanicalPlantState(\ReachabilityHorizonInstant)) \] The system is guaranteed safe over \([\TimeInstant, \ReachabilityTerminalTime]\) if and only if \(\ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant) \ge 0\). The zero-sublevel set of \(\ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant)\) corresponds exactly to the Backward Reachable Tube: \(\BackwardReachableTube(\TimeInstant) = \{ \MechanicalPlantState \mid \ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant) < 0 \}\).
Applying Bellman’s principle of dynamic programming in continuous state space yields the Hamilton-Jacobi-Isaacs (HJI) variational inequality: \[ \min \left\{ \frac{\partial \ReachabilityLevelFunction}{\partial \TimeInstant} + \ReachabilityHamiltonian(\MechanicalPlantState, \nabla \ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant)), \; \FailureMarginFunction(\MechanicalPlantState) - \ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant) \right\} = 0, \quad \ReachabilityLevelFunction(\MechanicalPlantState, \ReachabilityTerminalTime) = \FailureMarginFunction(\MechanicalPlantState) \] where the Hamiltonian \(\ReachabilityHamiltonian(\MechanicalPlantState, \ReachabilityCostate)\) evaluates the directional derivative of the value function along system dynamics under worst-case minimax competition: \[ \ReachabilityHamiltonian(\MechanicalPlantState, \ReachabilityCostate) = \max_{\ControlInput \in \AdmissibleControlSet} \min_{\AdversarialDisturbance \in \AdmissibleDisturbanceSet} \ReachabilityCostate^\top \PlantDynamicsFunction(\MechanicalPlantState, \ControlInput, \AdversarialDisturbance) \] The trajectory notation above is shorthand for a differential game with nonanticipating strategies: actions may depend on available state history, not future disturbances. The information pattern and regularity assumptions must match the Hamiltonian ordering; arbitrary open-loop input quantifiers alone do not establish a feedback safety guarantee. Here \(\ReachabilityCostate = \nabla \ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant) \in \mathbb{R}^\PlantStateDimension\) is the costate vector, representing the spatial gradient (directional sensitivity vector) of the safety boundary.
Because the value function \(\ReachabilityLevelFunction(\MechanicalPlantState, \TimeInstant)\) develops non-differentiable kinks and gradient discontinuities where characteristics collide or bifurcate, classical smooth solutions do not exist globally. The PDE is solved in the sense of viscosity solutions(Mitchell et al. 2005).4
4Viscosity Solutions and Level Set Methods: Introduced by Crandall and Lions, viscosity solutions provide the mathematically rigorous framework for continuous weak solutions to first-order Hamilton-Jacobi PDEs where spatial gradients \(\nabla \ReachabilityLevelFunction\) are discontinuous. High-order numerical schemes (such as WENO spatial discretization and TVD Runge-Kutta time integration) evaluate the sub- and super-differentials of \(\ReachabilityLevelFunction\) on regular Eulerian grids, tracking the implicit zero-level set without explicit front parameterization.
A regular grid with \(\AxisGridPointCount\) points per coordinate in state dimension \(\PlantStateDimension\) stores \(\mathcal{O}(\AxisGridPointCount^\PlantStateDimension)\) values, making direct reachability analysis expensive as dimension grows. If an infinite-horizon value converges under suitable game assumptions, its nonnegative set describes robust viability for that model. The limiting value need not be smooth and is not automatically a classical differentiable CBF.
At runtime, a gridded or learned approximation can support a safety filter only with controlled approximation error. Where a suitable smooth certificate is available, its directional derivatives supply barrier constraints; otherwise nonsmooth reachability methods are needed. Distilling a value function into a neural network does not preserve the offline guarantee without verification.
Spatial coordinate frames and lever-arm kinematics
Rigid-body pose in three-dimensional space is parameterized by the Special Euclidean group \(\SpecialEuclideanGroup\). A rigid transform from frame \(\Glyph{B}\) to frame \(\Glyph{A}\) is represented by the homogeneous transformation matrix \(\HomogeneousTransform \in \SpecialEuclideanGroup\): \[
\HomogeneousTransform = \begin{bmatrix} \TransformRotationBlock & \TransformTranslationBlock \\ \ZeroMatrix^\top & 1 \end{bmatrix}, \quad \TransformRotationBlock \in \SpecialOrthogonalGroup, \quad \TransformTranslationBlock \in \mathbb{R}^3
\] The rotation group \(\SpecialOrthogonalGroup\) consists of orthonormal rotation matrices with determinant one; \(\SpecialEuclideanGroup\) combines one such rotation with a translation. Thus \(\TargetFramePoint = \TransformRotationBlock \SourceFramePoint + \TransformTranslationBlock\) maps coordinates of a point from \(\Glyph{B}\) to \(\Glyph{A}\), and transforms along a chain compose as \(\ComposedChainTransform = \HomogeneousTransform \InnerChainTransform\). When sensors such as IMUs or cameras are displaced from the body center of rotation by lever arm \(\SensorLeverArm\), rotational motion induces apparent velocities and accelerations at sensor location \(\Glyph{P}\). The linear velocity \(\SensorPointVelocity\) and linear acceleration \(\SensorPointAcceleration\) satisfy: \[ \SensorPointVelocity = \RotationOriginVelocity + \AngularVelocity \times \SensorLeverArm \]\[ \SensorPointAcceleration = \RotationOriginAcceleration + \AngularAcceleration \times \SensorLeverArm + \AngularVelocity \times (\AngularVelocity \times \SensorLeverArm) \] All vectors in these sums must be expressed in the same frame, and \(\Glyph{P}\) must be fixed to the rigid body. Here \(\AngularVelocity\) is the body angular velocity, \(\AngularAcceleration = \AngularVelocityDerivative\) is the angular acceleration, \(\AngularAcceleration \times \SensorLeverArm\) is the tangential acceleration, and \(\AngularVelocity \times (\AngularVelocity \times \SensorLeverArm)\) 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
When an application processor stalls mid-aisle while the mobile base is cruising at \(1.5\text{ m/s}\), the base continues advancing \(1.5\text{ mm}\) per millisecond until the safety microcontroller’s lease expires and the brakes engage. 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 \(\SpeedAtDetection\) toward an obstacle with measured physical clearance \(\ClearDistance\). If the host processor or neural network fails to refresh the control setpoint, a hardware or firmware watchdog must intervene to prevent boundary violation. For a stop triggered by lease expiry, measure clearance at the supporting observation’s capture epoch. The elapsed time from that epoch until physical braking initiates is \(\LeaseExpiryDelaySubtotal + \DispatchAdmissionInterval\). The pre-brake subtotal \(\LeaseExpiryDelaySubtotal\) comprises five nonoverlapping intervals, a lease-expiry form of the stopping-budget delay whose hazard-detection form appears in Kinematic Stopping Envelopes and Information Age Lag: \[ \LeaseExpiryDelaySubtotal = \ChunkLease + \DetectionInterval + \FallbackHold + \DispatchObservationAge + \ActuatorResponseTime \] and the dispatch-to-admission interval \(\DispatchAdmissionInterval\) is charged beside it, never inside it. Here:
\(\ChunkLease\) runs from successful admission to lease expiry; receipt or publication alone does not renew it.
\(\DetectionInterval\) covers the post-expiry permission check and stop-command transport to the drive. It excludes the lease countdown already charged above and any separately charged hold.
\(\FallbackHold\) is an optional additional hold or fault-confirmation interval before the stop command reaches the drive; it is zero when the selected path has no such interval.
\(\DispatchObservationAge\) runs from evidence capture to perception’s dispatch of the observation, including sensing and perception inference. Clock-conversion uncertainty must enter the age bound or a justified spatial allowance once.
\(\DispatchAdmissionInterval\) runs from that dispatch through successful admission, including remaining proposer inference, transport, deserialization, validation, activation, and the wait for the next permission tick. Capture to admission is therefore \(\DispatchObservationAge + \DispatchAdmissionInterval\), and a stored dispatch age covers only its first term.
\(\ActuatorResponseTime\) runs from delivery of the stop command until the actuator establishes the braking deceleration assumed below.
The event boundaries follow the stalled-proposer timeline in Chunk Lease Sizing from the Stopping Budget. A heartbeat timeout is an alternative trigger with its own response path, not an extra countdown appended to an expired chunk lease. If clearance is instead referenced to a hazard’s appearance, the bound must also include any time until the supporting observation captures it.
Under constant cruising speed \(\SpeedAtDetection\) before brake onset, a verified lower bound on mechanical braking deceleration \(\CredibleBrakingDeceleration > 0\), a localization bound \(\LocalizationBound\), and a fixed protective clearance \(\ProtectiveClearanceMargin\), the total stopping distance bounded by available clearance \(\ClearDistance\) satisfies: \[ \SpeedAtDetection\,(\LeaseExpiryDelaySubtotal + \DispatchAdmissionInterval) + \frac{\SpeedAtDetection^2}{2 \CredibleBrakingDeceleration} + \LocalizationBound + \ProtectiveClearanceMargin \le \ClearDistance \] Solving directly for the maximum admissible validity lease \(\ChunkLease\) yields: \[ \ChunkLease \le \frac{\ClearDistance - \LocalizationBound - \ProtectiveClearanceMargin - \frac{\SpeedAtDetection^2}{2 \CredibleBrakingDeceleration}}{\SpeedAtDetection} - \left( \DetectionInterval + \FallbackHold + \DispatchObservationAge + \DispatchAdmissionInterval + \ActuatorResponseTime \right) \] This constant-deceleration model is a reference calculation, not the complete stopping budget of the running machine. A shaped stop must substitute its qualified stopping distance for \(\SpeedAtDetection^2/(2\CredibleBrakingDeceleration)\) and reserve tracking error and every other justified allowance, and the same substitution applies to the teleoperation bound below. If the right-hand side is nonpositive, this budget permits no positive execution lease: the enforcer must select the available fallback immediately, because a negative bound means that even an immediate response lacks the stated stopping reservation.
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:
\(\TeleoperationVideoLatency\) represent video camera exposure, encoding, and operator display processing, excluding network transport, which is charged in \(\TeleoperationRoundTripTime\).
\(\HumanReactionTime\) denote human neuromuscular reaction time (\(\HumanReactionTime \approx 150\text{--}250\text{ ms}\)).
\(\TeleoperationRoundTripTime = \TeleoperationDownlinkTime + \TeleoperationUplinkTime\) represent network round-trip transport latency.
\(\BrakingDistance = \frac{\SpeedAtDetection^2}{2 \CredibleBrakingDeceleration}\) represent the purely mechanical braking distance.
Total available time before available clearance \(\ClearDistance\) is exhausted is bounded by \(\ClearanceTimeBudget = (\ClearDistance - \BrakingDistance) / \SpeedAtDetection\). The allowable round-trip network latency ceiling is: \[ \TeleoperationRoundTripTime \le \frac{\ClearDistance - \BrakingDistance}{\SpeedAtDetection} - \left( \TeleoperationVideoLatency + \HumanReactionTime + \ActuatorResponseTime \right) \] If network jitter pushes \(\TeleoperationRoundTripTime\) 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 enforcer must evaluate physical invariants at a rate justified by its response budget, revoking operator command authority and executing autonomous braking before physical clearance is lost. Human Authority, Reaction Time, and Clearance uses this bound to size a handover, where the operator’s round trip spends the same pre-brake budget.
Kinodynamic bounds vs. 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). Wave variables (section 1.10) preserve the communication channel’s passivity under their delay assumptions. The coupled teleoperator still requires a valid energy argument for its other components. Passivity does not bound physical displacement. Safety requires satisfying both invariants simultaneously: wave variables bound interactive contact energy, while the kinodynamic latency inequality reserves spatial clearance under its constant-speed, fixed-obstacle, and braking assumptions.
Rigid Body Contact Mechanics
In free space, a small change in joint torque produces a smooth change in link acceleration; when a gripper finger or foot touches a rigid surface, the gap closes to zero and a normal reaction force appears abruptly to prevent interpenetration. Simulating and controlling physical interaction (locomotion, dexterous manipulation, tool assembly) requires modeling this boundary transition. The ideal unilateral, nonadhesive rigid-contact model uses complementarity: interaction forces arise strictly when bodies establish physical contact.
Let \(\GeneralizedCoordinates \in \mathbb{R}^\GeneralizedCoordinateCount\) denote generalized configuration coordinates. Let \(\ContactGapFunction(\GeneralizedCoordinates)\) be the signed distance (or gap function) between contact geometries:
\(\ContactGapFunction(\GeneralizedCoordinates) > 0\): Bodies are separated.
\(\ContactGapFunction(\GeneralizedCoordinates) = 0\): Bodies are touching (contact established).
\(\ContactGapFunction(\GeneralizedCoordinates) < 0\): Bodies are penetrating (a geometric interpenetration that rigid mechanics strictly forbids).
Let \(\ComplementarityNormalForce\) (or \(\Glyph{f}_n\)) denote the normal contact force pushing the bodies apart. In constrained mathematical optimization, the non-penetration condition corresponds to the inequality constraint \(\InequalityConstraintFunction(\GeneralizedCoordinates) = \ContactGapFunction(\GeneralizedCoordinates) \ge 0\). The governing physical law of unilateral rigid contact is precisely the Karush-Kuhn-Tucker (KKT) complementary slackness condition:
Discontinuous velocity jump across collision boundary
Step discontinuity that breaks smooth reverse-mode automatic differentiation
Just as Lagrange multipliers in constrained optimization become nonzero (\(\ConstraintLagrangeMultiplier > 0\)) only when constraints are active and tight (\(\InequalityConstraintFunction = 0\)), normal contact forces materialize exclusively upon contact closure:
If bodies are separated (\(\ContactGapFunction(\GeneralizedCoordinates) > 0\)), the constraint is inactive, enforcing \(\ComplementarityNormalForce = 0\).
If bodies are touching (\(\ContactGapFunction(\GeneralizedCoordinates) = 0\)), the constraint is active, permitting repulsive normal forces \(\ComplementarityNormalForce \ge 0\) to prevent interpenetration.
This nonadhesive contact model excludes tensile normal forces (\(\ComplementarityNormalForce<0\)); adhesion, grasping, and attached fixtures need other models.
The Delassus operator and acceleration-level LCP
At the velocity level, kinematic normal velocity is \(\ContactNormalVelocity = \ContactGapVelocity(\GeneralizedCoordinates) = \ContactNormalJacobian(\GeneralizedCoordinates)\GeneralizedVelocities\), where \(\ContactNormalJacobian(\GeneralizedCoordinates) = \nabla \ContactGapFunction(\GeneralizedCoordinates)^\top \in \mathbb{R}^{1 \times \GeneralizedCoordinateCount}\) is the contact normal Jacobian. Differentiating with respect to continuous time yields normal acceleration: \[ \ContactGapAcceleration(\GeneralizedCoordinates) = \ContactNormalJacobian(\GeneralizedCoordinates) \GeneralizedAccelerations + \ContactNormalJacobianDerivative(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities \] Substituting the multi-body equations of motion \(\GeneralizedInertiaMatrix(\GeneralizedCoordinates) \GeneralizedAccelerations + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) = \JointTorque + \ContactNormalJacobian(\GeneralizedCoordinates)^\top \ComplementarityNormalForce\) isolates configuration acceleration: \[ \GeneralizedAccelerations = \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1} \left( \JointTorque - \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities)\GeneralizedVelocities - \GravitationalTorqueVector(\GeneralizedCoordinates) + \ContactNormalJacobian(\GeneralizedCoordinates)^\top \ComplementarityNormalForce \right) \] Substituting \(\GeneralizedAccelerations\) back into the normal acceleration expression yields: \[ \ContactGapAcceleration = \DelassusOperator(\GeneralizedCoordinates) \ComplementarityNormalForce + \ContactDriftAcceleration \] where the scalar \(\DelassusOperator(\GeneralizedCoordinates) = \ContactNormalJacobian(\GeneralizedCoordinates) \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1} \ContactNormalJacobian(\GeneralizedCoordinates)^\top \ge 0\) is the Delassus operational space compliance operator (the apparent inverse inertia reflected at the contact point), and \(\ContactDriftAcceleration = \ContactNormalJacobian(\GeneralizedCoordinates) \GeneralizedInertiaMatrix(\GeneralizedCoordinates)^{-1}(\JointTorque - \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities)\GeneralizedVelocities - \GravitationalTorqueVector(\GeneralizedCoordinates)) + \ContactNormalJacobianDerivative(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities\) collects unforced kinematic and dynamic drift accelerations.
When contact is maintained (\(\ContactGapFunction = 0, \ContactGapVelocity = 0\)), non-penetration requires \(\ContactGapAcceleration \ge 0\). The contact mechanics form a canonical Linear Complementarity Problem (LCP): \[ \ContactGapAcceleration \ge 0, \quad \ComplementarityNormalForce \ge 0, \quad \ContactGapAcceleration \cdot \ComplementarityNormalForce = 0 \] Under polyhedral friction cone approximations, this generalizes to a vector LCP solvable via Lemke’s pivoting algorithm or quadratic programming. This is a rigid-contact idealization. Real interfaces deform, and the numerical contact solution must still be checked against the motion and force quantities the application needs.
Non-smooth collision manifolds and velocity jumps
When rigid bodies collide, the impact occurs over an infinitesimal duration \(\FiniteChange \TimeInstant \to 0\). For frictionless normal impact against a stationary rigid plane, let \(\PreImpactNormalVelocity < 0\) be the pre-impact normal velocity and \(\RestitutionCoefficient \in [0, 1]\) the coefficient of restitution. The Newton restitution model imposes the velocity jump: \[ \PostImpactNormalVelocity = -\RestitutionCoefficient \PreImpactNormalVelocity \implies \FiniteChange \ContactNormalVelocity = \PostImpactNormalVelocity - \PreImpactNormalVelocity = -(1 + \RestitutionCoefficient) \PreImpactNormalVelocity \] With effective normal mass \(\EffectiveImpactMass\), the normal impulse is \(\NormalContactImpulse = \EffectiveImpactMass \FiniteChange \ContactNormalVelocity\). In an idealized rigid impact, velocity changes discontinuously, meaning the acceleration is a Dirac delta distribution \(\UnitImpulseFunction(\TimeInstant - \RigidImpactInstant)\) rather than an ordinary finite derivative:5\[ \lim_{\VanishingTimeOffset \to 0^+} \frac{\lVert \GeneralizedVelocities(\RigidImpactInstant+\VanishingTimeOffset) - \GeneralizedVelocities(\RigidImpactInstant-\VanishingTimeOffset)\rVert}{2\VanishingTimeOffset} = \infty \]
5Moreau 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.
Failure modes for automatic differentiation and BPTT
For a differentiable discrete simulator \(\MechanicalPlantState_{k+1} = \SimulatorStepFunction_k(\MechanicalPlantState_k, \ModelParameters)\), define state sensitivity \(\StateParameterSensitivity_k = \partial\MechanicalPlantState_k/\partial\ModelParameters\). The chain rule propagates it as: \[ \StateParameterSensitivity_{k+1} = \frac{\partial \SimulatorStepFunction_k}{\partial\MechanicalPlantState_k}\StateParameterSensitivity_k + \frac{\partial \SimulatorStepFunction_k}{\partial\ModelParameters}. \] For a rollout loss \(\RolloutLoss = \sum_{k=0}^\RolloutStepCount\ell_k(\MechanicalPlantState_k, \ModelParameters)\), the parameter gradient is: \[ \nabla_\ModelParameters \RolloutLoss = \sum_{k=0}^\RolloutStepCount\left[\StateParameterSensitivity_k^\top\nabla_{\MechanicalPlantState_k}\ell_k + \nabla_\ModelParameters\ell_k\right]. \] Reverse-mode differentiation evaluates the same chain rule without storing every forward sensitivity. Contact complicates the derivatives of \(\SimulatorStepFunction_k\), not the chain rule itself. Within a fixed contact mode, a transition can have a finite derivative; at a mode change it can be nonsmooth, and the derivative must account for how the impact time changes with the initial state. An acceleration impulse alone does not imply that every state-transition Jacobian diverges.
A solver that differentiates only its currently active branch can miss the sensitivity of contact timing. Likewise, the contact force is zero while bodies are separated, but a distance-based loss can still supply a gradient toward contact. Whether a rollout has flat regions or abrupt changes depends on its dynamics, contact discretization, and objective.
Compliant contact replaces ideal impulses with deformation and finite-duration forces. This can improve differentiability, but the selected force law must remain repulsive and its stiffness can require smaller time steps. Implicit differentiation of converged contact constraints avoids differentiating every solver iteration, provided the active set and regularity conditions support a derivative. Stochastic policy gradients offer another route because they do not require differentiating through the contact transition; their variance and sampling cost remain separate concerns.
Elastodynamic impact force and compliance deformation
When an effective moving mass \(\EffectiveImpactMass\) strikes an obstacle or human tissue, rigid-body approximations break down because real materials deform elastically over a nonzero compression stroke \(\PenetrationDepth(\TimeInstant)\). Let \(\EffectiveSeriesStiffness\) denote the effective contact stiffness across the interface. In series compliance between the manipulator structural chain (\(\RobotStructuralStiffness\)) and human tissue or obstacle (\(\HertzianContactStiffness\)): \[ \frac{1}{\EffectiveSeriesStiffness} = \frac{1}{\RobotStructuralStiffness} + \frac{1}{\HertzianContactStiffness} \]
Unmitigated elastic impact force
For an unmitigated collision at impact velocity \(\SpeedBound\), initial kinetic energy \(\KineticEnergy = \frac{1}{2} \EffectiveImpactMass \SpeedBound^2\) converts entirely into elastic strain energy \(\ElasticStrainEnergy = \frac{1}{2} \EffectiveSeriesStiffness \PeakPenetrationDepth^2\) at the point of maximum compression (\(\PenetrationRate=0\)). Energy conservation yields the peak structural deflection: \[ \frac{1}{2} \EffectiveImpactMass \SpeedBound^2 = \frac{1}{2} \EffectiveSeriesStiffness \PeakPenetrationDepth^2 \implies \PeakPenetrationDepth = \SpeedBound \sqrt{\frac{\EffectiveImpactMass}{\EffectiveSeriesStiffness}} \] The resulting peak transient contact force \(\UnbrakedImpactForce = \EffectiveSeriesStiffness \PeakPenetrationDepth\) evaluates to: \[ \UnbrakedImpactForce = \SpeedBound \sqrt{\EffectiveSeriesStiffness \EffectiveImpactMass} \] This relation applies to a lossless linear spring with fixed effective mass and no additional energy input during compression. It identifies how speed, mass, and stiffness affect the modeled peak; a human-contact limit additionally depends on contact location, area, duration, and the applicable test conditions. The scalar spring calculation alone does not establish compliance with a safety standard.
Coupled contact indentation with mechanical braking
When active mechanical brakes engage with constant opposing force \(\EffectiveImpactMass \CredibleBrakingDeceleration\) throughout compression, the braking system performs mechanical work \(\BrakingWork = \EffectiveImpactMass \CredibleBrakingDeceleration \PenetrationDepth\) across the indentation depth \(\PenetrationDepth\). The coupled energy balance becomes: \[ \frac{1}{2} \EffectiveImpactMass \ImpactSpeed^2 = \frac{1}{2} \EffectiveSeriesStiffness \PeakPenetrationDepth^2 + \EffectiveImpactMass \CredibleBrakingDeceleration \PeakPenetrationDepth \] Rearranging into quadratic form: \[ \PeakPenetrationDepth^2 + \frac{2 \EffectiveImpactMass \CredibleBrakingDeceleration}{\EffectiveSeriesStiffness} \PeakPenetrationDepth - \frac{\EffectiveImpactMass \ImpactSpeed^2}{\EffectiveSeriesStiffness} = 0 \] Solving for the physically admissible positive deformation root: \[ \PeakPenetrationDepth = \sqrt{\left(\frac{\EffectiveImpactMass \CredibleBrakingDeceleration}{\EffectiveSeriesStiffness}\right)^2 + \frac{\EffectiveImpactMass \ImpactSpeed^2}{\EffectiveSeriesStiffness}} - \frac{\EffectiveImpactMass \CredibleBrakingDeceleration}{\EffectiveSeriesStiffness} \] When \(\CredibleBrakingDeceleration = 0\), this expression smoothly recovers the unmitigated deformation \(\PeakPenetrationDepth = \ImpactSpeed\sqrt{\EffectiveImpactMass/\EffectiveSeriesStiffness}\). For the unbraked spring model, the quarter-cycle compression duration is \(\QuarterCycleCompressionTime = \frac{\pi}{2}\sqrt{\EffectiveImpactMass/\EffectiveSeriesStiffness}\). This is not the peak time of the actively braked model. Reactive braking triggered only after physical contact cannot attenuate the unbraked initial force peak if sensor detection and brake engagement latency exceed this \(\QuarterCycleCompressionTime\). Consequently, safety cases cannot substitute reactive post-impact braking for verified pre-impact separation clearance envelopes (\(\PreImpactSeparationDistance \ge \StoppingDistance\)).
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 \(\LongitudinalSlipRatio\) quantifies the kinematic velocity mismatch between the wheel circumferential speed \(\WheelRadius\WheelAngularSpeed\) (wheel radius \(\WheelRadius\), angular rate \(\WheelAngularSpeed\)) and the vehicle forward longitudinal speed \(\LongitudinalVehicleSpeed\): \[
\LongitudinalSlipRatio = \begin{cases}
\frac{\WheelRadius\WheelAngularSpeed - \LongitudinalVehicleSpeed}{\WheelRadius\WheelAngularSpeed}, & \text{under acceleration } (\WheelRadius\WheelAngularSpeed > \LongitudinalVehicleSpeed \ge 0) \\[6pt]
\frac{\WheelRadius\WheelAngularSpeed - \LongitudinalVehicleSpeed}{\LongitudinalVehicleSpeed}, & \text{under braking } (\LongitudinalVehicleSpeed > \WheelRadius\WheelAngularSpeed \ge 0)
\end{cases}
\] For forward motion with nonnegative wheel speed, the same convention can be regularized near standstill: \[ \LongitudinalSlipRatio = \frac{\WheelRadius\WheelAngularSpeed - \LongitudinalVehicleSpeed}{\max(|\LongitudinalVehicleSpeed|, |\WheelRadius\WheelAngularSpeed|, \SlipRegularizationSpeed)} \in [-1, 1] \] The bound \([-1,1]\) assumes \(\LongitudinalVehicleSpeed\) and \(\WheelRadius\WheelAngularSpeed\) have the same sign; reverse motion needs a declared direction convention. The endpoint values below also assume the nonzero speed exceeds \(\SlipRegularizationSpeed\). Here \(\LongitudinalSlipRatio = 0\) corresponds to pure rolling without slip, \(\LongitudinalSlipRatio = 1\) denotes complete drive-wheel spin-out (\(\LongitudinalVehicleSpeed = 0, \WheelAngularSpeed > 0\)), and \(\LongitudinalSlipRatio = -1\) corresponds to complete wheel lock-up (\(\WheelRadius\WheelAngularSpeed = 0, \LongitudinalVehicleSpeed > 0\)).
Tractive force \(\TractiveForce\) scales with the normal contact load \(\WheelNormalLoad\) through the longitudinal friction coefficient \(\LongitudinalFrictionCoefficient(\LongitudinalSlipRatio)\): \[ \TractiveForce(\LongitudinalSlipRatio) = \LongitudinalFrictionCoefficient(\LongitudinalSlipRatio) \WheelNormalLoad \] At low slip ratios (\(|\LongitudinalSlipRatio| < \PeakSlipRatio\)), where the peak depends on tire and surface conditions, tire or tread rubber shears elastically, and tractive force is approximately linear in slip: \(\TractiveForce \approx \LongitudinalTractionStiffness \LongitudinalSlipRatio\), with longitudinal traction stiffness \(\LongitudinalTractionStiffness\). Beyond \(\PeakSlipRatio\), adhesion collapses into gross sliding friction: the slope \(\DerivativeOperator\LongitudinalFrictionCoefficient / \DerivativeOperator\LongitudinalSlipRatio\) turns negative, which can destabilize a torque-controlled wheel-speed equilibrium. If an ungrounded policy commands drive motor torque exceeding peak traction (\(\CommandedTractiveForce > \PeakTractionCoefficient \WheelNormalLoad\)), the wheel rapidly accelerates into the unstable slip regime (\(\DerivativeOperator\WheelAngularSpeed/d\TimeInstant = (\CommandedJointTorque - \WheelRadius \TractiveForce)/\WheelInertia > 0\)). The resulting slip can reduce lateral authority or disturb yaw, a cyber-physical disconnect that cannot be detected by directional agreement or cosine similarity metrics that evaluate only vector angles while ignoring control magnitude.
Grasp mechanics: Friction cones and force closure
The nonadhesive contact model above lets a contact push but never pull. In the parallel-jaw hold considered here, friction supports the object against downward motion. Coulomb’s law bounds the tangential force \(\FingerTangentialForce\) a contact can carry by the normal force \(\FingerNormalForce\) pressing it, \(\|\FingerTangentialForce\| \le \CoulombFrictionCoefficient \FingerNormalForce\), where \(\CoulombFrictionCoefficient\) is the friction coefficient between finger and object. Every admissible contact force therefore lies inside a friction cone about the contact normal, with half-angle \(\arctan \CoulombFrictionCoefficient\).
A parallel-jaw hold shows the consequence with nothing beyond Newton’s second law. Two fingers squeeze an object of mass \(\Mass\) on opposite vertical faces, each with normal force \(\FingerNormalForce\). Friction at the two contacts must carry the weight and any upward acceleration \(\GraspVerticalAcceleration\) the arm imposes while it moves: \[ 2\CoulombFrictionCoefficient \FingerNormalForce \ge \Mass\,(\Gravity + \GraspVerticalAcceleration) \quad\Longrightarrow\quad \FingerNormalForce \ge \frac{\Mass\,(\Gravity + \GraspVerticalAcceleration)}{2\CoulombFrictionCoefficient} \tag{1}\] The required squeeze grows with the arm’s acceleration and inversely with friction: halving \(\CoulombFrictionCoefficient\) doubles it. The object sets the other bound: a thin-walled cup or a ripe fruit deforms or breaks above some normal force. A grasp is feasible only when the interval between the two bounds is nonempty, and that interval is the band a guarded-contact check \(\GraspForceFloor \le \FingerNormalForce \le \GraspForceCeiling\) enforces (Grounded Affordances). In this two-finger model, the contact-force magnitude in that check denotes the normal squeeze at each finger, measured or conservatively inferred under qualified contact geometry. A generic force-vector norm or the net object wrench cannot substitute for that squeeze. Each term is a measurement the grasp needs: the object’s mass, the friction coefficient of this finger material on this surface, the planned peak acceleration, and the crush force. Horizontal acceleration and the moment of an off-center load add terms of the same form.
The general statement collects every contact into one linear map. A force \(\ContactPointForce_i\) applied at contact \(\ContactPointIndex\), located at \(\ContactPointOffset_i\) from the object’s reference point, exerts on the object the wrench formed by the force and its moment \(\ContactPointOffset_i \times \ContactPointForce_i\). Stacking all contact forces into \(\StackedContactForces\) gives the grasp map\(\GraspMap\), with object wrench \(\ObjectWrench = \GraspMap\StackedContactForces\) (the same bold letter as the Kalman gain, but a different matrix). A grasp has force closure when every external wrench \(\ExternalObjectWrench\) on the object can be balanced, \(\GraspMap\StackedContactForces = -\ExternalObjectWrench\), by contact forces that all lie inside their friction cones (Lynch and Park 2017). Approximating each cone by a few edge vectors makes the test finite: force closure holds when the origin lies strictly inside the convex hull of the primitive wrenches those edges generate. For two fingers in a plane, Nguyen’s condition is its geometric form: the segment joining the contact points must lie inside both friction cones (Nguyen 1988). In three dimensions, two point contacts cannot resist a torque about the line joining them; real fingertips resist it through torsional friction over a finite contact patch, the soft-finger contact model. Ferrari and Canny’s quality measure, the radius of the largest origin-centered ball inside the hull, turns the yes-or-no test into the smallest disturbance the grasp is guaranteed to resist per unit of contact force (Ferrari and Canny 1992).
Nguyen, Van-Duc. 1988. “Constructing Force-Closure Grasps.”The International Journal of Robotics Research 7 (3): 3–16. https://doi.org/10.1177/027836498800700301.
Ferrari, Carlo, and John Canny. 1992. “Planning Optimal Grasps.”Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2290–95. https://doi.org/10.1109/ROBOT.1992.219918.
Force closure is qualitative. It says a balancing set of forces exists, not that the gripper can produce them within its force limit or without crushing the object, which is why the magnitude check of equation 1 remains necessary for every grasp the machine admits.
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, and quantifying how rapidly estimation uncertainty grows across that blind interval requires continuous error-covariance dynamics and observability analysis. The discrete Kalman filter and its extended form, which the machine runs on dated measurements, are developed in Estimating State from Dated Measurements, and the consistency tests that decide whether a filter’s reported covariance can be trusted are in Testing whether the covariance is honest; this section treats the continuous-time limit that governs uncertainty growth between measurements.
Continuous Lyapunov and Riccati covariance dynamics
Consider a continuous-time linear dynamic system governed by: \[ \PlantStateRate(\TimeInstant) = \StateSpaceSystemMatrix \MechanicalPlantState(\TimeInstant) + \StateSpaceInputMatrix \ControlInput(\TimeInstant) + \ProcessNoiseVector(\TimeInstant) \]\[ \MeasurementVector(\TimeInstant) = \MeasurementMatrix \MechanicalPlantState(\TimeInstant) + \MeasurementNoiseVector(\TimeInstant) \] For the continuous white-noise model, \(\ProcessNoiseIntensity\) and \(\ContinuousMeasurementNoiseIntensity\) are process and measurement noise intensities, with covariance kernels \(\mathbb{E}[\ProcessNoiseVector(\TimeInstant)\ProcessNoiseVector(\PairedTimeInstant)^\top]=\ProcessNoiseIntensity\UnitImpulseFunction(\TimeInstant-\PairedTimeInstant)\) and \(\mathbb{E}[\MeasurementNoiseVector(\TimeInstant)\MeasurementNoiseVector(\PairedTimeInstant)^\top]=\ContinuousMeasurementNoiseIntensity\UnitImpulseFunction(\TimeInstant-\PairedTimeInstant)\). The model assumes suitable independence and \(\ContinuousMeasurementNoiseIntensity\succ0\). Measurement-noise intensity carries products of measurement-component units times seconds; per-sample measurement covariance carries products of measurement-component units.
Under an optimal continuous-time estimator (the Kalman-Bucy filter (Kalman 1960)), the error covariance matrix \(\EstimationErrorCovariance(\TimeInstant) = \mathbb{E}[(\MechanicalPlantState - \EstimatedPlantState)(\MechanicalPlantState - \EstimatedPlantState)^\top]\) evolves according to the continuous matrix differential Riccati equation:6\[ \ErrorCovarianceDerivative(\TimeInstant) = \StateSpaceSystemMatrix \EstimationErrorCovariance(\TimeInstant) + \EstimationErrorCovariance(\TimeInstant) \StateSpaceSystemMatrix^\top + \ProcessNoiseIntensity - \EstimationErrorCovariance(\TimeInstant) \MeasurementMatrix^\top \ContinuousMeasurementNoiseIntensity^{-1} \MeasurementMatrix \EstimationErrorCovariance(\TimeInstant) \]
Kalman, Rudolph Emil. 1960. “A New Approach to Linear Filtering and Prediction Problems.”Journal of Basic Engineering 82 (1): 35–45. https://doi.org/10.1115/1.3662552.
6Continuous 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.
When direct sensing is occluded or dropped (\(\MeasurementMatrix = \ZeroMatrix\)), the measurement correction term vanishes. The covariance growth collapses to the open-loop continuous Lyapunov differential equation: \[ \ErrorCovarianceDerivative(\TimeInstant) = \StateSpaceSystemMatrix \EstimationErrorCovariance(\TimeInstant) + \EstimationErrorCovariance(\TimeInstant) \StateSpaceSystemMatrix^\top + \ProcessNoiseIntensity \] The solution propagates initial uncertainty and accumulated \(\ProcessNoiseIntensity\) through \(e^{\StateSpaceSystemMatrix{}\TimeInstant}\). Unstable modes can grow exponentially; a random walk with \(\StateSpaceSystemMatrix=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.
Consider a one-dimensional double-integrator kinematic system modeling position \(\DoubleIntegratorPosition(\TimeInstant)\) and velocity \(\DoubleIntegratorVelocity(\TimeInstant)\) under unmeasured stochastic acceleration noise \(\AccelerationWhiteNoise(\TimeInstant)\): \[
\begin{bmatrix} \DoubleIntegratorPositionDerivative(\TimeInstant) \\ \DoubleIntegratorVelocityDerivative(\TimeInstant) \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \DoubleIntegratorPosition(\TimeInstant) \\ \DoubleIntegratorVelocity(\TimeInstant) \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \end{bmatrix} \AccelerationWhiteNoise(\TimeInstant)
\] where \(\AccelerationWhiteNoise(\TimeInstant)\) is zero-mean continuous white process noise with continuous spectral power intensity \(\AccelerationNoiseSpectralDensity > 0\), such that \(\mathbb{E}[\AccelerationWhiteNoise(\TimeInstant) \AccelerationWhiteNoise(\PairedTimeInstant)] = \AccelerationNoiseSpectralDensity \UnitImpulseFunction(\TimeInstant - \PairedTimeInstant)\). Here, \(\StateSpaceSystemMatrix = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}\) and the continuous process noise matrix is \(\ProcessNoiseIntensity = \begin{bmatrix} 0 & 0 \\ 0 & \AccelerationNoiseSpectralDensity \end{bmatrix}\). This intensity is not a per-sample acceleration variance: for position in meters and time in seconds, its units are square meters per cubic second, so multiplying it by elapsed time cubed gives positional variance in square meters.
Because \(\StateSpaceSystemMatrix^2=\ZeroMatrix\), the matrix exponential terminates after its linear term. The state transition is: \[
\ContinuousStateTransitionMatrix(\TimeInstant) = \IdentityMatrix + \StateSpaceSystemMatrix{}\TimeInstant = \begin{bmatrix} 1 & \TimeInstant \\ 0 & 1 \end{bmatrix}
\] During a blind occlusion window \(\TimeInstant \in [0, \FiniteChange \TimeInstant]\) with no measurement updates (\(\MeasurementMatrix = \ZeroMatrix\)), the total state covariance \(\EstimationErrorCovariance(\TimeInstant)\) evolves as the sum of unforced homogeneous propagation and accumulated process diffusion: \[
\EstimationErrorCovariance(\TimeInstant) = \ContinuousStateTransitionMatrix(\TimeInstant) \EstimationErrorCovariance_0 \ContinuousStateTransitionMatrix(\TimeInstant)^\top + \int_0^\TimeInstant \ContinuousStateTransitionMatrix(\TimeInstant - \ConvolutionTimeInstant) \ProcessNoiseIntensity \ContinuousStateTransitionMatrix(\TimeInstant - \ConvolutionTimeInstant)^\top d\ConvolutionTimeInstant
\] Let the initial state covariance at the instant of sensor loss (\(\TimeInstant = 0\)) be: \[
\EstimationErrorCovariance_0 = \begin{bmatrix} \InitialPositionStandardDeviation^2 & \InitialPositionVelocityCovariance \\ \InitialPositionVelocityCovariance & \InitialVelocityStandardDeviation^2 \end{bmatrix}
\] The homogeneous propagation term evaluates to: \[
\ContinuousStateTransitionMatrix(\TimeInstant) \EstimationErrorCovariance_0 \ContinuousStateTransitionMatrix(\TimeInstant)^\top = \begin{bmatrix} 1 & \TimeInstant \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \InitialPositionStandardDeviation^2 & \InitialPositionVelocityCovariance \\ \InitialPositionVelocityCovariance & \InitialVelocityStandardDeviation^2 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ \TimeInstant & 1 \end{bmatrix} = \begin{bmatrix} \InitialPositionStandardDeviation^2 + 2\InitialPositionVelocityCovariance \TimeInstant + \InitialVelocityStandardDeviation^2 \TimeInstant^2 & \InitialPositionVelocityCovariance + \InitialVelocityStandardDeviation^2 \TimeInstant \\ \InitialPositionVelocityCovariance + \InitialVelocityStandardDeviation^2 \TimeInstant & \InitialVelocityStandardDeviation^2 \end{bmatrix}
\] The accumulated stochastic diffusion term integrates across elapsed time: \[
\AccumulatedProcessNoiseCovariance(\TimeInstant) = \int_0^\TimeInstant \begin{bmatrix} 1 & \TimeInstant - \ConvolutionTimeInstant \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 & 0 \\ 0 & \AccelerationNoiseSpectralDensity \end{bmatrix} \begin{bmatrix} 1 & 0 \\ \TimeInstant - \ConvolutionTimeInstant & 1 \end{bmatrix} d\ConvolutionTimeInstant = \int_0^\TimeInstant \begin{bmatrix} \AccelerationNoiseSpectralDensity (\TimeInstant - \ConvolutionTimeInstant)^2 & \AccelerationNoiseSpectralDensity (\TimeInstant - \ConvolutionTimeInstant) \\ \AccelerationNoiseSpectralDensity (\TimeInstant - \ConvolutionTimeInstant) & \AccelerationNoiseSpectralDensity \end{bmatrix} d\ConvolutionTimeInstant = \begin{bmatrix} \frac{1}{3} \AccelerationNoiseSpectralDensity \TimeInstant^3 & \frac{1}{2} \AccelerationNoiseSpectralDensity \TimeInstant^2 \\ \frac{1}{2} \AccelerationNoiseSpectralDensity \TimeInstant^2 & \AccelerationNoiseSpectralDensity \TimeInstant \end{bmatrix}
\] Summing the homogeneous and stochastic contributions yields the closed-form positional variance \(\PropagatedPositionStandardDeviation^2(\TimeInstant) = \EstimationErrorCovariance_{11}(\TimeInstant)\): \[
\PropagatedPositionStandardDeviation^2(\TimeInstant) = \InitialPositionStandardDeviation^2 + 2 \InitialPositionVelocityCovariance \TimeInstant + \InitialVelocityStandardDeviation^2 \TimeInstant^2 + \frac{1}{3} \AccelerationNoiseSpectralDensity \TimeInstant^3
\] This derivation explains the distinct physical error regimes during sensor occlusion: initial velocity uncertainty \(\InitialVelocityStandardDeviation^2\) induces quadratic variance growth in time (\(\TimeInstant^2\)), while continuous stochastic disturbances \(\AccelerationNoiseSpectralDensity\) (such as air currents or unmodeled drivetrain chatter) inject cubic uncertainty growth (\(\frac{1}{3} \AccelerationNoiseSpectralDensity \TimeInstant^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
Measuring only a mobile base’s wheel speeds tells an estimator how fast the base is moving, but not where it started in the aisle: two trajectories shifted by \(0.5\text{ m}\) produce identical wheel-speed traces. In state estimation and machine learning, observability is the mathematical dual of reachability: whereas reachability asks whether latent internal states can be steered by control inputs, observability asks whether latent states can be reconstructed from external sensor observations \(\MeasurementVector(\TimeInstant)\).
In modern deep learning (such as world models, state-space models, and latent representation learning), neural encoders \(\EncodedLatentState = \LatentStateEncoder(\MeasurementVector)\) attempt to reconstruct underlying states from sensory observations. Observability establishes the fundamental, information-theoretic limit of this reconstruction: if a dynamical system is unobservable, the mapping from physical state trajectories to sensor observations is strictly non-injective. In that case, distinct physical states can produce mathematically indistinguishable observation sequences. No neural network, regardless of parameter count or training data, can reconstruct an unobservable state without additional information. Active excitation can help some nonlinear systems, but a known input cannot repair the unobservable subspace of a fixed LTI model.
A linear time-invariant system \((\StateSpaceSystemMatrix, \MeasurementMatrix)\) is observable on \([0, \FiniteTimeHorizon]\) if and only if the Kalman observability matrix has full column rank \(\StateDimension\): \[ \ObservabilityMatrix = \begin{bmatrix} \MeasurementMatrix \\ \MeasurementMatrix\StateSpaceSystemMatrix \\ \vdots \\ \MeasurementMatrix\StateSpaceSystemMatrix^{n-1} \end{bmatrix}, \quad \operatorname{rank}(\ObservabilityMatrix) = \StateDimension \] If \(\operatorname{rank}(\ObservabilityMatrix) < \StateDimension\), the unobservable subspace \(\ker(\ObservabilityMatrix)\) contains initial state trajectories that produce identical output measurements. No linear or nonlinear estimator can distinguish between initial states that differ along \(\ker(\ObservabilityMatrix)\).
For a smooth autonomous nonlinear system \(\PlantStateRate = \ControlAffineDriftField(\MechanicalPlantState)\), \(\MeasurementVector = \NonlinearOutputFunction(\MechanicalPlantState)\) where \(\NonlinearOutputFunction = [\OutputComponentFunction_1(\MechanicalPlantState), \dots, \OutputComponentFunction_{\OutputDimension}(\MechanicalPlantState)]^\top\), the Hermann-Krener observability criterion (Hermann and Krener 1977) generalizes the Kalman rank test via successive directional derivatives along system dynamics. Differentiating the measurement output equation \(\MeasurementVector(\TimeInstant) = \NonlinearOutputFunction(\MechanicalPlantState(\TimeInstant))\) with respect to continuous time yields: \[ \MeasurementVectorDerivative(\TimeInstant) = \frac{\partial \NonlinearOutputFunction}{\partial \MechanicalPlantState} \PlantStateRate = \frac{\partial \NonlinearOutputFunction}{\partial \MechanicalPlantState} \ControlAffineDriftField(\MechanicalPlantState) = \DriftLieDerivativeOperator \NonlinearOutputFunction(\MechanicalPlantState) \] Differentiating repeatedly produces the sequence of Lie derivatives, which are directional derivatives of the measurement channels along the unforced drift vector field \(\ControlAffineDriftField\): \[ \MeasurementVector^{(\LieDerivativeOrder)}(\TimeInstant) = \DriftLieDerivativeOperator^\LieDerivativeOrder \NonlinearOutputFunction(\MechanicalPlantState), \quad \text{where } \DriftLieDerivativeOperator^\LieDerivativeOrder \OutputComponentFunction_{\OutputIndex}(\MechanicalPlantState) = \nabla (\DriftLieDerivativeOperator^{\LieDerivativeOrder-1} \OutputComponentFunction_{\OutputIndex}(\MechanicalPlantState)) \cdot \ControlAffineDriftField(\MechanicalPlantState) \] These successive directional derivatives reveal how temporal variations in output channels encode higher-order kinematic and dynamic states (such as recovering joint velocities and accelerations from encoder or camera position sequences) without requiring real-time numerical differentiation of noisy signals.
Hermann, Robert, and Arthur J. Krener. 1977. “Nonlinear Controllability and Observability.”IEEE Transactions on Automatic Control 22 (5): 728–40.
7Hermann-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.
The Hermann-Krener observation space\(\NonlinearObservationSpace\) is the real vector space of all repeated Lie derivatives \(\{\DriftLieDerivativeOperator^\LieDerivativeOrder \OutputComponentFunction_{\OutputIndex}\}_{k \ge 0, 1 \le \OutputIndex \le \OutputDimension}\). Its gradient differentials span the observability codistribution: \[ \text{d}\NonlinearObservationSpace(\MechanicalPlantState) = \operatorname{span}\left\{ \text{d}\DriftLieDerivativeOperator^\LieDerivativeOrder \OutputComponentFunction_{\OutputIndex}(\MechanicalPlantState) \;\middle|\; \LieDerivativeOrder \ge 0, \; 1 \le \OutputIndex \le \OutputDimension \right\} \] where \(\text{d}\StateScalarFunction(\MechanicalPlantState) = \frac{\partial \StateScalarFunction}{\partial \MechanicalPlantState}\). The system satisfies the nonlinear observability rank condition at \(\MechanicalPlantState_0\) if:7\[ \operatorname{dim}(\text{d}\NonlinearObservationSpace(\MechanicalPlantState_0)) = \StateDimension \] When full rank \(\StateDimension\) is achieved, the directional gradients span the cotangent space \(\StateCotangentSpace \PlantStateSpace\), guaranteeing local weak observability: any infinitesimal deviation in latent state produces a distinguishable signature in the measurement sequence or its temporal derivatives.
Centroidal Dynamics and Floating-Base Control
Legged robots have an unactuated floating base as well as motorized joints. Internal joint motion can change the base pose, but only external forces change total linear momentum, and only external moments change total angular momentum. Ground contacts therefore determine which center-of-mass and momentum trajectories the robot can sustain against gravity.
Floating-base equations of motion
Let \(\GeneralizedCoordinates = [\FloatingBasePose^\top, \ActuatedJointCoordinates^\top]^\top \in \SpecialEuclideanGroup \times \mathbb{R}^\JointCount\) represent the generalized coordinates, where \(\FloatingBasePose \in \SpecialEuclideanGroup\) is the floating-base pose (position and orientation, 6 DoF) and \(\ActuatedJointCoordinates \in \mathbb{R}^\JointCount\) are the actuated joint angles. The total system possesses \(\JointCount + 6\) degrees of freedom. The displayed \(\GeneralizedVelocities\) and \(\GeneralizedAccelerations\) use a local coordinate convention; a global implementation must distinguish configuration coordinates from generalized velocities, particularly with quaternion orientation. The equations of motion are (Siciliano et al. 2009): \[ \GeneralizedInertiaMatrix(\GeneralizedCoordinates) \GeneralizedAccelerations + \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates) = \ActuationSelectionMatrix^\top \JointTorque + \sum_{k=1}^{\ContactCount} {\ContactPointJacobian}_k(\GeneralizedCoordinates)^\top \ContactPointForce_{k} \] where:
\(\ActuationSelectionMatrix = [\ZeroMatrix_{\JointCount \times 6}, \IdentityMatrix_{\JointCount \times \JointCount}] \in \mathbb{R}^{\JointCount \times (\JointCount+6)}\) is the actuation selection matrix separating the 6 unactuated floating-base coordinates from the \(\JointCount\) actuated joints.
\(\JointTorque \in \mathbb{R}^\JointCount\) is the commanded motor torque vector.
\({\ContactPointJacobian}_k(\GeneralizedCoordinates) = \partial \CartesianPosition_k / \partial \GeneralizedCoordinates \in \mathbb{R}^{3 \times (\JointCount+6)}\) is the contact point Jacobian for point-contact force \(\ContactPointForce_{k} \in \mathbb{R}^3\).
Partitioning the equations of motion into floating-base (\(\Glyph{b}\)) and actuated joint (\(\Glyph{j}\)) coordinates highlights the fundamental underactuation of legged machines: \[
\begin{bmatrix}
\FloatingBaseInertiaBlock(\GeneralizedCoordinates) & \BaseJointInertiaCoupling(\GeneralizedCoordinates) \\
\JointBaseInertiaCoupling(\GeneralizedCoordinates) & \GeneralizedInertiaMatrix_{jj}(\GeneralizedCoordinates)
\end{bmatrix}
\begin{bmatrix} \FloatingBaseAccelerations \\ \ActuatedJointAccelerations \end{bmatrix}
+
\begin{bmatrix} \FloatingBaseBiasForces(\GeneralizedCoordinates, \GeneralizedVelocities) \\ \ActuatedJointBiasForces(\GeneralizedCoordinates, \GeneralizedVelocities) \end{bmatrix}
=
\begin{bmatrix} \ZeroMatrix \\ \JointTorque \end{bmatrix}
+
\sum_{k=1}^{\ContactCount} \begin{bmatrix} \ContactJacobianBaseBlock^\top \\ \ContactJacobianJointBlock^\top \end{bmatrix} \ContactPointForce_{k}
\] where \(\GeneralizedBiasForces(\GeneralizedCoordinates, \GeneralizedVelocities) = \CoriolisCentrifugalMatrix(\GeneralizedCoordinates, \GeneralizedVelocities)\GeneralizedVelocities + \GravitationalTorqueVector(\GeneralizedCoordinates)\). Notice that the top 6 rows have identically zero direct motor authority (\(\ActuationSelectionBaseBlock^\top \JointTorque = \ZeroMatrix\)). The base equation still contains gravity and coupling to joint acceleration, so the base can accelerate even in flight: \[ \FloatingBaseInertiaBlock(\GeneralizedCoordinates) \FloatingBaseAccelerations + \BaseJointInertiaCoupling(\GeneralizedCoordinates) \ActuatedJointAccelerations + \FloatingBaseBiasForces(\GeneralizedCoordinates, \GeneralizedVelocities) = \sum_{k=1}^{\ContactCount} \ContactJacobianBaseBlock(\GeneralizedCoordinates)^\top \ContactPointForce_{k} \]
Centroidal momentum dynamics
Projecting the multi-body floating-base dynamics onto the robot’s center of mass \(\MassCenterPosition \in \mathbb{R}^3\) yields the Newton-Euler centroidal momentum equations(Lynch and Park 2017): \[ \CentroidalLinearMomentumDerivative = \Mass \MassCenterAcceleration = \sum_{k=1}^{\ContactCount} \ContactPointForce_{k} + \Mass \GravityVector \]\[ \CentroidalAngularMomentumDerivative = \sum_{k=1}^{\ContactCount}\left[(\CartesianPosition_k - \MassCenterPosition) \times \ContactPointForce_{k} + \ContactPointTorque_k\right] \] where \(\CentroidalLinearMomentum\) is linear momentum, \(\CentroidalAngularMomentum\) is angular momentum about the CoM, \(\Mass\) is the total robot mass, \(\GravityVector\) is the gravitational acceleration vector, and \((\CartesianPosition_k, \ContactPointForce_{k}, \ContactPointTorque_k)\) are the position, force, and moment at each active contact patch.8
8Centroidal 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.
The total centroidal momentum \(\CentroidalMomentum = [\CentroidalLinearMomentum^\top, \CentroidalAngularMomentum^\top]^\top \in \mathbb{R}^6\) linearly maps from generalized velocities through the Centroidal Momentum Matrix (CMM)\(\CentroidalMomentumMatrix(\GeneralizedCoordinates) \in \mathbb{R}^{6 \times (\JointCount+6)}\)(Lynch and Park 2017): \[ \CentroidalMomentum = \CentroidalMomentumMatrix(\GeneralizedCoordinates) \GeneralizedVelocities \] Differentiating with respect to time relates generalized accelerations to external contact wrenches: \[ \CentroidalMomentumRate = \CentroidalMomentumMatrix(\GeneralizedCoordinates) \GeneralizedAccelerations + \CentroidalMomentumMatrixDerivative(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities = \sum_{k=1}^{\ContactCount} \ContactWrenchTransform \ContactWrench_k + \begin{bmatrix} \Mass \GravityVector \\ \ZeroMatrix \end{bmatrix} \] where \(\ContactWrenchTransform\) transforms contact wrench \(\ContactWrench_k = [\ContactPointForce_{k}^\top, \ContactPointTorque_k^\top]^\top\) to the center of mass frame.
Physical contact feasibility constraints
For nonadhesive support contacts with finite friction, admissible contact wrenches must satisfy three physical invariants:
Unilateral Normal Force: Feet can only push against the ground: \[ \NormalContactForce = \ContactPointForce_{k}^\top \ContactNormal_k \ge 0 \]
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:9\[ \ZeroMomentPoint\in\SupportPolygon,\qquad \SupportPolygon=\operatorname{Conv}\!\left(\bigcup_k\ContactPatchRegion_k\right), \] where \(\ContactPatchRegion_k\) is each active contact patch projected onto that plane.
9Zero 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.
Hierarchical whole-body control quadratic program
In floating-base humanoid and legged systems, high-level policies (such as learned vision-language-action models or action chunking policies) propose desired kinematic behaviors: end-effector manipulation paths, torso orientation, gaze directions, or swing-foot trajectories. However, because the 6-DoF floating base is unactuated, these kinematic proposals cannot be executed directly by joint motors. The Hierarchical Whole-Body Control (WBC) Quadratic Program acts as a real-time multi-task constrained optimization projection layer, projecting competing operational-space task accelerations \(\DesiredTaskSpaceAcceleration_i\) through a prioritized optimization hierarchy while strictly enforcing rigid-body Newton-Euler dynamics, friction cones, and actuator limits. In optimization and machine learning terms, Whole-Body Control solves the physical counterpart of multi-task learning with conflicting gradients:
Concept
Multi-Task Deep Learning (PCGrad / MGDA)
Hierarchical Whole-Body Control (WBC)
Task Specifications
Loss functions \(\TaskSpecificLoss_1(\ModelParameters), \dots, \TaskSpecificLoss_{\TaskCount}(\ModelParameters)\)
Hard KKT constraints: dynamics, friction cones, torque saturation
Operational-space tasks and analytical null-space projection
Let task \(\ControlTaskIndex\) be defined by a kinematics mapping \(\TaskSpaceCoordinates_i = \TaskKinematicMap_i(\GeneralizedCoordinates) \in \mathbb{R}^{\TaskSpaceDimension_i}\) (e.g., end-effector Cartesian pose, center of mass position, or torso orientation). Differentiating twice yields the task acceleration: \[ \TaskSpaceAcceleration_i = \TaskJacobian_i(\GeneralizedCoordinates) \GeneralizedAccelerations + \TaskJacobianDerivative_i(\GeneralizedCoordinates, \GeneralizedVelocities) \GeneralizedVelocities \] A tracking controller generates desired task acceleration via proportional-derivative feedback: \[ \DesiredTaskSpaceAcceleration_i = \TaskSpaceReferenceAcceleration_i + \TaskProportionalGain (\TaskSpaceReference - \TaskSpaceCoordinates_i) + \TaskDerivativeGain (\TaskSpaceReferenceVelocity_i - \TaskSpaceVelocity_i) \] In complex humanoids, multiple tasks compete for limited degrees of freedom. Consider two prioritized tasks: Task 1 (highest priority, e.g., CoM balance and stance-foot ground contact) and Task 2 (secondary priority, e.g., end-effector trajectory tracking).
To ensure Task 2 never compromises Task 1, analytical Whole-Body Control projects Task 2 into the kinematic null space of Task 1. The unconstrained acceleration for Task 1 is: \[ \GeneralizedAccelerations_1^* = \TaskJacobianPseudoinverse_1 \left( \DesiredTaskSpaceAcceleration_1 - \TaskJacobianDerivative_1 \GeneralizedVelocities \right) \] Assuming \(\TaskJacobian_1\) has full row rank, \(\TaskJacobianPseudoinverse_1 = \TaskJacobian_1^\top (\TaskJacobian_1 \TaskJacobian_1^\top)^{-1}\) is the Moore-Penrose pseudo-inverse. The null-space projection matrix of Task 1 is: \[ \NullSpaceProjector_1 = \IdentityMatrix - \TaskJacobianPseudoinverse_1 \TaskJacobian_1 \] Notice the fundamental algebraic property: \[ \TaskJacobian_1 \NullSpaceProjector_1 = \TaskJacobian_1 \left( \IdentityMatrix - \TaskJacobianPseudoinverse_1 \TaskJacobian_1 \right) = \TaskJacobian_1 - \TaskJacobian_1 \TaskJacobianPseudoinverse_1 \TaskJacobian_1 = \ZeroMatrix \] Projecting the secondary task command \(\GeneralizedAccelerations_2\) through \(\NullSpaceProjector_1\): \[ \GeneralizedAccelerations^* = \GeneralizedAccelerations_1^* + \NullSpaceProjector_1 \GeneralizedAccelerations_2^* \] guarantees that Task 2 produces strictly zero acceleration in the direction of Task 1: \[ \TaskJacobian_1 \GeneralizedAccelerations^* = \TaskJacobian_1 \GeneralizedAccelerations_1^* + \TaskJacobian_1 \NullSpaceProjector_1 \GeneralizedAccelerations_2^* = \TaskJacobian_1 \GeneralizedAccelerations_1^* + \ZeroMatrix = \DesiredTaskSpaceAcceleration_1 - \TaskJacobianDerivative_1 \GeneralizedVelocities \] Task 2 operates exclusively within the redundant internal degrees of freedom left unconstrained by Task 1.
Embedded whole-body control quadratic program
While analytical pseudo-inverse projections provide elegant decoupling, they cannot enforce inequality constraints such as unilateral contact forces, friction cones, and motor torque limits. Real-time controller firmware resolves this by formulating Whole-Body Control as a convex Quadratic Program whose execution deadline is part of the controller design.
Dynamic Feasibility: The equality constraint enforces the coupled multi-body floating-base equations of motion, including joint coupling, gravity, and the modeled external contact forces. The stance-contact equality prevents assigning a supporting force to a point that is simultaneously commanded to move away from the support.
Multi-Task Null-Space Hierarchy: Finite objective weights \(\TaskWeight_i\) express a preference (\(\TaskWeight_1 \gg \TaskWeight_2 \gg \dots \gg \TaskWeight_{\TaskCount}\)). High-priority tasks (maintaining center-of-mass balance, stance-foot grounding, and trunk stability) dominate lower-priority goals (arm trajectory tracking or joint posture regularization toward nominal rest configurations). In strict hierarchical QP formulations, tasks are solved lexicographically in cascaded QPs, fixing the optimal residual of higher-priority tasks as hard constraints for subsequent stages.
Physical Contact and Torque Protection: The linear inequalities enforce contact friction pyramids (\(\FrictionPyramidMatrix_k \ContactPointForce_{k} \le \ZeroMatrix\)) and motor saturation limits (\(|\JointTorque| \le \JointTorqueLimits\)), restricting the solution to the chosen friction and torque model. Actual slip, electrical current, and joint-limit containment also depend on model error, sampling, and actuator response.
The Physical AI Synthesis: Whole-Body Control serves as the real-time deterministic gatekeeper (The Per-Tick Gatekeeper): high-level learned policies (such as Vision-Language-Action foundation models or diffusion trajectory planners) propose uncertified task goals \(\TaskSpaceReference\), while the WBC QP checks modeled task, contact, and torque feasibility before admission.
Here \(\FrictionPyramidMatrix_k\) describes the chosen inner approximation to the measured friction cone. The joint-acceleration bounds \(\GeneralizedAccelerationLowerBound, \GeneralizedAccelerationUpperBound\) must be derived to preserve velocity and position limits over the integration step, with reserves for uncertainty. Finite task weights do not implement a strict hierarchy; cascaded QPs must preserve each higher-priority optimum as a constraint. The solve deadline must be validated 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 feedback damping. Passivity evaluates a different property from tracking accuracy: whether a component can supply more energy through its ports than it has received or initially stored. This energy accounting is useful when connecting controllers to uncertain mechanical environments.
Energy storage and the passivity definition
Consider a continuous-time dynamical system with state \(\MechanicalPlantState(\TimeInstant)\), control input \(\PortInput(\TimeInstant)\) (an effort variable, such as force or torque \(\PortEffort\)), and measured output \(\PortOutput(\TimeInstant)\) (a flow variable, such as velocity \(\Velocity\)). The system is defined as passive if there exists a continuously differentiable, non-negative storage function \(\StorageFunction(\MechanicalPlantState) \ge 0\) (representing the total internal physical energy stored in mass momentum and structural compliance) such that: \[\StorageFunctionDerivative(\MechanicalPlantState) \le \PortInput(\TimeInstant)^\top \PortOutput(\TimeInstant) - \InternalDissipationPower(\MechanicalPlantState) \tag{2}\] where \(\PortInput(\TimeInstant)^\top \PortOutput(\TimeInstant)\) represents instantaneous power injected into the system through the physical port, and \(\InternalDissipationPower(\MechanicalPlantState) \ge 0\) denotes internal energy dissipation (such as mechanical damping, viscous friction, or electrical resistance). Integrating equation 2 over any operational duration \([0, \FiniteTimeHorizon]\) yields the fundamental passivity energy inequality: \[\int_0^T \PortInput(\TimeInstant)^\top \PortOutput(\TimeInstant) \, d\TimeInstant \ge \StorageFunction(\MechanicalPlantState(\FiniteTimeHorizon)) - \StorageFunction(\MechanicalPlantState(0)) + \int_0^T \InternalDissipationPower(\MechanicalPlantState(\TimeInstant))\,d\TimeInstant \ge -\StorageFunction(\MechanicalPlantState(0)) \tag{3}\] Under equation 3 and the declared port orientation, the net physical energy extractable from this modeled component is strictly bounded by its initial stored energy \(\StorageFunction(\MechanicalPlantState(0))\). A loaded mechanism can still release enough stored energy to injure or damage; passivity is an energy-bounding certificate, not an instantaneous contact-force limit.
Under a well-posed, power-conserving feedback interconnection, passive components admit a combined storage inequality. Stability conclusions additionally depend on the storage bounds and on strictness or detectability conditions; passivity alone does not guarantee every internal state converges. A human operator, powered tool, or active environment also cannot be assumed passive merely because it is connected to the robot.
The transport delay destabilization hazard
An untreated effort/flow communication channel need not remain passive when delayed. A fixed pure delay \(\PureTransportDelay\) contributes a frequency-dependent phase lag \(\DelayPhaseLag=-\AngularFrequency \PureTransportDelay\) at angular frequency \(\AngularFrequency\). In teleoperation or compliant interaction across wireless networks, late force feedback can arrive out of phase with the operator’s current hand motion.
A negative instantaneous input power means the component is returning energy through its port. That is compatible with passivity when the energy was previously stored. The delayed link violates passivity only if its net delivered energy exceeds its incoming and initially stored energy under the declared port signs. Such excess can reduce damping or destabilize the coupled loop; the outcome depends on the controllers and mechanical environment.
Passivity preservation via wave variables
To prevent delay from injecting artificial energy into the teleoperation loop, Anderson and Spong (1989) introduced the wave variable transformation. Inspired by microwave transmission lines and scattering theory, wave variables map physical effort \(\PortEffort\) and flow \(\PortFlow\) into incoming and outgoing wave channels with characteristic wave impedance \(\WaveImpedance>0\): \[\LeaderForwardWave(\TimeInstant) = \frac{\LeaderPortEffort(\TimeInstant) + \WaveImpedance \LeaderPortFlow(\TimeInstant)}{\sqrt{2\WaveImpedance}}, \quad \LeaderReturnWave(\TimeInstant) = \frac{\LeaderPortEffort(\TimeInstant) - \WaveImpedance \LeaderPortFlow(\TimeInstant)}{\sqrt{2\WaveImpedance}} \tag{4}\] Computing instantaneous power from the wave transformation in equation 4 reveals: \[\LeaderPortPower(\TimeInstant) = \LeaderPortEffort(\TimeInstant)^\top \LeaderPortFlow(\TimeInstant) = \frac{1}{2} \|\LeaderForwardWave(\TimeInstant)\|^2 - \frac{1}{2} \|\LeaderReturnWave(\TimeInstant)\|^2\] In wave coordinates, power transmitted across the network port is the difference between incoming wave power \(\frac{1}{2}\|\LeaderForwardWave\|^2\) and reflected wave power \(\frac{1}{2}\|\LeaderReturnWave\|^2\). For an ideal transmission channel with communication delay \(\PureTransportDelay\), transmitted waves are simply delayed: \(\FollowerForwardWave(\TimeInstant) = \LeaderForwardWave(\TimeInstant - \PureTransportDelay)\) and \(\LeaderReturnWave(\TimeInstant) = \FollowerReturnWave(\TimeInstant - \PureTransportDelay)\). The total energy stored in the communication channel in flight satisfies: \[ \ChannelStoredEnergy(\TimeInstant) = \frac{1}{2} \int_{t - \PureTransportDelay}^\TimeInstant \|\LeaderForwardWave(\DelayWindowIntegrationTime)\|^2 \DerivativeOperator\DelayWindowIntegrationTime + \frac{1}{2} \int_{t - \PureTransportDelay}^\TimeInstant \|\FollowerReturnWave(\DelayWindowIntegrationTime)\|^2 \DerivativeOperator\DelayWindowIntegrationTime \ge 0 \] Because the channel stores transmitted energy without amplifying wave amplitudes, the scattered communication link is passive and lossless under the stated constant-delay model; the delay line itself supplies no strict dissipation. Variable delay, packet loss, sampling, and saturation require additional analysis. Even the ideal passive channel does not bound spatial tracking errors; kinodynamic watchdog leases (section 1.6.5) must be evaluated concurrently to guarantee spatial obstacle clearance.
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.
Further Reading
For a deeper theoretical foundation in control theory, robotics, and dynamical systems, consult the following references:
Modern Robotics: Mechanics, Planning, and Control(Lynch and Park 2017) provides a rigorous treatment of kinematics and dynamics using screw theory and Lie groups.
Robotics: Modelling, Planning and Control(Siciliano et al. 2009) serves as a reference for multi-body robot dynamics, trajectory generation, and operational space control.
Feedback Systems: An Introduction for Scientists and Engineers(Åström and Murray 2021) establishes classical and state-space control theory, frequency-domain stability margins, and state estimation from first principles.
Applied Nonlinear Control(Slotine and Li 1991) provides a foundational treatment of nonlinear dynamical systems, Lyapunov stability certificates, and sliding mode control.
Control Barrier Functions: Theory and Applications(Ames et al. 2019) sets out the formulation of control barrier functions, safety-critical quadratic programming filters, and forward invariance.
A Time-Dependent Hamilton-Jacobi Formulation of Reachable Sets for Continuous Dynamic Games(Mitchell et al. 2005) establishes the level set method and viscosity solutions for continuous-state differential games and safety verification.
A Predictive Safety Filter for Learning-Based Control of Constrained Nonlinear Dynamical Systems(Wabersich and Zeilinger 2021) develops predictive safety filters and constrained MPC methods for safe learning on physical systems.
Lynch, Kevin M, and Frank C Park. 2017. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press.
Siciliano, Bruno, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo. 2009. Robotics: Modelling, Planning and Control. Springer Science & Business Media.
Åström, Karl Johan, and Richard M Murray. 2021. Feedback Systems: An Introduction for Scientists and Engineers. Princeton university press.
Slotine, Jean-Jacques E, and Weiping Li. 1991. Applied Nonlinear Control. Prentice Hall.
Ames, Aaron D., Samuel Coogan, Magnus Egerstedt, Gennaro Notomista, Koushil Sreenath, and Paulo Tabuada. 2019. “Control Barrier Functions: Theory and Applications.”2019 18th European Control Conference (ECC), 3420–31. https://doi.org/10.23919/ECC.2019.8796030.
Mitchell, Ian M., Alexandre M. Bayen, and Claire J. Tomlin. 2005. “A Time-Dependent Hamilton-Jacobi Formulation of Reachable Sets for Continuous Dynamic Games.”IEEE Transactions on Automatic Control 50 (7): 947–57. https://doi.org/10.1109/TAC.2005.851439.
Wabersich, Kim P., and Melanie N. Zeilinger. 2021. “A Predictive Safety Filter for Learning-Based Control of Constrained Nonlinear Dynamical Systems.”Automatica 129: 109647.