Add command queue to servo to account for latency

[pac48]

There is an issue where some robots experience non-smooth and noisy servo performance #2275. Currently, servo sends trajectory messages to the ros2_control JTC one waypoint at a time. However, This causes issues because when the JTC receives a new message, it replaces the old one entirely. My proposal to solve this issue is to send more than one waypoint at a time and make sure everything is time-synchronized. Below is an example diagram.

When messages are sent from servo to the JTC, there is an inherent noisy delay that should be accounted for. One strategy is to use a buffer that contains all generated servo commands from the current time to the current time plus the maximum expected latency. This range is shown in red. Any commands that are put in the buffer are considered "committed", meaning they will be executed by the JTC. For any time after the red region, there is still an opportunity to change commands. This is illustrated with the green trajectory.

I recorded and plotted the commands received by the JTC with the current servo implementation (left) vs. this PR (right) using the 'demo_pose.cpp' example in the servo package.

[codecov]

Codecov Report

Attention: 25 lines in your changes are missing coverage. Please review.

Comparison is base (74ba730) 50.31% compared to head (3217e03) 50.48%.

❗ Current head 3217e03 differs from pull request most recent head 279925f. Consider uploading reports for the commit 279925f to get more accurate results

Files	Patch %	Lines
moveit_ros/moveit_servo/src/servo.cpp	48.00%	13 Missing ⚠️
moveit_ros/moveit_servo/src/utils/common.cpp	82.36%	12 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2594      +/-   ##
==========================================
+ Coverage   50.31%   50.48%   +0.18%     
==========================================
  Files         386      386              
  Lines       32151    32228      +77     
==========================================
+ Hits        16172    16266      +94     
+ Misses      15979    15962      -17     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[AndyZe]

I think you could get rid of the complexity of the rolling window and simply always keep 2 waypoints in committed_commands_. I bet the majority of the performance gain comes from the latency parameter and the stop command point.

The latency parameter is identical to #886 but the stop command point is a good idea.

[AndyZe]

Adding that halt position waypoint will likely have strange effects if the position/vel/accel JTC interfaces are active, since that increases the spline interpolation order --> potential for large swings during interpolation. It should work well if just the position interface is active, though.

[AndyZe]

Need to rethink the end_state

[pac48]

Need to rethink the end_state

The one problem I see with using the haltJoints() function is that it seems like it needs to be called in the servo loop. The only scenario when end_state is even a factor is when the servo loop dies or is severely delayed. So it won't be easy to use.

One option is to actually call haltJoints() multiple times to generate a sequence of gradually slowing points, instead of a single one with zero velocity. The drawback is that it will increase the size of the trajectory message and slow down the servo loop.

Further realizing that this code path should never be executed anyway, unless the latency is extreme.

Another option is to not explicitly account for this case because the low-level controller will likely have a stopping mechanism. But I think it's safer to have it just in case. @AndyZe Do you have a suggestion to handle the edge case of extreme latency?

[AndyZe]

I guess the halt point is fine.

[AndyZe]

Suggested change

	// keep track of previously generated joint commands for constructing trajectory messages
	std::deque<KinematicState> committed_commands_;
	// rolling window of joint commands
	std::deque<KinematicState> joint_cmd_rolling_window_;

[AndyZe]

Not a fan of the "committed" terminology. I think people normally think of this as a rolling window.

[pac48]

I changed all references of "committed" to "joint_cmd_rolling_window"

[pac48]

Between @sea-bass comments about making the change more functional, e.g. passing the rolling window in, and @AndyZe concerns about performance, I think it is best to factor out the command queueing logic and have the calling code maintain it. I'll move the logic into the servo_node. It's better that way. Also, I'll make the name changes @AndyZe suggested,

[AndyZe]

Well, props for figuring out this long-standing issue!

[pac48]

Well, props for figuring out this long-standing issue!

Here is a result of running the realtime_servo demo in the tutorials. The new implementation is shown in the top plots and the old in the bottom plots. The left and right columns are position and position deltas as a function of time.

[sea-bass]

I see @AndyZe's point about scaling acceleration doing weird things to the direction being tracked, and left a comment inline asking if there is a way to "time scale" the bundled up trajectory in a way that is more correct.

The other comment about trying velocity filtering with the Butterworth plugin is something we could explore? I tried with these changes and it made my Gazebo simulation do horrible things, but that could have been a combination of Gazebo's physics and some other issues that may have been resolved since (including in this PR). I guess it's worth a shot to try it and get some data.

[sea-bass]

This 3 in here is a magic number, but I'm assuming this is related to you wanting 3 points in your trajectory, right? Is this the same as MIN_POINTS_FOR_TRAJ_MSG you defined in common.cpp?

If that's not the case, you can probably leave in the literal here (especially since it's also in the error message), but definitely add a comment explaining.

[pac48]

I think it's correct to relate the 3 here to MIN_POINTS_FOR_TRAJ_MSG. Do you think I should move MIN_POINTS_FOR_TRAJ_MSG to moveit_servo/utils/common.hpp so it can be accessed?

[sea-bass]

I think so!

[sea-bass]

I think @AndyZe's comment about direction switching is valid, and I hadn't thought of that since I was thinking about the full trajectory planning case where scaling down acceleration + scaling up time accelerating works equivalently to the unscaled case, as far as the space swept by the robot motion.

Is there any way to do the same within the current servo framework now that we have the rolling window? Like, time scaling https://www2.ece.ohio-state.edu/~zhang/RoboticsClass/docs/LN9_TrajectoryGeneration.pdf -- meaning, taking the existing trajectory's accelerations and scaling up the time_from_start to be higher as needed to meet the limits?

[pac48]

If we filtered velocities instead of positions --> problem solved. There's an issue about that.

@AndyZe If we filter in velocity, won't we run into the same issue of the robot not moving in the direction commanded? That is the main criticism of acceleration limiting.

[AndyZe]

True. Well, it's an optional plugin. Hopefully Ruckig will come along and solve all of our issues. Happy to help make that happen

[pac48]

True. Well, it's an optional plugin. Hopefully Ruckig will come along and solve all of our issues. Happy to help make that happen

@AndyZe Okay great. I removed the acceleration-based limits from this PR. I will follow up with an online_signal_smoothing optional plugin in a future PR if needed.

[AndyZe]

yay!

[sjahr]

@pac48 Can you address the conflicts? Then we should merge this, nice work!

[mergify]

This pull request is in conflict. Could you fix it @pac48?

[sea-bass]

@pac48 Can you address the conflicts? Then we should merge this, nice work!

I just addressed them, they were simple and from my recent PR, so let's do this!