Normalize quaternions when adding new or moving collision objects (#1119) #1420
Conversation
|
Thanks for helping in improving MoveIt! |
moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
Outdated
Show resolved
Hide resolved
| { | ||
| Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); | ||
| Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); | ||
| quaternion.normalize(); |
There was a problem hiding this comment.
Shouldn't this be added to tf2_eigen.h or even all conversion functions with quaternions? fromMsg() could have a default parameter normalize_quaternion=false.
There was a problem hiding this comment.
This would be indeed a nice to have. However, OSRF wasn't very responsive for ROS1 changes in the recent past.
There was a problem hiding this comment.
I can change the calls to tf2::fromMsg() in other places, however I am not so sure I this is really desired as then quaternion normlization is done more often. I read up on the discussion here, and for me it sounded like we don't want to normalize the quaternions everywhere. But maybe @rhaschke can clarify that?
There was a problem hiding this comment.
If that's the case it's fine with me. It just seems odd that collision objects are affected while attached collision objects are not. I approve this patch as is. @rhaschke do you have something to add here? Otherwise lgtm.
There was a problem hiding this comment.
I looked into this briefly a week ago and I agree that we should consider normalization in more places. Investigating the extent of required changes, I found some nasty code duplication between PlanningScene code and robot_state/.../conversions.cpp. I wanted to clean this up but didn't find time yet to do so.
There was a problem hiding this comment.
@henningkayser, let's merge for now. Remember to cherry-pick to master. This PR is targeted at melodic-devel currently.
Co-Authored-By: j-petit <petit.jens@gmail.com>
|
Congrats on getting your first MoveIt! pull request merged and improving open source robotics! |
…veit#1420) * normalize quaternion for added collision object (moveit#1119) * Refactered normalization into fcn * Added normalization for processNormalizationMove * Cleanup of artefacts * Changed fcn name and added docstring * Adapt docstring for doxygen Co-Authored-By: j-petit <petit.jens@gmail.com>
* Add setTrajectoryConstraints() to PlanningComponent * Add planning time to PlanningComponent::PlanSolution * Replace PlanSolution with MotionPlanResponse * Address review * Add MultiPipelinePlanRequestParameters Add plan(const MultiPipelinePlanRequestParameters& parameters) Add mutex to avoid segfaults Add optional stop_criterion_callback and solution_selection_callback Remove stop_criterion_callback Make default solution_selection_callback = nullptr Remove parameter handling copy&paste code in favor of a template Add TODO to refactor pushBack() method into insert() Fix selection criteria and add RCLCPP_INFO output Changes due to rebase and formatting Fix race condition and segfault when no solution is found Satisfy clang tidy Remove mutex and thread safety TODOs Add stopping functionality to parallel planning Remove unnecessary TODOs * Fix unused plan solution with failure * Add sanity check for number of parallel planning problems * Check stopping criterion when new solution is generated + make thread safe * Add terminatePlanningPipeline() to MoveItCpp interface * Format! * Bug fixes * Move getShortestSolution callback into own function * No east const * Remove PlanSolutions and make planner_id accessible * Make solution executable * Rename update_last_solution to store_solution * Alphabetize includes and include plan_solutions.hpp instead of .h * Address review * Add missing header * Apply suggestions from code review Co-authored-by: AndyZe <andyz@utexas.edu> Co-authored-by: AndyZe <andyz@utexas.edu>
Fixes #1119.
Description
As proposed in #1119 I implemented quaternion normalization when adding a new collision object and also when providing a new pose (move operation). Instead of using
tf2::fromMsgas before I wrote a new function which includes orientation normalization and replaced it in the appropriate spots.Checklist