command_delay_comp_campaign

MPC command-delay compensation on TIAGo Pro — knot / ramp campaign

This campaign is an attempt to reproduce, on a TIAGo Pro arm, the command-delay compensation approach from:

R. Subburaman and O. Stasse, “Delay Robust Model Predictive Control For Whole-Body Torque Control of Humanoids”, IEEE-RAS International Conference on Humanoid Robots (Humanoids), Nancy, France, 2024. https://ieeexplore.ieee.org/document/10769888/

The paper studies the effect of computation delay in a whole-body MPC running underneath a fast torque-tracking loop (validated on the TALOS humanoid), and proposes lightweight compensation policies that shift or interpolate the published MPC solution to account for that delay, instead of trying to make the solve itself faster.

Here we run the same class of policies on a different robot and a different, much simpler task: TIAGo Pro’s right 7-DoF arm, free motion, no locomotion, no contact. The goal of this specific campaign is to check that these policies still behave sensibly on this stack, and to get a first, real (not simulated) read on their delay/tracking/chatter effect.

Control stack and OCP

Two rates in cascade:

• agimus_controller (Python, Crocoddyl) solves an OCP at ~100 Hz (dt = 0.01 s, horizon = 30 nodes, i.e. a 0.3 s look-ahead) and publishes the result on /control.
• The Linear Feedback Controller (LFC, C++) runs the joint-torque loop at ~1 kHz, tracking the published state/gains with a PD+Riccati feedback law in between two /control updates.

The state is the arm’s [q, v] (7+7).

Running-node costs:

• a quadratic state-regularization residual (weak prior toward the reference state),
• a control-regularization residual that penalizes the commanded torque’s deviation from the gravity-compensation torque g(q),
• end-effector frame translation and rotation tracking (the tool-holder frame), the dominant costs — task-space tracking takes priority over the state prior.

Terminal-node costs: the same state-regularization and frame translation/rotation tracking terms (no control cost at the terminal node). No constraint is active in this campaign (collision-avoidance constraints exist in the OCP definition but are all switched off here — this is a free-motion, obstacle-free test).

Command-delay compensation policies under test (the paper’s terminology): publishing the horizon’s knot 0 as-is (“u⁰”, naive), publishing knot 1 instead to pre-empt one MPC period of delay (“u¹”, apply_knot), and interpolating the LFC’s feedback reference toward the next knot instead of holding it between two /control updates (“u²”, reference_interpolation), alone or combined.

Recorded data

Four bags, TIAGo Pro right arm, free motion, no contact.

run	policy (paper)	apply_knot	reference_interpolation	recording	duration	per-bag plots
ramp_knot_baseline	u⁰	0	false	20260907_142848	4.2 s	ocp_brush · torque_per_motor
ramp_knot_u0_interp	u⁰ + reference smoothing, no knot shift	0	true	20260907_143536	6.6 s	ocp_brush · torque_per_motor
ramp_knot_k1	u¹	1	false	20260909_115010	7.4 s	ocp_brush · torque_per_motor
ramp_knot_u2	u²	1	true	20260907_143659	6.5 s	ocp_brush · torque_per_motor

ramp_knot_u0_interp isolates the effect of interpolating the feedback reference from the effect of shifting the published knot.

Results

staircase_qact_qdes.html — the 4 runs stacked, one joint at a time: does the real robot track the commanded reference tightly, and does interpolating the reference (u0_interp/u2) visibly smooth out the tracking compared to holding it (baseline/k1)?

torque_uff_ufb.html — one run at a time, all joints: how much of the commanded torque is the OCP’s own plan vs. the LFC’s fast reactive correction? A large, jittery correction term is what we call “chatter” — this is where to see whether a policy makes the motion rougher or smoother.

ocp_brush.html (one per bag) — every OCP solve’s predicted horizon, all overlaid, against what actually happened: do the predictions consistently land close to reality, or drift off in some direction? Generated for every bag for completeness, not the focus of this campaign.

torque_per_motor.html (one per bag) — same question as torque_uff_ufb.html, but as a per-bag sanity check: does the commanded torque reconstructed from /control agree with what /lfc_debug reports?

run	x0-age at /control (mean / median)	OCP solve time (mean, max)	peak \|q_act-q_stair\|	RMS \|u_fb\|
baseline (u⁰)	7.16 / 6.56 ms	5.6 ms, 10.2 ms	0.025 rad (j5)	0.63 N·m
u0_interp	7.56 / 7.34 ms	6.2 ms, 11.2 ms	0.033 rad (j5)	1.05 N·m
k1 (u¹)	7.67 / 6.92 ms	6.1 ms, 11.2 ms	0.072 rad (j7)	3.19 N·m
u2	7.58 / 7.24 ms	6.3 ms, 10.9 ms	0.072 rad (j7)	2.89 N·m

• Solve time: 5.6-6.3 ms mean across all 4 runs, well inside the 10 ms budget (1.1 % of solves over budget at worst, none over 15 ms). No regression from the 2026-09-04/07 refactor (_APPLY_KNOT → ROS parameter, next_states moved into /control, feedback_gain_scale added, LFC try_publish/humble fix).
• x0-age at /control: essentially the same (~7-7.7 ms mean) across all 4 runs, apply_knot included. This is expected, not a null result: the metric measures how stale the state the OCP solved from is by the time /control is used — a function of solve time + pipeline latency. apply_knot doesn’t make that state fresher; it changes which knot of the already-solved horizon gets published, to compensate for the same staleness once it reaches the 1 kHz LFC loop. Its effect would show up in the q_act/q_stair phase relationship instead (staircase_qact_qdes.html), not in this metric.
• Peak tracking error / chatter (RMS \|u_fb\|): both are noticeably higher on k1 and u2 than on baseline/u0_interp, and on the same joint (j7 = arm_right_7_joint) for both. Read with caution: the 4 recordings are not length- or trajectory-matched (baseline is 4.2 s, the others 6.5-7.4 s), so part of this difference could come from more/different motion being captured rather than from the knot-1 shift itself. This campaign’s data alone doesn’t separate the two — a length-matched re-run would be needed to attribute the difference to apply_knot with confidence.