CMP: Robust Whole-Body Tracking for Loco-Manipulation via Competence Manifold Projection

Ziyang Cheng; Haoyu Wei; Hang Yin; Xiuwei Xu; Bingyao Yu; Jie Zhou; Jiwen Lu

cs.ROApr 8, 2026

CMP: Robust Whole-Body Tracking for Loco-Manipulation via Competence Manifold Projection

Authors: Ziyang Cheng, Haoyu Wei, Hang Yin, Xiuwei Xu, Bingyao Yu, Jie Zhou, Jiwen Lu

Organizations: Tsinghua University

Abstract

While decoupled control schemes for legged mobile manipulators have shown robustness, learning holistic whole-body control policies for tracking global end-effector poses remains fragile against Out-of-Distribution (OOD) inputs induced by sensor noise or infeasible user commands. To improve robustness against these perturbations without sacrificing task performance and continuity, we propose Competence Manifold Projection (CMP). Specifically, we utilize a Frame-Wise Safety Scheme that transforms the infinite-horizon safety constraint into a computationally efficient single-step manifold inclusion. To instantiate this competence manifold, we employ a Lower-Bounded Safety Estimator that distinguishes unmastered intentions from the training distribution. We then introduce an Isomorphic Latent Space (ILS) that aligns manifold geometry with safety probability, enabling efficient O(1) seamless defense against arbitrary OOD intents. Experiments demonstrate that CMP achieves up to a 10-fold survival rate improvement in typical OOD scenarios where baselines suffer catastrophic failure, incurring under 10% tracking degradation. Notably, the system exhibits emergent "best-effort" generalization behaviors to progressively accomplish OOD goals by adhering to the competence boundaries. Project website: https://shepherd1226.github.io/CMP/

Figures & tables

Figure 1: Robust Whole-Body Tracking via Competence Manifold Projection (CMP). The framework processes arbitrary task-space inputs for a legged manipulator, conceptually divided into In-Distribution (ID, green), Out-of-Distribution (OOD, blue), and Infeasible (red) regions. By projecting inputs onto a learned safety manifold: (Left) In OOD scenarios (e.g., a sideways push), the robot demonstrates emergent generalization, successfully tracking OOD trajectories by adhering to its competence boundary. (Right) For infeasible commands (e.g., a backward push far beyond model competence), the system exhibits “best-effort” behavior, safely interacting as close to the target as possible without crossing into unsafe states.

Method	Holistic	Competence-Aware	$\boldsymbol{\mathcal{O}(1)}$	Best-Effort
Decoupled WBC [ 31 , 27 ]	$\times$	$\times$	$-$	$-$
End-to-end WBC [ 18 , 28 , 24 ]	✓	$\times$	$-$	$-$
OOD Detectors [ 16 , 39 ]	$-$	✓	$\times$	$\times$
Inf.-time Steering [ 44 , 23 ]	$-$	✓	$\times$	✓
CMP (Ours)	✓	✓	✓	✓

Table 1: Comparison of representative approaches.

Figure 2: Evolution of the safety formulation. (a) The original problem involves infinite future horizons (Section 3.2 ). (b) We reduce this to a single-step latent inclusion problem, but face Boundary Ambiguity between the training distribution and true safe set (Section 3.3 ). (c) ILS enforces a safety–radius isomorphism, yielding a spherical boundary with $\mathcal{O}(1)$ verification and correction via norm truncation (Section 3.4 ).

Figure 3: Overview of the pipeline. Target trajectories relative to the current Tool Center Point (TCP) frame are encoded into a raw latent intention $z_{t}^{raw}$ by an Intent Encoder $\phi$ for execution by the Low-level Policy $\pi$ . A Safety Estimator $\omega$ is concurrently trained via TD targets to assess safety. To streamline inference, Isomorphic Latent Space (ILS) aligns safety contours to be spherical with safety decreasing radially. This permits $\mathcal{O}(1)$ safety enforcement without the estimator by simply truncating latent vectors that exceed the safety radius.

Method	$R_{safe}$	In-Distribution (ID)			OOD-Geometry			OOD-Sensor
Method	$R_{safe}$	SR (%) $\uparrow$	$e_{p}$ (cm) $\downarrow$	$e_{r}$ (rad) $\downarrow$	SR (%) $\uparrow$	$e_{p}$ (cm) $\downarrow$	$e_{r}$ (rad) $\downarrow$	SR (%) $\uparrow$	$e_{p}$ (cm) $\downarrow$	$e_{r}$ (rad) $\downarrow$
UMI-on-Legs	-	85.3	4.9	0.105	4.7	-	-	6.9	-	-
Latent Shielding	-	90.9	4.4	0.096	5.6	-	-	17.7	-	-
Neural CBF	-	95.9	5.4	0.123	7.1	-	-	19.8	-	-
CMP (Ours)	1.0	84.2	15.6	0.291	82.5	102.2	1.935	56.7	27.9	0.687
	1.5	95.2	6.0	0.139	62.4	90.8	1.767	49.7	15.9	0.475

Table 2: Performance Comparison across ID and OOD Scenarios in Simulation.

Method	Frame-Wise	Safety Estimator	ILS
UMI-on-Legs	$\times$	$\times$	$\times$
CVAE	✓	$\times$	$\times$
SCVAE	✓	✓	$\times$
CMP (Ours)	✓	✓	✓

Table 3: Ablation Study Configurations.

Figure 4: Accuracy–safety trade-off under varying

R_{safe}

. ID position error (log scale) is plotted against OOD-Geometry survival rate.

Method	ID ( $5$ tasks $\times$ $3$ trials)			Moderate OOD ( $5$ tasks $\times$ $3$ trials)			Extreme OOD ( $5$ tasks $\times$ $3$ trials)			Latency (ms) $\downarrow$
Method	SR (%) $\uparrow$	$e_{p}$ (cm) $\downarrow$	$e_{r}$ (rad) $\downarrow$	SR (%) $\uparrow$	$e_{p}$ (cm) $\downarrow$	$e_{r}$ (rad) $\downarrow$	SR (%) $\uparrow$	$e_{p}$ (cm) $\downarrow$	$e_{r}$ (rad) $\downarrow$	Latency (ms) $\downarrow$
UMI-on-Legs	80.0	$\mathbf{4.9\pm 1.6}$	$\mathbf{0.07\pm 0.02}$	0.0	-	-	0.0	-	-	$\mathbf{2.97\pm 0.15}$
Latent Shielding	73.3	$6.9\pm 4.3$	$0.09\pm 0.03$	33.3	$\mathbf{7.8\pm 6.6}$	$\mathbf{0.16\pm 0.11}$	20.0	-	-	$3.89\pm 0.49$
Neural CBF	80.0	$\mathbf{4.8\pm 1.9}$	$0.09\pm 0.03$	60.0	$\mathbf{7.0\pm 4.3}$	$\mathbf{0.17\pm 0.11}$	40.0	$\mathbf{19.3\pm 11.9}$	$\mathbf{0.24\pm 0.27}$	$5.36\pm 0.54$
CMP (Ours)	100.0	$\mathbf{5.1\pm 1.8}$	$0.09\pm 0.03$	93.3	$\mathbf{9.6\pm 7.2}$	$\mathbf{0.24\pm 0.10}$	86.7	$\mathbf{19.2\pm 11.1}$	$0.87\pm 0.68$	$\mathbf{2.99\pm 0.14}$

Table 4: Performance Comparison on 15 Real-World Tasks across ID and OOD Scenarios.

Figure 5: Robot behaviors under varying command difficulties (gripper removed to prevent damage). Color bars indicate outcomes: green for task success, blue for safe survival with reduced accuracy, and red for catastrophic failure. UMI-on-Legs fails catastrophically on OOD inputs; CMP generalizes to OOD tasks via “best-effort” behaviors.

Appendix figures & tables8 assets

Supplementary material from the paper’s appendix.

Appendix

Figure 6: (a) Extreme sparsity of competent trajectories in the 24D command space even with curriculum learning. (b) Latent space visualization confirming our ILS mechanism effectively reshapes intents into a statistical spherical boundary, tightening the $\max$ approximation at the origin.

Figure 7: Validation of the Safety Estimator. Top: Snapshots of the robot executing a raw OOD sideways push command without latent projection. Bottom: Time-series of Safety metric $W$ of the safest intention, raw input intention and the projected intention.

Human Label	$W_{\max}<0.6$	$0.6\leq W_{\max}\leq 0.8$	$W_{\max}>0.8$
Salvageable	0.4%	9.2%	57.8%
Unsalvageable	16.8%	14.4%	1.4%

Appendix

Table 5: Estimator Validation (500 random rollout states, 5 voters)

Figure 8: Defense against sensor-induced divergence. (Top) UMI-on-Legs [ 18 ] enters a positive feedback loop: VIO error $\rightarrow$ Unexpected input $\rightarrow$ Aggressive motion $\rightarrow$ Larger VIO error $\rightarrow$ Crash. (Bottom) CMP prevents the unexpected inputs from triggering unsafe motion, effectively blocking the hazardous feedback loop and preserving stability.

Module	Input	Hidden	Output	Activation
Encoder $\phi$	$s_{t},g_{t}$	$[256,256]$	6	ELU
Policy $\pi$	$s_{t},z_{t}$	$[256,256,256]$	12+6	ELU
Safety $\omega$	$s_{t},z_{t}$	$[256,256]$	1	ELU+Sig *

Appendix

Table 6: Network Architectures

Parameter	Policy Opt. ( $\pi,\phi$ )	Safety Opt. ( $\omega$ )
Optimizer	Adam	Adam
Num. Environments	4,096
Total Iterations	2,000
PPO Epochs	32
Mini-batches	4
Learning Rate (LR)	Adaptive $1\text{e-}3$	Fixed $1\text{e-}3$

Appendix

Table 7: Training Hyperparameters

Figure 9: (a) Safety value $W$ vs. Mahalanobis radius $r$ over $100$ rollout states. (b,c) PCA traces of $z_{t}$ under an OOD vs. an ID command.

Method	ID $e_{r}$ (rad) $\downarrow$	OOD-Geo SR (%) $\uparrow$	OOD-Rev SR (%) $\uparrow$	Other OOD Types
UMI-on-Legs	$\mathbf{0.105}$	$4.7$	$19.0$	$\times$
Command Clipping	$\mathbf{0.107}$	$\mathbf{90.2}$	$33.0$	$\times$
Rear-augmented	$0.130$	$87.0$	$30.2$	$\times$
CMP ( $R_{safe}=2$ )	$\mathbf{0.106}$	$46.9$	$\mathbf{65.2}$	$\checkmark$

Appendix

Table 8: Each method at its own best, matched budget.

Loco-Manipulation

See all 33 papers

Jun 23, 2026

FT-WBC: Learning Fault-Tolerant Whole-Body Control for Legged Loco-Manipulation

Legged manipulators combine the mobility of legged platforms with the manipulation capability of robotic arms. However, arm-induced Center-of-Mass shifts and dynamic disturbances make the system more prone to instability under actuator failures, potentially leading to falls, task failures, or safety risks. Existing fault-tolerant control methods mainly focus on locomotion alone, leaving the coupled problem of whole-body stability and arm reachability in fault-tolerant loco-manipulation largely unaddressed. To bridge this gap, we propose FT-WBC, a fault-tolerant loco-manipulation framework for robust whole-body control of legged manipulators under actuator failures. FT-WBC adopts a decoupled upper- and lower-body policy architecture and introduces two key modules: a Fault Estimator (FE) and a Posture Adaptation Module (PAM). The FE predicts faulty joints from lower-body proprioceptive histories, while the PAM uses this fault information to adapt the base posture plan generated by the arm policy, converting potentially unstable posture requests into safe and executable base posture commands. Through this fault-aware posture adaptation mechanism, FT-WBC synthesizes compensatory gaits under actuator failures and preserves as much arm workspace as possible while maintaining whole-body stability. Simulation and real-world experiments show that FT-WBC significantly improves survival rate and workspace under weakening or locked failures, and transfers zero-shot to a real legged manipulator in the real world.

Yudong Zhong, Pengfei Mai, Sikai Guo +6

1The Hong Kong University of Science and Technology (Guangzhou) · 2The University of Hong Kong

Robotic Control Legged Robotics Whole-Body Control Fault-Tolerant Control

Ask PDF

Jun 4, 2026

HANDOFF: Humanoid Agentic Task-Space Whole-Body Control via Distilled Complementary Teachers

Humanoid loco-manipulation benefits from one controller to coordinate locomotion, arm movements, and fall recovery. These behaviors are difficult to learn together from scratch because they require different skills and objectives. We present HANDOFF, a training architecture that distills motion-tracking, locomotion, and recovery teachers into one 29-DoF policy, using context-conditioned KL losses over separate action slices and a soft mixture of experts. Commanded velocity continuously blends body supervision, while a binary recovery flag assigns the full action to the recovery teacher. The resulting controller takes a compact 10-D task-space command rather than a dense kinematic stream and does not switch policies at runtime. On the Unitree G1, HANDOFF matches state-of-the-art velocity tracking and obtains the largest robust manipulation workspace in a matched adapted-interface benchmark. The same controller executes natural-language-driven, multi-stage loco-manipulation tasks in simulation and hardware experiments without further data collection or fine-tuning.

Lizhi Yang, Junheng Li, Nehar Poddar +5

California Institute of Technology · The Institute for Human & Machine Cognition

Humanoid Loco-Manipulation Robot Skill Learning Humanoid Robot Control Whole-Body Robotic Manipulation Whole-Body Control Policy Distillation

Ask PDF

Oct 5, 2026

Dataset-Free Compliant Humanoid Loco-Manipulation with Dynamic Online Posture

Most humanoid loco-manipulation controllers require human motion data to learn whole-body coordination and posture, leaving policies reliant on external sources to provide this data. We present OCLO (Online-posture Compliant LOco-manipulation), a humanoid loco-manipulation system trained without human motion data and commanded only through two end-effector targets. Because these targets do not uniquely determine whole-body posture, OCLO generates pelvis height and torso orientation online using an analytic reachability prior, further refined through policy-in-the-loop sampling with a task-agnostic cost. OCLO also learns whole-body compliance by displacing end-effector references according to measured forces through a spring-damper model, encouraging the legs, waist, and pelvis to yield to external loads. In simulation, using the reachability prior leads to a 77.8% success rate in acquiring the commanded reference, a vast improvement over the 37.8% success rate accomplished without the prior. Further, refinement reduces end-effector orientation error across all evaluated tasks. The same posture module improves a pretrained SONIC controller on four of five tasks. Without compliance training, policies tend to lose balance under disturbances rather than sacrifice tracking. On a Unitree G1, OCLO maintains balance under end-effector disturbances that cause its ablations to fail and performs seven loco-manipulation tasks, including crouched walking and picking up a box from a low surface. Project website: https://oclo-humanoid.github.io/

Seungho Yeom, Zhenyu Wu, Jaeyoung Huh +4

University of California San Diego, La Jolla, CA 92093, USA. · Yonsei University, Seoul 03722, Republic of Korea.

Whole-Body Robotic Manipulation Compliant Robot Control Humanoid Loco-Manipulation Robotic Control Humanoid Robot Control Robotic Loco-Manipulation+1

Ask PDF

Abstract

Loco-Manipulation

Explore similar work

FT-WBC: Learning Fault-Tolerant Whole-Body Control for Legged Loco-Manipulation

HANDOFF: Humanoid Agentic Task-Space Whole-Body Control via Distilled Complementary Teachers

Dataset-Free Compliant Humanoid Loco-Manipulation with Dynamic Online Posture