Move world object shapes from PlanningScene#3599
Conversation
Codecov ReportAttention: Patch coverage is
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## master #3599 +/- ##
==========================================
+ Coverage 47.15% 47.19% +0.05%
==========================================
Files 603 603
Lines 58999 59142 +143
Branches 6969 7016 +47
==========================================
+ Hits 27814 27909 +95
- Misses 30773 30815 +42
- Partials 412 418 +6 ☔ View full report in Codecov by Sentry. |
|
We could also add a new moveit_msg to move the shapes: # Shapes
# The length of the shape_indexes and shape_poses has to be identical.
# The indexes must be inside the objects shapes array bound
uint32[] shape_indexes
geometry_msgs/Pose[] shape_poses
# Internally, the CollisionObject shapes are added in this order: primitive, mesh and plane.
# Example:
# [primitive[0], primitive[1], mesh[0], mesh[1], mesh[2], plane[0]]
#
# To change only the first plane in the example above, use the index #5. |
rhaschke
left a comment
There was a problem hiding this comment.
I don't like the implicit assumption of a 1:1 correlation of poses to existing shapes.
However, to realize a fast update of many shapes, that is a good extension.
26cd8bf to
fb1c650
Compare
|
Moving meshes seems to not be cheap in terms of CPU time. It takes 0.3 seconds to update my object which contains 3 meshes. Only 2 of the 3 meshes are actually updated. These meshes are somewhat detailed. I think that the FCL collision environment removes the FCL object and recreates it instead of just updating the poses of it's shapes. Will try and fix that, but this is another issue, so not related to this PR. |
* MSA: ControllerWidget allow '0' character in controller names (moveit#3528) * Simplify code waiting for ActionBasedController - avoid code redundancy - fix warning interval to 5s Having an interval of timeout/3 is strange for very short or very long timeouts * CI: Fix building of ikfast plugins Ignore missing authentication for Indigo packages using --force-yes. * CI: Add new ROS repo key osrf/docker_images#697 (comment) * Warn if trajectory becomes invalid during execution (moveit#3536) Increase debug level from INFO to WARN. * Enable higher number of self-collision checking samples (moveit#3529) Replace density value label with spinbox to enable setting higher sampling densities * Add interrupt button for collision generation * Reuse existing finishGeneratingCollisionTable() in interrupt() When interrupted, we don't want to update the collision matrix. --------- Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> * Pass more distance information out from FCL collision check (moveit#3531) * RobotState: Initialize joint transforms of fixed joints (moveit#3541) RobotState doesn't update/initialize fixed joint transforms via update() - for optimization. They are only set to identity via getJointTransform() when called with a non-const RobotState. To initialize them once, we do that in initTransforms() now. Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> * rviz: correctly edit radian and degree joint values Before, the radio button radian/degree choice was not respected when editing unbound joints. I refactored the degree/radian logic into a custom data role `JointRangeFractionRole` for interaction with the event filter. * rviz: support enter to manually type joint values Without this, there was no keyboard-only way to edit unbound joints. I consider it a feature that this also works with *all* joints now, as I see no reason to prevent the user from spinbox-editing arbitrary joints if they wish to. * guard <Return> when joint editor is already active modelled after https://stackoverflow.com/a/18510433/21260084 . It seems there is no more convenient way to access whether an editor is already open. This prevents stderr output saying "edit: editing failed". * Use new location of boost::progress_display if available (moveit#3546) * Revert "Use new location of boost::progress_display if available (moveit#3546)" This reverts commit d03fade. * Update Dockerfile (moveit#3543) * use boost::timer::progress_display if available (moveit#3547) check for header to stay compatible with ubuntu 20.04. Support boost >= 1.83 Slightly ugly due to the double alias, but boost::timer was a class before 1.72, so using `boost::timer::progress_display` in the code breaks with older versions. * support new ompl cmake targets (moveit#3549) ompl's main branch changed omplConfig.cmake changed from OMPL_* variables to imported targets via ompl::ompl in October 2023. I generalized handling of fcl to support the new dependency. * fix link names with slashes when looking up rigid parents since subframes were added, RobotState::getRigidlyConnectedParentLinkModel did not see links anymore if they contained slashes. * cleanup inline-URDF with raw string notation * test links/object names with '/' * Sharpen existing unit test Not only test for the size of vectors, but also for the content. * Extend subframe test to handle object and subframe with slashes * Fix for subframes with slashes * Need to find pybind11_catkin early error (moveit#3552) * Fix docker configs - ci-testing requires specific apt key - use modern LABEL instead of deprecated MAINTAINER tag * use min instead of multiplication to combine relative and absolute check (moveit#3556) * 🛠️ Bump codecov/codecov-action from 3 to 4 (moveit#3559) * Cleanup const-ref arguments to double+int (moveit#3560) * Add missing word in error message * Replace const [double|int] reference arguments with plain const args There is no value in passing a const reference over passing the value directly. The latter saves an extra indirection. * [ROS-O] update Boost API (melodic-compatible) (moveit#3564) Debian testing finally warns about API that was already deprecated in Ubuntu 18.04. * Augment Python PlanningSceneInterface API (moveit#3534) * Make object creation functions public * Factor out make_plane * Provide attach_* methods * Use existing apply_planning_scene method instead of creating a new service instance * Replace dangerous default list argument with None https://pylint.readthedocs.io/en/latest/user_guide/messages/warning/dangerous-default-value.html * Expose get_planning_scene to python * Augment AllowedCollisionMatrix with setter methods * Add unit test for Python PlanningSceneInterface * [ROS-O] Fix Pilz unit tests (moveit#3561) * Avoid double-free of KDL::RotationalInterpolation in KDL >= 1.4.1 The underlying memory leak was fixed in KDL with orocos/orocos_kinematics_dynamics#180 * Decrease accuracy of quaternion check * PSM: add rudimentary tests including a check for whether the monitored scene does not change upon receiving full scenes (a current bug). * PSM: keep references to scene_ valid upon receiving full scenes plan_execution-related modules rely on `plan.planning_scene_` in many places to point to the currently monitored scene (or a diff on top of it). Before this patch, if the PSM would receive full scenes during execution, `plan.planning_scene_` would not include later incremental updates anymore because the monitor created a new diff scene. * Addressing discrepancies in parameter definition between IKFast solvers and the plugin template (moveit#3489) * Correct the inconsistent angle definition between IKFast solvers and the plugin template When determining the angle parameter to a IKFast solver of a type IKP_Translation*AxisAngle4D/IKP_Translation*AxisAngle*Norm4D, the current implementation takes the roll, pitch, and yaw value from the pose frame's orientation directly in each respective case, which does not match how angles are calculated in IKFast. In the solver-generating script of IKFast (ikfast.py), these angles are defined as (in the base link's frame): 1. the angle between the given axis (x/y/z) and the manipulator's direction vector, or 2. when the corresponding normal vector (z/x/y) is set, the angle between the given axis (x/y/z) and the projection of the manipulator's direction vector onto the plane of which the normal is the aforementioned one. Therefore, for example, if the orientation of a pose frame is described by (roll = PI, pitch = 0, yaw = 0), a direction vector (0, 0, 1) in the manipulator's frame will be mapped to (0, 0, -1); in that case, a TranslationXAxisAngle4D solver will view it as PI/2 from the x-axis, while the current implementation only comes up with 0 angle. Solution: This patch has implemented exactly the same way of angle calculation as done in IKFast, based on the assumption of where the manipulator's direction vector points to in its own frame. Note that this assumption is identical to the ones made in other cases, like IKP_Ray4D or IKP_TranslationDirection5D. * Allow users to change the direction vector defined in the plugin template through "create_ikfast_moveit_plugin.py" for customization There are several IK types (e.g. TranslationDirection5D) of which IKFast solvers require callers to pass in parameters calculated based on the manipulator's direction vector defined in its own frame; however, that vector is currently fixed to (0, 0, 1) in the plugin template, and this assumption might become incorrect if users have a different robot setup and manually generate their solver cpp with a customised wrapper XML specifying a different direction. Solution: Instead of letting users edit the plugin cpp every time after "create_ikfast_moveit_plugin.py" is run, this patch enhances the functionality of "create_ikfast_moveit_plugin.py" by allowing users to provide the direction vector of their choice via an input argument (--eef_drection); thus not only does the resulting plugin source file contain the desired setting, but the generated helping script "update_ikfast_plugin.sh" also keeps track of that argument, so that later updates will automatically stay consistent. Note that when not given in the input arguments, "--eef_drection" is set to (0, 0, 1) by default, and the generation falls back to using the original assumption. * Add a unit test for IKFast plugins of an iktype Translation*AxisAngle4D or Translation*AxisAngle*Norm4D To run this unit test, given the MoveIt source tree root being the current working directory, execute the following commands ./moveit_kinematics/test/test_4dof/test_4dof.sh catkin run_tests --no-deps -i moveit_kinematics Based on the current testing implementation for kinematics plugin (test_kinematics_plugin.cpp), the setup of this unit test involves 3 components: 1. Dedicated model description/config packages that are created for this unit test, which are packed into test_4dof/packages.tgz 2. A script (test_4dof/test_4dof-ikfast.test) to generate necessary packages, including IKFast plugins, that allow catkin to run a predefined test set 3. A test launch XML template (test_4dof/test_4dof-ikfast.test) describing test parameters and what tests to perform Note that for being able to reuse those existing test functions in test_kinematics_plugin.cpp, slight changes have been made to that cpp for dealing with the following two phenomena emerging when testing IKFast plugins targeting robots whose dof < 6: 1. Since an IKFast solver in this case can only produce part of pose information in the FK pass, the current implementation of getPositionFK() in the plugin template will always return false to indicate an error, causing the failure of every test run. To circumvent that behaviour and allow the IK functionality of plugins to be tested without being disrupted by getPositionFK(), a launch parameter "plugin_fk_support" with a default value "true" is introduced for indicating whether plugin's getPositionFK() is well supported; if it is set to false, poses from the FK pass will be obtained by updating the robot state with new angles and then calling moveit::core::RobotState::getGlobalLinkTransform(). 2. Since an IKFast solver in this case does not consume all pose information given for IK calculation, an IK solution can only guarantee a pose that matches the given one up to a certain extent; that also implies there might be no solution (approximately) equal to the joint states leading to the given pose. So evaluating solutions based on full pose/joint-states difference will inevitably leads to test failure. In order to have meaningful comparison, it is desired to only verify the information that is available and expected to stay unchanged between the FK/IK passes; and for cases where position checks remain valid (such as IKFast plugins of an iktype Translation*AxisAngle4D or Translation*AxisAngle*Norm4), that can be done via setting the newly added launch parameter "position_only_check" (false by default) to true to bypass full pose/joint-states checks and only measure the closeness between the derived position from solutions and the one from the original poses. * 🛠️ Bump pre-commit/action from 3.0.0 to 3.0.1 (moveit#3568) * CI: 🛠️ Bump action versions * MSA: Add missing filename attribute to gazebo plugin tag (moveit#3572) * Add joints_allowed_start_tolerance parameter (moveit#3287) ... for joint-specific start tolerances for TrajectoryExecutionManager Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> * Fixed order of return values in doc string of compute_cartesian_path() (moveit#3574) * Fix warehouse database destruction (moveit#3581) Standalone applications used the convenience function moveit_warehouse::loadDatabase() to load a database plugin. Internally, this uses a static plugin loader. As this static variable is released too late during app shutdown, we get an exception: ``` terminate called after throwing an instance of 'class_loader::LibraryUnloadException' what(): Attempt to unload library that class_loader is unaware of. ``` Explicitly instantiating the loader ensures correct destruction order and avoids the error. * Benchmarking with Google benchmark (moveit#3565) * Add benchmark dependency in moveit_core's package.xml * MoveGroupInterface: allow RobotState diffs as start state (moveit#3592) The old implementation used a RobotState instance for considered_start_state_, which was transformed into a RobotState message, when filling the actual MotionPlanRequest. This turned a provided diff message into a full message, which has performance drawbacks when there are many large attached object meshes. The new implementation uses the message format directly, avoiding unnecessary conversions. Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com> * Allow waiting for joint model group states and retrieval of group-specific timestamps (moveit#3580) This commit introduces: 1. per-group current state time retrieval 2. per-group complete robot state waiting This fixes the following issue: I want to call waitForCompleteState on group_A, which the API already supports. There is also another joint out_of_group_A in the robot model that is not available until some time after bringup. I'm calling waitForCompleteState(group_A, <timeout>) way earlier than out_of_group_A is published by its source. The problem with the current implementation is that it waits until <timeout> elapses, but returns true since all joints of group_A exist. I don't see a point in waiting for an out-of-group joint in this case. * Don't copy attached collision objects in CommandListManager::setStartState (moveit#3590) CommandListManager::setStartState shall update the start state in the next MotionPlanRequest to match the reached robot pose after previous planning steps. Copying attached collision objects is not necessary there, because the RobotState is initialized once in the beginning. In each iteration of CommandListManager::solveSequenceItems(), only joint states actually update. Copying the attached bodies is not only a performance penalty, but also introduces a (numerical) drift of the attached object poses as pointed out in moveit#3590 (comment) / moveit#3569. Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com> * Add moveit pro tutorials reference (moveit#3593) * Allow moving of all shapes of an object in one go (moveit#3599) * Rename ros-planning org (moveit#3603) * github.com/ros-planning -> github.com/moveit * codecov.io/gh/ros-planning -> codecov.io/gh/moveit * ros-planning.github.io -> moveit.github.io * 1.1.14 --------- Co-authored-by: Abe Maclean <abemaclean@gmail.com> Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> Co-authored-by: Michael Görner <me@v4hn.de> Co-authored-by: Salih Marangoz <12991962+salihmarangoz@users.noreply.github.com> Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> Co-authored-by: Hugal31 <hla@lescompanions.com> Co-authored-by: Lucas Walter <wsacul@gmail.com> Co-authored-by: Simon Schmeisser <simon.schmeisser@optonic.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com> Co-authored-by: nothingstopsme <50317951+nothingstopsme@users.noreply.github.com> Co-authored-by: Hugal31 <hugo.laloge@gmail.com> Co-authored-by: IrvingF7 <13810062+IrvingF7@users.noreply.github.com> Co-authored-by: Tom Noble <53005340+TSNoble@users.noreply.github.com> Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai> Co-authored-by: Captain Yoshi <captain.yoshisaur@gmail.com>

Description
Adds the ability to move the object shape poses from the PlanningScene.
One must supply all the shape poses. We could use some of the
CollisionObjectfield to only update the desired poses, but it would be hacky.move-shapes.mp4