feat: optional MPC command-delay compensation (Subburaman & Stasse 2024) by clementPene · Pull Request #159 · loco-3d/linear-feedback-controller

clementPene · 2026-09-02T13:16:11Z

What

Optional MPC command-delay compensation for the fast (LFC) loop, following
Subburaman & Stasse, Delay Robust MPC for Whole-Body Torque Control of
Humanoids (IEEE Humanoids 2024).

Four new parameters, all disabled by default.

param	default	effect
`reference_interpolation`	`false`	When true, the feedback reference `control.initial_state` (q, v) is linearly interpolated across the control period towards `control.next_states.front()`, instead of being held constant. Each state carries its own `header.stamp`; the interpolation period is read from the two stamps, not assumed. Falls back to holding the reference when the control carries no `next_states`.
`feedback_gain_scale`	`1.0`	Scales the Riccati feedback torque `K*(x-x_ref)` before it is added to the feedforward. `0.0` = feedforward only (no fast state feedback — pair with `remove_gravity_compensation_effort` if the low-level does its own gravity compensation). `0<s<1` lowers the fast-loop gain, e.g. to damp a closed-loop resonance.
`feedback_lowpass_cutoff_hz`	`0.0`	Optional 2nd-order Butterworth low-pass on `u_fb` before it is added to the feedforward (the paper's u³ step). 0 disables.
`debug_publish`	`false`	Publishes `/lfc_debug` (`std_msgs/Float64MultiArray`): `[time, alpha, q_ref, v_ref, u_ff, u_fb_raw, u_fb, u_cmd]`. Realtime-safe. For offline analysis; off in production.

Companion changes

linear_feedback_controller_msgs: Control.msg gains Sensor[] next_states
(fork branch feat/control-next-state, not yet a PR) — replaces the old
separate mpc_x_next topic. Each element carries its own header.stamp.
agimus_controller: fills next_states on /control instead of
publishing /mpc_x_next (PR #328, companion). This PR is a no-op until
both land and reference_interpolation is enabled.

Testing

Exercised on a TIAGo Pro 25 (real robot, fixed-base right-arm deburring demo):

reference_interpolation on: the ~1-MPC-period command lag (measured
≈ 11 ms) is compensated; the staircase reference becomes a ramp and u_fb
is smoother.
feedback_lowpass_cutoff_hz: works as a biquad. On our compliant SEA arm
an aggressive cutoff (40 Hz) excited a closed-loop resonance, so it is
deliberately opt-in and off by default; the paper's ~66 Hz is benign.
feedback_gain_scale: unit-tested (see below); not yet exercised on
robot.
debug_publish: used throughout for offline bag analysis.

Unit tests added for feedback_gain_scale and feedback_lowpass_cutoff_hz
at the LFController/LinearFeedbackController level. next_states
consumption (LinearFeedbackControllerRos) has no unit coverage -- that
class has no test harness at all, pre-existing gap.

Scope / limitations

Reference interpolation is fixed-base only (revolute joints -> linear
interp == pin::interpolate).
feedback_lowpass_cutoff_hz needs the controller update rate; disabled
with a warning if unknown.

Subburaman & Stasse, "Delay Robust MPC for Whole-Body Torque Control of Humanoids" (Humanoids 2024). Three new params, all off by default: - reference_interpolation: slide the feedback reference from the received knot to the next knot (read from a new mpc_x_next topic) across the MPC cycle instead of holding it, killing the staircase reference. - mpc_period: MPC solve period, to normalise the interpolation factor. - feedback_lowpass_cutoff_hz: optional 2nd-order Butterworth on u_fb. No-op until the MPC node publishes knot 1 + mpc_x_next and the param is on.Fixed-base only. Tested on TIAGo Pro 25.

debug_publish param -> publishes a std_msgs/Float64MultiArray each control cycle: [time, alpha, q_ref, v_ref, u_ff, u_fb_raw, u_fb, u_cmd], layout.dim labels the blocks. Realtime-safe (try-lock RealtimePublisher). Off by default; for offline analysis of the interpolation / low-pass.

for more information, see https://pre-commit.ci

Scalar param multiplying the Riccati feedback torque K*(x - x_ref) before it is added to the feedforward: control = feedforward + feedback_gain_scale * u_fb.

reference_interpolation now reads its interpolation target directly from input_control_.next_states.front() (populated by controlMsgToEigen from the /control message itself) instead of a separate mpc_x_next subscription.

for more information, see https://pre-commit.ci

MaximilienNaveau · 2026-09-07T14:45:51Z

@clementPene I greatly appreciate your contribution.
Please use nix locally or in the CI to make sure the CI is able to build the package in ALL distributions.
Thank you very much!

…F path

…o state

clementPene · 2026-09-09T09:24:09Z

@clementPene I greatly appreciate your contribution. Please use nix locally or in the CI to make sure the CI is able to build the package in ALL distributions. Thank you very much!

Yes but commits b5ca4e2 and e4b111b need to be revert one's loco-3d/linear-feedback-controller-msgs#81 is merged, because I modified messages.

MaximilienNaveau · 2026-09-09T10:55:39Z

@clementPene I greatly appreciate your contribution. Please use nix locally or in the CI to make sure the CI is able to build the package in ALL distributions. Thank you very much!

Yes but commits b5ca4e2 and e4b111b need to be revert one's loco-3d/linear-feedback-controller-msgs#81 is merged, because I modified messages.

@nim65s Will handle that once merged.

MaximilienNaveau · 2026-09-09T11:01:34Z

@olivier-stasse could you take quick look or at least acknowledge that you see that this PR exists.

@nim65s could you take a look?

I will review as will.

@LouiseMsn and @timtmit could you take look as well?

MaximilienNaveau

Some small things need to be debated.

I agree with the current implementation. I think we needed that for a while and thank you @clementPene for making it happen.

I would need the approval of @timtmit and @LouiseMsn and @olivier-stasse to make sure everyone is on the same page. You guys are the main users.

MaximilienNaveau · 2026-09-10T05:51:04Z

+  /// @brief Scale the feedback torque before it is added to the feedforward.
+  /// @param scale 1.0 = nominal, 0.0 = feedforward only, 0<s<1 lowers the
+  /// fast-loop gain. Applied after the optional low-pass.
+  void set_feedback_gain_scale(double scale) { fb_scale_ = scale; }


You should make a proper setter here in the sens that you need to check the validity of this parameter.

Either you add assert, and pray someone in release will have compile his code in debug first:

assert (scale_ <= 1.0 && scale_ >= 0.0 && "fb_scale_ must be in [0, 1]"

Or you forcibly bound the scale:

if (scale_ > 1.0) { fb_scale_ = 1.0; } else if (scale_ < 0.0) { fb_scale_ = 0.0; } else { fb_scale_ = scale; }

In anycase you need assume that the user will do something stupid...

MaximilienNaveau · 2026-09-10T05:53:24Z

+  /// @brief Enable a per-joint 2nd-order Butterworth low-pass on the feedback
+  /// torque u_fb = K*(x - x_ref). @param cutoff_hz 0 disables it (default).
+  /// @param sample_rate_hz the rate compute_control() is called at.
+  void configure_feedback_lowpass(double cutoff_hz, double sample_rate_hz);


I am not sure I am comfortable with this idea. You would introduce some delay here.
Is there a reference to the paper in this PR?
Do they also add a low pass filter?

I added the link to the PR description. yes, they use a low pass filter on u_fb because using interpolation creates some oscillations. I had the same result, you can see some graphs here

MaximilienNaveau · 2026-09-11T12:31:10Z

+      # (loco-3d/linear-feedback-controller-msgs#81, not merged upstream
+      # yet) -- revert to github:loco-3d/linear-feedback-controller-msgs#main
+      # once that PR is merged.
+      UPSTREAM_WORKSPACE: github:clementPene/linear-feedback-controller-msgs#feat/control-next-state


@nim65s to be fixed.

commits modifying CI are revert. But merging PR on lfc messages is not enough, it needs to be release

MaximilienNaveau · 2026-09-11T12:39:47Z

+        # (loco-3d/linear-feedback-controller-msgs#81, not merged upstream
+        # yet) -- remove this override once that PR is merged, CI will then
+        # go back to the nixpkgs-released version automatically.
+        rosOverrideAttrs.linear-feedback-controller-msgs = {


@nim65s to be fixed, loco-3d/linear-feedback-controller-msgs#81 is merged

…next-state" This reverts commit b5ca4e2.

This reverts commit e4b111b.

MaximilienNaveau · 2026-10-20T12:20:22Z

@nim65s could you check the nix integration so we can merge this with passing CI?

clementPene and others added 9 commits September 2, 2026 14:55

[pre-commit.ci] auto fixes from pre-commit.com hooks

d9e119f

for more information, see https://pre-commit.ci

feat(delay-comp): optional feedback_gain_scale on u_fb

2b13b28

Scalar param multiplying the Riccati feedback torque K*(x - x_ref) before it is added to the feedforward: control = feedforward + feedback_gain_scale * u_fb.

test(delay-comp): cover feedback_gain_scale and feedback_lowpass

fa58d31

[pre-commit.ci] auto fixes from pre-commit.com hooks

876995c

for more information, see https://pre-commit.ci

fix(build): gate try_publish on realtime_tools >= 3.0.0 for humble

3e4cc68

[pre-commit.ci] auto fixes from pre-commit.com hooks

54578c0

for more information, see https://pre-commit.ci

clementPene marked this pull request as ready for review September 4, 2026 15:17

clementPene requested a review from MaximilienNaveau September 7, 2026 12:21

clementPene added 4 commits September 9, 2026 10:20

CI(nix dependencies): update lfc messages to make CI pass

e4b111b

fix(tests): prime first_control_received_time_ before asserting the L…

4b85faf

…F path

fix(tests): compare desired config/velocity against the full pinocchi…

f2858d9

…o state

ci(ros2): temporarily pin UPSTREAM_WORKSPACE to feat/control-next-state

b5ca4e2

MaximilienNaveau reviewed Sep 11, 2026

View reviewed changes

clementPene added 2 commits September 11, 2026 16:00

Revert "ci(ros2): temporarily pin UPSTREAM_WORKSPACE to feat/control-…

63c2424

…next-state" This reverts commit b5ca4e2.

Revert "CI(nix dependencies): update lfc messages to make CI pass"

4e12e36

This reverts commit e4b111b.

MaximilienNaveau merged commit aee3a41 into loco-3d:main Oct 21, 2026
8 of 13 checks passed

Navigation Menu

Repository navigation

feat: optional MPC command-delay compensation (Subburaman & Stasse 2024) - #159

feat: optional MPC command-delay compensation (Subburaman & Stasse 2024)#159
MaximilienNaveau merged 15 commits into
loco-3d:mainfrom
clementPene:feat/mpc-delay-u3

clementPene commented Sep 2, 2026 •

edited

Loading

Uh oh!

MaximilienNaveau commented Sep 7, 2026

Uh oh!

clementPene commented Sep 9, 2026

Uh oh!

MaximilienNaveau commented Sep 9, 2026

Uh oh!

MaximilienNaveau commented Sep 9, 2026

Uh oh!

MaximilienNaveau left a comment

Uh oh!

MaximilienNaveau Sep 10, 2026

Uh oh!

MaximilienNaveau Sep 10, 2026

Uh oh!

clementPene Sep 11, 2026 •

edited

Loading

Uh oh!

MaximilienNaveau Sep 11, 2026

Uh oh!

clementPene Sep 11, 2026

Uh oh!

MaximilienNaveau Sep 11, 2026

Uh oh!

MaximilienNaveau commented Oct 20, 2026

Uh oh!

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

clementPene commented Sep 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

What

Companion changes

Testing

Scope / limitations

Uh oh!

MaximilienNaveau commented Sep 7, 2026

Uh oh!

clementPene commented Sep 9, 2026

Uh oh!

MaximilienNaveau commented Sep 9, 2026

Uh oh!

MaximilienNaveau commented Sep 9, 2026

Uh oh!

MaximilienNaveau left a comment

Choose a reason for hiding this comment

Uh oh!

MaximilienNaveau Sep 10, 2026

Choose a reason for hiding this comment

Uh oh!

MaximilienNaveau Sep 10, 2026

Choose a reason for hiding this comment

Uh oh!

clementPene Sep 11, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

MaximilienNaveau Sep 11, 2026

Choose a reason for hiding this comment

Uh oh!

clementPene Sep 11, 2026

Choose a reason for hiding this comment

Uh oh!

MaximilienNaveau Sep 11, 2026

Choose a reason for hiding this comment

Uh oh!

MaximilienNaveau commented Oct 20, 2026

Uh oh!

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

clementPene commented Sep 2, 2026 •

edited

Loading

clementPene Sep 11, 2026 •

edited

Loading