[JTC] Global cubic spline upsampling for positions-only action chunks by vedh1234 · Pull Request #2443 · ros-controls/ros2_controllers

vedh1234 · 2026-06-28T19:30:16Z

Description

Add InferenceBridgeController, a JTC extension that upsamples sparse, positions-only trajectory chunks into smooth global C2 cubic-spline trajectories.

Background:

AI / learned policies (e.g. diffusion policy, ACT) emit action chunks: short JointTrajectory messages containing only positions at the policy's output rate (e.g. 10–50 Hz), with no velocities or accelerations. Fed positions-only, JTC interpolates linearly (C0) producing velocity discontinuities and jerk spikes at every knot. This is unsuitable for real hardware.

Approach

The bridge intercepts incoming ~/joint_trajectory messages via JTC's topic_callback and, for positions-only chunks:

Synthesises timing from a configurable policy_frequency parameter when time_from_start is absent.
Solves knot velocities via fill_cubic_spline_velocities(), an O(n) tridiagonal Thomas solve with rest boundary conditions (v₀ = vₙ₋₁ = 0) and writes them into points[i].velocities.
Delegates to JTC, whose existing cubic-Hermite sampler reproduces the global C2 spline with no changes to the update loop or output path.

Chunks that already carry velocities pass through unchanged, the controller is a strict superset of JTC.

Is this user-facing behavior change?

Yes. Users can now load joint_trajectory_controller/InferenceBridgeController as a drop-in replacement for JTC. Positions-only trajectory messages are automatically upsampled to smooth C2 cubic splines. The only new parameter is policy_frequency (Hz), which controls the synthesised timing for chunks that arrive without time_from_start. Existing trajectories with velocities are unaffected.

Did you use Generative AI?

Yes, Google Gemini and Claude were used to make improvements in manually written code, architectural analysis, edge-case review. All generated code was reviewed, modified, and verified by the author.

Additional Information

Known limitations / future work:

Each chunk uses rest boundary conditions (v₀ = vₙ₋₁ = 0), so streaming chunks decelerate to a stop at each boundary. Cross-chunk C2 continuity feature is deferred and yet to be added.
Only the topic path (~/joint_trajectory) is upsampled. FollowJointTrajectory action goals bypass topic_callback and are not upsampled.

TODOs

To send us a pull request, please:

Fork the repository.
Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change.
Ensure local tests pass. (colcon test and pre-commit run (requires you to install pre-commit by pip3 install pre-commit)
Commit to your fork using clear commit messages.
Send a pull request, answering any default questions in the pull request interface.
Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.

Test Scenario:

Two position only motion profiles were sent on the using the ~/joint_trajectory. As we can see, InferenceBridgeController being C2 continuous, the staircase like discontinuity is not observed in velocity plot in both motion profiles. The sharp corners we observe are due to the lack of cross chunk continuity.

Pick & place	Sine Wave

codecov · 2026-06-28T20:01:29Z

Codecov Report

❌ Patch coverage is 91.59664% with 20 lines in your changes missing coverage. Please review.
✅ Project coverage is 86.99%. Comparing base (f6bcf6c) to head (1b1821d).
⚠️ Report is 8 commits behind head on master.

Files with missing lines	Patch %	Lines
...ory_controller/src/inference_bridge_controller.cpp	57.89%	11 Missing and 5 partials ⚠️
joint_trajectory_controller/src/trajectory.cpp	91.11%	2 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2443      +/-   ##
==========================================
+ Coverage   86.82%   86.99%   +0.16%     
==========================================
  Files         148      151       +3     
  Lines       16097    15336     -761     
  Branches     1361     1272      -89     
==========================================
- Hits        13976    13341     -635     
+ Misses       1615     1538      -77     
+ Partials      506      457      -49

Flag	Coverage Δ
unittests	`86.99% <91.59%> (+0.16%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	`62.50% <ø> (ø)`
...include/joint_trajectory_controller/tolerances.hpp	`92.22% <100.00%> (+12.42%)`	⬆️
...include/joint_trajectory_controller/trajectory.hpp	`87.50% <ø> (ø)`
..._trajectory_controller/validate_jtc_parameters.hpp	`88.23% <100.00%> (+4.90%)`	⬆️
...ntroller/test/test_inference_bridge_controller.cpp	`100.00% <100.00%> (ø)`
...ler/test/test_load_inference_bridge_controller.cpp	`100.00% <100.00%> (ø)`
...int_trajectory_controller/test/test_trajectory.cpp	`100.00% <100.00%> (ø)`
joint_trajectory_controller/src/trajectory.cpp	`90.18% <91.11%> (+0.18%)`	⬆️
...ory_controller/src/inference_bridge_controller.cpp	`57.89% <57.89%> (ø)`

... and 22 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

mergify · 2026-06-30T17:57:50Z

This pull request is in conflict. Could you fix it @vedh1234?

saikishor

Good job. The code looks good. However, the change is minimal here, I'm not sure if we should somehow inject this into JTC already and not to make a variant out of it. As we have plans to extend this, we can leave it, but we should reevaluate this soon

saikishor · 2026-07-10T18:44:10Z

+void InferenceBridgeController::topic_callback(
+  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg)
+{
+  // TODO: add staleness / max-age check for stamp=0 chunks


Should we add this already?

Yes, we can do it, I'll look into it.

saikishor · 2026-07-10T18:46:21Z

+  trajectory_msgs::msg::JointTrajectory & traj) const
+{
+  // Respect timing the policy already set; only synthesize when it is absent.
+  for (size_t i = 1; i < traj.points.size(); ++i)


why check from i=1?

time_from_start is measured from the start of the trajectory, point 0 (first waypoint) is always at t = 0 by definition. So no point in checking the first value as we would not know if user had added it or not.

saikishor · 2026-07-10T18:47:36Z

+inference_bridge_controller:
+  policy_frequency: {
+    type: double,
+    default_value: 30.0,


I wouldn't put any default value, if it is already a different controller

Okay, I'll keep it as a required field.

saikishor · 2026-07-10T18:49:39Z

  }
 }

+void fill_cubic_spline_velocities(trajectory_msgs::msg::JointTrajectory & traj)


May be return bool as you are handling failure cases here

Agreed, I'll make the required change.

saikishor · 2026-07-10T18:55:19Z

+      diagonal[i] = 2.0 * (1.0 / duration_prev + 1.0 / duration_next);
+      upper_diagonal[i] = 1.0 / duration_next;
+      rhs[i] = 3.0 * ((pos_next - pos_curr) / (duration_next * duration_next) +


What are these 2.0 and 3.0 here?

They're the coefficients of the cubic-Hermite equation. They come from second derivative of the equations.

saikishor · 2026-07-10T18:57:30Z

+TEST(TestInferenceBridgeController, preserves_header_stamp)
+{
+  TestableBridge bridge;
+  auto traj = make_positions_chunk({0.0, 0.1, 0.2, 0.1, 0.0}, 0.05);
+  // Clear timing so synthesize_timing actually runs, but keep a non-zero stamp.
+  for (auto & point : traj.points)
+  {
+    point.time_from_start = rclcpp::Duration::from_seconds(0.0);
+  }
+  traj.header.stamp.sec = 123;
+  traj.header.stamp.nanosec = 456u;
+
+  bridge.synthesize_timing(traj);
+  joint_trajectory_controller::fill_cubic_spline_velocities(traj);


It would be nice to add a test of the generated velocities

Okay I'll add a a dedicated velocity-value test.

Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>

vedh1234 added 7 commits June 28, 2026 16:58

fix: add inline to avoid ODR violation

fab35d2

fix(JTC): add virtual to topic callback for inheritance purpose

6ff937b

feat(JTC): add fill_cubic_spline_velocities + tests

aa6d904

test(JTC): add unit tests for inference_bridge_controller

67a283b

feat(JTC): add inference_bridge_controller

ce6b59d

feat(JTC): wire inference_bridge_controller into build + pluginlib

fe4301f

chore: pre commit run

eb205c8

vedh1234 added 3 commits June 29, 2026 13:01

docs: Enriched fill_cubic_spline_velocities doxygen

2277077

docs: update release notes

a8e29f1

docs: update user documentation

e91263f

Merge branch 'master' into feat/inference-bridge-controller

0e684b5

vedh1234 marked this pull request as ready for review July 1, 2026 12:38

github-actions Bot requested review from aprotyas, christophfroehlich, destogl, mamueluth and moriarty July 1, 2026 12:38

vedh1234 mentioned this pull request Jul 5, 2026

Add Example 19 for an end-to-end demo of the InferenceBridgeController ros-controls/ros2_control_demos#1159

Open

6 tasks

saikishor requested changes Jul 10, 2026

View reviewed changes

fix: Apply suggestions from code review

1b1821d

Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>

Uh oh!

Conversation

vedh1234 commented Jun 28, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Background:

Approach

Is this user-facing behavior change?

Did you use Generative AI?

Additional Information

TODOs

Test Scenario:

Uh oh!

codecov Bot commented Jun 28, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mergify Bot commented Jun 30, 2026

Uh oh!

saikishor left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

vedh1234 commented Jun 28, 2026 •

edited

Loading

codecov Bot commented Jun 28, 2026 •

edited

Loading