💻 computer science

Constrained Whole-Body Tracking for Humanoid Robots

This paper introduces ConstrainedMimic, a real-time, differentiable control framework that integrates operational space control and control barrier functions into reinforcement learning policies to enforce arbitrary safety constraints—such as collision avoidance and joint limits—on humanoid robots without significantly compromising their tracking performance.

Original authors: Daniel Morton, Pranit Mohnot, Marco Pavone

Published 2026-06-02

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Daniel Morton, Pranit Mohnot, Marco Pavone

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a humanoid robot as a highly talented, agile dancer who has learned to mimic human movements through trial and error (a process called Reinforcement Learning). This dancer is incredibly fast and can do backflips or complex teleoperation tasks. However, there's a problem: the dancer is so eager to move that they might accidentally trip over their own feet, bump into a table, or twist a joint in a way that breaks it. They learned to dance, but they didn't learn the rules of "don't hit things" or "stay balanced."

The paper introduces a new system called ConstrainedMimic. Think of this system as a super-vigilant safety coach standing right next to the dancer. This coach doesn't teach the dancer new moves; instead, they watch every move in real-time and gently nudge the dancer's limbs just enough to keep them safe, without ruining the dance.

Here is how the system works, broken down into simple concepts:

1. The Two Places to Apply the Safety Coach

The paper explains that you can apply this safety check at two different points in the robot's "brain":

The Input (The Plan): Before the dancer even starts moving, the coach looks at the "script" (the motion plan coming from a human or a computer). If the script says, "Cross your arms in a way that will hit your own face," the coach edits the script before the dancer sees it. This is called Constrained Retargeting.
The Output (The Action): Sometimes the script is fine, but the dancer gets too excited and moves too fast, causing them to overshoot and hit something. In this case, the coach waits until the dancer is about to move, then applies a "brake" or a "steering correction" to the actual muscle commands. This is called a Dynamic Safety Filter.

2. The "Invisible Wall" Analogy

The core technology behind this coach is something called Control Barrier Functions (CBFs).

Imagine the robot is walking through a room filled with invisible, rubbery walls.

Collision Avoidance: If the robot gets too close to a table (or its own arm), the rubber wall pushes back. The coach calculates exactly how much to push to keep the robot from touching the table, but no more.
Joint Limits: Imagine the robot's elbow has a rubber band that stops it from bending too far backward. The coach ensures the robot never pulls that band tight enough to snap.
Balance (Center of Mass): Imagine the robot is standing on a small, invisible platform (its feet). If the robot leans too far and its "center of gravity" starts to drift off the edge of the platform, the coach instantly shifts the robot's weight back to the center so it doesn't fall over.

3. Why Just One Check Isn't Enough

The paper tested this on a simulated robot (a Unitree G1) and found some interesting things:

The "Script" isn't enough: Even if you give the robot a "safe" plan, the robot might still overshoot and crash because it moves too fast. It's like giving a driver a map of a safe route; if they drive too fast, they might still miss a turn.
The "Action" check is crucial: You need the coach to check the actual movement (the output) to catch those overshoots.
The Best Combo: The safest method is to use both checks. Edit the plan and correct the action. This is like having a co-pilot who fixes the map and grabs the steering wheel if the driver swerves.

4. The "Minimal Nudge" Rule

A key feature of this system is that it is minimally invasive.
Imagine the robot is trying to do a delicate task, like threading a needle. If a safety constraint activates (like avoiding a bump), the coach doesn't stop the robot entirely. Instead, it makes the smallest possible adjustment to the movement to keep it safe, letting the robot finish the task. It respects the robot's original goal as much as possible.

5. Speed and Real-World Use

The system is incredibly fast. It runs on standard computer chips (CPUs, GPUs, and even TPUs) at speeds of 300 to 500 times per second.

Why speed matters: If the coach is slow, the robot might crash before the coach can react. Because this system is so fast, it can catch mistakes that happen in a split second, making it safe enough for real-world use.

Summary of Results

In their tests, the researchers simulated scenarios like:

Self-Collision: The robot trying to cross its arms and hit its own face.
The "Karate Chop": A human hand moving fast toward the robot's face (a scenario the robot wasn't trained for).
Balance: The robot leaning so far it would fall.

The findings were clear:

Without the coach, the robot crashed or violated safety rules frequently.
With just the "Plan" check, it was better but still crashed sometimes due to speed.
With just the "Action" check, it was very safe but sometimes too cautious.
With both, the robot stayed safe in almost every single test, avoiding collisions and staying balanced while still performing the tasks.

In short, ConstrainedMimic is a way to take a super-agile, learning-based robot and instantly give it a "safety instinct" that it can't learn on its own, ensuring it doesn't hurt itself or others, all without needing to retrain the robot or slow it down significantly.

Technical Summary: Constrained Whole-Body Tracking for Humanoid Robots

Problem Statement
Recent advances in reinforcement learning (RL) have enabled humanoid robots to achieve impressive whole-body agility in locomotion, teleoperation, and manipulation. However, a critical gap remains in ensuring safety and satisfying constraints—particularly those specified after training (e.g., novel obstacles, dynamic environments, or specific operational limits). Existing RL policies often lack inherent guarantees for collision avoidance, joint limits, or stability when faced with unforeseen scenarios. An ideal runtime safety framework must be minimally invasive, policy-agnostic, easily configurable for arbitrary constraints, and capable of operating at real-time speeds (≥50 Hz) without requiring policy retraining.

Methodology: ConstrainedMimic
The authors propose ConstrainedMimic, a control framework that integrates principles from operational space control and Control Barrier Functions (CBFs) to enforce safety within RL tracking policies. The framework operates at two natural intervention points in the motion-tracking stack: the input kinematic reference and the output joint commands. It employs three distinct methods for constraint enforcement, all formulated as differentiable Quadratic Programs (QPs) using the JAX library:

Constrained Retargeting (Input Level):
- Applied during the inverse kinematics (IK) retargeting process where human motion is mapped to robot kinematics.
- A CBF is embedded directly into the differential IK problem.
- The objective minimizes tracking error while satisfying safety constraints (e.g., self-collision avoidance) via a contact-constrained kinematic model.
- This ensures the reference motion fed to the RL policy is inherently safe.
Kinematic Safety Filters (Input Level):
- Applied to references that are already retargeted (e.g., offline motions).
- Acts as a filter that computes a minimally invasive modification to the reference motion to satisfy constraints.
- Utilizes a hierarchical objective: primary tasks track end-effectors and Center of Mass (CoM) in operational space, while secondary tasks track posture in the null space.
- The CBF constraints are built on full contact-constrained kinematics to respect the current contact mode (e.g., preventing foot slip).
Dynamic Safety Filters (Output Level):
- Applied to the joint commands (PD targets) output by the RL policy, just before execution.
- This is the final layer of defense, operating on the full contact-constrained and underactuated system dynamics.
- The hierarchy prioritizes contact force tracking for grounded feet, followed by motion and posture tasks defined in terms of accelerations.
- The QP solves for safe actuator torques ( $\Gamma_{body}$ ), which are then mapped back to safe PD targets for the robot.

Key Contributions

Framework Architecture: Identification of safety enforcement points at both the input (kinematics) and output (dynamics) of the policy, proposing a unified framework (ConstrainedMimic) that combines constrained retargeting, kinematic filters, and dynamic filters.
Task-Consistent Filtering: Construction of CBFs on full contact-constrained kinematics and dynamics, paired with QP objectives that respect the implicit task structure of motion tracking. This ensures that when constraints are active, the filter minimally restricts the policy by modifying only the lowest-priority components.
Differentiability and Performance: The entire system is fully differentiable and implemented in JAX, allowing it to run on CPU, GPU, and TPU. It achieves frequencies of 300–500 Hz, significantly exceeding typical policy frequencies (50 Hz), enabling safety enforcement at higher rates than the control loop.
Zero-Shot Adaptation: The framework allows for the enforcement of arbitrary runtime constraints on pre-trained policies without retraining.

Experimental Results
Experiments were conducted in simulation on a Unitree G1 humanoid using unmodified pre-trained policies (TWIST2 and SONIC).

Baseline Limitations: Unconstrained policies failed to satisfy safety constraints even for scenarios known at training time (e.g., self-collision) and failed to prevent collisions with external obstacles due to dynamic overshoot.
Effectiveness of Layers:
- Kinematic-only methods reduced constraint violations significantly but could not prevent violations caused by dynamic overshoot (e.g., a "karate chop" scenario where the robot overshoots a safe reference).
- Dynamic-only filters operating at the torque level significantly reduced violations but could still exhibit minor violations due to the complexity of the dynamics.
- Combined (Kinematic + Dynamic) enforcement provided the most reliable safety, reducing violation frames to near-zero (e.g., 0.02% for self-collision) and minimizing maximum violation distances.
Contact Constraints: The study highlighted that ignoring contact constraints in the CBF formulation (e.g., assuming unconstrained kinematics) leads to infeasible motions (e.g., feet leaving the ground) or constraint mismatches. The proposed method correctly accounts for contact modes.
Limitations Observed:
- Short-Horizon Failure: Single-step CBFs struggle with discrete mode changes required for long-horizon safety (e.g., deciding to "crouch" vs. "limbo" under an obstacle).
- Contact Mode Breaking: Maintaining safety against dynamic obstacles may require breaking the current contact mode (e.g., initiating a step), which requires higher-level discrete decision-making not fully automated in the current framework.

Significance and Claims
The paper claims that ConstrainedMimic bridges the gap between the agility of learning-based control and the safety guarantees of model-based control. By treating RL policies as "foundation models" and overlaying a runtime safety framework, the authors demonstrate that:

Safety constraints unknown at training time can be enforced without retraining.
The framework handles both dynamic feasibility and large numbers of non-convex state constraints in real-time.
The approach is computationally tractable and scalable, running efficiently on edge devices.

The authors remain modest regarding limitations, acknowledging that the current system relies on strong assumptions about state estimation and contact modes, and that higher-level semantic reasoning (e.g., deciding when to step) remains a target for future work. They also note that while dynamic filters are minimally invasive, they may potentially lead to out-of-distribution (OOD) states, a trade-off they plan to investigate further.