Inverse Dynamics Trajectory Optimization (IDTO)

• Inverse Dynamics Trajectory Optimization (IDTO) is a method that embeds inverse dynamics within trajectory planning to reconstruct forces, accelerations, and contact effects.
• It enhances convergence and reduces iterations by replacing forward dynamics with efficient inverse mapping techniques in direct transcription and MPC frameworks.
• Applications include contact-rich robotics, legged locomotion, and impaired-aircraft planning where computational efficiency and robustness are critical.

Searching arXiv for recent and foundational papers on inverse dynamics trajectory optimization and closely related formulations. Inverse Dynamics Trajectory Optimization (IDTO) denotes a class of trajectory-optimization formulations in which inverse dynamics is used as a primary computational object inside planning, rather than being reserved only for downstream tracking control. In the literature represented by direct transcription, shooting, and contact-implicit model predictive control, this usually means either enforcing dynamics through inverse-dynamics relations at transcription nodes, or optimizing a reduced trajectory representation—often generalized positions—and reconstructing velocities, accelerations, contact forces, and generalized forces from that trajectory. The resulting formulations are especially prominent in contact-rich robotics, underactuated systems, and applications where forward contact simulation is expensive or numerically fragile (Ferrolho et al., 2020, Kurtz et al., 2023, Chatzinikolaidis et al., 2021).

1. Definition and scope

Within simultaneous methods such as direct transcription, the central distinction is whether the nonlinear dynamics are enforced through forward dynamics or inverse dynamics. For a floating-base robot in contact, the equations of motion are written as

$$\mathbf{M}(\mathbf{q})\dot{\mathbf{v}} + \mathbf{h}(\mathbf{q},\mathbf{v}) = \mathbf{S}^\top \boldsymbol{\tau} + \mathbf{J}_s^\top(\mathbf{q})\boldsymbol{\lambda},$$

with generalized coordinates $\mathbf q$, generalized velocities $\mathbf v$, generalized accelerations $\dot{\mathbf v}$, joint torques $\boldsymbol\tau$, and contact forces or wrenches $\boldsymbol\lambda$. In the direct-transcription benchmark literature, forward-dynamics enforcement computes accelerations from $(q_k,v_k,\tau_k,\lambda_k)$, whereas inverse-dynamics enforcement treats the acceleration implied by neighboring velocity variables as known and constrains the required torque to match the torque decision variable (Ferrolho et al., 2020).

A second, more reduced, interpretation appears in contact-implicit MPC. There the optimization variables are only generalized positions over the horizon, while velocities, accelerations, contact forces, and generalized inverse-dynamics forces are reconstructed from the position sequence. In that formulation, inverse dynamics is not merely a post-processing step: it defines the optimization map from configuration trajectories to required generalized forces, and dynamic feasibility is reduced to the underactuation equalities (Kurtz et al., 2023).

The term does not cover every trajectory optimizer that later hands a kinematic or centroidal plan to a whole-body controller. A task-space method that optimizes base pose, footholds, contact forces, and foot velocities while using implicit inverse kinematics to recover configuration-dependent centroidal quantities is explicitly characterized as a hybrid centroidal/task-space trajectory optimization, not a full inverse-dynamics-consistent trajectory optimization at the joint torque level (Papatheodorou et al., 2023). Likewise, a control architecture that plans trajectories offline and then uses a floating-base inverse dynamics controller online is highly relevant to IDTO, but is not itself a pure end-to-end IDTO method (Reher et al., 2020).

2. Canonical formulations

Three formulations recur across the literature: inverse-dynamics direct transcription, position-only contact-implicit IDTO, and implicit-dynamics shooting methods.

Formulation	Decision variables	Dynamics enforcement
Direct transcription	$x_k=\{q_k,v_k\}$, $u_k=\{\tau_k,\lambda_k\}$	Inverse-dynamics defect constraints (Ferrolho et al., 2020)
Contact-implicit IDTO	$q_0,\dots,q_N$	Reconstruct $\mathbf q$0 from $\mathbf q$1; constrain underactuated rows (Kurtz et al., 2023)
Implicit DDP	Control sequence with implicit state transition	Implicit equation $\mathbf q$2 with inverse-dynamics-compatible sensitivities (Chatzinikolaidis et al., 2021)

In direct transcription, the decision vector aggregates all knot-point states and controls,

$\mathbf q$3

and the issue is how the defect constraints are written between adjacent mesh points. The 2020 benchmark showed that inverse-dynamics formulations converge faster, require less iterations, and are more robust to coarse problem discretization than forward-dynamics formulations for systems with rigid contacts (Ferrolho et al., 2020).

In position-only IDTO for contact-implicit MPC, the horizon is discretized into $\mathbf q$4 steps of length $\mathbf q$5, and the sole optimization variable is the configuration sequence

$\mathbf q$6

Velocities and accelerations are reconstructed by backward differences,

$\mathbf q$7

A smooth compliant contact model returns the contact force $\mathbf q$8, and inverse dynamics defines the generalized force required to realize the motion,

$\mathbf q$9

The resulting optimization problem is

$\mathbf v$0

where $\mathbf v$1 collects the unactuated rows of $\mathbf v$2 (Kurtz et al., 2023).

Implicit DDP generalizes the shooting paradigm by replacing an explicit state-update map with

$\mathbf v$3

This covers systems “via inverse dynamics and variational or implicit integrators,” and the required sensitivities are obtained from the implicit function theorem:

$\mathbf v$4

That structure enables inverse-dynamics-compatible DDP recursions while preserving the familiar feedforward-plus-feedback control law of DDP (Chatzinikolaidis et al., 2021).

3. Contact, underactuation, and feasibility

Contact treatment is one of the main axes along which IDTO formulations differ. In direct transcription with rigid contact, contact forces appear explicitly through the support Jacobian

$\mathbf v$5

and inverse-dynamics enforcement operates with the full rigid-body equations at each transcription node (Ferrolho et al., 2020).

By contrast, contact-implicit IDTO for MPC removes explicit complementarity variables and friction-cone constraints from the NLP. Contact is modeled as a smooth compliant algebraic function of state. The normal force is written

$\mathbf v$6

and tangential friction is regularized as

$\mathbf v$7

In this formulation, complementarity constraints, unilateral gap constraints, normal-force nonnegativity constraints, friction-cone constraints, joint limits, and actuator torque bounds are not explicit optimization constraints. Unilateral behavior is approximated through the compliant law, exact sticking is replaced by smooth regularization, and the only explicit equality constraints are the underactuation constraints $\mathbf v$8 (Kurtz et al., 2023).

Implicit DDP for contact-rich motions takes a different route. Starting from

$\mathbf v$9

the method works at acceleration level in contact space and introduces an invertible contact model. Contact-space acceleration is written

$\dot{\mathbf v}$0

and the forward contact solve is posed as a convex optimization problem with unilateral and friction-cone constraints. In the backward pass, the same contact model is inverted in closed form using a projection onto the friction cone, which avoids differentiating through the forward projected Gauss–Seidel solver (Chatzinikolaidis et al., 2021).

These distinctions delimit what should and should not be called IDTO. A reduced-order task-space optimizer that preserves centroidal balance, configuration-dependent inertia, and nonlinear angular-momentum effects but omits joint accelerations, torque variables, and full articulated rigid-body equations is better classified as a surrogate or approximation to full IDTO than as torque-level IDTO proper (Papatheodorou et al., 2023).

4. Numerical structure and solver design

The computational motivation for IDTO is explicit in the direct-transcription literature: rigid-body dynamics and their derivatives consume a significant fraction of computation time, and inverse dynamics is faster than forward dynamics in common libraries. The 2020 benchmark therefore asked whether that computational advantage survives transcription, and reported that inverse-dynamics formulations converge faster, require less iterations, and are more robust to coarse problem discretization. The authors conclude that inverse dynamics should be preferred to enforce nonlinear system dynamics in simultaneous methods such as direct transcription (Ferrolho et al., 2020).

In position-only IDTO for CI-MPC, the reduced problem is an equality-constrained nonlinear least-squares problem,

$\dot{\mathbf v}$1

with residual

$\dot{\mathbf v}$2

This least-squares structure motivates a custom Gauss–Newton trust-region method with dogleg steps. The method exploits a block pentadiagonal Gauss–Newton Hessian, uses diagonal trust-region scaling

$\dot{\mathbf v}$3

and employs a constrained dogleg variant for the underactuation equalities. The penalty version scales linearly in horizon length and cubically in number of DoFs, whereas the current Lagrange-multiplier implementation scales worse in horizon length because $\dot{\mathbf v}$4 is solved with dense algebra. In MPC mode, one solver iteration per control update is performed with warm starting from the previous solution (Kurtz et al., 2023).

Implicit DDP retains a shooting architecture rather than a sparse NLP. The local control update keeps the standard DDP form

$\dot{\mathbf v}$5

but every derivative entering the $\dot{\mathbf v}$6-function is generated from implicit sensitivities of $\dot{\mathbf v}$7. The framework also provides a Gauss–Newton or iLQR simplification that drops higher-order tensor terms, which is especially relevant when exact second derivatives of inverse dynamics and contact models are expensive (Chatzinikolaidis et al., 2021).

A recurrent practical theme is that derivatives remain costly. In the open-source CI-MPC solver, inverse-dynamics derivatives, including derivatives through contact geometry, are computed by finite differences and constitute the major computational bottleneck. The real-time performance therefore depends not only on inverse-dynamics structure, but also on sparse factorization, warm-start quality, and parallel derivative evaluation (Kurtz et al., 2023).

5. Applications and domain-specific variants

The most explicit contact-implicit MPC demonstration is the open-source IDTO solver for manipulation and locomotion. It is reported in simulation on spinner, quadruped, Allegro hand, and bi-manual tasks, and on hardware at over 100 Hz for a 20-degree-of-freedom bi-manual manipulation task. The optimized plan is not sent directly as open-loop torque; instead, cubic spline interpolation provides desired positions, velocities, and feedforward torques to a higher-rate feedforward PD tracking controller,

$\dot{\mathbf v}$8

which executes the current IDTO solution in receding horizon (Kurtz et al., 2023).

In legged locomotion, inverse dynamics also appears as a tracking layer on top of optimized trajectories. A compliant hybrid zero dynamic walking architecture plans a library of compliant walking trajectories offline and tracks them online using a floating-base inverse dynamics controller that generates dynamically consistent feedforward torques. This system demonstrates indoor flat-ground walking and outdoor disturbance rejection, but the paper explicitly does not present a pure end-to-end IDTO method (Reher et al., 2020).

A different hybridization appears in impaired-aircraft planning. The aircraft method optimizes a reduced trajectory with state

$\dot{\mathbf v}$9

and pseudo-controls

$\boldsymbol\tau$0

subject to the kinematic relations

$\boldsymbol\tau$1

terrain clearance, and maneuvering-flight-envelope bounds. After the optimizer selects a sequence of trim points $\boldsymbol\tau$2, the full 6-DoF trim controls and aerodynamic angles are recovered by solving a constrained inverse trimming problem with

$\boldsymbol\tau$3

The paper describes this as a combination of differential flatness theory, the pseudospectral method, nonlinear programming, and inverse dynamics, but the formulation is best understood as a trim-based hybrid or reduced-order IDTO variant rather than full-state direct inverse-dynamics optimization (Norouzi et al., 2024).

6. Adjacent methods, misconceptions, and limitations

A frequent misconception is to conflate IDTO with inverse optimal control. Inverse optimal control assumes the dynamics are known and the unknown object is the running cost or Lagrangian that makes observed trajectories optimal. Its central certificate is based on the Bellman residual

$\boldsymbol\tau$4

not on inverse-dynamics equations or transcription defects. That framework is highly relevant to recovering objectives from demonstrations, but it is not IDTO in the robotics sense of inverse-dynamics-constrained trajectory generation (Pauwels et al., 2014).

A second misconception is to treat any differentiable planner as IDTO. DiffTORI uses differentiable trajectory optimization as an implicit policy with learned forward latent dynamics

$\boldsymbol\tau$5

and Model-Based Diffusion solves model-based trajectory optimization by forward rollout and diffusion-inspired sampling over state-control sequences. Both are relevant to trajectory optimization and planner differentiation, but neither formulates inverse-dynamics constraints, torque balance equations, or collocation-style inverse-dynamics enforcement (Wan et al., 2024, Pan et al., 2024).

A third misconception is that “contact-implicit” necessarily implies explicit complementarity. In the open-source CI-MPC formulation, contact-implicit means that the optimizer is free to make or break contact because contact force is a smooth function of state rather than a discrete mode or complementarity variable. This design removes complementarity constraints and friction-cone inequalities from the NLP, but introduces compliant-contact approximations, regularized friction, and “force at a distance” through smoothing (Kurtz et al., 2023).

The main limitations therefore depend on formulation. Position-only IDTO sacrifices exact hard-contact mode enforcement, does not currently support actuator torque bounds or joint limits except potentially through penalties, and relies on delicate contact derivatives (Kurtz et al., 2023). Implicit DDP avoids explicit mode enumeration and can exploit recursive Newton–Euler inverse dynamics, but remains a local shooting method whose solution quality depends strongly on initialization (Chatzinikolaidis et al., 2021). Reduced-order hybrid methods for aircraft gain real-time performance by sequencing steady maneuvers and performing post hoc inverse trim recovery, but do not enforce full transient 6-DoF dynamic consistency inside the trajectory optimization itself (Norouzi et al., 2024).

Taken together, these papers suggest that IDTO is not a single algorithmic template but a family of formulations organized around one design decision: replace repeated forward-dynamics enforcement by an inverse map—torque reconstruction, inverse-dynamics defects, implicit dynamics sensitivities, or trim recovery—whenever that inversion yields a more tractable trajectory-optimization problem. In simultaneous methods, the evidence reported so far favors inverse dynamics over forward dynamics for enforcing nonlinear rigid-body dynamics under contact (Ferrolho et al., 2020).