Port over "update existing trajectory" to joint_trajectory_controller

[v-lopez]

As implemented in ROS1 control

There's a test implemented in test_trajectory_controller.cpp named test_execute_partial_traj_in_future that is disabled right now waiting for this feature.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[pastoriomarco]

I'm trying to figure out how to port the ros_controllers logic to ros2_controllers maintaining the original behavior and without changing the current one, but I'm wondering:

• The current behavior for trajectory update does work, just not as in the documentation. See here for a workaround.
• The way the current ros2_controllers trajectory update works requires to know the previous trajectory, but so does sending a new trajectory that can be successfully executed within the robot's limits, as the trajectory itself must be constructed to be coherent with the current robot's state.
• It's been almost 5 years since this issue was opened, so it doesn't seem something urgently needed by the user base, or it may have already been solved otherwise by those who need it. In the latter case, modifying it could break the current user applications that leverage it (like mine, potentially).

I think that the ros1 implementation is more elegant and complete, but is it necessary? I mean, after so much time wouldn't it be safer to update the documentation with the ros2_controllers behavior?

Sure, having the same behavior as in ROS1 would help the transition to ROS2 now that ROS1 is EOL, but it would also need to be consistent with the current ros2_controller's behavior to be safe with the current ROS2 applications.

PS: Some further considerations:
Currently, the trajectory substitution on ros_controllers seems to:

• Use absolute time, instead of relative time in ROS2: without keeping track of when the current traj was sent I don't think there's a way to get the actual time_from_start. At least on Humble, the actual.time_from_start field in the feedback's trajectory_msgs/JointTrajectoryPoint is always 0.
• Find the first valid traj point of the new traj that is in the future, and the last valid point that can be executed in the old traj before the first point of the new traj
• Discard everything before 'now'.
• Checks if the robot has joints with more than a complete revolution, and applies offsets to the new traj if needed
• Build a 'bridge' segment from the last point of the old traj to the fist point of the new traj

The automatic construction of a bridge segment is nice but, if I understood it correctly, there is no guarantee that the produced trajectory is executable. To be sure that the robot can physically execute the transition to the new traj I would still have to use the old traj (or at least part of it) and merge it with the new traj in a way that respects the robot's limits, for example running a time optimization algorithm that also considers scaling factors.
This is surely harder than just sending the new traj to the controller, but also safer and more stable.

Also, if I'm in the condition where the robot 'has_angle_wraparound', how can I be sure that the modified new traj doesn't have some joint that goes beyond its rotation limits after the offset is applied to the traj? Right now the ros_controllers implementation doesn't seem to check for the limits (if I understood it correctly), so the new traj could be modified to have unreachable points if the new traj's has points in the future that require the joint to rotate further towards its limits. If it did check for the limits, it would need to reject the new traj or to postpone the transition to a later point of the traj that is stil reachable considering the robot's limits, thus creating a different traj than the one originally sent.

I think that handling this functionality keeping both the ROS2 and ROS1 behaviors, while also making the trajectory substitution mechanism clear, robust and predictable is extremely complex (if feasible at all if we want to cover all the edge cases).

In my applications I can't have some edge cases that make the robot potentially behave erratically, especially for safety reasons in industrial settings. Thus, personally, I would still prefer leveraging the simpler mechanism adopted by the current ROS2 implementation, and handle the transition myself.

[christophfroehlich]

@pastoriomarco thank you for the detailed write-up.

• For your concern regarding breaking existing setups: We just could use a new parameter to opt-in the trajectory blending.
• regarding actual.time_from_start: This was fixed shortly after with added time_from_start to action feedback #1755 on humble, and there is a pending PR for rolling+jazzy.
• I transferred the documentation from the ROS 1 wiki to this codebase quite a long time ago, maybe not being aware that this blending does not exist yet. You are right, we should update the docs if we decide in not porting this method. I'll open a PR to do so, I think we would not backport this new feature to humble or jazzy at all. 📝 Add disclaimer on missing trajectory replacement implementation #1978, 📝 Remove wrong information about trajectory replacement #1979
• I don't think that we need to overcomplicate things regarding joint limits. The current interpolation method does not respect the limits at all, so why should we do it for the blending. IMHO, a valid use case would be the path planning and trajectory generation from a slower node, some sort of moving-horizon planner. This algorithm would consider the joint limits, and benefit from this to-be-added blending.