Whole-Body Control in UpstreamDrift
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.
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
@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.}
}