Precise \(SE(3)\) End-Effector Tracking
in Whole-Body Humanoid Control
Author names, affiliations, and the code repository are withheld until the review period ends.
All videos are recorded on hardware. Playback speed is stated in each caption where it is not 1×.
Overview
A three-minute overview of ResGAC on a Unitree G1: the method, then standing \(SE(3)\) tracking, arm holding while walking, peg-in-hole, and the Mime-man demo. Speed-ups are marked on screen in each clip.
TL;DR
ResGAC: A humanoid tracks \(SE(3)\) dual-arm end-effector poses while walking. A floating-base geometric admittance controller produces nominal arm joint targets from task-space pose feedback; residual RL adds a bounded correction in the same joint-position action space and controls the remaining joints for locomotion and balance. A ground-attached heading frame removes the kinematic transmission of pelvis roll, pitch, and heave to the end-effector while preserving planar motion. On hardware, ResGAC reaches 6.5 mm / 1.37° tracking error, beats every baseline we tested including SONIC, and succeeds in 90% of standing peg-in-hole trials against 50% for SONIC.
Abstract
Precise end-effector tracking during humanoid whole-body motion is challenging due to floating-base oscillations, gravity, dynamic coupling, and locomotion-induced disturbances. We propose ResGAC, a whole-body humanoid controller for precise end-effector pose tracking that combines geometric admittance control (GAC) with residual reinforcement learning. GAC provides structured \(SE(3)\) task-space feedback and generates nominal arm joint-position targets, while residual RL compensates for unmodeled dynamics and coordinates locomotion and balance in the shared joint-position action space. The left-invariant geometric formulation allows the same GAC law to be used across manipulation reference frames. This enables the use of a ground-attached heading frame that preserves planar locomotion while removing pelvis roll, pitch, and heave from the manipulation reference, thereby reducing reference-induced end-effector motion during locomotion. ResGAC is validated on a real Unitree G1 humanoid. Across four standing end-effector tracking benchmarks, ResGAC consistently outperforms representative baselines, including SONIC, achieving lower translational and rotational errors. Real-world experiments further demonstrate reduced propagation of pelvis motion to the desired end-effector pose using the proposed ground-attached heading frame. ResGAC achieves 90% success in a standing peg-in-hole task compared with 50% for SONIC, and accurate world-frame \(SE(3)\) end-effector pose tracking during lower-body motion.
Method
Figure 1: Precise \(SE(3)\) end-effector tracking in whole-body humanoid control. (a) ResGAC consists of a geometric admittance controller (GAC) that provides the nominal end-effector command and a residual RL policy that learns arm corrections while controlling the remaining whole-body joints. (b) The pelvis \(\{P\}\), heading \(\{H\}\), and ground-attached heading \(\{H_0\}\) frames: \(\{H\}\) removes pelvis roll and pitch, and \(\{H_0\}\) additionally removes heave, isolating planar \((x, y, \psi)\) motion from the manipulation task. (c) \(SE(3)\) end-effector pose tracking and manipulation tasks: arm-sweep, infinity, pick-and-place, peg-in-hole, and the Mime-man demo for world-frame end-effector tracking.
GAC is formulated on the floating base. Per control step it builds the geometric elastic wrench \(f_G\) from the \(SE(3)\) pose error, integrates a task-space admittance law, and maps the desired body-frame velocity to joint space through a damped pseudo-inverse with null-space regulation, yielding a nominal joint target \(a_{\text{GAC}}\). The policy adds a clipped residual on the arms and commands every other joint directly, so both act in the same joint-position action space and a zero residual recovers the nominal controller.
- Floating-base GAC. The relative Jacobian \(J^{E}_{RE}\) is constructed in the world-aligned Cartesian frame and transformed into the end-effector body frame, so the controller sees end-effector motion relative to the manipulation frame rather than to the world.
- Bounded residual in joint space. \(a_{\text{arm}} = a_{\text{GAC}} + s_r\,\mathrm{clip}(a_{\text{RL,arm}}, -c_r, c_r)\), with \(s_r = 0.75\), \(c_r = 1.0\), bounding the correction to \(\pm 0.75\) rad per arm joint. The nominal action is also fed to the actor, so the residual is conditioned on what it is correcting.
- Ground-attached heading frame \(\{H_0\}\). Yaw-only rotation with the origin projected to \(z=0\), removing roll, pitch, and vertical heave from the command while retaining the planar \((x,y,\psi)\) motion locomotion needs.
- Symmetry. Sagittal-plane reflection \(\mathcal{M} = \mathrm{diag}(1,-1,1)\) applied as replay augmentation and as explicit actor-equivariance and critic-invariance losses.
- No retargeted human motion data. Arm configurations are sampled, mapped through forward kinematics, and interpolated in \(SE(3)\) with randomized speeds.
Reward terms and final-stage weights — Table I, all 25 active terms
Total reward \(r_t = \sum_i w_i r_{i,t}\). \(\exp(\cdot)\) denotes an exponential kernel. Values are the final training stage; curriculum-dependent changes are in the curriculum section.
| Group | Term | Form | σ / Param. | \(w_i\) |
|---|---|---|---|---|
| Manip. | EEF position | \(\exp(L_2^2 + L_1)\) | 0.001 / 0.01 | 4.0 |
| EEF orientation | \(\exp(SO(3))\) | 0.02 | 3.0 | |
| Loco. | Linear velocity | \(\exp(L_2^2)\) | 0.25 | 2.0 |
| Angular velocity | \(\exp(L_2^2)\) | 0.25 | 1.5 | |
| Swing-foot trajectory | \(\exp(L_2^2)\) | 0.008 | 5.0 | |
| Pelvis orientation | \(\exp(L_2^2)\) | 0.04 | 1.0 | |
| Torso orientation | \(\exp(L_2^2)\) | 0.04 | 1.0 | |
| Base height | \(\exp(L_2^2)\) | 3.0 | 1.0 | |
| Reg. | Arm residual | \(L_2^2\) | — | −0.05 |
| EEF body twist | \(L_2^2\) | — | −0.05 | |
| Arm action rate | \(\lVert \Delta a\rVert^2\) | — | −0.5 | |
| Non-arm action rate | \(\lVert \Delta a\rVert^2\) | — | −1.0 | |
| Mean-action accel. | \(\lVert \Delta^2 \mu\rVert^2\) | — | −0.10 | |
| Arm joint velocity | \(L_2^2\) | — | −0.05 | |
| Pose deviation | Weighted \(L_2^2\) | — | −1.0 | |
| Joint-position limit | Linear excess | 95% | −1.0 | |
| Foot proximity | Threshold | 0.15 m | −10 | |
| Foot orientation | \(L_2\) | — | −5 | |
| Turn-in-place drift | Gated \(L_2^2\) | — | −2.5 | |
| Heading drift | Gait-avg. \(L_2^2\) | — | −10 | |
| Lateral drift | Gait-avg. \(L_2^2\) | — | −2 | |
| Standing yaw rate | Gated \(L_2^2\) | — | −2.5 | |
| Foot contact force | Threshold \(L_2^2\) | 400 N | −2 | |
| Pre-touchdown velocity | Gated \(L_2^2\) | 0.15 m/s | −2 | |
| — | Alive / termination | Indicator | — | 10 / −200 |
Every term is defined in full in §1 of the additional details.
Training curriculum — Table II, six stages, 201,911 iterations, ~16 h on one L40S
| A | B | C | D | E | F | |
|---|---|---|---|---|---|---|
| Task | Stand Fixed | Stand Traj. | Walk Fixed | Walk Traj. | Walk Traj. | Walk Traj. |
| Iterations | ≤ 10k | ≤ 10k | ≤ 20k | 50k | 60k | 60k |
| Cmd. range \((v_x,v_y,\omega_z)\) | 0 | 0 | .3/.3/.5 | .7/.7/.7 | .7/.7/.7 | .7/.7/.7 |
| \(\sigma_P\) | .02 | .01 | .02 | .005→.001 | .001/.01 | .001/.01 |
| \(\sigma_R\) | .20 | .10 | .10 | .10→.02 | .02 | .02 |
| \(\sigma_v\) | .25 | .25 | .25 | .25→.10 | .10 | .10 |
| Height cmd. range [m] | 0.73 | [.68, .78] | [.68, .78] | [.55, .80] | [.55, .80] | [.55, .80] |
| Init. \(q\) scale | [.9, 1.1] | [.85, 1.15] | [.85, 1.15] | [.7, 1.3] | [.7, 1.3] | [.7, 1.3] |
| \(w_{\text{rot}}\) | 2.5 | 2.5 | 2.5 | 2.5 | 3.0 | 3.0 |
| \(w_{\text{res}}\) | −.10 | −.10 | −.10 | −.10→−.05 | −.05 | −.05 |
| \(w_{\text{pose}}\) | −.5 | −.5 | −.5 | −1.0 | −1.0 | −1.0 |
| \(w_{\dot q,\text{arm}}\) | −.0125 | −.0125 | −.0125 | −.02 | −.05 | −.05 |
| \(w_{\text{twist}}\) | 0 | 0 | 0 | −.02 | −.05 | −.05 |
Stages A–C stop early once stable, hence the \(\le\) on their budgets. \(\sigma_P\) for stages E and F lists \((\sigma_2, \sigma_1)\) of the combined \(L_2^2 + L_1\) kernel. Arrows denote in-stage linear annealing, not a step between stages. Policy and optimizer checkpoints transfer sequentially between stages.
Controller gains and training setup — every number needed to rerun this
| Quantity | Value |
|---|---|
| Admittance mass \(M\) | \(\mathrm{diag}(1.5 I_3,\; 2.25 I_3)\) |
| Stiffness \(K_p = K_R\) | \(5000 I_3\) |
| Damping \(K_d\) | set so damping ratio = 2 |
| Damped pseudo-inverse \(\lambda_{\text{null}}\) | 0.0025 |
| Null-space gain \(k_{\text{null}}\) | 50 |
| Residual scale / clip \(s_r,\, c_r\) | 0.75, 1.0 |
| Joint-position action scale | 0.25 |
| Control rate (GAC and policy) | 50 Hz (sim 200 Hz, decimation 4) |
| Episode length | 20 s = 1000 steps |
| Parallel environments | 4096 |
| Hardware / wall-clock | 1× NVIDIA L40S, ~16 h |
| Algorithm | FastSAC, distributional critic |
| Actor / critic hidden dim | 512 / 768 |
| Total iterations (actual) | 201,911 |
| Deployment kinematics | pinocchio |
PD gains follow BeyondMimic. The critic additionally receives pelvis linear velocity, which is never provided to the actor during training or deployment. The nominal budget is 210k iterations; early stopping in stages A–C brought the actual run to 201,911. Full optimizer and network settings are on the additional details page.
Tracking Benchmarks
Four held-out reference trajectories, all evaluated while the humanoid maintains a standing posture so that tracking precision is isolated from locomotion-induced disturbance. None of these trajectories appears in the training curriculum.
Pick a trajectory: each tab shows all six controllers, then a head-to-head against SONIC-Teleop-v1.1.
