Momentum-Constrained Trajectory Optimization

• Momentum-Constrained Trajectory Optimization is a framework that explicitly embeds momentum exchange and conservation constraints into trajectory planning across various domains such as spacecraft, legged robots, and impact-aware manipulation.
• It leverages manifold-constrained dynamics, centroidal state modeling, and stochastic chance constraints to ensure physically feasible motion while reducing control effort and enhancing maneuver performance.
• Applications demonstrate significant improvements in maneuver cost, computation time, and dynamic consistency compared to traditional kinematic or simplified torque-based planning methods.

Searching arXiv for recent and foundational papers related to Momentum-Constrained Trajectory Optimization. arxiv_search(query="Momentum-Constrained Trajectory Optimization centroidal momentum spacecraft CMG trajectory optimization", max_results=10) arxiv_search(query="Momentum-Constrained Trajectory Optimization centroidal momentum spacecraft CMG trajectory optimization", max_results=10) Momentum-Constrained Trajectory Optimization (MTO) denotes, in the cited literature, a class of trajectory optimization formulations in which momentum-related quantities are embedded directly in the planning problem as conserved quantities, state variables, dynamics, or feasibility constraints. The constrained quantity depends on the domain: in CMG-driven spacecraft it is the constant inertial angular momentum manifold; in legged locomotion it is typically centroidal linear and angular momentum together with contact-force feasibility; in humanoid running flight phases it is conserved centroidal angular momentum; in impact-aware manipulation it is the impulsive momentum balance at collision; and in assistive navigation it appears as momentum-aware regularization intended to suppress abrupt velocity and acceleration variations (Dearing et al., 2022, Herzog et al., 2015, Gazar et al., 2022, Sovukluk et al., 29 Jan 2025, Stouraitis et al., 2020, Zeng et al., 19 Sep 2025).

1. Conceptual scope and defining idea

Taken together, these formulations suggest that MTO is not a single algorithmic template but a modeling stance: trajectory generation is organized around physically executable momentum exchange rather than around purely kinematic interpolation. In the spacecraft setting, the state is restricted to the manifold

$$X(h_0)=\left\{ x \in \mathbb{R}^{3m+7} \;:\; \|q\|=1,\;\; h_0 = C(q)\bar{h}(x)\right\},$$

so unit quaternion normalization and conserved inertial angular momentum are part of the feasible set itself (Dearing et al., 2022). In multi-contact humanoid planning, the reduced state is the center of mass and total linear and angular momentum, while contact forces, torques, and contact-point trajectories are optimized so that the momentum equations are satisfied and contact feasibility is maintained (Herzog et al., 2015).

A broader reading of the literature shows that explicit hard momentum constraints are only one realization of the idea. In stochastic centroidal planning for quadrupeds, momentum is optimized through the centroidal state $$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$ and through force constraints with chance-constrained back-offs (Gazar et al., 2022). In humanoid running, the decisive fact is conservation during flight,

$\dot{\mathbf{k}}_G=\mathbf{0},$

which makes limb-swing trajectories a direct determinant of touchdown orientation (Sovukluk et al., 29 Jan 2025). In manipulation, the relevant quantity is the impact balance

$M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$

with compliance and contact timing optimized so that a large impulse is distributed over a physically realizable transient rather than left to a low-level controller (Stouraitis et al., 2020). This suggests that the defining feature of MTO is the explicit encoding of momentum exchange in the optimal-control structure, even when the formulation is hybrid, stochastic, or only indirectly “momentum-constrained.”

2. State spaces, invariants, and momentum dynamics

The dynamical backbone of MTO varies substantially across domains, but each formulation identifies a momentum quantity whose evolution has to be respected exactly or approximately. For CMG-driven spacecraft, the model is quaternion-based and retains full momentum exchange physics, including variable inertia $J_{st}(\delta)$, rather than replacing the actuator array with an approximate body torque input. The body-frame angular momentum is

$$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$

and the conserved inertial-frame momentum is

$h_0 = C(q)\,\bar{h}(\omega,\delta,h_{ga},h_{swr}).$

The paper explicitly contrasts this with the simplified torque model

$$\dot{q} = \frac{1}{2} O_L(q)\,\widetilde{\omega}, \qquad J\dot{\omega} = -\widehat{\omega}\,J\omega + \tau_r,$$

which is often paired with a pseudoinverse steering law and becomes ill-conditioned near singular CMG configurations (Dearing et al., 2022).

In legged systems, the dominant reduced-order model is centroidal. A representative formulation uses

$$M\dot{r} = l, \qquad \dot{l} = Mg + \sum f_i, \qquad \dot{\kappa} = \sum \tau_i + \sum (p_i - r)\times f_i,$$

so the optimizer controls the evolution of linear and angular momentum through contact wrenches and contact geometry (Herzog et al., 2015). A stochastic extension writes the centroidal state as

$$\boldsymbol{x}_k \in \mathbb{R}^9 = (\boldsymbol{c}_k, \boldsymbol{l}_k, \boldsymbol{\kappa}_k),$$

with controls

$$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$0

and discrete dynamics

$$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$1

Here momentum is not a byproduct of kinematics; it is the primary reduced state that the optimizer propagates and constrains (Gazar et al., 2022).

In humanoid running flight phases, the same centroidal viewpoint becomes a conservation law. Starting from

$$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$2

the angular part of the centroidal momentum matrix is decomposed as

$$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$3

Two stated properties are central: $$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$4, and $$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$5 is always invertible. During flight, since $$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$6, one has $$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$7, and the body rotational velocity satisfies

$$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$8

Joint motion therefore determines how the conserved angular momentum is partitioned between torso and limbs, which is why swing-leg and arm trajectories directly affect touchdown posture (Sovukluk et al., 29 Jan 2025).

3. Optimization formulations and constraint architectures

The most explicit manifold-constrained formulation in the cited literature is the spacecraft rest-to-rest transfer problem

$$(\boldsymbol{c}, \boldsymbol{l}, \boldsymbol{\kappa})$$9

subject to

$\dot{\mathbf{k}}_G=\mathbf{0},$0

with $\dot{\mathbf{k}}_G=\mathbf{0},$1, both endpoints assumed to be non-singular equilibria satisfying $\dot{\mathbf{k}}_G=\mathbf{0},$2 and $\dot{\mathbf{k}}_G=\mathbf{0},$3. The running and terminal costs are quadratic,

$\dot{\mathbf{k}}_G=\mathbf{0},$4

and the constraints include the full nonlinear CMG/VSCMG dynamics, unit quaternion normalization, conserved inertial angular momentum, and implicit manifold feasibility $\dot{\mathbf{k}}_G=\mathbf{0},$5 (Dearing et al., 2022).

A canonical legged formulation is the multi-contact momentum-control problem in which contact forces $\dot{\mathbf{k}}_G=\mathbf{0},$6, torques $\dot{\mathbf{k}}_G=\mathbf{0},$7, and contact locations $\dot{\mathbf{k}}_G=\mathbf{0},$8 are parameterized by polynomials. The objective is

$\dot{\mathbf{k}}_G=\mathbf{0},$9

with constraints on center of pressure, torque, and friction cone feasibility sampled along the horizon. The activation of each contact is handled by the predefined mode-scheduling switch $M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$0, so the optimizer works within a known contact schedule rather than deciding the contact sequence itself (Herzog et al., 2015).

More elaborate formulations generalize this template in different directions. One extension introduces stochastic dynamics and chance constraints,

$M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$1

with Gaussian uncertainty on contact positions and additive disturbances, covariance propagation, and deterministic back-offs derived from the inverse Gaussian CDF (Gazar et al., 2022). Another introduces a momentum-inertia-aware centroidal stage in which an equimomental ellipsoid parameterization makes centroidal inertia itself an optimization variable for “inertia shaping,” especially in twisting jumps and zero-gravity reorientation (Zhou et al., 2022). A flight-phase-specific humanoid formulation instead optimizes polynomial coefficients $M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$2 to minimize

$M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$3

subject to touchdown and liftoff placement, velocity, and ground-clearance constraints, with only the joints that significantly contribute to angular momentum and inertia reconfiguration included in the decision vector (Sovukluk et al., 29 Jan 2025).

Hybrid contact problems depart further from a single smooth OCP. In impact-aware manipulation, the problem is written over a fixed sequence of contact-control modes $M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$4, and the optimizer simultaneously chooses object motion, contact forces, contact timings, and the compliance parameter $M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$5. A force-transmission model,

$M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$6

is embedded so that impact is treated as a planned transient rather than an unmodeled discontinuity (Stouraitis et al., 2020). In assistive navigation, MTO is inserted downstream of quintic Frenet-frame sampling and third-order endpoint smoothing. The optimization is expressed through a Lagrangian

$M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$7

and is explicitly described as a mechanism that “suppresses abrupt velocity and acceleration variations” (Zeng et al., 16 Apr 2026).

4. Solvers, feedback design, and computational strategies

The numerical treatment of MTO is as heterogeneous as its dynamical models. For spacecraft on a constrained manifold, a local LQ regulator is designed on the tangent space and then lifted back to the ambient coordinates. If $M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$8 is an orthonormal basis for $M(\mathrm{v}^+ - \mathrm{v}^-)=\Lambda \delta t,$9, the reduced linearization is

$J_{st}(\delta)$0

and the resulting $J_{st}(\delta)$1, $J_{st}(\delta)$2, and $J_{st}(\delta)$3 are used inside PRONTO, a projection-operator-based Newton method whose iterates remain dynamically feasible. The initial guess is generated from the Singularity Robust steering law of Oh and Vadali (1991), with $J_{st}(\delta)$4 as the initial guess window (Dearing et al., 2022).

In multi-contact humanoid planning, the optimized momentum trajectory is followed by a time-varying LQR law of the form

$J_{st}(\delta)$5

computed in a receding-horizon fashion and embedded in a hierarchical inverse-dynamics controller formulated as QP cascades. The reported hierarchy includes Newton-Euler equations, contact constraints, center-of-pressure inequalities, friction-cone inequalities, joint-acceleration limits, LQR momentum control, swing-foot control, posture PD control, contact-force control, and base-link orientation (Herzog et al., 2015). A related quadrupedal pipeline alternates between centroidal optimization and whole-body optimization until “dynamics consensus” is reached, then tracks the result online with a convex MPC that jointly optimizes contact forces and joint accelerations in a single optimization (Zhou et al., 2022).

For stochastic centroidal planning, the nonlinear problem is convexified and solved by Sequential Convex Programming. The paper uses linearized dynamics, linearized chance constraints, and an $J_{st}(\delta)$6 trust-region penalty applied only to angular momentum, because angular momentum is identified as the only strongly nonlinear part of the centroidal dynamics in that setting (Gazar et al., 2022). The humanoid running paper takes the opposite computational route: it implements a custom C++ solver that computes numerical gradients of the equality constraints, stacks them into a Jacobian matrix, projects search directions into the nullspace of already satisfied constraints, and uses a line search; with the reduced joint set and $J_{st}(\delta)$7 dynamics samples, this makes the flight-phase limb optimizer real-time capable (Sovukluk et al., 29 Jan 2025).

A distinct but related algorithmic use of “momentum” appears in constrained stochastic SCA. CoSTA introduces recursive momentum gradient tracking,

$J_{st}(\delta)$8

together with convex surrogates and a parameterized MFCQ condition that yields explicit dual-variable bounds. In that paper, “momentum” refers to the optimizer’s recursive variance-reduction mechanism rather than to physical momentum of the system being planned, which is an important terminological distinction (Idrees et al., 2024).

5. Representative domains and reported outcomes

The cited literature spans spacecraft attitude control, humanoid and quadrupedal locomotion, impact-aware manipulation, and assistive navigation. The representative formulations and reported outcomes are summarized below.

Paper	System and MTO mechanism	Reported outcomes
"Attitude Trajectory Optimization and Momentum Conservation with Control Moment Gyroscopes" (Dearing et al., 2022)	Rest-to-rest attitude transfer on the manifold $J_{st}(\delta)$9 with full CMG/VSCMG dynamics and PRONTO	Rooftop array: maneuver cost from 83.56 to 39.90, control effort from 109.77 to 25.40 N·m·s, maneuver time from 95.70 s to 47.93 s, final attitude error from 0.83° to 0.06°; Pyramid array: maneuver cost from 91.53 to 34.07, control effort from 140.21 to 26.05 N·m·s, maneuver time from 100.35 s to 37.39 s, final attitude error from 1.68° to 0.10°
"Trajectory generation for multi-contact momentum-control" (Herzog et al., 2015)	Reduced-order planning of CoM, linear momentum, angular momentum, contact forces, torques, and CoP for a Sarcos humanoid	polynomials of order 3, 4 minutes planning time, convergence after two iterations, successful traversal of stepping stones of different heights and orientations, gains recomputed every 10 ms over a 2 s horizon with 200 time steps
"Nonlinear Stochastic Trajectory Optimization for Centroidal Momentum Motion Generation of Legged Robots" (Gazar et al., 2022)	Stochastic centroidal momentum optimization with chance constraints and alternating whole-body DDP / centroidal SCP	The paper states that the stochastic formulation reduces the amount of foot slippage for different gaits while achieving better performance over deterministic planning
"Momentum-Aware Trajectory Optimization and Control for Agile Quadrupedal Locomotion" (Zhou et al., 2022)	Alternating centroidal–whole-body planning with equimomental ellipsoid inertia shaping and online convex MPC	Demonstrated cantering, parkour-like hopping over a gap, zero-gravity body reorientation, and 180-degree twisting jump; on the Mini Cheetah the proposed controller successfully executes the full twist while baseline convex MPC and a joint PD controller fail to achieve the full rotation
"Realtime Limb Trajectory Optimization for Humanoid Running Through Centroidal Angular Momentum Dynamics" (Sovukluk et al., 29 Jan 2025)	Real-time nonlinear optimization of limb polynomials during flight under conserved centroidal angular momentum	Kangaroo biped: 24 decision variables, 14 nonlinear constraints, 11 points, 1.54 ms; Unitree G1 humanoid: 32 decision variables, 14 nonlinear constraints, 11 points, 1.92 ms
"Multi-mode Trajectory Optimization for Impact-aware Manipulation" (Stouraitis et al., 2020)	Hybrid trajectory optimization over fixed contact-control modes with explicit force-transmission dynamics and compliance optimization	Impact-aware TO: average computation time 141 ms versus 63 ms for impact-agnostic TO; planned force duration enlarged from $$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$0 s to $$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$1 s; on hardware, maximum contact force is below 20 N in a full-workspace case and below 30 N in a reduced-workspace case for object speed about 0.66 m/s
"Momentum-constrained Hybrid Heuristic Trajectory Optimization Framework with Residual-enhanced DRL for Visually Impaired Scenarios" (Zeng et al., 19 Sep 2025)	Frenet-frame heuristic sampling, MTO smoothing, dual-stage cost modeling, and residual-enhanced actor-critic selection	Stable policy performance in approximately half the training iterations required by the PPO baseline; average cost reduced by 30.3%, cost variance by 53.3%, and ego and obstacle risks by over 77%

These examples show that MTO can be instantiated as a manifold OCP, a reduced-order centroidal planner, a hybrid contact optimizer, or a front-end smoothing layer for sampled trajectories. A plausible implication is that the unifying concept is not the specific solver class but the decision to encode momentum evolution, momentum exchange, or momentum continuity before downstream tracking and control are considered.

6. Tradeoffs, limitations, and recurring misunderstandings

Several limitations recur across the literature. In the spacecraft case, the full CMG/VSCMG model preserves momentum conservation and avoids many singularity-related failures, but the formulation is more complex than simplified torque models; the examples are restricted to transfers between non-singular equilibrium points with $$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$2, and the optimization uses no explicit slew-rate constraints (Dearing et al., 2022). In the multi-contact humanoid setting, the contact schedule is predefined through $$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$3, so the optimizer does not choose contact modes or support transitions autonomously (Herzog et al., 2015). In stochastic centroidal planning, robustness is obtained through conservative back-offs and equal risk allocation based on Boole’s inequality, which improves feasibility but necessarily reduces nominal aggressiveness (Gazar et al., 2022).

A second recurring issue is that “momentum-constrained” does not always mean a hard equality constraint on momentum. In flight-phase humanoid running, the essential condition is conservation of centroidal angular momentum; in impact-aware manipulation, the optimizer manages momentum exchange indirectly by shaping compliant force transients; in assistive navigation, MTO is a variational smoothing layer whose “momentum-aware” character comes from motion-energy, guidance, social, smoothness, and perceptual-uncertainty terms rather than from a centroidal or inertial momentum balance (Sovukluk et al., 29 Jan 2025, Stouraitis et al., 2020, Zeng et al., 16 Apr 2026). A common misunderstanding is therefore to equate MTO with a single mathematical template. The cited papers do not support that interpretation.

A third issue is objective mismatch. The spacecraft paper identifies a “critical shortcoming” in objective functions that penalize only the norm of the control input rather than electrical power usage. It reports that total control effort decreases by about 77%–81%, but the optimal maneuver uses 3–4 times more true electrical energy than the initial guess because minimizing motor-torque norm is not the same as penalizing power when gimbal and wheel speeds are variable (Dearing et al., 2022). This is a direct reminder that physically meaningful momentum constraints do not by themselves guarantee physically meaningful cost functions.

Finally, the term “momentum” itself is overloaded in optimization. CoSTA uses recursive momentum to accelerate constrained stochastic SCA and proves $$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$4 complexity with adaptive step sizes and $$h = J_{st}\,\omega + A_s h_{swr} + A_g h_{ga}, \qquad J_{st}(\delta)=J + A_s J_s A_s^\top + A_t J_t A_t^\top,$$5 with non-adaptive step sizes, but that momentum is a gradient-tracking device rather than a physical conservation law (Idrees et al., 2024). This distinction matters because MTO, in its physical sense, is about how trajectories respect or exploit momentum exchange in the underlying system. The literature suggests that the strongest MTO formulations are those in which that physical structure is explicit enough to shape both feasibility and control design, yet abstract enough to remain computationally tractable across manifolds, contacts, impacts, and uncertainty.