---
title: Momentum-Constrained Trajectory Optimization
url: https://www.emergentmind.com/topics/momentum-constrained-trajectory-optimization-mto
type: topic
---

# Momentum-Constrained Trajectory Optimization

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 [2211.02110, 1507.04380, 2205.13264, 2501.17351, 2006.13374, 2509.15582].

## 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 [2211.02110]. 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 [1507.04380].

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 [2205.13264]. 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 [2501.17351]. 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 [2006.13374]. 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 [2211.02110].

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 [1507.04380]. 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{u}_k \in \mathbb{R}^{3n_c} = (\boldsymbol{f}_{e,0}, \dots, \boldsymbol{f}_{e,n_c}),
\]
and discrete dynamics
\[
\begin{bmatrix} \boldsymbol{c}_{k+1} \\ \boldsymbol{l}_{k+1} \\ \boldsymbol{\kappa}_{k+1} \end{bmatrix}
=
\begin{bmatrix}
\boldsymbol{c}_{k} + \frac{1}{m}\boldsymbol{l}_k\Delta_k \\
\boldsymbol{l}_{k} + m \boldsymbol{g}\Delta_k + \sum^{n_c}_{i=0} \boldsymbol{f}_{e,i,k}\Delta_k \\
\boldsymbol{\kappa}_{k} + \sum^{n_c}_{i=0} (\boldsymbol{p}_{e,i,k} - \boldsymbol{c}_k)\times \boldsymbol{f}_{e,i,k}\Delta_k
\end{bmatrix}.
\]
Here momentum is not a byproduct of kinematics; it is the primary reduced state that the optimizer propagates and constrains [2205.13264].

In humanoid running flight phases, the same centroidal viewpoint becomes a conservation law. Starting from
\[
\mathbf{h}_G=\mathbf{A}_G(\mathbf{q})\boldsymbol{\nu},
\]
the angular part of the centroidal momentum matrix is decomposed as
\[
\mathbf{A}_k= \begin{bmatrix} \mathbf{A}_v & \mathbf{A}_\omega & \mathbf{A}_j \end{bmatrix}.
\]
Two stated properties are central: \(\mathbf{A}_v=\mathbf{0}\), and \(\mathbf{A}_\omega\) is always invertible. During flight, since \(\mathbf{f}_c=\mathbf{0}\), one has \(\dot{\mathbf{k}}_G=\mathbf{0}\), and the body rotational velocity satisfies
\[
\boldsymbol{\omega}_b = \mathbf{A}_\omega^{-1}\big(\mathbf{k}_{Gf}-\mathbf{A}_j\boldsymbol{\nu}_j\big).
\]
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 [2501.17351].

## 3. Optimization formulations and constraint architectures

The most explicit manifold-constrained formulation in the cited literature is the spacecraft rest-to-rest transfer problem
\[
\min_{x(\cdot),u(\cdot)} \int_0^T \ell(x(t),u(t))\,dt + m(x(T))
\]
subject to
\[
\dot{x}=f(x,u), \qquad x(0)=x_0,
\]
with \(x_0,x_d \in X(h_0)\), both endpoints assumed to be non-singular equilibria satisfying \(f(x,0)=0\) and \(\omega=0\). The running and terminal costs are quadratic,
\[
\ell(x,u) \coloneqq \tfrac{1}{2}\|x-x_d\|_{Q(x_d)}^2 + \tfrac{1}{2}\|u\|_R^2, \qquad
m(x) \coloneqq \tfrac{1}{2}\|x-x_d\|_{P(x_d)}^2,
\]
and the constraints include the full nonlinear CMG/VSCMG dynamics, unit quaternion normalization, conserved inertial angular momentum, and implicit manifold feasibility \(x(t)\in X(h_0)\) [2211.02110].

A canonical legged formulation is the multi-contact momentum-control problem in which contact forces \(f_i(t)\), torques \(\tau_i(t)\), and contact locations \(p_i(t)\) are parameterized by polynomials. The objective is
\[
J = \sum_{t_0}^T \Big( \| l(t_i)\|_{W_1}^2 + \| l(t_i) - l_{des}(t_i)\|_{W_2}^2 + \| r(t_i) - r_{des}(t_i)\|_{W_3}^2 + \| \kappa(t_i)\|_{W_4}^2 + \| \kappa(t_i) - \kappa_{des}(t_i)\|_{W_5}^2 \Big),
\]
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 \(\alpha(t)\), so the optimizer works within a known contact schedule rather than deciding the contact sequence itself [1507.04380].

More elaborate formulations generalize this template in different directions. One extension introduces stochastic dynamics and chance constraints,
\[
\Pr(\boldsymbol{H}\boldsymbol{x}_k \le \boldsymbol{h}) \ge \alpha_x, \qquad
\Pr(\boldsymbol{G}\boldsymbol{u}_k \le \boldsymbol{g}) \ge \alpha_u,
\]
with Gaussian uncertainty on contact positions and additive disturbances, covariance propagation, and deterministic back-offs derived from the inverse Gaussian CDF [2205.13264]. 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 [2203.01548]. A flight-phase-specific humanoid formulation instead optimizes polynomial coefficients \(\mathbf{\Gamma}\) to minimize
\[
f_{\text{cost}} = \|\boldsymbol{\theta}_b(t_f)\|,
\]
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 [2501.17351].

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 \( \boldsymbol{z}=\{z_0,\dots,z_J\} \), and the optimizer simultaneously chooses object motion, contact forces, contact timings, and the compliance parameter \(\boldsymbol{\alpha}\). A force-transmission model,
\[
\ddot {\mathrm{f}}(t) + 2 \alpha \dot {\mathrm{f}}(t) + \alpha^2 {\mathrm{f}}(t) = \alpha^2 {\mathrm{f}}_d,
\]
is embedded so that impact is treated as a planned transient rather than an unmodeled discontinuity [2006.13374]. In assistive navigation, MTO is inserted downstream of quintic Frenet-frame sampling and third-order endpoint smoothing. The optimization is expressed through a Lagrangian
\[
L_{i}(t)=\frac{1}{2} m_{i}\left\|v_{i}(t)\right\|^{2}
- F_{\text{asst }(t)} \cdot v_{i}(t)
- F_{\text{obs }(t)} \cdot\left(v_{i}(t)-v_{j}(t)\right)
+\lambda_{s}\left\|\dot{v}_{i}(t)\right\|^{2}
+\lambda_{u} \cdot \operatorname{Tr}\left[\Sigma_{i}^{\text {perception }(t)}\right],
\]
and is explicitly described as a mechanism that “suppresses abrupt velocity and acceleration variations” [2604.14986].

## 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(x)\) is an orthonormal basis for \(T_xX\), the reduced linearization is
\[
\dot{s} = A_s(x)\,s + B_s(x)\,v,
\qquad
A_s(x) \coloneqq M(x)A(x)M(x)^\top,\quad B_s(x) \coloneqq M(x)B(x),
\]
and the resulting \(Q(x_d)\), \(P(x_d)\), and \(K(x_d)\) 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 \(T = 180\ \text{s}\) as the initial guess window [2211.02110].

In multi-contact humanoid planning, the optimized momentum trajectory is followed by a time-varying LQR law of the form
\[
\lambda = \lambda^* - K_t(x - x^*),
\]
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 [1507.04380]. 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 [2203.01548].

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 \(L_1\) 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 [2205.13264]. 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 \(11\) dynamics samples, this makes the flight-phase limb optimizer real-time capable [2501.17351].

A distinct but related algorithmic use of “momentum” appears in constrained stochastic SCA. CoSTA introduces recursive momentum gradient tracking,
\[
\z_{t+1} = \nabla f(\x_t,\xib_t) + (1-\beta_t)\big(\z_t-\nabla f(\x_{t-1},\xib_t)\big),
\]
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 [2404.11790].

## 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" [2211.02110] | Rest-to-rest attitude transfer on the manifold \(X(h_0)\) 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" [1507.04380] | 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" [2205.13264] | 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" [2203.01548] | 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" [2501.17351] | 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" [2006.13374] | 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 \(\delta t \approx 0.15\) s to \(\delta t \approx 0.9\) 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" [2509.15582] | 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 \(\omega=0\), and the optimization uses no explicit slew-rate constraints [2211.02110]. In the multi-contact humanoid setting, the contact schedule is predefined through \(\alpha(t)\), so the optimizer does not choose contact modes or support transitions autonomously [1507.04380]. 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 [2205.13264].

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 [2501.17351, 2006.13374, 2604.14986]. 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 [2211.02110]. 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 \(\tilde{\mathcal{O}}(\epsilon^{-3/2})\) complexity with adaptive step sizes and \(\mathcal{O}(\epsilon^{-3/2})\) with non-adaptive step sizes, but that momentum is a gradient-tracking device rather than a physical conservation law [2404.11790]. 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.

Source: https://www.emergentmind.com/topics/momentum-constrained-trajectory-optimization-mto