Ensure the robot state is up-to-date before Servoing#2954
Conversation
8950e60 to
9975f8b
Compare
There was a problem hiding this comment.
Fixes look good at a glance!
One thing to watch out for, from my own experience, is what happens if you pause/stop servo, move the robot through some other free-space motion planner, and then resume servo. (Maybe @pac48 and @sjahr can test with MoveIt Pro and double check?)
Far as I remember, this was one reason for the liberal sprinkling of resetSmoothing(), but I don't doubt this PR fixes that. I just want to make sure it wasn't pruned too much to affect the interaction with other motions.
Essentially, there are 2 alternatives:
- Keep the smoothing going even when servo is stopped,
- Ensure that transitioning servo from stopped to not stopped triggers a reset to the latest state when restarting.
There are merits to both, but just wanted to bring it up and have people test this case.
e876ca4 to
66f59b7
Compare
I had to add one line to fix this. Good catch. Here's a video of several cycles of Servo -> pause -> joint command -> unpause -> Servo again servo_pausing.webm |
3089351 to
3686dae
Compare
sea-bass
left a comment
There was a problem hiding this comment.
Awesome, thanks for that test!
3686dae to
37bc770
Compare
Description
Previously we didn't wait to get a current robot state before servoing began. That caused the initial joint angles to be all-zero, which caused a small jump at the beginning of motion. This is a fix.
Test with:
ros2 launch moveit_servo demo_ros_api.launch.pyros2 run moveit_servo servo_keyboard_input