Revert central differencing calculation in servo

[sea-bass]

This is effectively a revert of #2080, just that flat out reverting the commit had a lot of merge conflicts due to newer PRs that landed.

We found this was affecting us in MoveIt Studio in that the velocity scaling was not kicking in correctly. For example, going from override_velocity_scaling_factor value of 0.1 to 1.0 did not actually increase the target velocity 10x; it was being damped out by the central difference.

After a bunch of debugging and thinking, I've convinced myself that central differencing is not what we need here -- it's useful for state estimation, but not when we're trying to calculate a command given a known current state and a desired next state. We know exactly what our target velocity should be.

To test:

• Start some kind of Servo node with the ability to command a robot
• Try change speed scaling: ros2 param set /servo_node moveit_servo.override_velocity_scale_factor 0.1
• Check that the velocity actually scales linearly when not scaled down by other factors (collision, singularity, etc.)
• Compare w/ main

[sea-bass]

@ibrahiminfinite FYI

[codecov]

Codecov Report

Patch coverage: 86.67% and project coverage change: -0.01 ⚠️

Comparison is base (9782d9a) 50.52% compared to head (0c8a01f) 50.51%.

❗ Current head 0c8a01f differs from pull request most recent head cc3cfb8. Consider uploading reports for the commit cc3cfb8 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2203      +/-   ##
==========================================
- Coverage   50.52%   50.51%   -0.01%     
==========================================
  Files         386      386              
  Lines       31736    31735       -1     
==========================================
- Hits        16032    16028       -4     
- Misses      15704    15707       +3     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	71.14% <71.43%> (ø)
moveit_ros/moveit_servo/src/utilities.cpp	72.80% <100.00%> (-0.21%)	⬇️

... and 2 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[ibrahiminfinite]

Do we still need to remove the central difference ?

[sea-bass]

Do we still need to remove the central difference ?

Yes, I think so. I filled in more details in the issue description.

[bgill92]

Tested with Moveit Studio and Gazebo, reverting the central difference does allow for proper velocity scaling.

[AndyZe]

I don't have a convenient way to test this at the moment, but I'm concerned about a joint jump if Servo is stopped, the robot moves, then Servo starts again. So could you try:

• launch Servo
• ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger {}\
• move the robot somehow (maybe rqt_joint_trajectory_controller)
• ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {}\

And look carefully for a joint jump

[sea-bass]

I don't have a convenient way to test this at the moment, but I'm concerned about a joint jump if Servo is stopped, the robot moves, then Servo starts again.

We fixed this with #2186. It wasn't related to the velocity differencing calculation, but rather to the low-pass filters retaining old state from before the servo calcs were stopped.

[AndyZe]

Please test regardless.

[sea-bass]

Please test regardless.

Yes, we have been testing this exact thing with MoveIt Studio and the PR I mentioned before got rid of the issue. @bgill92 and @chancecardona have also tested.

EDIT: Here's a video :)

2023-06-01.11-39-41.mp4