Distributed Inverse Dynamics Control for Quadruped Robots using Geometric Optimization

In plain words

A walking robot pushes its feet against the ground, and the ground can only push back inside a limited cone of directions set by friction. If a controller asks for a push outside that cone, the foot slips. Many controllers avoid the problem by using a rough shape with flat sides, or by solving a large general optimisation problem at every control step. This paper keeps the real cone and the full robot model. It finds the foot forces with a simple iterative method that takes a step, then pushes any foot force that has left the cone back onto its edge.

The rest of the controller is a way of splitting the job. The body must follow its planned motion, and the joints must follow theirs. The paper arranges the joint commands so that they do not disturb the body's motion. A small planner decides where the feet land, and a state estimator works out the body's position and speed from the motion sensors.

The authors test the controller in simulation and on a Unitree Go2 robot. They report less foot slipping, better body tilt tracking and a faster solve than a generic optimisation solver.

TL;DR

The paper's controller turns a desired body and leg motion into joint torques, using the full rigid-body model of an 18-degree-of-freedom quadruped §2.1, §3. It enforces the exact friction cone for the contact forces through a custom projected-gradient solver, GPGD, instead of linearising the cone for a generic QP §4. Joint tracking is placed in the null space of the base map so it does not change the base motion §3. All results are author-reported on a Unitree Go2 in MuJoCo and on hardware, and the paper gives no official code.

Problem

• Reactive controllers for quadrupeds often use a simplified model, or treat the friction cone as a linear pyramid so that a standard QP solver applies §1, §2.3, §4.
• Whole-body controllers use the full model but are heavy to run at high rates on an onboard processor, and they usually also use the friction pyramid §1.
• The null-space inverse dynamics controller does not enforce the friction constraints, so the contact forces it produces can slip §2.2, §6.1.1.
• The QP balance controller ignores how leg dynamics affect the base, and it adds joint torques directly to the base torques, which changes the base motion §2.3.
• Assumptions:
  • the contact is no-slip at each foot in contact §2.1;
  • the Jacobians used by the controller never become rank-deficient; the gait and step length are chosen to ensure this §3;
  • at least two feet are in contact at any time for the base map to have full row rank §3.
• Scope: the experiments are on flat ground with a trotting gait §6, §6.1.1.

Method

Model §2.1

• Generalised coordinates are q = [q_b^T\ q_j^T]^T, with 6 base DOFs and 12 leg joints, ordered FL, FR, RL, RR §2.1.
• Equation of motion: M\ddot q + \eta = S^T\tau + J_c^T F_c, with S selecting the 12 actuated joints eq. 1. The no-slip contact constraint is J_{c,i}\dot q = 0 and J_{c,i}\ddot q + \dot J_{c,i}\dot q = 0 for feet in contact eq. 2.

Fully actuated view and commanded accelerations §3

1. Treat the robot as fully actuated and compute the generalised force that would produce a commanded acceleration: \tau_f = \hat M \ddot q_{cmd} + \hat\eta eq. 5. The paper splits it as \tau_f = [\tau_b^T\ \tau_j^T]^T §3.
2. Per DOF, the commanded acceleration is a PD law with a desired acceleration: \ddot q_{cmd} = \ddot q_{des} + K_p(q_{des} \ominus q) + K_d(\dot q_{des} - \dot q) eq. 9. The operator \ominus is a space-preserving subtraction for orientation, so the orientation error stays in \mathbb R^3 §3.
3. If the model is exact, the closed loop is \ddot q = \ddot q_{cmd} eq. 7, eq. 8. The model error is \epsilon = M^{-1}(\tilde M \ddot q_{cmd} + \tilde\eta) eq. 8.

Base wrench to contact forces §3

4. The base wrench is produced by the stance feet: J_{ab}^T F_c = \tau_b eq. 10, where J_{ab}^T \in \mathbb R^{6 \times 3n_c} has identity blocks for the feet and skew-symmetric lever-arm blocks for the COM offsets eq. 11.
5. Find the contact forces with a regularised least-squares problem and the contact constraints eq. 12:
   • cost: (J_{ab}^T F - \tau_b)^T S_1 (J_{ab}^T F - \tau_b) + F^T W F + (F - F_{prev})^T V (F - F_{prev});
   • constraints: normal force bounded, F^z_{max} \ge F^z \ge F^z_{min} \ge 0 eq. 13, and friction cone \sqrt{(F^x)^2 + (F^y)^2} \le \mu |F^z| eq. 14.
   • S_1 weights the base wrench, W penalises force, V penalises change from the previous solution §3.
6. Stance-leg torques that realise the forces: \tau_1 = -J_{aa}^T F_c^* eq. 15.

Null-space split of joint tracking §3

7. Define J_{a,b} = -J_{aa}^T (J_{ab}^T)^\dagger eq. 16, so that \hat\tau_b = (J_{a,b})^\dagger \tau eq. 17 is the base wrench a torque vector produces.
8. Project the joint torques into the null space of J_{a,b}, N_{a,b} = I - J_{a,b} (J_{a,b}^T J_{a,b})^{-1} J_{a,b}^T eq. 18; see Pitfalls on the printed form. The final command is \tau = -J_{aa}^T F_c^* + N_{a,b}\,\tau_j eq. 19.
9. The paper shows that (J_{a,b})^\dagger N_{a,b} = 0, so the joint torques do not change the base wrench, and the base task has priority over the joint task §3.

Force solver GPGD §4, alg. 1

10. Write eq. (12) in standard form f^T Q f + P f + R with f = F_c and eq. 20:
    • Q = J_{ab} S_1 J_{ab}^T + W + V;
    • P = -2(\tau_b^T S_1 J_{ab}^T + f_{prev}^{*T} V);
    • R = \tau_b^T S_1 \tau_b + f_{prev}^{*T} V f_{prev}^*.
11. Gradient and Hessian: \nabla J = 2Qf + P^T and \nabla^2 J = 2Q §4.
12. Run the iteration in alg. 1 (below). It takes a step with the inverse Hessian, then projects each stance foot's force onto the unilateral bounds and onto the friction cone, and stops when the projected point moves less than 10^{-2} alg. 1.
13. The projection onto the cone surface, as printed in alg. 1, gives the normal part (\mu f_t + f_z)/(\mu^2+1) and sets the tangential part to \mu times that normal part, along the direction of the original tangential force. Points inside the polar cone are set to zero.

# Geometric projected gradient descent  [alg. 1; §4]
# Cost f^T Q f + P f + R over stacked stance forces (3 per foot)
# Reading: alg. 1 never copies the new iterate into f_n. We assume
# forces = projected at the end of each pass (see Pitfalls).

def gpgd_solve(Q, P, start, stance, mu, f_min, f_max, max_iters):
    # Warm start from the previous solution  [alg. 1, Initialize]
    forces = start
    projected = zeros_like(start)
    for iteration in range(max_iters):
        # Gradient of the cost at the current iterate  [§4]
        grad = 2 * Q @ forces + P
        # Newton-type step with the Hessian 2Q  [alg. 1]
        trial = forces - solve(2 * Q, grad)
        for foot in stance:
            # Unilateral bounds on the normal force  [alg. 1; eq. 13]
            f_z = clip(trial[foot.z], f_min, f_max)
            f_t = norm(trial[foot.xy])
            # Inside the friction cone: keep the point  [alg. 1]
            if f_t <= mu * f_z:
                projected[foot] = (trial[foot.xy], f_z)
            # Inside the polar (dual) cone: force is zero  [alg. 1]
            elif f_t <= -f_z / mu:
                projected[foot] = zeros(3)
            # Otherwise project onto the cone surface  [alg. 1]
            else:
                f_z_p = (mu * f_t + f_z) / (mu**2 + 1)
                f_z_p = clip(f_z_p, f_min, f_max)
                f_xy_p = mu * f_z_p * trial[foot.xy] / f_t
                projected[foot] = (f_xy_p, f_z_p)
        # Stop when the projected point barely moves  [alg. 1; tol 1e-2]
        if norm(projected - forces) < 1e-2:
            return projected
        forces = projected   # our reading; not printed in alg. 1
    # Sub-optimal result if max_iters is reached  [alg. 1]
    return projected

Controller step, assembled §3, eqs. 5-19

The paper does not print this as one listing; it is assembled from the equations above.

# One inverse-dynamics step  [§3; eqs. 5, 9, 10, 15, 16, 18, 19]
# Output: tau (12,) joint torques

def didc_step(q, dq, desired, stance, gains):
    # Commanded acceleration: decoupled PD per DOF  [eq. 9]
    # Orientation error uses the space-preserving minus  [§3]
    error_q = space_minus(desired.q, q)
    acc_cmd = (
        desired.acc
        + gains.Kp @ error_q
        + gains.Kd @ (desired.dq - dq)
    )
    # Generalised force for the fully actuated model  [eq. 5]
    tau_full = M_hat @ acc_cmd + eta_hat      # (18,)
    tau_base = tau_full[:6]                   # (6,)
    tau_joint = tau_full[6:]                  # (12,)
    # Jacobian blocks, rows only for feet in contact  [eq. 10, eq. 11]
    J_ab = contact_jacobian_base(q, stance)   # (3*n_c, 6)
    J_aa = contact_jacobian_joint(q, stance)  # (3*n_c, 12)
    # Cost matrices in standard form  [eq. 20]
    Q = J_ab @ S_1 @ J_ab.T + W + V           # (3*n_c, 3*n_c)
    P = -2 * (tau_base @ S_1 @ J_ab.T + prev_forces.T @ V)
    # Contact forces from the cone-constrained solver  [eq. 12-14]
    forces = gpgd_solve(Q, P, prev_forces, stance, params)
    # Stance-leg torques that produce the base wrench  [eq. 15]
    tau_1 = -J_aa.T @ forces                  # (12,)
    # Base-to-joint map and its null-space projector  [eq. 16, eq. 18]
    J_base = -J_aa.T @ pinv(J_ab.T)           # (12, 6)
    null_proj = (
        eye(12) - J_base @ inv(J_base.T @ J_base) @ J_base.T
    )
    # Joint tracking acts only in the null space  [eq. 19]
    return tau_1 + null_proj @ tau_joint      # (12,)

Gait and foothold planning §5.1

• Base motion: the desired base acceleration is a first-order gain on the velocity error, saturated at a maximum acceleration §5.1.1. The desired velocity and position are integrated from it eq. 21, eq. 22.
• The maximum horizontal acceleration is the normal force limit times the friction coefficient, divided by the total mass §5.1.1. Yaw is planned the same way with a separate rate and acceleration limit §5.1.1.
• Gait: four parameters, gait period t_g, duty factor \phi_{st,i}, phase offset \psi_{o,i} and step height h_z, in the style of Hildebrand's gait parameters §5.1.2.
• Contact schedule: phase \phi_{ph,i} = \mathrm{mod}(t/t_g + \psi_{o,i}, 1); leg i is in stance when \phi_{ph,i}/\phi_{st} < 1 §5.1.2.
• Foothold: the hip position plus a Raibert offset d_i = \tfrac12 \dot r_h \phi_{st} t_g eq. 23, eq. 24; §5.1.2. The swing trajectory is sinusoidal §5.1.2.
• Reach limit: d_{max} = \sqrt{\beta l_e^2 - (r_h^z)^2} with \beta \in [(r_h^z/l_e)^2, 1) eq. 25. The theoretical top speed is v_{max} = 2 d_{max}/(\phi_{st} t_g) eq. 26.
• Alg. 2 shortens any offset that exceeds the reach limit and lowers the base while that happens, or raises it otherwise, then clamps the base height alg. 2.

# Adaptive step length and base height  [alg. 2; §5.1.2; eq. 25]
# Shortens offsets that exceed the reach limit, then adjusts the height

def adapt_step_length(hip_vel, hip_z, base_z, phi_st, t_g, dt, params):
    limited = False
    offsets = {}
    for leg in legs:
        # Raibert offset for this leg  [alg. 2; §5.1.2]
        offset = 0.5 * hip_vel[leg].xy * phi_st * t_g
        # Reach limit at this hip height  [eq. 25]
        reach = sqrt(params.beta * params.l_e**2 - hip_z[leg]**2)
        if norm(offset) >= reach:
            limited = True
            # Scale the offset back to the reach limit  [alg. 2]
            offset = offset / norm(offset) * reach
        offsets[leg] = offset
    # Lower the base if a step was shortened, else raise it  [alg. 2]
    if limited:
        base_z = base_z - params.alpha_minus * dt
    else:
        base_z = base_z + params.alpha_plus * dt
    # Clamp the base height to its limits  [alg. 2]
    base_z = clip(base_z, params.z_min, params.z_max)
    return offsets, base_z

State estimation §5.2

• Base pose and velocity: an extended Kalman filter with IMU data in the prediction step; stance feet are assumed not to slip, and this is the correction step for base position and velocity §5.2.1. The framework is based on Bloesch et al. (2013) and Bledt et al. (2018) §5.2.1.
• Contact force: the contact force is estimated from measured joint torques and the equation of motion, using a filtered finite difference for joint acceleration §5.2.2.
• Contact probability: a Kalman-filter method from the literature, fed with the estimated force, estimated foot height and gait phase §5.2.2.
• Contact state: hysteresis on the probability, on at 0.6 and off at 0.4, otherwise unchanged §5.2.2.

Gains §6

• Per DOF, the gains come from an LQR with double-integrator dynamics, state weights Q = \mathrm{diag}(100, 1) and input weight R = 10^{-3} §6.
• For the base in x and y, the paper overrides the result to K_p = 0 and K_d = 10.4 §6.

Hyperparameters

Results

Author-reported. Setup: MuJoCo simulation on an Apple M1 Pro, and a Unitree Go2 on hardware, with a Vicon motion-capture reference for velocity §6. Baselines are NSPIDC and QP balance control (BC) §2.2, §2.3. Solver comparison: 10 continuous runs with the same command schedule §6.1.2.

• Solver ratios as stated in the text: GPGD about 2.47 times faster than qpOASES, about 31% lower residual, and about 99% lower constraint violation §6.1.2.
• Orientation: both controllers keep the roll and pitch error within 0.1 rad during the hardware run §6.2.
• Simulation: NSPIDC lost foot grip at high speed and the run was cut short, so its high-speed plots are partial §6.1.1 fig. 5.

Paper vs code

No official code.

What was searched:

1. The paper's text, abstract and bibliography for a repository link. Only a link to CycloneDDS, a third-party dependency, appears.
2. GitHub repository search for: "distributed inverse dynamics quadruped", "DIDC quadruped", "geometric projected gradient descent friction cone", "unitree go2 inverse dynamics" and "go2 mujoco inverse dynamics". The one relevant hit is a third-party Go2 MuJoCo project whose owner is not an author; it was not used.
3. GitHub user search for the author names: two personal accounts with no link to the paper; not used. No GitHub organisation for the authors' labs was found.

No discrepancy check against code was possible.

Pitfalls

• Alg. 1 never updates its iterate. As printed, it computes the gradient from f_n and f_{n+1} from f_n, and then only writes the projected point into f_p. The loop compares f_p with f_n, but f_n is never reassigned alg. 1. A literal implementation repeats the same step, so it either stops at the first pass or runs to the iteration limit. The card's code assumes the intended update f_n \leftarrow f_p; the paper does not state it.
• The "gradient descent" step is a Newton step. The update uses (\nabla^2 J)^{-1} = (2Q)^{-1} alg. 1; §4. For a quadratic with no constraints, one such step reaches the minimum. The iteration matters only because of the projections.
• Initialisation is unclear. Alg. 1 starts from F^*_{c_k} alg. 1, but eq. (12) uses F^*_{c,prev} from the previous time step §3. The paper does not say whether these are the same vector, so the warm start may be ambiguous.
• Convergence test is not scale-free. The tolerance is an absolute 10^{-2} on the norm of the force change alg. 1. Units and the scale of the forces are not stated, so the same tolerance means different things for different robots.
• Dual-cone branch versus F^z_{min}. Alg. 1 sets forces to zero for points inside the polar cone alg. 1, but also clamps the normal force to F^z_{min} earlier. If F^z_{min} > 0, zero is outside the allowed range. The paper gives no value for F^z_{min}, so this cannot be checked.
• The printed null-space projector does not multiply out. Eq. (18) reads I - J_{a,b}(J_{a,b}^T J_{a,b})^{-1} J_{a,b}. With J_{a,b} of size 12 \times 6, the product is not conformable. The proof §3 uses J_{a,b}^T on the right, so implement N = I - J(J^TJ)^{-1}J^T, which is the projector onto the complement of the range of J_{a,b} when it has full column rank.
• Sign conventions matter. The minus signs in eq. (15) and eq. (16) carry through the orthogonality argument §3. Dropping either changes \hat\tau_b.
• Row rank depends on the gait. The base map J_{ab}^T needs full row rank, which needs at least two stance feet. With exactly two feet, the 6\times 6 matrix must be invertible and the QP only gives the closest solution. With fewer than two feet there is no exact solution, and the paper's solver returns a least-squares result with a residual §3. The paper ensures the condition through the gait and step length, not by code it shows §3.
• Swing legs are implicit. J_{aa}^T is 12 \times 3n_c for stance feet only §3. Zero-padding of swing legs when forming \tau_1 is implied but not written out.
• Missing values for the cone. The friction coefficient \mu and the normal-force limits are not given numerically eq. 13, eq. 14. An implementation cannot run as written without choosing them.
• Model estimates are unspecified. The controller needs \hat M and \hat\eta eq. 5, but the paper does not say where they come from (URDF, identification, or another source). It also does not analyse how model error \epsilon eq. 8 affects stability beyond the no-error case.
• Joint gains are not printed. The paper derives them from LQR weights §6, so an implementer must solve the Riccati equation. The values in the Hyperparameters table are our derivation and should be checked.
• Weights W and V. The text gives them as scalars §6, but they enter Q as matrices eq. 20. The card assumes scalar multiples of the identity. The paper does not state this.
• Planner values mostly absent. Acceleration limits, the first-order gain K_b, the yaw limits, the base height limits, and the alg. 2 rates are named but not given §5.1, alg. 2. The reach parameter \beta is given only as an interval §5.1.2.
• Solver comparison is not like for like. GPGD uses a naive implementation and qpOASES an optimised one; the paper says so §6.1.2. The violation metrics differ: GPGD uses the cone, qpOASES its linear pyramid §6.1.2. The qpOASES violation of about -0.97 is large for a QP that should satisfy its own constraints, and the paper does not explain it. Units for the residual and the violation are not stated.
• Speed claim scope. The abstract's speed-up claim over reactive controllers with generic QP solvers rests on the solver timing against qpOASES alone §6.1.2. The text gives no controller-level timing.
• Softer gains change the comparison. The foot-slip comparison of about 32% uses softer NSPIDC gains, because NSPIDC fails with the original gains §6.1.1. The gain values are not given.
• Figure wording. The text calls the orientation study in §6.1.1 an "experiment", but the caption says simulation §6.1.1 fig. 10.
• Hardware evidence is limited. The number of hardware runs is not stated, and error statistics are shown only as figures §6.2. Power is reported only in simulation.
• Onboard processor not named. The abstract says the controller runs efficiently on embedded processors. The text does not name the onboard computer or its timing §6.
• Equation and section numbers were counted from the LaTeX source, which numbers equations continuously; the compiled PDF was not checked. The text refers to "Section II" for the reactive-control background §6.1.1, §6.2; this card cites that section as §2.

Lineage

• Builds on: the QP-based contact-force distribution of MIT Cheetah 3 (Bledt et al., 2018) and the QP balance controller of Gehring et al. (2013) §3, §2.3; the projection step from Tasora and Anitescu (2011) §4; the null-space inverse dynamics line of work, including Mistry et al. (2010) and Buchli et al. (2009) §1; hierarchical control of Liu et al. (2016) §3. The bibliography gives no arXiv ids, so none is linked.
• Compared against: NSPIDC §2.2 and QP-based balance control §2.3 on the same robot, in simulation and on hardware §6. The solver GPGD is compared with qpOASES §6.1.2.
• Related exact-cone approach: Aghili (2017), a QCQP with the exact friction cone solved by interior-point methods; the authors note it had no onboard implementation §1.
• Superseded by: not known from this wiki's content.
• Future work named by the authors: testing in a two-layer predictive-reactive architecture, and extending the projection approach to other constraints §7.