Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Servo] Update last_sent_command_ at ServoCalcs start #2249

Merged
merged 4 commits into from
Aug 26, 2020

Conversation

AdamPettinger
Copy link
Contributor

Description

I moved the initialization of last_sent_command_ from the ServoCalcs constructor to the start() function. The last_sent_command_ is stored and published if Joint or Cartesian servoing is not done.

This PR fixes a logic error resulting in bad behavior:

  1. Start servo
  2. Servo around
  3. Stop servo
  4. Move robot with something besides servo
  5. Start servo
  6. last_sent_command_ is stored as where servo stopped in 3 and if published may result in the manipulator jumping back to that position.

This PR fixes by saving last_sent_command_ as the current position every time servo is started.

There could still be some unexpected behavior if you start the servo while the arm is moving from another input. This will suddenly stop the arm as the last_sent_command_ is set to the current position and 0 velocity. I am unsure what should happen in this case, but am open to suggestions.

@codecov
Copy link

codecov bot commented Aug 11, 2020

Codecov Report

Merging #2249 into master will increase coverage by 0.09%.
The diff coverage is 100.00%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master    #2249      +/-   ##
==========================================
+ Coverage   57.80%   57.89%   +0.09%     
==========================================
  Files         326      326              
  Lines       25625    25627       +2     
==========================================
+ Hits        14812    14837      +25     
+ Misses      10813    10790      -23     
Impacted Files Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp 72.95% <ø> (ø)
moveit_ros/moveit_servo/src/servo_calcs.cpp 61.48% <100.00%> (+0.46%) ⬆️
...e/src/parameterization/model_based_state_space.cpp 72.22% <0.00%> (-2.78%) ⬇️
moveit_core/robot_model/src/joint_model_group.cpp 59.33% <0.00%> (-2.72%) ⬇️
...ics_plugin_loader/src/kinematics_plugin_loader.cpp 60.54% <0.00%> (-2.05%) ⬇️
...on/pick_place/src/approach_and_translate_stage.cpp 72.80% <0.00%> (-0.80%) ⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp 78.92% <0.00%> (-0.45%) ⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp 69.44% <0.00%> (-0.45%) ⬇️
moveit_core/utils/src/robot_model_test_utils.cpp 73.07% <0.00%> (ø)
...tils/include/moveit/utils/robot_model_test_utils.h 100.00% <0.00%> (ø)
... and 13 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 3e4d3ac...e9e20e8. Read the comment docs.

@AndyZe
Copy link
Member

AndyZe commented Aug 13, 2020

Thanks for a good description of what's going on. It makes sense; I don't have any suggestion for the edge case you pointed out.

@AndyZe
Copy link
Member

AndyZe commented Aug 13, 2020

I just noticed some folders are not being installed. Would you mind adding that here? In CMakeLists, it would be:

install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

If you install the binary (sudo apt install ros-melodic-moveit-servo), it's causing some issues. Oops.

Copy link
Member

@AndyZe AndyZe left a comment

Choose a reason for hiding this comment

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

I tested switching back and forth between trajectory and streaming control. No issues, code looks good too.

@AdamPettinger
Copy link
Contributor Author

I just noticed some folders are not being installed. Would you mind adding that here?

Done!

@AndyZe
Copy link
Member

AndyZe commented Aug 19, 2020

@tylerjw you will probably want to merge this before the Noetic release. It adds these key lines:

install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

Copy link
Member

@tylerjw tylerjw left a comment

Choose a reason for hiding this comment

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

looks good, thank you for this fix

@tylerjw tylerjw merged commit bc6ac4e into moveit:master Aug 26, 2020
@gavanderhoorn
Copy link
Contributor

I read the title of this PR as "Update [..] at ServoCakes start" and was rather confused for a while.

🍰

@tylerjw tylerjw mentioned this pull request Oct 23, 2020
57 tasks
tylerjw pushed a commit to tylerjw/moveit that referenced this pull request Oct 23, 2020
* Move last_sent_command_ update from constructor to start
* Tweak demo configs
* Add install folders to CMakeLists
* Add clarity to parameter warning
@AdamPettinger AdamPettinger deleted the update_servo_start branch October 25, 2020 03:19
tylerjw pushed a commit to tylerjw/moveit that referenced this pull request Oct 27, 2020
* Move last_sent_command_ update from constructor to start
* Tweak demo configs
* Add install folders to CMakeLists
* Add clarity to parameter warning
tylerjw pushed a commit that referenced this pull request Nov 19, 2020
* Move last_sent_command_ update from constructor to start
* Tweak demo configs
* Add install folders to CMakeLists
* Add clarity to parameter warning
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants