[GSoC 2026] Physical AI Inference and Trajectory Upscaling for ros2_control

[vedh1234]

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

Physical AI Inference and Trajectory Upscaling for ros2_control

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.

[Sattyam_Jain]

One thing worth scoping in early, from having got this wrong: what counts as a failure.

Trajectory upscaling and inference will both produce almost right outputs. Without a stated oracle, “it works” ends up meaning “it did not visibly crash”, and that is not a signal you can regression-test against for the rest of the project.

Two cheap oracles that work in ros2_control specifically:

1. Joint-limit and velocity-limit violations, read straight off the controller state. Binary, no labelling, already published on a topic.
2. Commanded versus achieved pose error over the trajectory, with a threshold you pick once and write down. Not perfect, but it turns “looks smooth” into a number that moves.

And one thing to measure that is easy to skip: run the pipeline with the inference step disabled, on the same inputs, and record how often that baseline also violates the limits. It is usually not zero, and if you do not have that number, you cannot tell whether your improvements are improvements.

Happy to be more concrete if it is useful. I work on failure measurement for VLA policies in simulation, so this is the part I have spent time on, though on manipulation rather than ros2_control specifically.

[MariaCristina]

Really interesting work, especially the trajectory blending and position upsampling. The hardware demo makes the improvements much easier to understand. The follow-on project ideas also look useful for anyone wanting to build on this work.

[Panav]

I reached out to Vedhas @vedh1234 in person,
What he’s worked on and documented really well is what i keep visiting discourse for.

However I am a little nauseous from the wallposting @Sattyam_Jain
I am genuinely curious with 16 thread responses, 2 posts within 8 minutes of read time, 0 likes taken or given, are you using an agent looped in here?

Even if not, I would genuinely appreciate if you read through what people have worked on, it is slightly insulting if someone would copy paste responses from agents pointing out “shortcomings” without trying out or understanding it.

It is understandable if people want AI to summarize a long paragraph or translate before posting here but the jargon heavy inorganic content over and over again is not what I check out discourse for every morning.

[vedh1234]

Thanks @MariaCristina and @Panav, I am very glad that you found this project useful and liked the demos.

@Sattyam_Jain thanks for the feedback but regardless I would like to make some clarifications for how these features interact with limits:

Blending and JTC upsampling does not violate joint position limits as blending only reorders the waypoints to form a mutated trajectory message using the waypoints that already existed in the old and new trajectory; upsampling writes/fills the velocities so again it doesn’t touch commanded positions. Neither generates a joint position that wasn’t already there, so a violation on those paths comes from upstream, the policy or planner that produced the waypoints. Velocity limits can be an issue and worth watching only if a policy runs near its velocity ceiling.
Though this is a documented behaviour of cartesian_trajectory_controller and was not part of the scope of this project hence listed as a spinoff idea (I’ll also make it an explicit point): Currently the IK runs in non-RT loop and is a differential IK, it is not aware of joint limits and nullspace control. That is called out in this post under “What is next” above, and example_20’s verifier has a dedicated joint-limit test case for exactly this. Though this could be a really great next contribution to this project.