Distributed Inverse Dynamics Control

• DIDC is an advanced control architecture for legged robots that models full rigid-body dynamics and enforces exact non-linear friction cone constraints in real time.
• It utilizes a custom Geometric Projected Gradient Descent solver to compute optimal contact forces efficiently, reducing slip and enhancing orientation tracking.
• Experimental results show DIDC's significant improvements in slip reduction, base velocity and orientation tracking, and computational efficiency over conventional methods.

A distributed inverse dynamics controller (DIDC) is an advanced control architecture for legged robots that leverages full rigid-body dynamics modeling and geometric optimization to achieve accurate, real-time force allocation while accounting for exact friction constraints. Unlike traditional reactive or whole-body controllers that either simplify dynamics or linearize key constraints for computational tractability, DIDC explicitly models and solves for the coupling between base and joint motions, enforces non-linear friction cones, and distributes computation to achieve high-rate operation on embedded platforms. Recent work demonstrates implementation on quadruped systems, showing improvements in slip reduction, orientation tracking, and computational efficiency compared to established methods (Khandelwal et al., 2024).

1. Motivation and Limitations of Prior Controllers

Conventional reactive controllers for legged locomotion often employ simplified dynamic models, such as Single Rigid Body Dynamics (SRBD) or centroidal models, sacrificing accuracy in the base wrench calculation due to their neglect of link-to-link inertia coupling and Coriolis/centrifugal effects. These simplifications can cause significant errors, particularly at elevated speeds or in dynamic maneuvers (Khandelwal et al., 2024).

Most approaches linearize the non-linear friction cone constraint

$\sqrt{(F_c^x)^2 + (F_c^y)^2} \leq \mu F_c^z$

into a "friction pyramid" of half-space constraints for QP solvers’ tractability. However, this approximation admits inadmissible force solutions, causing foot slip under high tangential loads.

Full-body controllers using comprehensive rigid-body dynamics (RBD) and exact cone constraint formulations require solving large QP or QCQP problems. Onboard embedded processors struggle to meet real-time control deadlines (≥ 500 Hz); e.g., ANYmal WBC at 400 Hz necessitates an Intel Core i7–class CPU.

2. Rigid-Body Dynamics and Force Decomposition

The DIDC framework adopts the complete RBD formulation for the robot, partitioning the generalized coordinates $q ∈ ℝ^{18}$ into base and joint components:

• $q_b ∈ ℝ^6$: base position and orientation
• $q_j ∈ ℝ^{12}$: joint angles

The system dynamics with contact forces $F_c ∈ ℝ^{3n_c}$ are given by:

$M(q) \ddot{q} + η(q, \dot{q}) = S^T τ + J_c^T F_c$

where $M(q)$ is inertia, $η(q, \dot{q})$ encapsulates Coriolis, centrifugal, and gravity terms, $S$ selects actuated torques, and $J_c$ is the stacked contact Jacobian.

To resolve underactuation, DIDC introduces an "all-actuated" generalized torque $q ∈ ℝ^{18}$0:

$q ∈ ℝ^{18}$1

This $q ∈ ℝ^{18}$2 is split into a base wrench $q ∈ ℝ^{18}$3 and a joint torque $q ∈ ℝ^{18}$4. The base wrench is realized via contact force optimization, and joint torque tracking commands are systematically projected into the null space of the base mapping, ensuring strict orthogonality between objectives.

3. Friction Cone Constraints and Contact Model

DIDC enforces physically accurate contact constraints. At each stance foot $q ∈ ℝ^{18}$5:

• $q ∈ ℝ^{18}$6 (unilateral contact)
• $q ∈ ℝ^{18}$7 (maximum normal force)
• $q ∈ ℝ^{18}$8 (exact friction cone)

The quadratic (circular) friction cone constraint is treated directly, unlike prior methods employing linearizations which fail to guarantee slip-free operation under significant tangential loading.

4. Geometric Optimization via Projected Gradient Descent

DIDC employs a custom Geometric Projected Gradient Descent (GPGD) solver to compute the optimal contact forces $q ∈ ℝ^{18}$9 by solving a QP subject to non-linear cone and box constraints:

$q_b ∈ ℝ^6$0

subject to the exact constraints above.

The GPGD algorithm iterates, at each step performing:

• Hessian-corrected gradient updates.
• Projection of each foot force onto the admissible friction cone and unilaterality region.
• Convergence checks on each control cycle.

This scheme achieves efficient real-time operation at 500 Hz with typical ARM64 CPUs, reporting a mean GPGD solve time of 0.0093 ms (std. 0.00017 ms), approximately 2.5× faster than qpOASES while reducing cone constraint violation by ≈99%.

5. Orthogonal Composition and Distributed Control Structure

Once $q_b ∈ ℝ^6$1 is determined, DIDC calculates the associated base wrench's joint-space effect and computes desired joint torques for trajectory tracking:

$q_b ∈ ℝ^6$2

where $q_b ∈ ℝ^6$3, $q_b ∈ ℝ^6$4 is the projector into the null space of the base mapping, and $q_b ∈ ℝ^6$5 is derived from desired joint accelerations, $q_b ∈ ℝ^6$6, and $q_b ∈ ℝ^6$7. This formulation guarantees that joint-level tracking commands do not interfere with base wrench realization.

The control cycle is distributed in the following structure:

• State estimation via EKF.
• Motion planning and foot placement heuristics.
• Parallelized per-foot optimization of $q_b ∈ ℝ^6$8.
• Joint torque dispatch per limb via middleware (CycloneDDS over ROS2).

6. Experimental Validation and Performance Metrics

DIDC was evaluated both in simulation (MuJoCo) and on hardware (Unitree Go2) at 500 Hz. Key quantitative results include:

• Foot slip for DIDC maintained $q_b ∈ ℝ^6$9 2 cm/s versus 32% higher slip in non-friction enforcing controllers at moderate velocities.
• Mean base velocity tracking error $q_j ∈ ℝ^{12}$0 0.05 m/s at reference ±1.5 m/s.
• Orientation (roll/pitch) error: DIDC mean ± std = {0.0156±0.006 rad, 0.012±0.005 rad} vs QP-balance {0.0234±0.009 rad, 0.018±0.008 rad}.
• Average power consumption ≈ 5% less than QP-balance.
• GPGD consistently outperformed qpOASES in solution accuracy and constraint adherence.

Solver	Residual $q_j ∈ ℝ^{12}$1	Time $q_j ∈ ℝ^{12}$2 [ms]	Constraint violation $q_j ∈ ℝ^{12}$3
GPGD	$q_j ∈ ℝ^{12}$4	$q_j ∈ ℝ^{12}$5	$q_j ∈ ℝ^{12}$6
qpOASES	$q_j ∈ ℝ^{12}$7	$q_j ∈ ℝ^{12}$8	$q_j ∈ ℝ^{12}$9

Hardware deployment demonstrated similar benefits, with DIDC yielding $F_c ∈ ℝ^{3n_c}$030% reduction in base orientation tracking error and maximum slip of 0.91 m/s (vs 1.02 m/s in NSPIDC) during touchdown.

7. Advantages, Limitations, and Prospective Extensions

DIDC advances the state-of-the-art in legged robot control by:

• Accurately modeling full-body dynamics and exact friction constraints, reducing slip and improving orientation control.
• Implementing an orthogonality-preserving decomposition for simultaneous base and joint tracking.
• Achieving efficient, real-time operation ($F_c ∈ ℝ^{3n_c}$1500 Hz) on embedded CPUs with lower power consumption.
• Enabling fast convergence—at least twice as fast as generic QP-based implementations—critical for dynamic locomotion.

Limitations include the requirement for non-singular $F_c ∈ ℝ^{3n_c}$2 and $F_c ∈ ℝ^{3n_c}$3 (precluding some gaits), solver convergence dependence on previous solution initialization, and reliance on nominal dynamics models without explicit compensation for uncertainty.

Future directions noted comprise integration within hierarchical MPC+DIDC frameworks for terrain adaptation, extension to compliant/uneven substrates by adapting cone projections, deployment in multi-legged or collaborative robotic systems, and leveraging accelerators (GPU/TPU) for scaling or advanced warm-starting strategies (Khandelwal et al., 2024).