Organization: Open Source Robotics Foundation / ros2_control
Contributor: Vedhas Talnikar (GitHub)
Mentors: Dr. Bence Magyar (Locus Robotics), Sai Kishor Kothakota (PAL Robotics)
Full write-up: control.ros.org - Physical AI Inference and Trajectory Upscaling
Full demo video: YouTube
Work product: GitHub - vedh1234/gsoc2026-ros2-control · GitHub
Learned manipulation policies (ACT, diffusion policies, VLAs) emit joint targets at 20 to 50 Hz and replan every 50 to 500 ms. The hardware underneath wants a fresh command every control cycle, at 500 Hz to 2 kHz. Forward those sparse waypoints as they arrive and you get a velocity discontinuity and an acceleration spike at every new waypoint.
This project closed that gap in ros2_controllers, in three parts. We demonstrated below runs on hardware: a 7-DOF AgileX Nero.
1. Trajectory replacement with blending
JTC used to discard the active trajectory when a new one arrived. A future-stamped message killed the running motion the moment it landed and ramped to its first point, and any joint the message omitted froze in place.
It now merges the two at the instant the message arrives: prefix (the old path still to run), bridge (one point sampled from the old spline, so the seam carries a real velocity), new points, and suffix (joints the message never mentioned finish their original motion).
Enabled by allow_trajectory_replacement, default true.
allow_trajectory_replacement: false |
allow_trajectory_replacement: true |
|---|---|
 |
 |
| The new trajectory replaces the old one outright, so the arm cuts straight toward the first new waypoint and drops off the scan height early (red points). | The prefix keeps the old path running until the handoff, so the arm holds the scan height and only then descends. |
2. Positions-only action chunks
A chunk carrying only positions falls back to linear interpolation: which leads to a staircase velocity and an impulse acceleration at every knot. With positions_upsampling enabled, the controller solves the knot velocities that a global cubic spline would have (one tridiagonal system per joint, O(n), in the non-real-time subscription callback) and synthesizes time_from_start from the policy rate when the chunk arrives without timing.
Nothing in the real-time path changes, and messages that already carry velocities pass through untouched, so the feature is a strict superset of the previous behaviour.
| No controller | Upsampling off | Upsampling on |
|---|---|---|
 |
 |
 |
| Policy setpoints streamed to the hardware at 15 Hz. Position is discontinuous, so every derivative is an impulse train. | 15 Hz to 200 Hz by linear interpolation. Position is continuous, velocity is a staircase that steps once per policy waypoint. | 15 Hz to 200 Hz through a C2 cubic spline. Velocity is continuous and acceleration is bounded. |
The three sit one rung apart on the continuity ladder: a zero-order hold, a first-order hold, and a cubic spline.
3. Cartesian trajectory controller
Interpolating in joint space between two configurations does not move the tool along a straight line, and several policy families emit end-effector poses rather than joint angles. cartesian_trajectory_controller subclasses JTC and accepts trajectory_msgs/MultiDOFJointTrajectory on ~/cartesian_reference. Per message it interpolates the path in Cartesian space (cubic Hermite for translation, SLERP for rotation), resamples it densely, runs differential inverse kinematics through kinematics_interface, and hands an ordinary joint trajectory to the base class.
All of the kinematics runs once per message in the non-real-time callback, so the real-time loop stays as fast as plain JTC.
| Task-space path | Policy in Cartesian space |
|---|---|
 |
 |
| 49 published poses become a continuous 200 Hz trajectory. The pen holds one commanded orientation for the whole word, tracked to 0.41 degrees. | The same pick-and-place task with an ACT policy trained in Cartesian space, emitting end-effector poses instead of joint angles. |
What is next
solve_ik_along_path has no joint-limit awareness and no nullspace control yet, so on a 7-DOF arm the redundant degree of freedom drifts. The work product lists eight follow-on projects with requirements and priorities, from factoring the trajectory upscaling primitives into control_toolbox so any controller can reuse them, to an IKFast plugin for kinematics_interface and a FollowCartesianTrajectory action server.
Full walkthrough
Acknowledgements
Thanks to @bmagyar, @saikishor and @christophfroehlich for the guidance and reviews throughout, to the ros2_control maintainers and contributors for reviewing the pull requests, and to Prof. Pushpak Jagtap and the FoCAS Lab, IISc Bangalore, for hardware and lab facilities.