Skip to main content

Whole-Body Control in UpstreamDrift

reference
robotics
Inverse-dynamics QP structure, task hierarchy, friction pyramid, and known limitations on UpstreamDrift main.
Author

Dieter Olson

Published

November 11, 2026

Modified

November 11, 2026

Abstract
Reference for WholeBodyController on UpstreamDrift main at 3afe09c1: equality and inequality stacked QP, contact force decision variables, pyramidal friction with mu 0.5, and inverse dynamics torques with optional clipping. Documents weighted versus hierarchical modes and notes that sorted priority iteration may invert the documented 0-equals-highest convention until a pending fix lands.

Plain-Language Summary

This reference describes the whole-body inverse-dynamics QP in UpstreamDrift: task costs, friction pyramid, torque mapping, and defaults. It flags a hierarchy ordering mismatch on main. It is code documentation, not a walking controller certification.

Key Takeaways

  • The QP stacks dynamics equalities, friction inequalities, and torque limits over contact forces and accelerations.
  • Friction uses a four-sided pyramid with mu 0.5 hard-coded, not a second-order cone.
  • Task priority is documented as 0 equals highest, but hierarchical solve order on main may disagree.
  • Default contact force bounds are plus or minus 10 000 N with millisecond discretization.

Whole-body control (WBC) in UpstreamDrift is implemented by WholeBodyController (src/robotics/control/whole_body/wbc_controller.py) with tasks in task.py and QP assembly in qp_solver.py. This page states what main at 3afe09c1 actually solves.

WarningPending Fix for Contact Torques and Hierarchy

UpstreamDrift PR #15025 (issue #15012) corrects contact-consistent torques on actuated joints and reworks strict hierarchical (HQP) level ordering. On current main, hierarchical levels are visited in descending numeric Task.priority while task.py documents 0 = highest—treat hierarchical solutions as unreliable until that PR merges. Weighted-sum mode (WBCConfig.use_hierarchical=False) is unaffected by the ordering bug but shares the same post-solve torque extraction path.

Dynamics and Decision Variables

With \(n_v\) velocity degrees of freedom and \(3 n_c\) contact force variables, the weighted or hierarchical solve stacks decision vector \(\mathbf{x} = [\ddot{\mathbf{q}}, \mathbf{f}_c]\).

Soft tasks minimize \(\sum_i \|\mathbf{J}_i \ddot{\mathbf{q}} - \mathbf{b}_i\|_W^2\) (task Jacobians are \(n_{\mathrm{task}}\times n_v\)).

When contacts exist, equality constraints enforce

\[ \mathbf{M}(\mathbf{q})\,\ddot{\mathbf{q}} - \mathbf{J}_c^{\mathsf T}\mathbf{f}_c = -\mathbf{nle}, \]

with n_v rows—all degrees of freedom, including actuated joints—using only the linear \(3\times n_v\) blocks of each contact Jacobian (rotational rows dropped for point contacts).

Joint torques are not QP decision variables. They are recovered after the solve as

\[ \boldsymbol{\tau} = \mathbf{M}\ddot{\mathbf{q}} + \mathbf{nle} - \sum_i \mathbf{J}_{c,i}^{\mathsf T}\mathbf{f}_{c,i}, \]

then clipped to torque_limits if configured (_extract_solution_from_x).

Friction Pyramid

For each contact, normal force \(f_z \ge 0\) is enforced via \(-f_z \le 0\). Tangential forces use a four-sided pyramid with coefficient mu = 0.5 hard-coded in _build_inequality_constraints (not a circumscribed cone factor). This is a linearized Coulomb model, not a second-order cone.

Task Priority Convention (Document Once)

Task.priority is documented as 0 = highest (task.py:54). Strict hierarchy builds a null-space projector from equality rows accumulated at each level (_build_priority_level_cost).

On main, _solve_hierarchical iterates sorted(priority_groups.items(), reverse=True), so larger integers are processed first—opposite the docstring convention. Prefer weighted mode or wait for PR #15025 before trusting cascade results.

Configuration Defaults

Field Default Role
dt 0.001 s Velocity-limit to acceleration bound
regularization 1e-6 \(\ddot{\mathbf{q}}\) Tikhonov term
contact_force_regularization 1e-4 Contact force penalty
use_hierarchical True HQP vs weighted sum
Contact force bounds \(\pm 10{,}000\) N Numerical stability

Citation

BibTeX citation: Download .bib
@online{olson2026,
  author = {Olson, Dieter},
  title = {Whole-Body {Control} in {UpstreamDrift}},
  date = {2026-11-11},
  url = {https://affinedrift.com/articles/upstreamdrift-whole-body-control.html},
  langid = {en},
  abstract = {Reference for WholeBodyController on UpstreamDrift main at
    3afe09c1: equality and inequality stacked QP, contact force decision
    variables, pyramidal friction with mu 0.5, and inverse dynamics
    torques with optional clipping. Documents weighted versus
    hierarchical modes and notes that sorted priority iteration may
    invert the documented 0-equals-highest convention until a pending
    fix lands.}
}
For attribution, please cite this work as:
Olson, Dieter. 2026. “Whole-Body Control in UpstreamDrift.” November 11. https://affinedrift.com/articles/upstreamdrift-whole-body-control.html.