Paper deep dive
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
Intelligence
Status: succeeded | Model: google/gemini-3.1-flash-lite-preview | Prompt: intel-v1 | Confidence: 96%
Last extracted: 4/10/2026, 4:03:36 AM
Summary
The paper introduces Competence Manifold Projection (CMP), a framework for robust whole-body control in legged mobile manipulators. CMP addresses the fragility of learned policies against Out-of-Distribution (OOD) inputs by transforming infinite-horizon safety constraints into a computationally efficient single-step manifold inclusion. It utilizes a Lower-Bounded Safety Estimator and an Isomorphic Latent Space (ILS) to align safety probability with latent geometry, enabling O(1) real-time projection of unsafe commands onto a safe competence boundary.
Entities (5)
Relation Signals (3)
CMP → utilizes → Isomorphic Latent Space
confidence 98% · We then introduce an Isomorphic Latent Space (ILS) that aligns manifold geometry with safety probability
CMP → employs → Lower-Bounded Safety Estimator
confidence 97% · To instantiate this competence manifold, we employ a Lower-Bounded Safety Estimator
CMP → improves → Whole-Body Control
confidence 95% · To improve robustness against these perturbations without sacrificing task performance and continuity, we propose Competence Manifold Projection (CMP).
Cypher Suggestions (0)
No Cypher suggestions yet.
Abstract
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. Result videos are available at: this https URL.
Tags
Links
- Source: https://arxiv.org/abs/2604.07457v1
- Canonical: https://arxiv.org/abs/2604.07457v1
Trouble viewing inline? Open PDF directly →
Full Text
72,125 characters extracted from source content.
Expand or collapse full text
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 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. Result videos are available at: https://shepherd1226.github.io/CMP/. 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 (green), Out-of-Distribution (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. I Introduction Legged mobile manipulators represent a versatile class of robots capable of traversing unstructured environments while performing complex manipulation tasks. To fully exploit their potential, recent paradigms, exemplified by UMI-on-Legs [18], have shifted towards global end-effector tracking in the task space. This approach offers seamless compatibility with cross-embodiment data collection pipelines like UMI [10] and FastUMI [56], end-effector-centric teleoperation interfaces, and high-level planning policies, effectively mobilizing whole-body coordination. By receiving target poses in the global frame, localizing the current end-effector via Visual-Inertial Odometry (VIO), and computing the relative target pose for policy execution, this framework guarantees native spatial translation generalization for the Whole-Body Control (WBC) policy while stably tracking global targets. However, this unified coordination introduces significant reliability challenges. The learned policies are typically only competent for specific trajectories within the training dataset or limited spatial distributions (e.g., the natural manipulation workspace located in front of the robot [18, 28, 24]). Here, competence denotes the policy’s ability to operate safely. When inputs exceed this range—falling Out-of-Distribution (OOD, referring to commands outside the policy’s competence distribution rather than the training distribution, as detailed in Section I)—due to VIO drift, teleoperation latency, or infeasible user commands, the policy often manifests unpredictable and physically unsafe behaviors [55, 50]. While fully or partially decoupled WBC architectures [31, 27, 54, 36, 15] offer robustness, they inherently limit whole-body synergy or compatibility with task-space paradigms [18, 10, 56]. Furthermore, existing safety solutions fail to adequately address three critical issues in this context. First, inference-time policy steering methods [43, 23, 2, 57, 44, 37, 35] face severe real-time constraints, often being too computationally expensive for high-frequency and highly dynamic WBC loops. Second, standard OOD detectors [51, 19, 16] confuse “Out-of-Training-Distribution” with “Out-of-Safe-Distribution”, thus failing to identify actions that lie within the training distribution but were never successfully mastered. Finally, traditional reactive triggers [30, 52, 33] or emergency stops [römerFailurePredictionRuntime2025] inevitably disrupt task continuity, lacking a “best-effort” mechanism to smoothly degrade performance while enforcing safety. To address these issues, we propose Competence Manifold Projection (CMP). First, we establish a Frame-Wise Safety Scheme to decouple temporal safety, effectively transforming the intractable infinite-horizon constraint into a verifiable single-step manifold inclusion condition. To ground this manifold, we then develop a Lower-Bounded Safety Estimator that distinguishes mastered from unmastered behaviors, thereby resolving safety boundary ambiguity. Finally, these components are unified via the Isomorphic Latent Space (ILS), which aligns safety probability with manifold geometry to enable efficient (1)O(1) projection. This integrated pipeline ensures a seamless continuum of control: it preserves full performance for safe inputs while autonomously degrading to the closest feasible behavior for unsafe ones, enabling “best-effort” tracking along the capability boundary. Experimental evaluations show that CMP enhances survival rates by up to a factor of 10 across typical OOD scenarios in both simulation and real-world deployments, while strictly bounding in-distribution tracking degradation to under 10%. The main contributions of this work are summarized as follows: • Problem Reduction: We formulate a safety metric that provides an inherent reduction from the infinite-horizon safety problem to a frame-wise condition, which is highly beneficial for legged robots yet remains under-analyzed in prior works. • Mechanism Design: We introduce Isomorphic Latent Space, driven by a Lower-Bounded Safety Estimator, which naturally aligns latent geometry with safety probability via dynamic KL regularization. • Efficient Deployment: We achieve (1)O(1) seamless OOD detection and handling, offering a run-time tunable trade-off between tracking precision and safety. • Generalization Behaviors: We demonstrate emergent capabilities to progressively accomplish OOD goals by adhering to competence boundaries, significantly enhancing robust generalization on unseen tasks. I Related Work I-A Whole-Body Control for Legged Manipulators Control architectures for legged manipulators have evolved to navigate the trade-off between modular stability and whole-body synergy. Early approaches explicitly decouple locomotion from manipulation using Model Predictive Control (MPC) [5, 41] or hierarchical learning [31, 27], typically treating the arm as a disturbance or solving for motions sequentially. While this modularity accommodates robust navigation planners [13, 8, 20] and simplifies sim-to-real transfer [26, 17], it inherently restricts the reachable workspace by neglecting whole-body momentum [54]. To recover coordination, unified policies often define objectives in the floating-base frame [36, 15]. However, this formulation introduces high-frequency jitter transmission [15] and suffers from interface mismatch with large-scale global-pose datasets like UMI [10, 56]. Recent end-to-end frameworks [18, 28, 24] directly track global end-effector poses to leverage onboard state estimation for agile maneuvers. Yet, without intrinsic competence awareness, these policies remain notoriously fragile against OOD commands induced by sensor noise or infeasible user inputs [55, 50]. Addressing this, our framework maintains the dynamic advantages of holistic whole-body coordination and seamless compatibility with mainstream global-pose interfaces, while introducing a latent projection layer to robustify the policy against arbitrary upper-level perturbations. I-B Physical Safety and OOD Handling Ensuring reliability in learning-based control requires addressing both physical constraints and distributional shifts efficiently [7]. Traditional mechanisms often rely on “break-then-fix” reactive recoveries [30, 52, 22, 33, 25], which intervene only after stability is compromised, or impose conservative constraints that hinder exploration [38, 46, 11, 9]. While predictive shielding via Control Barrier Functions (CBFs) [6, 53, 3, 14, 48] or Hamilton-Jacobi reachability [21, 4, 47] offer foresight, they necessitate manual, task-specific safety function design and struggle to scale. Conversely, data-driven OOD detectors [51, 19, 16] and failure monitors [1, 12] typically function as open-loop alarms or emergency stops [römerFailurePredictionRuntime2025], lacking active correction capabilities. Recent inference-time steering methods [43, 23, 2, 57, 44, 37, 35, 34] actively utilize gradient guidance or predictive models to enforce safety constraints or mitigate covariate shift [39], but their reliance on iterative sampling introduces prohibitive latency for high-frequency whole-body control (>50>50 Hz) [29, 49]. In contrast, our (1)O(1) projection enables seamless, high-frequency “best-effort” steering without iterative latency or disruptive emergency stops. I Problem Formulation Figure 2: Evolution of the safety formulation. (a) The original safety problem is inherently coupled with infinite future horizons (Section IV-B). (b) We reduce this to a single-step latent inclusion problem, yet face the challenge of Boundary Ambiguity where the training distribution differs from the true safe set (Section IV-C). (c) Finally, ILS enforces an isomorphism between safety probability and geometric radius, creating a spherical competence boundary that enables (1)O(1) safety verification and correction via norm truncation (Section IV-D). We model the whole-body control task as a Markov Decision Process (MDP) defined by the tuple (,,,)(S,A,P,G). Here, ,,S,A,P denote the spaces of robot state, action, and environment dynamics respectively, while G encapsulates task goals that dictate the reward formulation. We designate a subset safe⊂S_safe as physically safe states, constrained by limits such as torque saturation and contact forces. The objective is to ensure that for any initial state s0s_0 and arbitrary goal sequence =gtt=0∞g=\g_t\_t=0^∞, the induced trajectory satisfies: ∀t≥0,st∈safe,∀ t≥ 0, s_t _safe, (1) subject to the policy at∼π(⋅|st,gt)a_t π(·|s_t,g_t) and dynamics st+1∼(⋅|st,at)s_t+1 (·|s_t,a_t). However, the policy’s input pair (st,gt)(s_t,g_t)—whether due to VIO estimation errors or aggressive user commands—may exceed the policy’s competence, driving the system towards failure states s∉safes _safe. Enforcing Eq. (1) under these conditions presents three challenges: • Temporal-Spatial Complexity (Fig. 2a): Safety is inherently coupled with the infinite-dimensional temporal sequence t:∞∈∞g_t:∞ ^∞. A command gtg_t is safe only if there exists a future trajectory strictly within safeS_safe. Since commands are end-effector trajectories rather than simple position setpoints, this feasible subspace is topologically fragmented and extremely sparse. Direct (1)O(1) constraint in this original command space is computationally intractable (see Appendix VII-A for details). • Boundary Ambiguity (Fig. 2b): Let safe⊂×D_safe ×G denote the true set of safe state-goal pairs. We only possess the training distribution trainD_train. Since train≠safeD_train _safe (training distributions may contain unmastered failures), standard OOD detection relying merely on (st,gt)∈train(s_t,g_t) _train is insufficient for robustness. • Seamless Enforcement (Fig. 2c): Abrupt emergency stops cause stability issues and interrupt task execution, while post-fall recovery risks hardware damage. This requires finding the semantically closest safe command online to maintain stability. We seek an efficient projection Φ:→ :G mapping gtg_t to gt′g _t such that (st,gt′)∈safe(s_t,g _t) _safe, minimizing ‖gt−gt′‖semantic\|g_t-g _t\|_semantic. IV Method Figure 3: Overview of the pipeline. Target trajectories relative to the current Tool Center Point (TCP) frame are encoded into a raw latent intention ztrawz_t^raw by an Intent Encoder ϕφ for execution by the Low-level Policy π. A Safety Estimator ω 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 (1)O(1) safety enforcement without the estimator by simply truncating latent vectors that exceed the safety radius. IV-A Overview To achieve robust whole-body control under any input commands, we propose the Competence Manifold Projection (CMP) framework. As illustrated in Fig. 3, our architecture builds upon a hierarchical structure comprising an Intent Encoder ϕφ, a Low-level Policy π, and a Safety Estimator ω. We train a unified policy capable of tracking diverse trajectories using a single set of network weights. Following Ha et al. [18], we omit observation history in state representation and simplify the notation to single-frame states. IV-B Frame-Wise Safety Scheme This section addresses Challenge 1 (Section I) by resolving the coupled temporal and spatial complexities of safety through a hierarchical latent framework. To handle the spatially fragmented safe regions in the original high-dimensional space, we aim to map diverse target trajectories into a compact latent space. Drawing inspiration from Conditional Variational Autoencoders (CVAE) [42], we separate the control logic into two levels. The Intent Encoder ϕφ, implemented as a Multi-Layer Perceptron (MLP) with hidden layers [256, 256], encodes task goal gtg_t under the condition of sts_t into a latent distribution (μϕ,Σϕ)N( _φ, _φ), from which the latent command ztz_t is sampled. The Low-level Policy π, utilizing an MLP with hidden layers [256, 256, 256], decides whole-body action ata_t according to the latent intention ztz_t conditioned on current state sts_t. To maximize task performance, the policy π and encoder ϕφ are jointly optimized via the Proximal Policy Optimization (PPO) [40] surrogate loss. Crucially, both modules take the current state sts_t as a condition. This ensures the latent space purely encodes the variation in future trajectory intent relative to the current state, allowing us to reshape the distribution of safe trajectories into a continuous, regular manifold within the latent space Z. Furthermore, this structure enables us to eliminate the temporal dependency of safety assurance. By analyzing a special definition of safety, we inherently reduce the intractable temporal safety problem into a verifiable single-step spatial inclusion condition. For any given conditioned Low-level policy π(⋅|st,zt)π(·|s_t,z_t), we define the Maximum Probability of Perpetual Safety, Wπ(st,zt)W^π(s_t,z_t), to quantify the long-term viability of executing the latent command ztz_t. Specifically, this metric represents the probability that the system remains within safeS_safe indefinitely, under the condition that: (1) The agent commits to executing ztz_t at the current time step; (2) All future latent commands are selected optimally to maximize survival chances. Let τt:∞=skk=t∞ _t:∞=\s_k\_k=t^∞ denote the state trajectory and t+1:∞z_t+1:∞ denote the future sequence of latent commands. The safety value is defined as: Wπ(st,zt)≜maxt+1:∞ℙ(∀k≥t,sk∈safe|st,zt,t+1:∞,π).W^π(s_t,z_t) _z_t+1:∞P (∀ k≥ t,s_k _safe\; |\;s_t,z_t,z_t+1:∞,π ). (2) By exploiting the Markov property, this infinite-horizon objective decomposes into the following Bellman recursive equation: Wπ(st,zt)=safe(st)⋅st+1[maxz′∈Wπ(st+1,z′)],W^π(s_t,z_t)=I_S_safe(s_t)·E_s_t+1 [ _z W^π(s_t+1,z ) ], (3) where safeI_S_safe is the indicator function and the expectation is over the dynamics st+1∼(⋅|st,π(st,zt))s_t+1 (·|s_t,π(s_t,z_t)). Based on WπW^π, we define the Competence Manifold δπ(s)C_δ^π(s) as the set of latent commands from which the policy π can maintain safety with probability at least 1−δ1-δ: δπ(s)≜z∈∣Wπ(s,z)≥1−δ.C_δ^π(s) \z W^π(s,z)≥ 1-δ\. (4) This definition provides a critical analytical linkage: Let P(t+1:∞)≜ℙ(⋂k=t∞sk∈safe∣st,zt,t+1:∞,π)P(z_t+1:∞) ( _k=t^∞\s_k _safe\ s_t,z_t,z_t+1:∞,π) be the theoretical survival probability executing a specific future sequence. The requirement that there exists at least one future latent sequence ensuring continuous safety with probability ≥1−δ≥ 1-δ naturally translates to maxt+1:∞P(t+1:∞)≥1−δ _z_t+1:∞P(z_t+1:∞)≥ 1-δ. By our definition in Eq. (2), this is exactly captured as Wπ(st,zt)≥1−δW^π(s_t,z_t)≥ 1-δ, directly implying the single-step inclusion zt∈δπ(st)z_t _δ^π(s_t). Thus, we have established a clear safety metric that inherently possesses the critical property of reducing the complex infinite-horizon safety requirement into a verifiable single-step membership test. This reduction is exact and does not rely on quasi-static assumptions, making it particularly suitable for dynamic legged robots. IV-C Lower-Bounded Safety Estimator While the established scheme connects safety to the Competence Manifold, the true boundary of this manifold remains ambiguous since training data is not guaranteed to be perfectly safe (Section I). To identify mastered intentions, we train a Safety Estimator ω to approximate the infinite-horizon safety probability Wπ(st,zt)W^π(s_t,z_t), serving as a verifiable criterion beyond simple distribution matching. However, directly computing the Bellman update (Eq. (3)) faces two hurdles: the unknown transition dynamics and the intractable maximization over the continuous latent space. We address these through a conservative approximation strategy. The estimator uses an MLP with hidden layers [256, 256] and is trained independently via Binary Cross-Entropy (BCE) loss to track the safety target. First, to bypass the maximization maxz′Wπ _z W^π, we substitute it with the value at the latent origin: maxz′∈Wπ(st+1,z′)≈W^ω(st+1,). _z W^π(s_t+1,z )≈ W_ω(s_t+1,0). (5) This approximation relies on the property that the peak safety probabilistically aligns with the latent origin. Both theoretical and empirical analyses of its validity are provided in Appendix VII-C. Second, to balance estimation bias and signal propagation, we employ Temporal Difference (TD(λsafe _safe)) [45]. While n-step expansion accelerates failure signal backpropagation, it constitutes a theoretical lower bound of true safety that increasingly loosens with larger n (proof in Appendix VII-C). The TD(λ) mechanism balances these multi-step returns, ensuring conservative estimation. In practice, the training target Wtarget,tW_target,t is computed recursively backward through the trajectory: Wtarget,t=0,if failure1,if timeout(1−λsafe)W^ω(st+1,)+λsafeWtarget,t+1,otherwise,W_target,t= cases0,&if failure\\ 1,&if timeout\\ (1- _safe) W_ω(s_t+1,0)&\\ + _safeW_target,t+1,&otherwise cases, (6) where λsafe=0.8 _safe=0.8. The estimator is then optimized via Binary Cross-Entropy: ℒBCE=BCE(W^ω(st,zt),Wtarget,t).L_BCE=BCE ( W_ω(s_t,z_t),\ W_target,t ). (7) IV-D Isomorphic Latent Space With the safety metric available, the final challenge is to enforce it seamlessly under strict real-time constraints (Section I). Since online optimization of WπW^π is computationally prohibitive, we propose Isomorphic Latent Space (ILS). This technique reshapes the latent space to align safety levels with geometry, reducing complex safety assurance to efficient (1)O(1) operations. Standard latent spaces employ static KL divergence to maintain continuity, typically clustering high-frequency samples near the origin. However, our objective—real-time (1)O(1) verification and minimal-distortion projection—demands a geometric organization where safety monotonically decreases with radial distance, forming a spherical competence boundary. To reconcile this geometric constraint with the semantic continuity of the latent representation, we exploit the property that high-dimensional Gaussian mass concentrates on spherical shells, and dynamically modulate the KL prior. This distributes intentions onto specific radial shells according to their safety probability, thereby aligning geometric structure with safety while preserving the underlying semantic topology. IV-D1 Mapping Construction To map safety probability to geometry, we require a variance mapping function ℛR that is monotonically decreasing. We adopt a cubic inverse formulation: Rt=min(Rmax,max(Rmin,1(Wtarget,t+ϵ)3)),R_t= (R_ , (R_ , 1(W_target,t+ε)^3 ) ), (8) where we set ϵ=1−W¯batchε=1- W_batch. This adaptive term ensures the denominator centers around 1, maintaining a stable gradient scale for RtR_t regardless of shifts in the batch’s average safety. The min-max clipping prevents KL divergence explosion at extreme probability values. IV-D2 Implicit Isomorphism The Intent Encoder ϕφ minimizes this dynamic KL divergence: ℒILS=DKL((μϕ,Σϕ)∥(0,Rt2I)).L_ILS=D_KL (N( _φ, _φ) (0,R_t^2I) ). (9) This implicitly enforces isomorphism since a Gaussian sample z∈ℝdz ^d from (0,R2I)N(0,R^2I) concentrates mass around the shell ‖z‖2≈dR\|z\|_2≈ dR. Thus, minimizing ℒILSL_ILS organizes the latent space such that ‖z‖∝R\|z\| R. Lower safety probabilities map to larger latent norms, reshaping potentially irregular competence boundaries into regular hyperspheres. IV-D3 Runtime Competence Manifold Projection Based on the probability-geometry isomorphic structure, the complex safety verification z∈δπz _δ^π simplifies to a norm check ‖ztraw‖≤Rsafe\|z_t^raw\|≤ R_safe, where RsafeR_safe is a chosen radius threshold corresponding to the desired safety confidence 1−δ1-δ. When an OOD command results in a latent vector extending beyond the competence boundary, we apply Competence Manifold Projection (CMP): ztsafe=ztraw⋅min(1,Rsafe‖ztraw‖2).z_t^safe=z_t^raw· (1, R_safe\|z_t^raw\|_2 ). (10) This operation strictly bounds ‖ztsafe‖2≤Rsafe\|z_t^safe\|_2≤ R_safe. Following our temporal-to-frame-wise equivalence analysis (Section IV-B), this geometric bound ensures the existence of a perpetually safe future trajectory (P≥1−δP≥ 1-δ). Crucially, due to the continuity of the latent space enforced by KL regularization, this projection does not arbitrarily reset the system. Instead, it finds the closest feasible intent to the original command, enabling the robot to perform “best-effort” tracking at the limit of its capabilities rather than freezing. Visualizations confirming the empirical behavior of ILS are presented in Appendix VII-B. V Simulation Experiments We conduct simulation experiments in Isaac Gym [32]. The robot model comprises a Unitree Go2 quadruped equipped with a Hexfellow Saber robotic arm and a UMI gripper. V-A Overall Performance Comparison TABLE I: Performance Comparison across ID and OOD Scenarios (3 Random Seeds). Method RsafeR_safe In-Distribution (ID) OOD-Geometry OOD-Sensor SR (%) ↑ epe_p (cm) ↓ ere_r (rad) ↓ SR (%) ↑ epe_p (cm) ↓ ere_r (rad) ↓ SR (%) ↑ epe_p (cm) ↓ ere_r (rad) ↓ 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 2.0∗ 94.7 4.5 0.106 46.9 79.0 1.661 40.3 11.8 0.367 2.5 91.3 4.1 0.097 34.1 69.4 1.612 34.5 9.9 0.308 3.0 94.7 3.8 0.091 26.3 - - 29.3 - - 100 94.9 3.5 0.086 5.1 - - 16.1 - - • Note: Bold indicates the best results among the baselines and the selected CMP configuration (denoted as ∗), including ties within the omitted standard deviation margin (<0.2<0.2 cm or 0.00020.0002 rad). To avoid survivorship bias, tracking metrics (epe_p and ere_r) are omitted (-) for Survival Rate (SR) <30%<30\%. We quantitatively evaluate the tracking performance and Survival Rate (SR) across different algorithms and safety radii. We test on three scenarios, with 7,000 trajectories of 13 seconds each. The training dataset shares the same scale and distribution as the In-Distribution (ID) validation set but consists of a distinct set of trajectories. We compare against UMI-on-Legs [18] alongside safety approaches like Latent Shielding [35] and Neural CBF [34]. We exclude safe RL, which is inherently fragile to OOD commands, and MPC safety formulations, which remain limited to locomotion or fixed-base arms rather than dynamic whole-body tracking. • In-Distribution (ID): Targets are mostly within the frontal yaw range of [−60∘,60∘][-60 ,60 ], which constitutes the natural workspace for legged manipulators. The ID dataset consists of trajectories collected from representative tasks, combined with augmented and randomly generated trajectories, aiming to achieve general end-effector tracking rather than task-specific motions. Detailed generation methods for the three datasets are provided in Appendix VII-E. • OOD-Geometry: Target yaw ∈[120∘,240∘]∈[120 ,240 ] (Rear), requiring geometric adaptation. • OOD-Sensor: ID trajectories corrupted by random, high-magnitude sensor noise injection to simulate VIO failure. Metrics: We report Position Error (epe_p), Orientation Error (ere_r), and Survival Rate (SR). Following Ha et al. [18], an episode is terminated as a failure if it encounters physically unsafe conditions such as collisions or excessive contact forces (see Appendix VII-F for details). The SR metric represents the percentage of episodes that are not early-terminated. Quantitative Analysis: Table I summarizes the results. While UMI-on-Legs [18] achieves good In-Distribution (ID) tracking precision, it fails catastrophically under OOD conditions. Neural CBF [34] and Latent Shielding [35] offer only marginal SR improvements. Mechanistically, Neural CBF struggles because its required Lie derivative conditions are frequently violated by complex legged dynamics, whereas Latent Shielding’s hard thresholds abruptly interrupt tasks to enforce safety, forcing a severe trade-off where preserving overall tracking performance inevitably sacrifices survivability. In contrast, our CMP matches the ID tracking precision of baselines while substantially boosting survival rates. Operating at a balanced safety radius of Rsafe=2.0R_safe=2.0, CMP achieves exactly a 10-fold SR improvement in OOD-Geometry (46.9% vs. 4.7%) and a nearly 6-fold increase under OOD-Sensor noise compared to UMI-on-Legs. Unlike abrupt shielding methods, CMP serves as a best-effort projection that smoothly bounds actions back to competence boundaries, effectively preserving intent semantics while ensuring survival. V-B Ablation Studies To verify the contribution of each component, we conduct an ablation study. Table I details the method configurations: • CVAE: Implements the Frame-Wise Safety Scheme (Section IV-B), filtering outliers by simply truncating the norm of latent commands ztz_t that fall far from the distribution center to a safe radius RsafeR_safe during inference. • Safe-CVAE (SCVAE): Incorporates the Safety Estimator (Section IV-C) into CVAE. To leverage the Safety Estimator to selectively encode only safe actions into the latent space, SCVAE applies the KL divergence loss only to samples where the estimated safety W(st,zt)W(s_t,z_t) exceeds the batch average W¯batch W_batch, while ignoring unsafe samples. This serves as a direct filtering strategy without geometric shaping. • CMP: Further employs Isomorphic Latent Space (ILS, Section IV-D) to structurally align the latent space geometry with safety probability. TABLE I: Ablation Study Configurations. Method Frame-Wise Safety Estimator ILS UMI-on-Legs × × × CVAE ✓ × × SCVAE ✓ ✓ × CMP (Ours) ✓ ✓ ✓ TABLE I: Performance Comparison on 15 Real-World Tasks across ID and OOD Scenarios (3 Random Seeds). Method ID (55 tasks× 33 trials) Moderate OOD (55 tasks× 33 trials) Extreme OOD (55 tasks× 33 trials) Latency (ms) ↓ SR (%) ↑ epe_p (cm) ↓ ere_r (rad) ↓ SR (%) ↑ epe_p (cm) ↓ ere_r (rad) ↓ SR (%) ↑ epe_p (cm) ↓ ere_r (rad) ↓ UMI-on-Legs 80.0 4.9±1.64.9± 1.6 0.07±0.020.07± 0.02 0.0 - - 0.0 - - 2.97±0.152.97± 0.15 Latent Shielding 73.3 6.9±4.36.9± 4.3 0.09±0.030.09± 0.03 33.3 7.8±6.67.8± 6.6 0.16±0.110.16± 0.11 20.0 - - 3.89±0.493.89± 0.49 Neural CBF 80.0 4.8±1.94.8± 1.9 0.09±0.030.09± 0.03 60.0 7.0±4.37.0± 4.3 0.17±0.110.17± 0.11 40.0 19.3±11.919.3± 11.9 0.24±0.270.24± 0.27 5.36±0.545.36± 0.54 CMP (Ours) 100.0 5.1±1.85.1± 1.8 0.09±0.030.09± 0.03 93.3 9.6±7.29.6± 7.2 0.24±0.100.24± 0.10 86.7 19.2±11.119.2± 11.1 0.87±0.680.87± 0.68 2.99±0.142.99± 0.14 • Note: Best results and those within the error margin are bolded. CMP utilizes Rsafe=2.0R_safe=2.0 for all experiments. To avoid survivorship bias, tracking metrics (position error epe_p and orientation error ere_r) are omitted (-) for Survival Rate (SR) <30%<30\%. Figure 4: Trade-off between Accuracy and Safety. We sweep the safety radius RsafeR_safe to examine the relationship between ID position error in logarithmic axis and OOD-Geometry survival rate. Trade-off Analysis: We sweep RsafeR_safe to evaluate the conservatism-agility trade-off (Fig. 4). SCVAE outperforms CVAE at larger radii by filtering unsafe data but degrades below CVAE at small radii. This occurs because SCVAE lacks structured latent organization: unlike CVAE (centering high-frequency data) or CMP (centering safe data via ILS), SCVAE’s latent origin is neither density- nor safety-optimized. Consequently, aggressive truncation yields latent codes that are neither accurate nor safe. By contrast, CMP achieves a superior trade-off curve. By correlating safety with the latent geometry through ILS, CMP preserves a richer set of functional behaviors at smaller radii, whereas baselines suffer rapid performance degradation (further visualizations of ILS effects in the latent space are detailed in Appendix VII-B). V-C Validation of Safety Estimator Figure 5: 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. We use a rollout trajectory to qualitatively validate the effectiveness of the Safety Estimator W(s,z)W(s,z) (for a quantitative study, see Appendix VII-D). We execute the policy using raw latent codes ztrawz_t^raw without safety projection, allowing us to observe the safety metric’s response to dangerous behaviors. Three phases are observed in Fig. 5: 1. Normal State (e.g., t=0.2s): The robot tracks a feasible target. Safety metrics Wmax=W(s,)W_max=W(s,0), Wproj=W(s,ztsafe)W_proj=W(s,z_t^safe), and Wraw=W(s,ztraw)W_raw=W(s,z_t^raw) are all high. Since ztrawz_t^raw is safe, Wmax>Wraw≈WprojW_max>W_raw≈ W_proj, validating the safety assessment. 2. OOD Target (e.g., t=1.0s): The target becomes OOD. We observe Wmax>Wproj>WrawW_max>W_proj>W_raw, indicating the estimator correctly penalizes the risky ztrawz_t^raw, while projection yields a safer ztsafez_t^safe. This validates the sensitivity of W to z. 3. Near Fall (e.g., t=1.4s): Unsafe actions drive the robot to a near-fall state. All W values drop significantly, confirming W(s,z)W(s,z) effectively captures state-dependent risks. VI Real-world Experiments VI-A Experimental Setup We validate the proposed approach on a physical platform with the same configuration as the simulation: a Unitree Go2 quadruped robot and a 6-DoF Hexfellow Saber robotic arm. However, the UMI gripper is removed to prevent potential hardware damage during the evaluation of safety-critical failure modes and extreme OOD tracking tasks, ensuring consistent experimental conditions. Visual-Inertial Odometry (VIO) from an onboard Intel RealSense T265 camera estimates the base pose in the global frame, while the user command (pre-set trajectories) provides a global target pose. The policy input is the computed relative target pose. Consequently, OOD inputs arise from two sources: intrinsic sensor anomalies (e.g., VIO drift) and extrinsic infeasible user commands. VI-B Robustness to OOD Commands Figure 6: Visual comparison of robot behaviors under varying command difficulties. Note that the gripper is removed to prevent damage during failure modes. The color bars denote the outcome: green for task success, blue for safe survival (despite lower accuracy), and red for catastrophic failure. The top row (UMI-on-Legs [18]) and bottom row (CMP) show snapshots from representative trials. While UMI-on-Legs succeeds in ID tasks, it suffers catastrophic failures in OOD scenarios. CMP generalizes to some moderate OOD tasks and survives arbitrary commands, demonstrating seamless “best effort” behaviors. We evaluate system performance in Table I across 15 target trajectories categorized by difficulty: In-Distribution (ID), Moderate OOD, and Extreme OOD. These encompass not only spatial deviations (forward, sideways, and backward mapping) for basic tasks like pushing and tossing, but also dynamic intensity variations such as rapid jumping and fast cup manipulation. As in the simulation experiments, we compare CMP against UMI-on-Legs [18], Latent Shielding [35], and Neural CBF [34]. Due to space constraints, Fig. 6 visually compares only UMI-on-Legs and CMP on 9 representative spatial tracking tasks. The results are analyzed as follows: • ID Scenarios: CMP is the only method achieving a 100% survival rate across standard tasks, compared to ∼ 80% for UMI-on-Legs and Neural CBF. The tracking errors of CMP are not significantly different from the baselines given the standard deviations, confirming that our safety projection does not hinder nominal precision. • Moderate OOD: This category poses spatial or dynamic challenges (e.g., sideways tracking, diagonal jumping) that the training distribution does not cover. UMI-on-Legs consistently fails (0% SR) due to aggressive lateral actions destabilizing the base. While Latent Shielding and Neural CBF yield partial resilience (33.3% and 60.0% SR), they still frequently lose balance. Conversely, CMP achieves a 93.3% survival rate by projecting these intentions into the Competence Manifold, exhibiting a “best-effort” strategy—such as performing small, safe turns for sideways pushing—to seamlessly accomplish originally unstable tasks. • Extreme OOD: These tasks (e.g., backward tracking, extreme side-jumping) represent severe deviations from the training distribution. All baselines struggle heavily (UMI-on-Legs 0.0%, Latent Shielding 20.0%, Neural CBF 40.0%). Conversely, CMP effectively truncates unsafe command components to prioritize stability, achieving an 86.7% survival rate. Notably, even in these extreme cases, CMP generates safe motions that structurally resemble the target intents, preserving the semantic meaning of the command as much as possible. Beyond survival and performance, Latent Shielding (extra forward pass) and Neural CBF (multiple backward passes) computationally incur latency overloads (3.893.89 ms and 5.365.36 ms respectively) detrimental to high-frequency whole-body control. In contrast, CMP’s implicit (1)O(1) projection algorithm achieves a fast 2.992.99 ms latency, closely matching the unshielded baseline. Across the 45 hardware trials of CMP (Table I), we observed 3 failures. We analyze their causes to guide future improvements: • Estimator Inaccuracy (1 time): Imperfect network learning, compounded by the theoretical lower bound (conflating marginal and perfect safety), caused an over-prediction of safety, resulting in extreme, oscillatory motions. • Imperfect Latent Mapping (2 times): The spherical boundary is statistical. Projected commands may remain unsafe and fail to salvage the execution, typically causing the robot to fall. Note that CMP handles command distribution shifts, not dynamics shifts (e.g., carrying loads or disturbances), though it readily accommodates environment-adaptive policies via context conditioning. VI-C Robustness to Sensor Divergence Figure 7: Defense against sensor-induced divergence. (Top) UMI-on-Legs [18] enters a positive feedback loop: VIO error → Unexpected input → Aggressive motion → Larger VIO error → Crash. (Bottom) CMP prevents the unexpected inputs from triggering unsafe motion, effectively blocking the hazardous feedback loop and preserving stability. We investigate robustness against sensor-induced OOD inputs by commanding the robot to track a sinusoidal forward trajectory that necessitates rapid lateral body adjustments. Such rapid motion often causes the VIO odometry to jitter or drift significantly. Fig. 7 illustrates the critical divergence mechanism triggered by the T265 sensor’s bandwidth limitation: (1) Rapid oscillation induces VIO drift, creating an erroneous, distorted relative goal gtg_t. (2) For UMI-on-Legs [18], this OOD goal elicits aggressive corrective actions. (3) These actions intensify body oscillation, forming a positive feedback loop that rapidly destabilizes the system (Fig. 7 Top). In contrast, CMP (Fig. 7 Bottom) detects the low survival probability associated with the anomalous goal and projects the latent command to a safe region, dampening the response to sensor noise. This effectively blocks the dangerous feedback loop, preventing error amplification and maintaining stability. VII Conclusion In this paper, we introduced Competence Manifold Projection (CMP) to secure whole-body controllers against OOD perturbations. Our approach addresses the safety challenge through three contributions: First, a Frame-Wise Safety Scheme reduces intractable infinite-horizon constraints to single-step latent inclusions. Second, a Lower-Bounded Safety Estimator quantifies the maximum viability of arbitrary intents. Finally, an Isomorphic Latent Space aligns this metric with latent geometry, transforming verification into an efficient (1)O(1) projection. Extensive experiments confirm that CMP achieves up to a 10-fold improvement in survival rates across typical OOD scenarios in simulation and real-world setups. These gains incur less than 10% In-Distribution tracking degradation. Beyond passive safety, the system exhibits emergent “best-effort” behaviors, maximizing task progress along the competence boundary. This work effectively bridges the gap between high-performance learning policies and deployment reliability. Future work will scale this framework to higher-dimensional humanoid robots. Additionally, we aim to develop adaptive mechanisms for online auto-tuning of the safety radius RsafeR_safe, dynamically balancing safety and performance in response to environmental complexity. References [1] C. Agia, R. Sinha, J. Yang, Z. Cao, R. Antonova, M. Pavone, and J. Bohg (2024) Unpacking failure modes of generative policies: runtime monitoring of consistency and progress. In Conference on Robot Learning (CoRL), Cited by: §I-B. [2] A. Ajay, Y. Du, A. Gupta, J. Tenenbaum, T. Jaakkola, and P. Agrawal (2023) Is conditional generative modeling all you need for decision-making?. In International Conference on Learning Representations (ICLR), Cited by: §I, §I-B. [3] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada (2019) Control barrier functions: theory and applications. In 2019 18th European control conference (ECC), p. 3420–3431. Cited by: §I-B. [4] S. Bansal, M. Chen, S. Herbert, and C. J. Tomlin (2017) Hamilton-jacobi reachability: a brief overview and recent advances. In 2017 IEEE 56th Annual Conference on Decision and Control (CDC), p. 2242–2253. Cited by: §I-B. [5] C. D. Bellicoso, K. Krämer, M. Stäuble, D. Sako, F. Jenelten, M. Bjelonic, and M. Hutter (2019) ALMA - articulated locomotion and manipulation for a torque-controllable robot. In 2019 International Conference on Robotics and Automation (ICRA), Vol. , p. 8477–8483. Cited by: §I-A. [6] R. M. Bena, G. Bahati, B. Werner, R. K. Cosner, L. Yang, and A. D. Ames (2025) Geometry-aware predictive safety filters on humanoids: from poisson safety functions to cbf constrained mpc. In 2025 IEEE-RAS 24th International Conference on Humanoid Robots (Humanoids), p. 1–8. Cited by: §I-B. [7] L. Brunke, M. Greeff, A. W. Hall, Z. Yuan, S. Zhou, J. Panerati, and A. P. Schoellig (2022) Safe learning in robotics: from learning-based control to safe reinforcement learning. Annual Review of Control, Robotics, and Autonomous Systems 5 (1), p. 411–444. Cited by: §I-B. [8] R. Buchanan, L. Wellhausen, M. Bjelonic, T. Bandyopadhyay, N. Kottege, and M. Hutter (2021) Perceptive whole-body planning for multilegged robots in confined spaces. Journal of Field Robotics 38 (1), p. 68–84. Cited by: §I-A. [9] R. Cheng, G. Orosz, R. M. Murray, and J. W. Burdick (2019) End-to-end safe reinforcement learning through barrier functions for safety-critical continuous control tasks. In Proceedings of the AAAI conference on artificial intelligence, Vol. 33, p. 3387–3395. Cited by: §I-B. [10] C. Chi, Z. Xu, C. Pan, E. Cousineau, B. Burchfiel, S. Feng, R. Tedrake, and S. Song (2024) Universal manipulation interface: in-the-wild robot teaching without in-the-wild robots. In Proceedings of Robotics: Science and Systems, Cited by: §I, §I, §I-A, §VII-A, §VII-E1. [11] G. Dalal, K. Dvijotham, M. Vecerik, T. Hester, C. Paduraru, and Y. Tassa (2018) Safe exploration in continuous action spaces. arXiv preprint arXiv:1801.08757. Cited by: §I-B. [12] J. Duan, W. Pumacay, N. Kumar, Y. R. Wang, S. Tian, W. Yuan, R. Krishna, D. Fox, A. Mandlekar, and Y. Guo (2025) AHA: a vision-language-model for detecting and reasoning over failures in robotic manipulation. In International Conference on Learning Representations (ICLR), Cited by: §I-B. [13] T. Dudzik, M. Chignoli, G. Bledt, B. Lim, A. Miller, D. Kim, and S. Kim (2020) Robust autonomous navigation of a small-scale quadruped robot in real-world environments. In 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 3664–3671. Cited by: §I-A. [14] Y. Emam, G. Notomista, P. Glotfelter, Z. Kira, and M. Egerstedt (2022) Safe reinforcement learning using robust control barrier functions. IEEE Robotics and Automation Letters. Cited by: §I-B. [15] Z. Fu, X. Cheng, and D. Pathak (2023) Deep whole-body control: learning a unified policy for manipulation and locomotion. In Proceedings of the 6th Conference on Robot Learning, Vol. 205, p. 138–149. Cited by: §I, §I-A. [16] M. Ganai, R. Sinha, C. Agia, D. Morton, L. Di Lillo, and M. Pavone (2025) Real-time out-of-distribution failure prevention via multi-modal reasoning. arXiv preprint arXiv:2505.10547. Cited by: §I, §I-B. [17] X. Gu, Y. Wang, and J. Chen (2024) Humanoid-gym: reinforcement learning for humanoid robot with zero-shot sim2real transfer. arXiv preprint arXiv:2404.05695. Cited by: §I-A. [18] H. Ha, Y. Gao, Z. Fu, J. Tan, and S. Song (2024) UMI-on-legs: making manipulation policies mobile with manipulation-centric whole-body controllers. In Proceedings of the 8th Conference on Robot Learning, Vol. 270, p. 5254–5270. Cited by: §I, §I, §I-A, §IV-A, §V-A, §V-A, §V-A, Figure 6, Figure 7, §VI-B, §VI-C, 2nd item, §VII-E1, §VII-F, §VII-G2. [19] N. He, S. Li, Z. Li, Y. Liu, and Y. He (2024) ReDiffuser: reliable decision-making using a diffuser with confidence estimation. In International Conference on Machine Learning (ICML), Cited by: §I, §I-B. [20] D. Hoeller, L. Wellhausen, F. Farshidian, and M. Hutter (2021) Learning a state representation and navigation in cluttered and dynamic environments. IEEE Robotics and Automation Letters 6 (3), p. 5081–5088. Cited by: §I-A. [21] K. Hsu, V. R. Royo, C. J. Tomlin, and J. F. Fisac (2021) Safety and liveness guarantees through reach-avoid reinforcement learning. In Robotics: Science and Systems XVII, Cited by: §I-B. [22] T. Huang, J. Ren, H. Wang, Z. Wang, Q. Ben, M. Wen, X. Chen, J. Li, and J. Pang (2025) Learning humanoid standing-up control across diverse postures. arXiv preprint arXiv:2502.08378. Cited by: §I-B. [23] M. Janner, Y. Du, J. B. Tenenbaum, and S. Levine (2022) Planning with diffusion for flexible behavior synthesis. arXiv preprint arXiv:2205.09991. Cited by: §I, §I-B. [24] K. Jiang, Z. Fu, J. Guo, W. Zhang, and H. Chen (2025) Learning whole-body loco-manipulation for omni-directional task space pose tracking with a wheeled-quadrupedal-manipulator. IEEE Robotics and Automation Letters 10 (2), p. 1481–1488. Cited by: §I, §I-A. [25] V. Kumar (2021) Learning control policies for fall prevention and safety in bipedal locomotion. Ph.D. Thesis, Georgia Institute of Technology. Cited by: §I-B. [26] J. Lee, J. Hwangbo, L. Wellhausen, V. Koltun, and M. Hutter (2020) Learning quadrupedal locomotion over challenging terrain. Science robotics 5 (47), p. eabc5986. Cited by: §I-A. [27] M. Liu, Z. Chen, X. Cheng, Y. Ji, R. Qiu, R. Yang, and X. Wang (2024) Visual whole-body control for legged loco-manipulation. In Proceedings of the 8th Conference on Robot Learning, Cited by: §I, §I-A. [28] X. Liu, B. Ma, C. Qi, Y. Ding, N. Xu, G. Zhang, P. Chen, K. Liu, Z. Jia, C. Guan, et al. (2025) Mlm: learning multi-task loco-manipulation whole-body control for quadruped robot with arm. IEEE Robotics and Automation Letters 11 (1), p. 81–88. Cited by: §I, §I-A, 2nd item. [29] G. Lu, Z. Gao, T. Chen, W. Dai, Z. Wang, W. Ding, and Y. Tang (2024) Manicm: real-time 3d diffusion policy via consistency model for robotic manipulation. arXiv preprint arXiv:2406.01586. Cited by: §I-B. [30] Y. Ma, F. Farshidian, and M. Hutter (2023) Learning arm-assisted fall damage reduction and recovery for legged mobile manipulators. In 2023 IEEE International Conference on Robotics and Automation (ICRA), p. 12149–12155. Cited by: §I, §I-B. [31] Y. Ma, F. Farshidian, T. Miki, J. Lee, and M. Hutter (2022) Combining learning-based locomotion policy with model-based manipulation for legged mobile manipulators. IEEE Robotics and Automation Letters 7 (2), p. 2377–2384. Cited by: §I, §I-A. [32] V. Makoviychuk, L. Wawrzyniak, Y. Guo, M. Lu, K. Storey, M. Macklin, D. Hoeller, N. Rudin, A. Allshire, A. Handa, et al. (2021) Isaac gym: high performance gpu based physics simulation for robot learning. In Thirty-fifth Conference on Neural Information Processing Systems Datasets and Benchmarks Track (Round 2), Cited by: §V. [33] Z. Meng, T. Liu, L. Ma, Y. Wu, R. Song, W. Zhang, and S. Huang (2025) SafeFall: learning protective control for humanoid robots. arXiv preprint arXiv:2511.18509. Cited by: §I, §I-B. [34] K. Nakamura, A. L. Bishop, S. Man, A. M. Johnson, Z. Manchester, and A. Bajcsy (2025) How to train your latent control barrier function: smooth safety filtering under hard-to-model constraints. arXiv preprint arXiv:2511.18606. Cited by: §I-B, §V-A, §V-A, §VI-B. [35] K. Nakamura, L. Peters, and A. Bajcsy (2025) Generalizing safety beyond collision-avoidance via latent-space reachability analysis. arXiv preprint arXiv:2502.00935. Cited by: §I, §I-B, §V-A, §V-A, §VI-B. [36] G. Pan, Q. Ben, Z. Yuan, G. Jiang, Y. Ji, S. Li, J. Pang, H. Liu, and H. Xu (2025) RoboDuet: learning a cooperative policy for whole-body legged loco-manipulation. IEEE Robotics and Automation Letters. Cited by: §I, §I-A. [37] H. Qi, H. Yin, Y. Du, and H. Yang (2025) Strengthening generative robot policies through predictive world modeling. arXiv preprint arXiv:2502.00622. Cited by: §I, §I-B. [38] A. Ray, J. Achiam, and D. Amodei (2019) Benchmarking safe exploration in deep reinforcement learning. arXiv preprint arXiv:1910.01708 7 (1), p. 2. Cited by: §I-B. [39] S. Ross, G. Gordon, and D. Bagnell (2011) A reduction of imitation learning and structured prediction to no-regret online learning. In Proceedings of the fourteenth international conference on artificial intelligence and statistics, p. 627–635. Cited by: §I-B. [40] J. Schulman, F. Wolski, P. Dhariwal, A. Radford, and O. Klimov (2017) Proximal policy optimization algorithms. arXiv preprint arXiv:1707.06347. Cited by: §IV-B. [41] J. Sleiman, F. Farshidian, and M. Hutter (2023) Versatile multicontact planning and control for legged loco-manipulation. Science Robotics 8 (81), p. eadg5014. Cited by: §I-A. [42] K. Sohn, H. Lee, and X. Yan (2015) Learning structured output representation using deep conditional generative models. In Advances in Neural Information Processing Systems, C. Cortes, N. Lawrence, D. Lee, M. Sugiyama, and R. Garnett (Eds.), Vol. 28. Cited by: §IV-B. [43] Z. Sun and S. Song (2025) Latent policy barrier: learning robust visuomotor policies by staying in-distribution. arXiv preprint arXiv:2508.05941. Cited by: §I, §I-B. [44] Z. Sun, Y. Wang, D. Held, and Z. Erickson (2024) Force-constrained visual policy: safe robot-assisted dressing via multi-modal sensing. IEEE Robotics and Automation Letters. Cited by: §I, §I-B. [45] R. S. Sutton and A. G. Barto (2018) Reinforcement learning: an introduction. Second edition, MIT Press, Cambridge, Massachusetts. Cited by: §IV-C. [46] C. Tessler, D. J. Mankowitz, and S. Mannor (2018) Reward constrained policy optimization. arXiv preprint arXiv:1805.11074. Cited by: §I-B. [47] B. Thananjeyan, A. Balakrishna, S. Nair, M. Luo, K. Srinivasan, M. Hwang, J. E. Gonzalez, J. Ibarz, C. Finn, and K. Goldberg (2021) Recovery RL: safe reinforcement learning with learned recovery zones. IEEE Robotics and Automation Letters 6 (3), p. 4915–4922. Cited by: §I-B. [48] Z. Wang, X. Yang, J. Zhao, J. Zhou, T. Ma, Z. Gao, A. Ajoudani, and J. Liang (2025) End-to-end humanoid robot safe and comfortable locomotion policy. arXiv preprint arXiv:2508.07611. Cited by: §I-B. [49] L. Wei, H. Feng, P. Hu, T. Zhang, Y. Yang, X. Zheng, R. Feng, D. Fan, and T. Wu (2024) Closed-loop diffusion control of complex physical systems. arXiv preprint arXiv:2408.03124. Cited by: §I-B. [50] W. Xing, M. Li, M. Li, and M. Han (2025) Towards robust and secure embodied ai: a survey on vulnerabilities and attacks. arXiv preprint arXiv:2502.13175. Cited by: §I, §I-A. [51] C. Xu, T. K. Nguyen, E. Dixon, C. Rodriguez, P. Miller, R. Lee, P. Shah, R. Ambrus, H. Nishimura, and M. Itkina (2025) Can we detect failures without failure data? Uncertainty-aware runtime failure detection for imitation learning policies. In Robotics: Science and Systems (RSS), Cited by: §I, §I-B. [52] C. Yang, K. Yuan, Q. Zhu, W. Yu, and Z. Li (2020) Multi-expert learning of adaptive legged locomotion. Science Robotics 5 (49), p. eabb2174. Cited by: §I, §I-B. [53] L. Yang, B. Werner, R. K. Cosner, D. Fridovich-Keil, P. Culbertson, and A. D. Ames (2025) SHIELD: safety on humanoids via cbfs in expectation on learned dynamics. arXiv preprint arXiv:2505.11494. Cited by: §I-B. [54] N. Yokoyama, A. Clegg, J. Truong, E. Undersander, T. Yang, S. Arnaud, S. Ha, D. Batra, and A. Rai (2024) ASC: adaptive skill coordination for robotic mobile manipulation. IEEE Robotics and Automation Letters 9 (1), p. 779–786. Cited by: §I, §I-A. [55] H. Zhang, R. Dai, G. Solak, P. Zhou, Y. She, and A. Ajoudani (2025) Safe learning for contact-rich robot tasks: a survey from classical learning-based methods to safe foundation models. arXiv preprint arXiv:2512.11908. Cited by: §I, §I-A. [56] Zhaxizhuoma, K. Liu, C. Guan, Z. Jia, Z. Wu, X. Liu, T. Wang, S. Liang, P. Chen, P. Zhang, H. Song, D. Qu, D. Wang, Z. Wang, N. Cao, Y. Ding, B. Zhao, and X. Li (2025) FastUMI: a scalable and hardware-independent universal manipulation interface with dataset. arXiv preprint arXiv:2409.19499. Cited by: §I, §I, §I-A, §VII-A. [57] G. Zhou, S. Swaminathan, R. V. Raju, J. S. Guntupalli, W. Lehrach, J. Ortiz, A. Dedieu, M. Lázaro-Gredilla, and K. Murphy (2024) Diffusion model predictive control. arXiv preprint arXiv:2410.05364. Cited by: §I, §I-B. Appendix VII-A Necessity of Latent Space Safety Unlike simpler relative-to-base tracking methods, modern task-space Whole-Body Control paradigms (compatible with UMI [10] and FastUMI [56]) receive full end-effector trajectories expressed in global or TCP coordinate systems. These commands typically consist of multiple keyframes spanning the full 6 Degrees of Freedom (e.g., 6-DoF × 4 keyframes = 24-dimensional space). This dramatic increase in command dimensionality makes naive solutions to OOD robustness unviable: • Command-Space Constraints: The feasible region in this 24D space exhibits extreme sparsity and fragmentation. A minor modification to a coordinate may cause the arm to hit a singularity, necessitating an entirely distinct system-level solution (such as a 180-degree base reorientation) to track properly. Furthermore, temporal order matters significantly, as trajectories are fundamentally distinct from simple setpoints. For instance, successfully learning to track a trajectory forward does not imply the ability to track it in reverse. Consequently, direct (1)O(1) feasibility verification in the raw command space is intractable. • Curriculum Learning: Unlike simple setpoint reaching, trajectory tracking requires expert trajectories in RL as reasonable references. As visualized in Fig. 8a, prior works such as MLM [28] employ curriculum learning to acquire richer trajectories than UMI-on-Legs [18], but still fall far from full coverage of the command space. The mastered command distribution may seem dense in 3D space, but it remains extremely sparse in the 24D space. VII-B Visualization of Isomorphic Latent Space To empirically verify the spatial organization governed by our Isomorphic Latent Space (ILS), we utilized PCA to visualize the latent space of 3 random states (Fig. 8b). As theoretically modelled in Section IV-D, ILS establishes a dynamic inverse correlation such that low safety predictions induce expansive radii through a monotonically decreasing KL divergence boundary RtR_t. From the properties of high-dimensional Gaussians (z∼(0,Rt2I)z (0,R_t^2I)), probability mass concentrates heavily on spherical shells corresponding to their safety estimates, statistically aligning safety levels with geometry. Figure 8: (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. VII-C Proof of Lower Bound Property This section provides the theoretical proof from Section IV-C, verifying that the n-step expansion and the TD(λsafe _safe) training target constitute a strict lower bound of the true safety probability. VII-C1 Definitions Let (s)I(s) denote the safety indicator safe(s)I_S_safe(s). We define the Optimal Safety Value at state s as the maximum safety probability achievable from s: V∗(s)≜maxz∈Wπ(s,z).V^*(s) _z W^π(s,z). (11) The multiplicative Bellman equation (Eq. (3)) can be rewritten using this notation as: Wπ(st,zt)=(st)⋅st+1|st,zt[V∗(st+1)].W^π(s_t,z_t)=I(s_t)·E_s_t+1|s_t,z_t [V^*(s_t+1) ]. (12) VII-C2 The n-step Expansion Operator The n-step probability expansion operator (n)P^(n) is defined as the expected safety calculated using the rollout trajectory τ∼rolloutτ _rollout for the first n steps and assuming optimal control thereafter: (n)≜τ[(∏i=0n−1(st+i))V∗(st+n)].P^(n) _τ [ ( _i=0^n-1I(s_t+i) )V^*(s_t+n) ]. (13) For n=1n=1, this simplifies to (1)=Wπ(st,zt)P^(1)=W^π(s_t,z_t). VII-C3 Monotonicity Proof We now prove that the sequence is monotonically non-increasing, i.e., (n)≥(n+1)P^(n) ^(n+1). First, we expand V∗(st+n)V^*(s_t+n) using the Bellman equation: V∗(st+n) V^*(s_t+n) =maxz′((st+n)s′|st+n,z′[V∗(s′)]) = _z (I(s_t+n)E_s |s_t+n,z [V^*(s ) ] ) ≥(st+n)⋅st+n+1|st+n,zt+n[V∗(st+n+1)]. (s_t+n)·E_s_t+n+1|s_t+n,z_t+n [V^*(s_t+n+1) ]. (14) The inequality holds because the maximum over all z′z is inherently greater than or equal to the expectation over the specific intention zt+nz_t+n sampled from the rollout policy’s encoder. Substituting Eq. (14) back into the definition of (n)P^(n): (n) ^(n) ≥τ[(∏i=0n−1(st+i))(st+n)st+n+1[V∗(st+n+1)]] _τ [ ( _i=0^n-1I(s_t+i) )I(s_t+n)E_s_t+n+1[V^*(s_t+n+1)] ] =τ[(∏i=0n(st+i))V∗(st+n+1)] =E_τ [ ( _i=0^nI(s_t+i) )V^*(s_t+n+1) ] =(n+1). =P^(n+1). (15) This establishes the chain Wπ(st,zt)=(1)≥(2)≥⋯≥(n)W^π(s_t,z_t)=P^(1) ^(2)≥… ^(n), confirming that the n-step rollout provides a lower bound estimation. VII-C4 TD(λsafe _safe) Target and Tightness The training target GtλsafeG_t _safe is defined as the geometric weighted average of these n-step expansions: Gtλsafe=(1−λsafe)∑n=1∞λsafen−1(n).G_t _safe=(1- _safe) _n=1^∞ _safe^n-1P^(n). (16) Since (n)\P^(n)\ is a monotonically non-increasing sequence, the convex combination is upper-bounded by its first term: Gtλsafe≤(1)=Wπ(st,zt).G_t _safe ^(1)=W^π(s_t,z_t). (17) Thus, GtλsafeG_t _safe remains a strict lower bound of the true optimal safety. The choice of λsafe _safe involves a trade-off among four sources of error: • Propagation Delay: λsafe _safe determines the weight of multi-step returns. As λsafe→0 _safe→ 0, the target degenerates to single-step bootstrapping (st)V∗(st+1)I(s_t)V^*(s_t+1), leading to slow back-propagation of failure signals. Increasing λsafe _safe enables direct signal propagation across time steps via eligibility traces. • Variance: As λsafe→1 _safe→ 1, the target relies on longer stochastic rollout trajectories. Due to the randomness in environment dynamics and policy execution, the variance of the cumulative probability estimate increases significantly. • Estimator Bias: The term V∗(st+n)V^*(s_t+n) in (n)P^(n) relies on the approximation by the Safety Estimator network ω. A smaller λsafe _safe causes earlier bootstrapping, making the target highly dependent on the potentially inaccurate ω. • Lower Bound Gap: As λsafe _safe increases, the weight shifts towards (∞)P^(∞). This implies that the estimated value transitions from the “theoretical optimal safety” to the “on-policy safety,” which constitutes a more conservative lower bound. We select λsafe=0.8 _safe=0.8. This value strikes a balance between suppressing estimator bias and controlling sampling variance, while enabling rapid back-propagation of failure signals. VII-C5 Approximation of the Maximization (Eq. (5)) The substitution of maxz′Wπ(st+1,z′) _z W^π(s_t+1,z ) with the value at the latent origin W^ω(st+1,) W_ω(s_t+1,0) introduces an approximation. Theoretically, because maxz′Wπ(st+1,z′)≥Wπ(st+1,) _z W^π(s_t+1,z )≥ W^π(s_t+1,0), this substitution yields an even more conservative learning target Wtarget,t≤maxz′Wπ(st+1,z′)W_target,t≤ _z W^π(s_t+1,z ), reinforcing the theoretical lower-bound sequence proved above. Practically, this bound is tight (Fig. 8b showing the safety at origin is very close to peak). Initially, the KL divergence inherently tends to encode frequently executed actions near the origin. As the RL policy improves, these frequently chosen actions naturally correspond to relatively safe and competent behaviors. This serves as an initial training signal for the Safety Estimator. Then a virtuous training circle emerges: the initially trained estimator guides the Isomorphic Latent Space (ILS) to further concentrate safer intentions to the center. This in turn tightens the assumption maxz′Wπ(st+1,z′)≈W^ω(st+1,) _z W^π(s_t+1,z )≈ W_ω(s_t+1,0), which subsequently provides more accurate targets for the estimator, leading to progressively better training of both the estimator and the latent space. VII-D Quantitative Validation of Safety Estimator Our Safety Estimator W(s,z)W(s,z) intrinsically targets the metric formalised in Eq. (2): whether any command exists that can salvage the state from failure. Specifically, computing this exact conditional maximum maxW W analytically is computationally intractable, making ground truth comparisons practically impossible. To validate the estimator, we instead correlate the network’s predictions against human labels. In Table IV, we randomly sampled 500 rollout states from OOD executions. For each state, 5 independent human evaluators provided a binary vote: “Salvageable” (could the robot recover if the best possible command sequence were given?) or “Unsalvageable” (is a fall inevitable regardless of future inputs?). The Safety Estimator scores Wmax=W^ω(st+1,)W_ = W_ω(s_t+1,0) were logged. TABLE IV: Estimator Validation (500 random rollout states, 5 voters) Human Label Wmax<0.6W_ <0.6 0.6≤Wmax≤0.80.6≤ W_ ≤ 0.8 Wmax>0.8W_ >0.8 Salvageable 0.4% 9.2% 57.8% Unsalvageable 16.8% 14.4% 1.4% When the estimator evaluates that a state has low probability to be salvageable (Wmax<0.6W_ <0.6), the conditional probability that a human perceives the situation as unsalvageable reaches 16.8%/(16.8%+0.4%)≈97.7%16.8\%/(16.8\%+0.4\%)≈ 97.7\%. Conversely, when the estimator implies high certainty in safety (Wmax>0.8W_ >0.8), the probability that humans confirm it is indeed salvageable is 57.8%/(57.8%+1.4%)≈97.6%57.8\%/(57.8\%+1.4\%)≈ 97.6\%. This strong correlation validates that our Safety Estimator effectively captures the underlying safety semantics as perceived by human evaluators, confirming its practical utility for real-time safety assessment and correction in OOD scenarios. VII-E Dataset Generation Details To ensure the diversity and robustness of the learned policy, we curate a composite dataset derived from both real-world human demonstrations and procedural generation. All trajectories are unified to a fixed duration of T=2500T=2500 steps at 200 Hz (12.5 seconds). We define the global coordinate system such that the robot base initially faces the +x+x direction, with gravity acting along the −z-z axis. VII-E1 In-Distribution (ID) Dataset The training dataset consists of 7,000 trajectories, constructed from two primary sources: Augmented UMI Data We utilize the open-source dataset from UMI-on-Legs [18], specifically incorporating 1,090 “cup in the wild” trajectories and 101 “tossing” trajectories collected via the UMI [10] data collection pipeline. To balance the distribution, the tossing subset is oversampled by a factor of three. We sample 2,000 trajectories from this collected pool and apply rigid body transformations to augment the workspace coverage: • Centering & Translation: Trajectories are first centered at the origin, then translated along the x-axis by a random offset δx∼(−0.2,0.2) _x (-0.2,0.2) m. • Rotation: We apply a random yaw rotation θ∼(−30,30)∘θ (-30,30) around a pivot point defined at (−0.3,0,0)(-0.3,0,0) m relative to the robot base. This simulates variations in task orientation within the frontal workspace. Procedural Random Pushes To enhance the policy’s tracking capability across the full kinematic range, we generate 5,000 synthetic trajectories. • Position: The end-effector follows a random walk sequence. In the XY plane, each way-point is generated by moving a random distance d∼(0.1,0.5)d (0.1,0.5) m towards a direction ϕ∼(−45,45)∘φ (-45,45) relative to the forward (+x+x) axis, with a travel speed sampled from (0.01,0.4)U(0.01,0.4) m/s. The Z-axis movements vary independently within [0.02,0.6][0.02,0.6] m, with vertical speeds sampled from (0.01,0.2)U(0.01,0.2) m/s. • Orientation: Target orientations are generated by linearly interpolating between random Euler configurations (Roll∈[−30,30]∘Roll∈[-30,30] , Pitch∈[15,60]∘Pitch∈[15,60] , and Yaw∈[−45,45]∘Yaw∈[-45,45] ). The interpolation speed (angular velocity) is randomized for each segment, sampled from (0.01,1.0)U(0.01,1.0) rad/s. An independent ID test set is generated using the same protocol but with different random seeds. VII-E2 Out-of-Distribution (OOD) Datasets We evaluate generalization using two challenging variants, each containing 7,000 trajectories derived from the full ID dataset. OOD-Geometry (Rear Workspace) This dataset evaluates the agent’s competence in reaching targets completely outside the training distribution (specifically, behind the robot). We take the completed ID dataset as a base and re-apply the augmentation pipeline described above, sampling 7,000 times, but modifying the rotation parameter to sample θ∼(179,181)∘θ (179,181) . This effectively mirrors the entire distribution of frontal tasks to the rear of the robot, requiring significant whole-body reorientation. OOD-Sensor (Drift & Jumps) This dataset simulates severe state estimation failures such as VIO drift. We process every trajectory in the ID dataset by injecting discrete drift events at random intervals Δt∈[1,5] t∈[1,5] s. At each event, a persistent bias is added to the remainder of the trajectory: • Position Drift: Additive Gaussian noise δp∼(0,0.22) _p (0,0.2^2) m. • Orientation Drift: Multiplicative rotation noise derived from Euler angles δr∼(0,302)∘ _r (0,30^2) . VII-F Termination Conditions We largely follow the termination protocols established in UMI-on-Legs [18] to define the safety boundary safeS_safe. An episode is terminated immediately as a failure if any of the following physical safety constraints are violated specifically: • Invalid Body Contacts (Falls): A fall is detected if any risk-sensitive rigid body comes into contact with the environment (ground) with a force magnitude exceeding 1.01.0 N. The specific links triggering termination are: – Base & Torso: Base configuration links, Hip links, Thigh links, and the Head. – Manipulator Arm: All arm segments, including the Base Arm Link and Links 1-6. Note that the feet are explicitly permitted to contact the ground to support locomotion. Other operational constraints, such as joint limits, torque saturation, and action rates, are configured as soft constraints. Violations of these limits result in negative reward penalties rather than episode termination. VII-G Implementation Details All algorithms are implemented in PyTorch and trained on a single NVIDIA RTX A6000 GPU. The total training duration is approximately 2.5 hours for 2,000 iterations. VII-G1 Network Architectures We implement all network modules using Multi-Layer Perceptrons (MLPs). The detailed architectural configurations are summarized in Table V. TABLE V: Network Architectures Module Input Hidden Output Activation Encoder ϕφ st,gts_t,g_t [256,256][256,256] 6 ELU Policy π st,zts_t,z_t [256,256,256][256,256,256] 12+6 ELU Safety ω st,zts_t,z_t [256,256][256,256] 1 ELU+Sig* * ELU for hidden layers; Sigmoid for the output layer. VII-G2 Training Hyperparameters The training process involves separate optimizers for the policy and the Safety Estimator. We utilize a reward formulation consistent with UMI-on-Legs [18] for the PPO surrogate loss. A detailed summary of hyperparameters is provided in Table VI. TABLE VI: Training Hyperparameters Parameter Policy Opt. (π,ϕπ,φ) Safety Opt. (ω) Optimizer Adam Adam Num. Environments 4,096 Total Iterations 2,000 PPO Epochs 32 Mini-batches 4 Learning Rate (LR) Adaptive 1e-31e-3 Fixed 1e-31e-3 Discount (γ) 0.9 - GAE (λ) 0.95 - Clip Range (ϵε) 0.2 - KL Coef. (βKL _KL) 0→1k1e-30 1k1e-3 - Safety Disc. (λsafe _safe) - 0.8 Loss Function ℒPPO+βKLℒILSL_PPO+ _KLL_ILS ℒBCEL_BCE