[JTC] Add spline_upsampling feature for positions-only trajectories (no separate controller)

[vedh1234]

Description

Add spline_upsampling feature to JointTrajectoryController to upsample 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 controller intercepts incoming ~/joint_trajectory messages via topic_callback and, if spline_upsampling.enable is true and it receives positions-only chunks:

1. Synthesises timing from a configurable spline_upsampling.policy_frequency parameter when time_from_start is absent.
2. 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.
3. Passes through to JTC validation, where the 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, meaning the feature is a strict superset of existing JTC behavior and operates as a safe no-op when disabled.

Is this user-facing behavior change?

Yes, as an opt-in feature. Users can now enable spline_upsampling.enable (default: false) in their JTC configuration. When enabled, positions-only trajectory messages on the topic interface are automatically upsampled to smooth C2 cubic splines. The spline_upsampling.policy_frequency parameter (Hz) controls the synthesised timing for chunks that arrive without time_from_start. Existing trajectories with velocities are unaffected, and because it is off by default, there is no breaking change for existing users.

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, with upsampling enabled and 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