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

FT-WBC: Learning Fault-Tolerant Whole-Body Control for Legged Loco-Manipulation
Jun 23, 2026Yudong Zhong, Pengfei Mai, Sikai Guo +6Robotic Control Legged Robotics
1The Hong Kong University of Science and Technology (Guangzhou) · 2The University of Hong Kong
HANDOFF: Humanoid Agentic Task-Space Whole-Body Control via Distilled Complementary Teachers
Jun 4, 2026Lizhi Yang, Junheng Li, Nehar Poddar +5Humanoid Loco-Manipulation Robot Skill Learning
California Institute of Technology · The Institute for Human & Machine Cognition
Dataset-Free Compliant Humanoid Loco-Manipulation with Dynamic Online Posture
Oct 5, 2026Seungho Yeom, Zhenyu Wu, Jaeyoung Huh +4Whole-Body Robotic Manipulation Compliant Robot Control
University of California San Diego, La Jolla, CA 92093, USA. · Yonsei University, Seoul 03722, Republic of Korea.

Jun 23, 2026J/K move · Enter open · S save

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

Yudong Zhong, Pengfei Mai, Sikai Guo, Jiahang Cao+5

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

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

Open paper AskPDF arXiv

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.

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