[JTC] cross chunk continuity for positions upsampling

[vedh1234]

Description

Add cross-chunk velocity continuity to the positions_upsampling feature to eliminate ringing and velocity dips when streaming trajectory chunks from AI policies.

Background

The initial positions_upsampling PR#2491 added smooth cubic splines within chunks but relied on rest boundary conditions (v₀ = vₙ₋₁ = 0). When streaming continuous chunks from a policy (e.g., 5Hz replan), this forced the robot to decelerate at the start of every chunk, causing high-frequency velocity ringing and massive acceleration spikes.

Is this user-facing behavior change?

No. This is a purely mathematical improvement to the existing positions_upsampling.enable feature.

Did you use Generative AI?

Yes, Google Gemini and Claude were used to assist. All generated code was reviewed, modified, and verified by the author.

Additional Information

• Added the trajectory blending diagram along with the documentation
• Future Work: The end of the chunk still relies on a rest boundary condition (vₙ₋₁ = 0), which is acceptable as streaming chunks typically overwrite the trajectory long before the tail is reached.

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

A 5Hz replan with a 30Hz target rate was tested using a continuous sine wave reference. Option A suggests appending commanded velocity at first waypoint of the trajectory but that would make the velocity same for current point and first waypoint in case of t=dt which causes discontinuity in accelaration. Option B: we prepend the commanded state waypoint in the message itself and accordingly calculate velocity at the first waypoint leading to a smooth trajectory as shown in blue colour.

[codecov]

Codecov Report

❌ Patch coverage is 96.62921% with 3 lines in your changes missing coverage. Please review.
✅ Project coverage is 87.07%. Comparing base (94e74de) to head (58e7837).

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	93.33%	1 Missing and 1 partial ⚠️
joint_trajectory_controller/src/trajectory.cpp	66.66%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2573      +/-   ##
==========================================
+ Coverage   87.00%   87.07%   +0.06%     
==========================================
  Files         157      157              
  Lines       17528    17615      +87     
  Branches     1489     1493       +4     
==========================================
+ Hits        15251    15338      +87     
+ Misses       1722     1720       -2     
- Partials      555      557       +2     

Flag	Coverage Δ
unittests	87.07% <96.62%> (+0.06%)	⬆️

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	40.00% <ø> (ø)
...include/joint_trajectory_controller/trajectory.hpp	100.00% <ø> (+12.50%)	⬆️
...int_trajectory_controller/test/test_trajectory.cpp	100.00% <100.00%> (ø)
...ler/test/test_trajectory_controller_upsampling.cpp	100.00% <100.00%> (ø)
...ntroller/test/test_trajectory_controller_utils.hpp	83.79% <100.00%> (+0.04%)	⬆️
joint_trajectory_controller/src/trajectory.cpp	90.07% <66.66%> (-0.34%)	⬇️
...ory_controller/src/joint_trajectory_controller.cpp	83.83% <93.33%> (+0.18%)	⬆️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

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

[christophfroehlich]

this looks great, but the "upsampling enabled" version is now obsolete right? So this picture is great for the PR, but not for the general docs?

[vedh1234]

Yeah I agree, I'll just update the original upsampling diagram instead with this plot.