Body Schema Policy

Body as the Vision–Action Interface for Efficient Robot Loco-Manipulation

Lechen Zhang, Chenyuan Chen, Muxin Zhou, Zewen He, Yoshihiko Nakamura

Mohamed bin Zayed University of Artificial Intelligence

Three panels. Left, human motor control: a person reaching for a box, annotated with body schema, body-centered vision and internal model. Middle, Body Schema Policy: a Unitree G1 humanoid making the same reach, annotated with latent body schema, body-queried vision and action denoising. Right: success rate against action chunking latency for eight policies, marker size showing parameter count; Ours sits at the top right.
From the human body schema to a robot's latent body schema. Left: the human body schema links perception with the body and its movement. Middle: Body Schema Policy (BSP) organizes visual queries and action generation around features on the robot's latent body schema. Right: success rate versus action chunking latency on simulated G1 whole-body tasks.

Abstract

Currently, robots can learn whole-body loco-manipulation through scale: the strongest policies have billions of parameters and still need task-specific fine-tuning. Humans have limited processing capacity, yet learn and act efficiently through a body schema that organizes what they see and how they move. We introduce Body Schema Policy (BSP), an imitation learning policy that builds such a schema into the vision–action interface. Its latent body schema places joint and link tokens on the robot's kinematic tree, where learned screw motions carry features between links. Through body-queried vision, each body part retrieves its own visual evidence, which conditions the denoising of its actions. With this inductive bias, BSP returns an action chunk faster than any policy we compare. Compact, body-specific visual readouts also emerge without spatial supervision: manipulation commands depend strongly on task objects, while locomotion commands respond more broadly to the scene. On nine simulated Unitree G1 whole-body tasks, BSP matches the Ψ0 humanoid foundation model with 75× fewer parameters and nearly doubles Diffusion Policy's success. The same holds on a real G1 and a Fetch mobile manipulator.

Method

Architecture diagram. An egocentric image passes through a visual encoder into visual tokens. Robot state, the denoising step and the noisy action chunk initialize the latent body schema: joint, link and global tokens on the robot's kinematic tree. Neural screw blocks move parent-link features to the child link and update the joint; periodic blocks add body self-attention and body-queried cross-attention into the visual tokens. The final schema gives one condition per action coordinate to a shared temporal decoder, which predicts the action noise.
Body Schema Policy architecture. The state, the denoising step and the current action hypothesis initialize joint, link and global tokens. Each neural screw block transports parent-link features to child links, then updates the corresponding joint tokens. Periodic blocks add body self-attention over links and the global token, and body-queried cross-attention, which retrieves image features with body tokens as queries and visual tokens as keys and values. The final schema supplies one condition per action coordinate to a shared temporal decoder, which predicts action noise.

Mechanism analysis

Expressivity

The G1 humanoid at one configuration outside the training range, as predicted by MLP, GCN, Transformer, Body Transformer (BoT), Rodrigues and Screw (ours), next to the ground truth. The four generic networks scatter links across the scene, shaded dark blue for large error; Rodrigues stays mostly intact, with visible error on a leg and a hand; ours matches the ground truth. Color scale: fitting error (MSE), log scale from 3.0e-07 to 5.86e-01.
Kinematics Extrapolation. Generic networks failed to recover an unseen pose, while our Neural Screw Operator reproduces the ground truth.
The Fetch mobile manipulator at one configuration outside the training range, as predicted by MLP, GCN, Transformer, Body Transformer (BoT), Rodrigues and Screw (ours), next to the ground truth. The four generic networks keep the robot in one piece, with error shading on the arm and gripper, darkest for GCN. The Rodrigues panel is almost empty: the robot is displaced out of the frame, and only a small gripper fragment remains. Ours matches the ground truth. Color scale: fitting error (MSE), log scale from 3.00e-07 to 1.13e+00.
Kinematics Extrapolation on Fetch. Fetch's mobile base translates, a motion the rotation-only Rodrigues operator cannot represent: it shifts every link by about 3.3 m, so the robot leaves the frame. Our Neural Screw Operator reproduces the ground truth.
The Franka arm at one configuration outside the training range, as predicted by MLP, GCN, Transformer, Body Transformer (BoT), Rodrigues and Screw (ours), next to the ground truth. The four generic networks break the arm apart, with the upper links scattered and shaded dark blue for large error. Rodrigues keeps the arm in one piece, with blue shading on the upper arm and the hand hanging apart from the wrist. Ours matches the ground truth. Color scale: fitting error (MSE), log scale from 3.00e-07 to 3.41e-01.
Kinematics Extrapolation on Franka. Generic networks break the arm apart and the Rodrigues operator misplaces the hand, while our Neural Screw Operator reproduces the ground truth.
The Stretch mobile manipulator at one configuration outside the training range, as predicted by MLP, GCN, Transformer, Body Transformer (BoT), Rodrigues and Screw (ours), next to the ground truth. The four generic networks misplace the wrist and gripper, shaded dark blue; for GCN, Transformer and BoT they float apart from the arm. Rodrigues and ours keep the robot in one piece, Rodrigues with blue shading on the gripper and ours with fainter shading. Color scale: fitting error (MSE), log scale from 3.00e-07 to 2.47e-01.
Kinematics Extrapolation on Stretch. Generic networks misplace the wrist and gripper. The Rodrigues operator and our Neural Screw Operator both stay close to the ground truth, ours with the lower error (7.0×10−6 against 9.7×10−5).

Denoising efficiency

Denoising with the Latent Body Schema. Our model denoises an action chunk in fewer steps than Diffusion Policy's U-Net and a Rodrigues Network of the same size that read the same patch tokens.

Body-queried vision

Body-Queried Visual Sensitivity. We visualize what the denoiser reads. In our model, compact, task- and joint-specific visual representations emerge without spatial supervision: manipulation-related parts focus only on the task-relevant object, while body-pose and locomotion joints watch more of the environment. In a U-Net that cross-attends to the same patch tokens, every command reads nearly the same map. Each map is normalized to its own maximum.

Results

Simulation

Nine whole-body tasks from the SIMPLE G1 benchmark. Every method runs the same 30 episodes per task.

Real world

Pedal Trash BinSuccess

Grasp · Open Bin · Throw

Push TrolleySuccess

Loco. · Grasp · Push