Constrained Whole-Body Tracking for Humanoid Robots
Where does safety fit into a learning-based humanoid motion tracking stack? We approach safety from both the kinematics and dynamics levels, addressing safety from both sides (input and output) of the policy. On the kinematics side, constraints can naturally fit into the IK-based retargeting process between human and robot form-factors, or be applied as a safety filter on motions already mapped to the robot geometry. On the dynamics side, a potentially unsafe output from the policy can be passed through a safety filter prior to execution on the robot.
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.
Overview
Purely kinematic notions of safety are insufficient for policies with tracking error
Given an unsafe input to a whole-body tracking policy (a kinematic reference), safety constraints can be applied
at this level either via a kinematic filter, or via constrained retargeting. While this filtered reference may
satisfy constraints, deploying it on the robot does not necessarily lead to safe behavior, due to not accounting
for inertial effects, overshoot, and general tracking errors of the underlying policy.
Here, we demonstrate a "karate chop" motion where we enforce a constraint between the hand and a planar surface.
Although kinematic safety methods can constrain an unsafe behavior (A) and produce a safe reference (B), tracking
this with a general motion tracker (C) leads to overshoot and constraint violation. Accounting for dynamics (D/E)
maintains safety, whether the original input was safe or not.
A safety filter is beneficial even for conditions known at training time
While a key benefit of an online safety filter is the ability to enforce runtime-specified constraints
that were unknown at training time, these filters are still useful for conditions that are known at
training time.
Consider self-collision avoidance: the full model of the robot kinematics and link geometry is known
during training, and often, self-collisions are heavily penalized in the reward function. Still, the
tracking policy provides no guarantee that this constraint is implicitly represented, and tracking an
unsafe reference (A) can still lead to collisions (B) without a filter (C).
Safety filters for humanoid whole-body control must account for the contact-constrained kinematics and dynamics
For safety filters to work properly, they must have an accurate model of the kinematics (for first-order, input-level
filters) and dynamics (for second-order, output-level filters). Naively enforcing safety without considering the
contact state, though, will result in a mismatch that leads to unsafe and unexpected behavior.
Consider an antigravity lean, which violates a center
of mass stability constraint. Unless we account for both the contact-constrained kinematics and a task-consistent
objective for this motion (E), we either get physically-implausible, unsafe, or undesired motions from the filtered
output (A-D).
Discrete kinematic modes impact the dynamic feasibility of single-step constraint enforcement
CBFs are a single-step method of enforcing safety, and some constraints inherently require longer-horizon reasoning
over discrete actions or modes.
Consider walking under an obstacle at head-height: if whole-body collision avoidance is enforced during the motion,
the resulting output is not necessarily dynamically-feasible, depending on the kinematic mode of the robot when
the constraint is active. A head-on approach leads to an unstable "limbo" motion, whereas approaching it from a
slight crouch can allow the robot to further duck under the obstacle, maintaining a stable posture throughout.
Certain constraints inherently require breaking the current contact mode to maintain safety
Consider a humanoid standing at rest with a dynamic obstacle approaching. With the feet fixed in a double-support
contact mode, the robot cannot locomote, inevitably leading to a collision! In cases like this, breaking the current contact mode
is necessary to maintain safety.
To address this, we propose a simple bilevel heuristic: a reduced-order planar dynamics model runs in parallel to
a whole-body safety method, with a lower-body planner (e.g. from SONIC) generating the full kinematic reference
to track both the desired upper-body motion and the safe planar motion.
On hardware, enforcing safety at both the kinematics and dynamics levels most reliably delivers a safe output, even under minor model mismatch.
On the Unitree G1 hardware, we recreate our "karate chop" experiment, seeking to understand how the results differ
during real hardware deployment. As in our simulation experiments, the kinematic filter alone leads to small constraint
violation, but under minor dynamics mismatch, the dynamic filter alone also leads to
minor violation (whereas in simulation, this was the least conservative approach that maintained safety).
In general, we find that kinematic and dynamic safety methods complement eachother for reliable constraint enforcement:
kinematics alone fails due to a lack of consideration of inertia and tracking error; dynamics alone fails due to imperfect
modeling and estimation at runtime; considering both effects together ultimately leads to safe behavior from the robot.
All methods run well above policy frequency, even on edge devices and with hundreds of constraints
Even with upwards of 300 constraints, our filters can be deployed at well over 1 kHz on desktop computers, and over 100 Hz on
edge devices such as an Orin Nano.
All software is publicly available, and try out our other software libraries that we use in our deployment stack!
BibTeX
@article{morton2026constrained,
author={Morton, Daniel and Mohnot, Pranit and Pavone, Marco},
title={Constrained Whole-Body Tracking for Humanoid Robots},
journal={arXiv preprint arXiv:2606.00374},
year={2026},
note={Accepeted to the Conference on Robot Learning (CoRL), 2026},
}