Teaching Whole-Body Robots to Improve Safely from Their Own Safety Filter

Robots that learn by copying human demonstrations, a method called imitation learning, can only be as good as the demonstrations they copy. Getting past that ceiling means letting the robot try new actions and learn from the results. For a mobile manipulator, a robot whose wheeled or legged base, torso and arm move together as one body, those new actions can tip the base, slam the arm into furniture, or overload a joint. We propose a way to let such a robot explore beyond its demonstrations without a human hovering over an emergency stop. The robot proposes small corrections to what its imitation policy would do. A conventional whole-body controller, the classical layer that converts desired hand motions into coordinated base and arm motions, enforces hard physical limits on those corrections. The central idea concerns what happens when that controller has to clip, slow, or redirect a command. Each such event is recorded as a "near miss" and attributed to the body part whose limit was hit, and that record becomes the training signal for a learned safety model. The robot learns which of its own ideas the controller would have had to fight. Over time it stops proposing them, and it gets the information it needs to learn about danger without actually crashing.

Two other routes are worse. Pure trial-and-error learning needs too many risky interactions on hardware. Bolting a safety filter onto a learner without feedback has the opposite problem: the robot freezes in front of obstacles and never learns why. Turning the filter's interventions into labeled, per-limb cost data targets both problems at once. It addresses the scarcity of safety-relevant data that cripples learned safety models, and it addresses the lack of any way to tell whether the base or the arm was responsible for a risky situation.

Core Claim

Self-improvement beyond an imitation ceiling on a whole-body mobile manipulator can be made autonomous, safe during training, and sample-efficient if three things hold. First, exploration consists of bounded residual edits to action chunks (short blocks of future actions) proposed by a frozen imitation policy. Second, an analytical whole-body controller projects every command onto a set of hard instantaneous constraints. Third, the size of each projection, split by body segment, is used as a dense cost label to train segment-specific cost critics, which are models that predict accumulated future near-miss cost. The critics then restrict which edits are proposed at all.

Concretely, let $a$ be the commanded whole-body action, $\Pi(a)$ its projection by the controller onto joint, torque, support-margin and collision constraints, and $a_k$ the components belonging to segment $k$ (base, torso, arm). The near-miss cost for segment $k$ is

$$c_k(s,a) = \lVert a_k - \Pi(a)_k \rVert + \beta \,\mathbb{1}[\text{a segment-}k\text{ constraint is active}],$$

where $s$ is the state and $\beta$ a fixed weight. The attribution follows constraint ownership. Support-polygon and tipping constraints are charged to base and torso, reach and joint limits to the arm, and a collision constraint to the segment whose link would collide. At each control step the policy picks, among $N$ candidate chunks $\tilde a = a_{\text{BC}} + \delta$ formed from frozen-policy proposals plus residuals $\delta$,

$$\tilde a^\star = \arg\max_{\tilde a} Q_r(s,\tilde a) \quad \text{s.t.} \quad \bar Q_{c,k}(s,\tilde a) \le b_k \ \text{ for every segment } k,$$

where $Q_r$ is the task value critic, $\bar Q_{c,k}$ is an upper confidence estimate of segment $k$'s discounted near-miss cost from an ensemble, and $b_k$ is a per-segment budget. If no candidate satisfies every constraint, the robot executes the unedited imitation proposal through the controller.

The near-miss cost is not the true safety cost. It is a conservative surrogate that is non-zero well before a real violation would occur. That is why it remains plentiful even as the policy becomes safe.

Why This Is Not Already Done

The closest work splits along exactly the lines this proposal joins. Hierarchical Value-Decomposed Offline RL for Whole-Body Control (HVD) learns task values split into base, torso and arm critics from the imperfect 50-hour WB-50 dataset. It raises Wipe Board success from 18% for the π0 vision-language-action baseline to 32%. It is offline only and has no cost model; we reuse its decomposition idea but apply it to safety cost and to online data collection.

Two papers show how to improve on a frozen imitation policy. Beyond Imitation: Off-Policy Q-Planning freezes the imitation policy and reranks its sampled action chunks with a learned critic, taking real bimanual stack-cups from 40% to 90%. Its authors state that it cannot reach actions the frozen policy never proposes. EXPO-FT learns residual edits to those chunks and reaches 100% success in 19.1 minutes on average. It does this only on fixed-base arms, with a human holding a SpaceMouse to catch dangerous actions. We adopt EXPO-FT's residual edits to escape Q-planning's support limit, and we replace the human with the controller plus learned per-segment cost critics.

Two papers supply the safety machinery we adapt to high-dimensional bodies. Safe Exploration via Policy Priors (SOOPER) achieves zero training violations on a physical race car by handing control back to a safe prior whenever predicted cost becomes uncertain. We borrow that fallback logic. Breaking Safety Paradox with Feasible Dual Policy Iteration shows that learned safety boundaries degrade as violations vanish from the replay buffer. This is the observation that motivates near-miss labels.

HoMeR supplies the action interface. Its policy outputs end-effector targets and an analytical whole-body controller tracks them, which gives 79.17% success from 20 demonstrations. That controller omits collision constraints and cannot self-improve. Robotic Table Wiping via RL and Whole-Body Trajectory Optimization does enforce collisions in a whole-body optimizer, but only beneath a policy that chooses fixed wipe primitives, and the optimizer never feeds back into learning. No paper in the file uses a controller's interventions as segment-attributed training labels.

Research Program

The first component is the system. We take a HoMeR-style mobile manipulator stack in which an imitation policy (a diffusion policy, i.e. a generator that produces actions by iteratively denoising noise, following Diffusion Policy) outputs end-effector and base targets in 8-step chunks. We extend its kinematic controller into a quadratic program with four sets of constraints: joint and velocity limits, a quasi-static support-margin constraint on the projected center of mass, signed-distance collision constraints against a voxelized scene from depth, and torque bounds from a rigid-body model. On top of this sit a residual edit policy, a task critic $Q_r$ trained with categorical (HL-Gauss) regression as in Q-planning, and an ensemble of per-segment cost critics. The task critic and cost critics are pretrained offline on WB-50, with subtask rewards and near-miss costs relabeled by replaying each logged command through the controller. This step costs nothing on hardware, and that is part of the sample-efficiency argument.

The second component is the online loop. The robot runs autonomous episodes with residual radii initialized small. A segment's residual radius grows only where the ensemble disagreement on that segment's cost is low, which follows SOOPER's logic of expanding only where uncertainty is low, applied separately per limb. A held-out "true violation" monitor counts real events: support margin below a hard threshold, unplanned contact force above a threshold, or torque saturation. These counts are used only for evaluation and never for training. This separation lets us measure whether the near-miss surrogate actually keeps real violations at zero.

The third component is analysis. We test two hypotheses. The first is that per-segment cost critics catch delayed hazards that a scalar critic blurs. For example, the base parks at a pose from which the arm can complete the reach only by approaching the tipping margin. The second is that near-miss labels keep the cost critic accurate late in training, when real violations no longer occur. For the second, we track the cost critic's calibration on a fixed probe set of risky states across training. The prediction is that a critic trained on real violations alone degrades as violations disappear from the buffer, as the safety-paradox paper predicts, while the near-miss critic does not.

Baselines

The naive reference is the frozen imitation policy executed through the same constraint-enforcing controller. This is HoMeR's recipe with our added collision and support constraints. It defines the ceiling we claim to break and the controller-only safety level.

The strongest existing self-improvement method is Q-planning (Beyond Imitation) adapted to whole-body control. It reranks $N$ frozen-policy chunks with $Q_r$ and executes them through the same controller. It gets the same WB-50 pretraining, the same $N$, the same critic architecture, and the same number of autonomous robot-minutes. Its reported result is 40% to 90% on real stack-cups, with Filtered SFT stalling at 55%. That result comes from a fixed-base task, and no reported result was found on Lacuna for Q-planning on any whole-body task. Beating it means a higher final success rate at equal robot-minutes on tasks where its success plateaus, with real violations no higher. We specifically expect this on tasks whose demonstrations never include the needed body posture, such as leaning the base in to reach a far shelf edge.

The strongest offline method is HVD trained on WB-50. Its reported result is 32% versus 18% for π0 on Wipe Board. Beating it means the online loop exceeds 32% on Wipe Board after a bounded number of autonomous hours. We will also report HVD at equal total data. No reported HVD number was found on Lacuna for BEHAVIOR-1K, so there we run it ourselves.

The most important ablations isolate the key ingredient. The first is EXPO-FT-style residual edits behind the controller with no cost critic, where the filter replaces the human. The second uses a single scalar cost critic in place of the per-segment ones. The third trains the cost critics only on real violations instead of near-miss labels. The fourth removes the fallback to the frozen policy. All variants share the controller, the data, and an equal hyperparameter-tuning budget of a fixed number of simulated sweeps before any hardware run.

Why not just…?

Why not just put the safety filter on a residual learner and skip the cost critic? This is the most serious threat. The first ablation above is exactly this system. The filter alone guarantees the same instantaneous constraints. What it cannot do is anticipate states from which every continuation forces heavy clipping. The on-ramp merging work (RL with MPC for On-Ramp Merging) found that MPC overrides tend toward conservatism and dead ends. It also found that the RL agent learned nothing about safety from them, crashing 85.1% of the time when the supervisor was removed under unfamiliar traffic. If the filter-only ablation matches our method on success, freeze rate, and real violations, the learned cost critics are unnecessary and we will report that. The decisive measurements are the freeze rate (episodes in which the controller reduces motion to near zero for more than a set time) and success when the controller is tightened at test time.

Why not just add the near-miss cost to the reward as a penalty? This alternative uses our own signal, so it is a fair challenge. Safety-Gymnasium (Safety-Gymnasium) documents how penalty weights swing between a robot that stands still and one that accepts large risks. A single penalty also mixes limbs together. We run this alternative with a sweep over penalty weights. It counts as a real competitor if any single weight matches our method across all tasks without being retuned per task.

Why not just collect more teleoperated demonstrations? Whole-body teleoperation of 21 or more joints is exhausting, according to HVD, and imitation cannot learn from failures. Filtered SFT stalled at 55% in Q-planning's experiments. We test this directly. We give imitation alone extra demonstrations equal in operator wall-clock time to our method's total human time, which covers resets only, and compare success rates.

Why not just train in simulation and transfer? We do pretrain in simulation, but transfer alone is weak. BEHAVIOR-1K reports about 22% real-world success for a simulation-trained Tiago mobile manipulator (BEHAVIOR-1K). Our loop is meant to run after transfer, and we report sim-only performance as a reference point.

Proposed Experiments

In simulation we use the HoMeR simulated tasks (cube picking, dishwasher opening, cabinet opening) and a subset of BEHAVIOR-1K activities in OmniGibson that require base, torso and arm coordination, with assistive grasping disabled. Simulation lets us count true violations densely and run many seeds. Offline pretraining and relabeling use WB-50. On hardware we run the five WB-50 tasks, including Wipe Board, on a whole-body mobile manipulator, with humans performing scene resets only.

For metrics, we report three primary quantities: final success rate; true violations per hour during training, which should be zero for our method; and autonomous robot-minutes needed to reach the Q-planning plateau. We also report freeze rate, intervention magnitude per segment over time, and cost-critic calibration on the fixed risky-probe set.

Three checks decide the outcome. The first is whether our method beats both the Q-planning plateau and HVD's 32% on Wipe Board without raising true violations above the frozen-policy-plus-controller reference. The second is whether the third ablation, trained on real violations only, loses critic calibration late in training while the near-miss version does not. The third is whether the per-segment critic outperforms the scalar one specifically on tasks with base-placement hazards. If the filter-only ablation ties us on all three, the contribution reduces to applying EXPO-FT to whole-body robots with a controller in place of the human operator. We would report it as that.