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

FrameMeaning
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 inHand behaviour when the body swaysGood for
world frame + re-anchoringstays still in the worldgrasping, placing, touching a fixed object
base framemoves with the bodygestures, carrying an object close to the body

Module vector layout

SignalSymbolDim
base position3
base orientation4
base linear velocity3
base angular velocity3
world target pose7
world target twist6
output: base frame pose7
output: base frame twist6

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