Overview
- a humanoid arm is mounted on a moving base (pelvis or torso), so a target fixed in the world drifts in the base frame every time the body sways or steps
- re-anchoring recomputes the arm target in the base frame every control tick, from the live base pose
- this cancels body sway at the hand, as long as the arm loop is faster than the sway and the base pose estimate is clean
Mathematical Formulation
Frames
| Frame | Meaning |
|---|---|
| world (odometry) frame | |
| base frame where the arm IK is solved (pelvis, or torso after the waist joints) | |
| camera frame | |
| goal (object or grasp) frame |
Core equation
The target is stored in the world frame. Each tick, express it in the base frame:
with the homogeneous inverse
If the arm root sits after the waist joints :
Re-anchoring a whole trajectory
Plan the Cartesian trajectory in the world frame (S-curve or quintic for position, SLERP for orientation), then re-express each sample:
Velocity feedforward
With base linear velocity and angular velocity in the world frame, the desired hand twist seen from the moving base is:
Without this term the IK only sees position error, so it always lags the sway by one loop.
Where the target comes from
A camera detection at time gives . Store it in the world frame using the estimate at that time:
Re-anchoring at a later time then gives:
Only the relative motion matters. A global offset in the estimator cancels, and the error equals the drift since detection, so short term odometry quality is what counts.
Latency compensation
If the estimate is seconds old, predict it forward with the base twist :
where is the matrix form of the twist.
World frame vs base frame planning
| Plan in | Hand behaviour when the body sways | Good for |
|---|---|---|
| world frame + re-anchoring | stays still in the world | grasping, placing, touching a fixed object |
| base frame | moves with the body | gestures, carrying an object close to the body |
Module vector layout
| Signal | Symbol | Dim |
|---|---|---|
| base position | 3 | |
| base orientation | 4 | |
| base linear velocity | 3 | |
| base angular velocity | 3 | |
| world target pose | 7 | |
| world target twist | 6 | |
| output: base frame pose | 7 | |
| output: base frame twist | 6 |
Input , output .
Algorithmic / Architectural Flow
flowchart LR CAM["Camera detection T_CG at t0"] --> W["Store T_WG with T_WB at t0"] W --> TG["World frame trajectory T_Wd(t)"] EST["State estimator T_WB, twist"] --> PR["Predict forward by latency"] PR --> LP["Low pass and jump guard"] LP --> RA["T_Bd = inv(T_WB) T_Wd"] TG --> RA RA --> CH{"Reachable?"} CH -- yes --> IK["IK / QP for q_U des"] CH -- no --> HOLD["Hold, clamp or ask legs to step"]
Per control tick:
def tick(t):
T_WB, xi_B = estimator.pose_and_twist()
T_WB = predict(T_WB, xi_B, latency) # latency compensation
T_WB = smoother.update(T_WB) # remove foot impact spikes
T_Wd, V_Wd = traj.sample(t) # world frame S-curve + SLERP
T_Bd = inv(T_WB) @ T_Wd
V_Bd = twist_to_base(V_Wd, T_WB, xi_B)
if not workspace.contains(T_Bd):
return hold_or_request_step()
return ik.solve(T_Bd, V_Bd, q_U)Sim-to-Real & Hardware Failure Modes
Things that break on hardware:
- foot impact jitter: each heel strike puts a spike into , re-anchoring passes it straight to the hand, low pass the base pose and accept a little lag
- estimator jumps: an EKF correction or a map relocalisation makes jump, which turns into a step in , pass the re-anchored target through a jerk limited online filter before IK
- yaw drift: IMU and leg odometry drift slowly in yaw, which rotates a far target around the robot, re-detect the object before the final approach
- timestamp mismatch: using the camera pose from now with a detection from 100 ms ago places the target wrong by the sway in that time, look up the transform at the detection stamp
- frame mix up: pelvis link, torso link and the URDF base link are different frames, and quaternion order may differ between libraries
- coupled oscillation: the arm cancels pelvis sway, its reaction moves the pelvis again, and the loop can ring near the gait frequency, keep the arm bandwidth well above the sway frequency and add damping, or re-anchor only in the final approach
- target leaves the workspace: a big sway or a step moves the base so far that is out of reach, the IK then saturates at a singular pose, check reach first and hold or ask locomotion to step closer
Cross-Links
- why the pelvis moves at all: Upper-Lower Body Decoupling
- transform lookup at a given timestamp: Coordinate Frames (tf2)
- where comes from: Extended Kalman Filter (EKF)