Section 7.6: Operational-space and whole-body control (preview for humanoids)

A Careful Control Loop

This section develops the technical contract for Operational-space and whole-body control (preview for humanoids) into a usable mental model. First we define the object of study, then we connect it to the agent loop, then we test it with a compact implementation.

The key question in Operational-space and whole-body control (preview for humanoids) is practical: what must the agent know, what can it observe, what action is available, and what evidence shows that the action worked under the stated conditions?

Theory

For Operational-space and whole-body control (preview for humanoids), the practical design rule is to make the interface inspectable before optimization begins: inputs, outputs, units, latency, bounds, and failure labels should all be visible in the saved artifact.

Operational-space control asks for motion in task coordinates, such as hand pose, foot force, or center of mass, rather than raw joint coordinates. The Jacobian $J(q)$ maps joint velocity to task velocity, $\dot x=J(q)\dot q$, so the controller can reason about what the robot's body does in the world. Whole-body control adds priority: balance and contact constraints usually outrank a hand motion, and remaining degrees of freedom can be used for posture, joint-limit avoidance, or energy management.

Worked Example

Whole-body control resolves task priorities with null-space projection. Given a primary task velocity \(\dot x_d\) and its Jacobian \(J\), the joint command is \(\dot q = J^\dagger \dot x_d + (I - J^\dagger J)\,u_\text{null}\). The first term meets the primary task; the projector \((I - J^\dagger J)\) restricts the secondary objective \(u_\text{null}\) to the directions that leave the primary task untouched. Code Fragment 7.6.1 runs this on a planar 3-link arm: the primary task is end-effector motion, the secondary is posture (pulling joints toward a rest pose). It then shows the priority guarantee saturating: once enough independent tasks are stacked, the null space collapses and there is no freedom left for posture.

import numpy as np

L = np.array([1.0, 0.8, 0.6])           # planar 3-link arm

def fk_jac(q):
    # 2xN Jacobian of end-effector position w.r.t. joint angles.
    J, ang = np.zeros((2, 3)), np.cumsum(q)
    for i in range(3):
        Jx = Jy = 0.0
        for k in range(i, 3):
            Jx += -L[k] * np.sin(ang[k]); Jy += L[k] * np.cos(ang[k])
        J[0, i], J[1, i] = Jx, Jy
    return J

q = np.array([0.3, 0.5, -0.4])
J = fk_jac(q)
Jp = np.linalg.pinv(J)

xdot_d = np.array([0.2, 0.0])           # primary: move +x, hold y
u_null = 1.0 * (np.zeros(3) - q)        # secondary: pull joints toward rest pose
Nproj = np.eye(3) - Jp @ J              # null-space projector of the primary task
qdot = Jp @ xdot_d + Nproj @ u_null

print("primary task error ||J*qdot - xdot_d|| =",
      round(float(np.linalg.norm(J @ qdot - xdot_d)), 6))
print("secondary realized in null space, ||N u_null|| =",
      round(float(np.linalg.norm(Nproj @ u_null)), 4))

# Stack a third independent task (end-effector orientation): null space saturates.
J3 = np.vstack([J, np.ones((1, 3))])    # x, y, and orientation = sum of joint angles
N3 = np.eye(3) - np.linalg.pinv(J3) @ J3
print("null-space trace with 3 independent tasks =", round(float(np.trace(N3)), 3),
      "(0 => no DOF left for posture)")

Practical Recipe

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

Production Pattern

Operational-space and whole-body control (preview for humanoids) sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Operational-space control must name task priorities, contact assumptions, and null-space behavior. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

Use this recipe when turning Operational-space and whole-body control (preview for humanoids) into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Write the control objective, measured state, actuator command, update rate, and saturation policy.
2. Run a step-response test before adding learning, with overshoot, settling time, and steady-state error logged.
3. Compare the hand controller with python-control, CasADi, Drake, do-mpc, or ROS 2 control on the same plant model.
4. Record latency, missed deadlines, saturation events, constraint violations, and recovery actions.
5. Only compare controllers and policies when they share sensors, action limits, disturbance tests, and safety checks.

A learned policy can hide a whole-body priority conflict until contact changes. Check frame conventions, Jacobian ordering, contact mode, torque limits, null-space projection, and fallback behavior before scaling training. For this section, first reproduce one small task-space command by hand, then rerun it through Drake, Pinocchio, MuJoCo, or the robot controller stack. If the two disagree, inspect frame transforms, contact assumptions, and which lower-priority task was sacrificed.