Skip to content

Revert central differencing calculation in servo#2203

Merged
sjahr merged 3 commits intomoveit:mainfrom
sea-bass:revert-servo-central-difference
Jun 2, 2023
Merged

Revert central differencing calculation in servo#2203
sjahr merged 3 commits intomoveit:mainfrom
sea-bass:revert-servo-central-difference

Conversation

@sea-bass
Copy link
Copy Markdown
Contributor

@sea-bass sea-bass commented May 25, 2023 •
edited
Loading

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 sea-bass requested a review from AndyZe May 25, 2023 03:35
@sea-bass
Copy link
Copy Markdown
Contributor Author

sea-bass commented May 25, 2023

@ibrahiminfinite FYI

@sea-bass sea-bass force-pushed the revert-servo-central-difference branch from d79f325 to 8d360ee Compare May 25, 2023 03:37
@codecov
Copy link
Copy Markdown

codecov bot commented May 25, 2023 •
edited
Loading

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.

@sea-bass sea-bass requested review from bgill92 and sjahr May 25, 2023 13:53
@ibrahiminfinite
Copy link
Copy Markdown
Contributor

ibrahiminfinite commented May 31, 2023

Do we still need to remove the central difference ?

@sea-bass
Copy link
Copy Markdown
Contributor Author

sea-bass commented May 31, 2023

Do we still need to remove the central difference ?

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

bgill92
bgill92 approved these changes May 31, 2023
View reviewed changes
Copy link
Copy Markdown
Contributor

@bgill92 bgill92 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@AndyZe
Copy link
Copy Markdown
Member

AndyZe commented Jun 1, 2023 •
edited
Loading

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

AndyZe
AndyZe reviewed Jun 1, 2023
View reviewed changes
@sea-bass
Copy link
Copy Markdown
Contributor Author

sea-bass commented Jun 1, 2023

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
Copy link
Copy Markdown
Member

AndyZe commented Jun 1, 2023

Please test regardless.

sjahr pushed a commit to PickNikRobotics/moveit2 that referenced this pull request Jun 1, 2023
@sea-bass @sjahr
REMOVE WHEN moveit#2203 IS MERGED Revert central differencing calcula…
c03c806 
…tion in servo
@sea-bass
Copy link
Copy Markdown
Contributor Author

sea-bass commented Jun 1, 2023 •
edited
Loading

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

@sjahr sjahr merged commit f9d44b6 into moveit:main Jun 2, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@AndyZe AndyZe AndyZe approved these changes

@sjahr sjahr sjahr approved these changes

+1 more reviewer

@bgill92 bgill92 bgill92 approved these changes

Reviewers whose approvals may not affect merge requirements

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

5 participants

@sea-bass @ibrahiminfinite @AndyZe @bgill92 @sjahr