Constrained Whole-Body Tracking for Humanoid Robots

arXiv:2606.00374 · cs.RO · Submitted 2026-05-29 · Read on arXiv

Listen

Transcript

Introduction to the show: ident: Robotics Radio. Generated commentary on the latest robotics and control papers.

Rosa: Today's paper: "Constrained Whole-Body Tracking for Humanoid Robots".

Dev: The gist: ConstrainedMimic is a control framework that leverages whole-body kinematics and dynamics for real-time constraint enforcement within RL tracking policies,

Rosa: First, who's behind it and why it matters.

Paper summary: Rosa: So we’re talking about this paper called Constrained Whole-Body Tracking for Humanoid Robots. Basically it addresses the problem that while reinforcement learning shows these humanoid robots can move around really well, making sure they stay safe and follow rules after they've been trained is still a big challenge.

Dev: Yeah, the core idea here is introducing this control framework called ConstrainedMimic which uses whole-body kinematics and dynamics to enforce constraints in real time while the reinforcement learning policy is tracking motion. It’s about making sure the robot doesn't violate any rules during operation.

Taro: So what does it claim to do for these humanoid systems, Rosa? Is it just patching things up, or is it a more fundamental change?

Rosa: It claims they use principles from operational space control and control barrier functions, or CBFs, to satisfy arbitrary runtime constraints on both the kinematic reference motion and the underlying dynamics. They show that you can enforce these constraints efficiently at frequencies above one kHz <ref:2606.00374#pg1>.

Dev: That high frequency is pretty important for real time performance because it means we can react quickly when things go wrong in the control loop. The paper demonstrates this using experiments on a simulated Unitree G1 with a learned policy, showing they can handle collision avoidance, joint limits, and center of mass stability constraints.

Rosa: And the main point is that by staying consistent with what the robot is currently doing and what it’s trying to track, these safety mechanisms only minimally restrict the policy's capabilities when those constraints are active. It doesn't shut down the learning entirely.

Taro: That sounds like a clever way to integrate safety without completely crippling the learning process, but how does that integration actually happen inside the system?

Dev: They identify two natural intervention points in an RL tracking stack: first, the input kinematic reference and second, the output joint commands. This creates three different ways to enforce constraints: constrained retargeting, kinematic filters, and dynamic filters.

Rosa: So you have these three methods for applying safety checks to the motion generation process itself. It sounds like they are checking things before the motion gets sent to the robot's motors.

Taro: Right, and for each of those methods, they build Control Barrier Functions on the full contact-constrained kinematics and dynamics of the humanoid. They ensure that whatever motion is chosen respects things like foot slip during contact mode changes.

Dev: The specific safety constraints they look at are pretty comprehensive: collision avoidance with both the robot body and external obstacles, workspace limits, joint limits, singularity avoidance, and stability constraints for the center of mass.

Rosa: When we look at their math on page two of Constrained Whole-Body Tracking for Humanoid Robots they show a constraint like Lfh(z)+Lgh(z)u≥−α(h(z)) which is used in an optimization-based controller or safety filter to keep the system safe <ref:2606.00374#pg1,Constrained Whole-Body Tracking for Humanoid Robots>.

Paper summary: Taro: That structure, using that barrier function h with respect to the state z as a differentiable function, allows them to build these constraints into a quadratic program or QP formulation. It’s how they turn those abstract safety goals into something solvable for the robot's control inputs u.

Dev: For example, when they look at whole-body kinematically constrained retargeting, they minimize something like the weighted squared error between the desired motion and what the kinematics allow, subject to that constraint equation.

Rosa: That means they are trying to generate a reference trajectory that is physically possible given all the robot's physical limits and contact conditions before it even reaches the policy layer. It’s a check on whether we can even reach that point safely.

Taro: Then they have kinematic safety filters which take motions already defined on the robot kinematics and compute a minimally invasive modification to that reference motion to maintain constraint enforcement. It's an adjustment, not a complete rewrite of the plan.

Dev: And finally, they have dynamic safety filters which analyze the output joint commands using a control-affine model of the contact-constrained and underactuated system dynamics. This filter is run at a high frequency, like two hundred fifty Hz in their experiments.

Rosa: That final dynamic layer is what seems to be the last line of defense before anything moves physically on the robot, checking the actual dynamics against those safety conditions. It’s running at five times the policy rate in those tests.

Taro: So, if you ask a person walking or driving what they'd ask about this paper, they might wonder if it works outside of a perfect simulation environment.

Dev: That’s exactly what Rosa is interested in: whether these results hold up when you take the system out of the lab and into the real world for an extended period.

Rosa: We need to see if this whole ConstrainedMimic framework can actually handle the messy, unpredictable nature of physical interaction beyond a controlled simulation setting.

Taro: And what it means for us is that we’re moving toward policies that aren't just agile in the lab, but are also fundamentally robust when dealing with real-world disturbances and unexpected situations.

Dev: The main implication is enabling safety for learning-based motion tracking where the constraints themselves might be unknown at training time. They show you can enforce arbitrary safety constraints on top of a pre-trained policy without needing to retrain it.

Rosa: So, the title Constrained Whole-Body Tracking for Humanoid Robots points directly to this idea of tracking while actively constraining the system's behavior in real time using these kinematic and dynamic tools.

Taro: It suggests that future autonomy won't just be about chasing a goal; it will also be about rigorously adhering to physical boundaries while pursuing that goal.

Dev: For control engineers, the focus on those high frequencies and the specific QP constructions for each filter gives us concrete things to build upon when designing real-time safety layers.

Rosa: And for field robotics, it shows a path toward deploying learning-based systems in environments where strict physical constraints matter more than just reaching a target.

Conclusion: Rosa: So we’re wrapping up our look at Constrained Whole-Body Tracking for Humanoid Robots. Essentially, this paper presents a way to make sure learning policies don't break things when they're running on real robots.

Dev: It’s about using whole-body kinematics and dynamics to keep constraints active in real time, even when the policy is making decisions.

Rosa: The authors are showing how you can satisfy those arbitrary runtime constraints on both the robot's movement and its underlying physics without needing to retrain the policy.

Dev: That means for a robot already trained, we can layer safety on top of it minimally invasively, at frequencies up to one thousand hertz.

Rosa: It’s about putting guardrails around learning-based motion tracking so the robot doesn't just crash when things get messy in the real world.

Dev: What this means for us is that we might actually be able to deploy these learning models in more physically demanding environments, where safety isn't a nice-to-have but a must have.

Rosa: It shifts the focus from just chasing a target to rigorously adhering to physical boundaries while you’re getting there.

Dev: It also shows how important those high loop rates are when you're dealing with contact constraints and underactuated systems.

Rosa: We’ll be looking at how robust this framework is when we take it out of the simulation and into actual hardware for extended periods.

Stanford University

cs.RO

Submitted: 2026-05-29

Updated: 2026-10-14

Comments: To be presented at the 2026 Conference on Robot Learning, Austin TX

Code: https://github.com/kevinzakka/mink

Project page: https://stanfordasl.github.io/constrainedmimic

License: http://creativecommons.org/licenses/by/4.0/

Importance score: 83/100

The gist: The gist: ConstrainedMimic is a control framework that leverages whole-body kinematics and dynamics for real-time constraint enforcement within RL tracking policies, enabling the satisfaction of

Key concepts

Control Barrier Functions (CBFs): CBFs are mathematical functions used in control systems to guarantee that a system stays within a safe region. They define a set of constraints, such as avoiding collisions or staying within joint limits. The framework uses CBFs to ensure the robot's motion respects these safety rules at every time step.
Whole-Body Kinematic-Constrained Retargeting: This method enforces safety during the initial reference generation phase by solving a constrained optimization problem based on inverse kinematics. It ensures that the desired reference motion respects complex physical constraints, like maintaining proper contact modes (e.g., preventing foot slip) while minimizing tracking error.
Whole-Body Dynamic Safety Filters: This final safety layer analyzes the robot's actual dynamics using a control-affine model to check for immediate dangers. It runs at a high frequency to compute minimal modifications to joint commands that prevent violations of dynamic constraints, such as center of mass stability or collision avoidance.
Task-Consistent Filtering Objectives: These are objectives used within the CBF framework that respect the implicit goals of motion tracking. They ensure that while safety is enforced, the robot still tries to achieve its primary task—following a desired trajectory—by balancing safety requirements with tracking accuracy.

Terminology

Control Barrier Functions (CBFs) — 1× in this paper · explained in 6 episodes
Reinforcement Learning — 1× in this paper · explained in 4 episodes
Reinforcement Learning (RL) — 1× in this paper · explained in 25 episodes

Summary

The gist: ConstrainedMimic is a control framework that leverages whole-body kinematics and dynamics for real-time constraint enforcement within RL tracking policies, enabling the satisfaction of arbitrary runtime constraints on both the kinematic reference motion and the underlying dynamics<ref:2606.00374#pg2>

How it works

ConstrainedMimic addresses safety in a learning-based motion-tracking stack by identifying two natural intervention points: the input kinematic reference, and the output joint commands <ref:2606.00374#pg3>. This leads to three methods for constraint enforcement: constrained retargeting, kinematic filters, and dynamic filters <ref:2606.00374#pg2>. For each method, the framework constructs Control Barrier Functions (CBFs) on the full contact-constrained kinematics and dynamics with task-consistent filtering objectives that respect the implicit task structure of motion tracking <ref:2606.00374#pg2>.

Safety Constraints

The paper defines safety constraints as a variety of conditions such as collision avoidance, workspace limits, joint limits, singularity avoidance, and stability constraints <ref:2606.00374#pg2>. These are incorporated into an optimization-based controller or safety filter through CBF constraints where the condition requires Lfh(z)+Lgh(z)u≥−α(h(z)) <ref:2606.00374#pg2>. Specific barrier functions used in experiments include:

Joint limits, defined as q−qmin qmax−q <ref:2606.00374#pg2>.
Collision avoidance, defined as the distance to collision between two bodies (i,j) in a set of collision pairs P with radii ri rj" <ref:2606.00374#pg2>.
Center of mass (CoM) stability, defined as b−AxCoM(q) <ref:2606.00374#pg2>.

Whole-Body Kinematically-Constrained Retargeting

This method enforces safety during the reference generation phase by constructing a CBF constraint within the differential inverse kinematics problem where minimize u Xnt i=1∥Wi(Jicu−νi)∥2 2 subject to Lfh(z)+Lgh(z)u≥−α(h(z)) <ref:2606.00374#pg2>. The constraints are built on the full contact-constrained kinematics or dynamics of the humanoid, such that the safe motion respects the contact mode (e.g., avoiding foot slip) <ref:2606.00374#pg2>.

Whole-Body Kinematic Safety Filters

For motions already defined on robot kinematics, a kinematic safety filter computes a minimally-invasive modification to the reference motion to maintain constraint enforcement <ref:2606.00374#pg2>. This is achieved by posing the CBF-QP as minimize q˙ WtJtc WnNtc(q˙ −q˙ nom) 2 subject to Lfh(z)+Lgh(z)u≥−α(h(z)) <ref:2606.00374#pg2>.

Whole-Body Dynamic Safety Filters

This final layer enforces safety on the output by analyzing dynamics using a control-affine model of the contact-constrained and underactuated system dynamics <ref:2606.00374#pg2>. The objective is posed as a QP minimizing Γbody WcJ¯T c S T WtJtcM−1S T WnM−1NT tc S T (Γbody−Γbody,nom) 2 subject to Lfh(z)+Lgh(z)u≥−α(h(z)) <ref:2606.00374#pg2>. This filter is run at a high frequency, such as 250 Hz (5x policy rate) in experiments <ref:2606.00374#pg2>.

Performance Analysis

Experiments on a Unitree G1 with policies like TWIST2 and SONIC show that while the base policy fails to maintain safety, adding both kinematic and dynamic filters most reliably leads to safe operation <ref:2606.00374#pg2>. The results consistently show that enabling constraints on both input and output levels most reliably leads to safe operation <ref:2606.00374#pg2>. For instance, the dynamic safety filter reduces constraint violation significantly due to operating at the final, lowest level of the stack <ref:2606.00374#pg2>. The paper concludes that ConstrainedMimic allows for enforcing arbitrary safety constraints (unknown at training time) on top of a pre-trained policy, in a minimally-invasive manner, and without any need for re-training <ref:2606.00374#pg2>.

Limitations and Future Work

Limitations include focusing primarily on single-step constraint enforcement as safety often requires higher-level logic (long-horizon or semantic reasoning) for discrete decisions <ref:2606.00374#pg2>. Future work will explore integration with onboard estimation for improved generality and quantify the effect of distribution shift from kinematic filters <ref:2606.00374#pg2>. The framework also does not currently account for fall-inducing disturbances or fully integrate with onboard perception data <ref:2606.00374#pg2>.

Acknowledgments

Daniel Morton was supported by a NASA Space Technology Graduate Research Opportunity, and Pranit Mohnot was supported by a NSF Graduate Research Fellowship <ref:2606.00374#pg2>. Thanks to Yanjie Ze and William Chong for helpful discussions throughout the project <ref:2606.00374#pg2>.

References

[1] Y. Ze, S. Zhao, W. Wang, A. Kanazawa, R. Duan, P. Abbeel, G. Shi, and J. W. C. K Liu., Twist2: Scalable, portable and holistic humanoid data collection system., arXiv preprint arXiv:251102832,, 9

[2] Q. Liao, T E Truong, X Huang, Y Gao, G Tevet, K Sreenath, and C K Liu., Beyondmimic: From motion tracking to versatile humanoid control via guided diffusion., arXiv preprint arXiv:250808241,, 9

[3] Z Luo, Y Yuan, T Wang, C Li, S Chen, F Castaneda, Z A Cao, J Li, D Minor, Q Ben, X Da,, Sonic: Supersizing motion tracking for natural humanoid whole-body control., arXiv preprint arXiv:251107820,, 9

[4] Y Ze, Z Chen, J P Araujo, Z a Cao, X B Peng, J Wu, and K Liu., Twist: Teleoperated whole-body imitation system., In J Lim, S Song, and H W Park editors,, Proceedings of The 9th Conference on Robot Learning,, 2143–2154,, 9

[5] T He, Z Luo, X He, W Xiao, C Zhang, W Zhang, K Kitani, C Liu., Omnih2o: Universal and dexterous human-to-humanoid whole-body teleoperation and learning., In Conference on Robot Learning,, 9

[6] Q Ben, F Jia, J Zeng, J Dong, D Lin, and J Pang., HOMIE: Humanoid Loco-Manipulation with Isomorphic Exoskeleton Cockpit., In Proceedings of Robotics: Science and Systems,, LosAngeles, CA,, June 2025., 9

[7] T He, W Xiao, T Lin, Z Luo, Z Xu, Z Jiang, J Kautz, C Liu,, Hover: Versatile neural whole-body controller for humanoid robots., In 2025 IEEE International Conference on Robotics and Automation (ICRA), pages 9989–9996,, 10

[8] A D Ames, S Coogan, M Egerstedt, G Notomista, K Sreenath, and P Tabuada., Control barrier functions: Theory and applications., In 2019 18th European Control Conference (ECC), pages 4542–4548,, 10

[9] S C Hsu, X Xu, and A D Ames., Control barrier function based quadratic programs with application to bipedal robotic walking., In 2015 American Control Conference (ACC), pages 827–834,, 10

[10] Q Nguyen, A Hereid, J W Grizzle, A D Ames, and K Sreenath., 3d dynamic walking on stepping stones with control barrier functions., In 2016 IEEE 55th Conference on Decision and Control (CDC), pages 827–834,, 10

[11] C Khazoom, D Gonzalez-Diaz, Y Ding, and S Kim., Humanoid self-collision avoidance using whole-body control with control barrier functions.

Improvements for AI systems

Improved AI systems can maintain safety during deployment by integrating ConstrainedMimic, a runtime safety framework for learned whole-body motion-tracking policies. This allows policies to handle arbitrary runtime constraints on both the kinematic reference motion and the underlying dynamics while remaining minimally invasive.
The system can enforce specific physical limits by utilizing three enforcement methods: constrained retargeting, kinematic filters, and dynamic filters, which are constructed with CBFs on the full contact-constrained kinematics and dynamics. This enables real-time enforcement of constraints such as collision avoidance (both with the robot body and external obstacles), joint limits, and center of mass stability constraints.
The AI system can handle complex failure modes by combining kinematic and dynamic safety checks. By employing both methods, the system can achieve superior performance: enabling constraints on both input and output levels most reliably leads to safe operation, as demonstrated by the performance comparison in Table 1 showing a reduction in violation frames compared to individual methods.
The robot can adapt its motion when encountering unforeseen obstacles or required contact mode changes. The framework addresses this by proposing a heuristic: in parallel with the whole-body CBFs, enforce an obstacle-avoidance CBF on a reduced-order model of the planar locomotion dynamics and then pass the safe velocity to a lower-body kinematic planner when necessary.
The system can operate effectively on resource-constrained hardware with high fidelity. The methods are fully differentiable and can be deployed at up to 300-500 Hz, running on CPU, GPU, and TPU, which unlocks the ability to enforce safety at higher rates than the policy while maintaining computational tractability.

Abstract

Recent advances in reinforcement learning (RL) have demonstrated impressive whole-body agility for humanoid robots, yet ensuring safety and satisfying constraints -- particularly those specified after training -- remains a challenge. Towards this goal, we present ConstrainedMimic, a control framework that leverages whole-body kinematics and dynamics for real-time constraint enforcement within RL tracking policies. By integrating principles from operational space control and control barrier functions, we enable the satisfaction of arbitrary runtime constraints on both the kinematic reference motion and the underlying dynamics, with efficient computation at frequencies above 1 kHz. In whole-body motion-tracking experiments in simulation and hardware, we demonstrate collision avoidance (both with the robot body and external obstacles), joint limits, and center of mass stability constraints. By remaining consistent with the current contact mode and tracking objectives, we minimally restrict the capabilities of the policy when constraints are active. Project page: https://stanfordasl.github.io/constrainedmimic.