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

Add object pose #2037

Merged
merged 16 commits into from
Aug 16, 2021
Merged

Add object pose #2037

merged 16 commits into from
Aug 16, 2021

Conversation

felixvd
Copy link
Contributor

@felixvd felixvd commented Apr 27, 2020

As discussed in #2025, this defines CollisionObjects with a PoseStamped instead of a header, so that an object can be moved together with its subframes and shapes.

I would like to get this in before the Noetic release and make it easy to review, so this PR includes only the very minimum that I believe this functionality needs. The tests passed locally, so I'd like to hear your thoughts.

Depends on moveit/moveit_msgs#69.

New summary here.


Checklist

  • Required by CI: Code is auto formatted using clang-format
  • Extend the tutorials / documentation reference
  • Document API changes relevant to the user in the MIGRATION.md notes
  • Create tests, which fail without this PR reference
  • While waiting for someone to review your request, please help review another open pull request to support the maintainers

MIGRATION.md Outdated Show resolved Hide resolved
moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved
moveit_core/collision_detection/src/world.cpp Outdated Show resolved Hide resolved
@tylerjw
Copy link
Member

tylerjw commented May 1, 2020

I'm not sure how it passes locally because I can't get it to build locally and neither can CI.

@felixvd
Copy link
Contributor Author

felixvd commented May 4, 2020

Did you include this change to moveit_msgs? It won't build without, which is why Travis is failing.

@tylerjw
Copy link
Member

tylerjw commented May 4, 2020

@felixvd It now works for me. Sorry, I didn't see that I needed that other change.

@felixvd felixvd mentioned this pull request May 6, 2020
3 tasks
@v4hn
Copy link
Contributor

v4hn commented May 21, 2020

@felixvd when do you get around to update this patch?

@felixvd
Copy link
Contributor Author

felixvd commented May 21, 2020

I just rebased this and it built fine on my machine.

@felixvd felixvd changed the title Add object origin Add object pose May 21, 2020
@felixvd
Copy link
Contributor Author

felixvd commented May 21, 2020

If I'm understanding right, the subframe test fails. I'll have a look at it this weekend. For some reason starting up the subframes tutorial gives me this error. Any ideas?

rviz: /build/ogre-1.9-B6QkmW/ogre-1.9-1.9.0+dfsg1/OgreMain/include/OgreAxisAlignedBox.h:252: void Ogre::AxisAlignedBox::setExtents(const Ogre::Vector3&, const Ogre::Vector3&): Assertion `(min.x <= max.x && min.y <= max.y && min.z <= max.z) && "The minimum corner of the box must be less than or equal to maximum corner"' failed.

moveit_core/collision_detection/src/world.cpp Outdated Show resolved Hide resolved
moveit_core/collision_detection/src/world.cpp Outdated Show resolved Hide resolved
moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved
moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved
@felixvd
Copy link
Contributor Author

felixvd commented May 24, 2020

I added the changes from the reviews and rebased on master, but there's something going on that I don't understand. When I try to start up the subframes_tutorial, Rviz sometimes crashes with the error I quoted further up. When Rviz doesn't crash, the subframe tutorial works fine, so I'm hopeful that this problem is what is causing the test to fail.

I think it has something to do with uninitialized memory, because the error occurs consistently with some builds but not at all with others, and object.pose (see below) always takes on the same random-looking value after a build, but the value changes in between builds (even when I change other unrelated things in the code, like debug messages). I couldn't find the cause for this behavior, but I pushed two commits of which one works and the other crashes on my machine.

I added these debugging outputs to the lines 1714-1735, and found that after the subframes tutorial node publishes the box and the cylinder, processCollisionObjectAdd in planning_scene.cpp is executed twice more, and at those times the object.pose is not empty anymore, but has random-seeming values:

object.pose: position: 
  x: 4.66536e-62
  y: 5.40156e-67
  z: 8.31857e-72
orientation: 
  x: 3.00735e-67
  y: -6.9902e-77
  z: -1.47439e+179
  w: 0.5

In the other build, where I changed nothing but the debug statements, it had this value:

object.pose: position: 
  x: 16.9477e-310
  y: 0
  z:  6.78681e+199
orientation: 
  x: 01.03984e+61
  y: 0
  z: -0
  w:  4.11911e+99

I can't continue debugging today, but my open questions are:

  1. Why is processCollisionObjectAdd running twice for the CollisionObjects as shown in the gist? Does this have something to do with the PlanningSceneMonitor, getPlanningSceneMsg, or getPlanningSceneDiffMsg?
  2. Why does object.pose have these weird values in the second run? It is especially confusing that object.id and object.header.frame_id work.

I'd be grateful if someone could confirm that it's not all an issue of my workspace setup or something similarly dumb. I pulled the master-source docker image again and double checked that all the other packages in the workspace are up-to-date, but you never know.


edit (going through my own posts to clean up):
The answer to question 1) is that the MotionPlanning plugin maintains its own PlanningScene. Launching rviz and move_group in different terminals solves this.
The answer to question 2) was probably due to returning addresses to non-persistent variables in world.

@jliukkonen
Copy link
Contributor

I approved the changes you made based on my suggestions but of course the issues you described above need to be resolved and debugged before merge. 👍

Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

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

So far this PR just adds the pose member to CollisionObjects, but doesn't really employ it.
I would expect that shape and subframe poses are also stored relative to the CollisionObject's pose.
Of course, this might will produce extra overhead when computing (absolute) collision transforms for all shapes. The desired behavior for this needs to be discussed!

Your spurious error is probably related to an inconsistent workspace: Some object files or libs still employ the old API, while some already use the new.

moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved
@@ -225,9 +225,14 @@ bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& tr
if (transform.isApprox(Eigen::Isometry3d::Identity()))
return true; // object already at correct location
ensureUnique(it->second);
for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i)
it->second->pose_ = transform * it->second->pose_;
for (Eigen::Isometry3d& shape_pose : it->second->shape_poses_)
Copy link
Contributor

Choose a reason for hiding this comment

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

If these poses are relative to pose_, why should they get adapted?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

At the moment, all the transformations in world.h are global, so I stuck with that convention. Making the poses (and subframes) relative to the pose would be a step towards a scene graph, which I don't think should be in the scope of this PR.

Copy link
Contributor

Choose a reason for hiding this comment

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

If you restrict the scope of this PR in this fashion, you don't need to explicitly store the pose of the collision object at all. Just transform all shape transforms accordingly.
This will avoid the initialization issues we see.

Copy link
Contributor

Choose a reason for hiding this comment

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

But if the pose is explicitly stored the pose of an object becomes clearly visible in the data structure too (at the cost of an additional transform to store for each object).

I would also prefer to store relative transforms with this patch.
It's the main point of this refactoring after all and puts the additional transform to good use.

If you do not include it, because you would run into API problems or unexpected complexity,
please add an appropriate issue (best case assigned to yourself 😎 ).

Copy link
Contributor

Choose a reason for hiding this comment

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

Good that we agree on this. I was kind of confused that the feature of relative transforms wasn't utilized yet.
However, we should discuss, maybe in tomorrow's maintainer meeting, whether we want to accept the additional matrix multiplication necessary to get absolute transforms (for each collision update).

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I could store the relative shape/subframe transforms and change all of the getTransform calls so they return pose*transform, but I suspect that the overhead of doing the multiplication every time will be significant. I would tend towards storing both global and local transforms, as suggested here, but @v4hn said you were opposed at some point(?). Would be great to have a benchmark for this decision.

I know you also brought up the idea of lazy lookups, which sounds good but this discussion of data structure options was inconclusive, and something like that would be yet a step further.

Maybe we can decide on something in the maintainer meeting.

Copy link
Contributor Author

@felixvd felixvd May 28, 2020

Choose a reason for hiding this comment

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

I just noticed that global and relative subframe poses are already stored in the attached_body since this PR, so I would say that adding relative and global subframes to the world objects as well makes the most sense.

edit: Although on second thought, that would be required for all the shapes as well. Hm!

Copy link
Contributor

Choose a reason for hiding this comment

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

I don't like that we attempt to mess up the whole frame handling even more instead of going for a clean scene graph interface.
If you start storing local and global transforms for subframes, you will need to do the same for the object shapes as well... Cleaning this up later is the same effort as adding it now...

Copy link
Contributor Author

Choose a reason for hiding this comment

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

And I am worried adding a graph structure will blow this change out of proportion, since the scene graph implementation isn't any clearer, and it would add the need for lazy lookups.

Storing all the transformations as relative ones from the object's pose may be the best bet for now.

moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

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

The error you observe, @felixvd, is due to the World::Object::pose_ not being initialized in World::addToObject, when a new object is created.
Furthermore, you need to correctly initialize moveit_msgs::CollisionObject.pose.orientation (the quaternion should read 0,0,0,1) in a lot of places all over the code base! There is still a lot of work to do.

@felixvd
Copy link
Contributor Author

felixvd commented May 27, 2020

you need to correctly initialize moveit_msgs::CollisionObject.pose.orientation (the quaternion should read 0,0,0,1) in a lot of places all over the code base!

One might think so, but this won't be a breaking change because we merged #2089. We could change from a WARN to a DEBUG message though, to make the change less noisy.

The error you observe, @felixvd, is due to the World::Object::pose_ not being initialized in World::addToObject, when a new object is created.

Thanks! :) Reminds me that the pose should also be stored in the AttachedBody class for completeness. I'll revise.

@v4hn
Copy link
Contributor

v4hn commented May 27, 2020

One might think so, but this won't be a breaking change because we merged #2089. We could change from a WARN to a DEBUG message though, to make the change less noisy.

That is a rather lousy excuse to ignore formal correctness. It's a warning for a reason and it needs to be addressed.

@felixvd felixvd force-pushed the add-object-origin branch 2 times, most recently from e5ebdd1 to 62d8ae4 Compare June 2, 2020 19:01
@felixvd
Copy link
Contributor Author

felixvd commented Aug 16, 2021

Alright, CI succeeds! We have been using this internally for over 6 months, the videos I linked in #2810 have been recorded including these changes, and it's over a year old.

I think all your comments have been adressed @rhaschke . We should merge and build on top of this.

Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

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

Thanks for addressing the remaining comments.

@rhaschke rhaschke merged commit 9503bfb into moveit:master Aug 16, 2021
@felixvd
Copy link
Contributor Author

felixvd commented Aug 16, 2021

Thanks for finding the hidden (and hopefully last) bug and seeing this through!

felixvd added a commit to felixvd/moveit_msgs that referenced this pull request Aug 16, 2021
With moveit/moveit#2037 merged, this disclaimer can be removed
v4hn pushed a commit to moveit/moveit_msgs that referenced this pull request Aug 16, 2021
With moveit/moveit#2037 merged, this disclaimer can be removed
Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

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

dammit, I never handed in the review!
Please have a look nonetheless @felixvd

if (update_subframe_poses)
{
obj->global_subframe_poses_ = obj->subframe_poses_; // TODO (felixvd): Inefficient copy, but iterating through two
// maps is complicated to write for this prototype
Copy link
Contributor

Choose a reason for hiding this comment

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

for this prototype ? 😄
You probably want to change either the comment or the implementation.
FixedFrameTransformsMap uses string-comparison in the map template, so I would expect the two maps have the exact same order.
As this is the most debated method (overhead of the update) it would be great to have it implemented without reallocating the whole RB-tree each time.

moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved
@@ -1708,6 +1697,8 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
return false;
}

// TODO (felixvd): Factor these 3 conditions out of here and processAttachedCollisionObject,
// and fill the pose vectors with Identity transforms instead of stopping.
Copy link
Contributor

Choose a reason for hiding this comment

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

Yes please!

This is an important migration step for me: If you used a single shape per object, you can fill in the pose field instead of the single entry in *_poses from now on.
Also it takes one line less to fill in (no resize needed) in user code.

If this is addressed somewhere else already, you might remove the TODO instead.

wxmerkt added a commit to wxmerkt/moveit that referenced this pull request Dec 8, 2021
…e format

The serialization format for the .scene files changed in
moveit#2037. This commits a
testcase using the old scene format. It will fail and a subsequent
commit to introduce backwards compatibility to the scene-file parsing
will make it pass.
v4hn pushed a commit that referenced this pull request Dec 10, 2021
* [moveit_core] test_planning_scene: Add failing unit test for old scene format

The serialization format for the .scene files changed in
#2037. This commits a
testcase using the old scene format. It will fail and a subsequent
commit to introduce backwards compatibility to the scene-file parsing
will make it pass.

* [moveit_core] PlanningScene: Add backwards compatibility for old scene version format

This commit adds a mechanism for detecting the version of the scene file
format to enable the loadGeometryFromStream method to read old version
scene files without having to migrate them. To detect the version of the
scene format, we use the content of the line following the start of an
object: In the old version of the format, this specified the number of
shapes in the object (a single int). In the new version of the format,
it is the translational part of the pose of the object (i.e. three
double values separated by spaces). To detect the format, we check for
the number of spaces after trimming the string.

* Simplify code: Avoid reading full stream

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
captain-yoshi added a commit to captain-yoshi/urdf_to_scene that referenced this pull request Feb 21, 2022
MoveIt version 1.1.6 introduced a new reference frame for collision
objects (moveit/moveit#2037).
vatanaksoytezer pushed a commit to moveit/moveit2 that referenced this pull request May 16, 2022
* Improve error messages

- Downgrade ERROR to WARN
- Report affected joint name
- Quote (possibly empty) planner id

* Avoid duplicate error messages

* Don't complain about missing limits for irrelevant JMGs

When planning an arm motion, Pilz's PTP planner shouldn't complain (and bail out)
on missing joint limits of hand joints!

* Fix unittests by providing a valid JMG

* Remove deprecated xacro --inorder

* Fix typo: demangel -> demangle

* Drop the minimum velocity/acceleration limits for TOTG (#2937)

Just complain about negative / zero values.

* CI: Explicitly add always() to if conditions

Otherwise, on a failure, the implicitly used success() will skip the step.

* Reset markers on display_contacts topic after a new planning attempt

* Silent warning about virtual_joint in Gazebo setups

Gazebo requires a fixed joint from world to the first robot link.
This resembles the virtual_joint of SRDF.
However, the RobotModel parser issues the following warning:
Skipping virtual joint 'xxx' because its child frame 'xxx' does not match the URDF frame 'world'

* Remove execution_type argument from real-robot controller_manager.launch

* Fix definition of real-robot moveit_controller_manager

Fixes the following error (occurring since 61d18f2)
```
[FATAL] ros.moveit_ros_planning.trajectory_execution_manager:
  Exception while loading controller manager 'robot':
  According to the loaded plugin descriptions the class robot
  with base class type moveit_controller_manager::MoveItControllerManager does not exist.
```

As we introduced `pass_all_args="true"`, the value of the argument
`moveit_controller_manager` was the robot name.

* Rework moveit_controller_manager handling

There are 3 basic MoveIt controller manager plugins:
- fake = `moveit_fake_controller_manager::MoveItFakeControllerManager`
  Used in demo.launch. Doesn't really control the robot, but just
  interpolates between via points. Allows these execution_types:
  - via points: just jumps to the via points
  - interpolate: linearly interpolates between via points (default)
  - last point: jumps to the final trajectory point (used for fast execution testing)

- ros_control = `moveit_ros_control_interface::MoveItControllerManager`
  Interfaces to ros_control controllers.

- simple = `moveit_simple_controller_manager/MoveItSimpleControllerManager`
  Interfaces to action servers for `FollowJointTrajectory` and/or `GripperCommand`
  that in turn interface to the low-level robot controllers (typically based on ros_control)

However, so far move_group.launch distinguished between `fake` and `robot` only.
The argument moveit_controller_manager now allows switching between all 3 variants.
Adding more *_moveit_controller_manager.launch files allows for an extension of this scheme.

* Cleanup generation of ros_controllers.yaml

* Fix handling of sensors_3d.yaml

- Reading both, the default and the existing package's sensors_3d.yaml
  into the config, the config file was growing by 2 configs each time.
- Not visiting the Perception tab, was writing the default config with 2 entries
- Selecting "None" was writing an invalid config:
  sensors:
    - {}
    - {}

* Add gazebo_controllers.yaml

* Rework controller config generation

We should write separate controller config files for different controller managers:
- simple_moveit_controllers.yaml handles everything relevant for SimpleMoveItControllerManager
- ros_controllers.yaml handles ros_control config
- gazebo_controllers.yaml handles controllers required for Gazebo

* Fix ros_controllers.yaml: always handle joints as sequence

* Rename ROSControlConfig -> ControllerConfig

* Rename functions *ROSController* -> *Controller*

* Rename ros_controllers_config_ -> controller_configs_

* Rename files ros_controllers_widget.* -> controllers_widget.*

* Rename ROSControllersWidget -> ControllersWidget

* Update widget texts to speak about generic controllers

* Simplify code

* Fix controller choice

- Provide all types of JointTrajectoryController as well as
  FollowJointTrajectory and GripperCommand (use by simple manager)
- Use effort_controllers/JointTrajectoryController as default
- Create FollowJointTrajectory entries for any JointTrajectoryController

* Formatting

* demo.launch: start joint + robot-state publishers in fake mode only

This will facilitate re-use of demo.launch.

* Modularize demo_gazebo.launch: draw on demo.launch

* gazebo.launch: Load URDF via xacro if neccessary

* gazebo.yaml: Allow initial_joint_positions

* gazebo.launch: delayed unpause

Only unpause simulation when robot model was loaded.
This ensures that the initial pose is actually held.

* Pilz unittests: use test_environment.launch

* Rename launch argument execution_type -> fake_execution_type

... to clarify that this parameter is only used for fake controllers

* moveit.rviz template: remove link names

* moveit.rviz: Use Orbit view controller

* Improve instructions

* Allow checking/unchecking multiple files for generation

* Simplify definition of `planning_plugin` parameter

There is no means to declare the planning_plugin as an arg first.

* Pilz: Define default planner

* Enable compiler warnings

* Use catkin_tools_devel builder from industrial_ci

ros-industrial/industrial_ci#707

* Add comments

* Use test_environment.launch in unittests

* Remove unused moveit_planning_execution.launch

* Fix orientation of subframe offset in Pilz planners (#2890)

Fix #2879 by reorienting the subframe offset applied to a goal pose in the PTP planner,
so that it is applied in the correct direction.
Also add the same offset computation to LIN and CIRC generators,
thus allowing them to work with subframes.

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

* CI: rename cross_platform_ci -> robostack

* CI: prerelease.yaml: only allow manual triggering

* README.md: Update badges

* 1.1.6

* GHA: auto-sync noetic-devel to master (#2952)

* Fixup auto-sync noetic-devel to master

* Upload controller_list for simple controller manager (#2954)

* Fixup auto-sync noetic-devel to master

Fix push permissions.

* Add ns for depth image & pointcloud octomap updaters (#2916)

* MSA: Notice file updates (#2964)

This commit fixes a MSA bug causing files in a loaded MoveIt config to be incorrectly classified as externally modified
after being written by the "Generate Package" button.
As this status is solely based on the file timestamp relative to the timestamp stored in the .setupassistant file,
we need to update this timestamp when we wrote the files.

* RDFLoader: clear buffer before reading content (#2963)

* Add waypoint duration to the trajectory deep copy unit test (#2961)

* Add waypoint duration to the trajectory deep copy test

* Slightly more accurate comments

* MoveitCpp - added ability to set path constraints for PlanningComponent. (#2959)

* Joints widget: avoid flickering of the nullspace slider

Show a (disabled) dummy slider if there is no nullspace.
This avoids flickering between zero and one slider, which is the most common case.
Also provide some tooltips to explain the usage.

* MPD: Avoid flickering of the progress bar

The progress bar shows the number of pending background jobs.
If there is only one job pending, the progress bar is shown and
immediately hidden as soon as the process is finished.
Thus, we shouldn't show the progress bar if there is only one job
and thus no actual progress to show.

Use the default size and color scheme.

* Switch to std::bind (#2967)

* boost::bind -> std::bind
  grep -rlI --exclude-dir=.git "boost::bind" | xargs sed -i 's/boost::bind/std::bind/g'
* Convert bind placeholders
  grep -rlI --exclude-dir=.git " _[0-9]" | xargs sed -i 's/ _\([0-9]\)/ std::placeholders::_\1/g'
* Update bind include header
  grep -rlI --exclude-dir=.git "boost/bind" | xargs sed -i 's#boost/bind.hpp#functional#'

* Add marker for subgroups even if no endeffector is defined for them (#2977)

For single groups, the old logic fell back to add a marker
for the last link if IK is supported for it and no endeffector is defined.

That (quite reasonable) fallback did not yet work for subgroups though.

* Use termination condition for simplification step (#2981)

... to allow canceling the simplification step

* Add backwards compatibility for old scene serialization format (#2986)

* [moveit_core] test_planning_scene: Add failing unit test for old scene format

The serialization format for the .scene files changed in
moveit/moveit#2037. This commits a
testcase using the old scene format. It will fail and a subsequent
commit to introduce backwards compatibility to the scene-file parsing
will make it pass.

* [moveit_core] PlanningScene: Add backwards compatibility for old scene version format

This commit adds a mechanism for detecting the version of the scene file
format to enable the loadGeometryFromStream method to read old version
scene files without having to migrate them. To detect the version of the
scene format, we use the content of the line following the start of an
object: In the old version of the format, this specified the number of
shapes in the object (a single int). In the new version of the format,
it is the translational part of the pose of the object (i.e. three
double values separated by spaces). To detect the format, we check for
the number of spaces after trimming the string.

* Simplify code: Avoid reading full stream

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* Allow restricting collision pairs to a group (#2987)

* Increase ccache size (#2990)

Increase ccache size to 10G. This is particularly needed for ccov's debug build, which uses 8G according to the stats.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* RoboStack CI: Pin tinyxml2 version (#2993)

* quietly use backward_cpp/ros if available (#2988)

This is simply convenient and you always need it when you did not explicitly add it.

Follow @tylerjw's initiative to add it to MoveIt2:
#794

* add comment to mention loading of old scene files

Followup to 9d78be4

* Provide MOVEIT_VERSION_CHECK macro (#2997)

- Rename MOVEIT_VERSION -> MOVEIT_VERSION_STR
- MOVEIT_VERSION becomes a numeric identifier
- Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0)

* feat(simple_controller_manager): add `max_effort` parameter to GripperCommand action (#2984)

This commit adds the `max_effort` parameter to the GripperCommand
declaration in the `controller_list` (see issue #2956). This value is
only used when effort is set in the requested gripper trajectory.

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>

* Remove all remaining usage of robot_model

* round_collada_numbers.py: python 2/3 compatibility (#2983)

Python3 requires the files to be opened in binary mode read a bytes object instead of a string, which is needed in turn by etree.parse().

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* Do not assert on printTransform with non-isometry (#3005)

instead print a tag and the matrix
building a Quaternion from non-isometries is undefined behavior in Eigen, thus the split.

* add pilz planner to moveit_planners dependency

It should have been there for quite some time now...

* pilz: report joint name with invalid limits in start state

- remove unused utility method
  it does not provide enough feedback, is almost trivial and does redundant checks in the single case it's called from.

* pilz: restrict start state check to active group

* Add API stress tests for TOTG

* TOTG: catch division by 0

This bug is already in the original implementation:

https://github.com/tobiaskunz/trajectories/blob/master/Path.cpp

In case the dot product between the two vectors is close to +/-1,
angle becomes +/-PI and cos/tan of 0.5 * PI in the lines below will
produce a division by 0.

This happens easily if a optimal trajectory is processed by TOTG, i.e.,
if a trajectory is processed by TOTG twice.

* Fix MoveGroupInterface uninitialized RobotState (#3008)

* RobotState::attachBody: Migrate to unique_ptr argument (#3011)

... to indicate transfer of ownership and simplify pointer handling

* Disable (flaky) timing tests in DEBUG mode (#3012)

* Pass xacro_args to both, urdf and srdf loading

* Modernize loops

* Move MoveItErrorCode class to moveit_core (#3009)

... reducing code duplication and facilitating re-use

* Adapt to API changes in srdfdom

* Don't fill all ACM entries by default

* Improve formatting of comments

* ACM: specific pair entries take precedence over defaults

Reverts c72a857

* Add comment to prefer setDefaultEntry() over setEntry()

... because the former will consider future collision entries as well.

* Optimization: Check for most common case first

* Adapt message passing of AllowedCollisionMatrix

- Serialize full current state (previously pairs with a default, but no entry were skipped)
- Only initialize matrix entries that deviate from the default.

* ACM:print(): show default value

* Adapt to API changes in srdfdom

@v4hn requested splitting of collision_pairs into (re)enabled and disabled.

* Unify initialization of ACM from SRDF

* Move MoveItConfigData::setCollisionLinkPairs to collisions_updater.cpp

This method is only used there to update disabled collision entries.

* Disable slow robot_interaction tests in DEBUG mode (#3014)

These tests are known to run for a very long time in debug builds. So let's disable them in this case.
If you still insist to run them, you can do so via `locked_robot_state_test --gtest_also_run_disabled_tests`

* 1.1.7

* collision_distance_field: Fix undefined behavior vector insertion (#3017)

Co-authored-by: andreas-botbuilt <94128674+andreas-botbuilt@users.noreply.github.com>

* Fix deprecation warning in moveit_cpp (#3019)

Fixup for #3009.

* Fix wrong transform in distance fields' determineCollisionSpheres() (#3022)

* Make TimeParameterization classes polymorphic (#3021)

* MSA: Add STOMP + OMPL-CHOMP configs (#2955)

- Add stomp planner to MSA
- Add OMPL-CHOMP planner to MSA
- Remove obsolete CHOMP parameters
- Update CHOMP config parameters to match code defaults
- Create CHOMP config via template (instead of code)

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* Fix CI: update apt packages before install

* CI: Fetch srdfdom from source

* Update .rosinstall: Include srdfdom

* Resolve ambiguous function specification (#3040)

As Eigen introduced construction from brace-initializers as well, we do need to distinguish between
void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>&) and
void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd&)

* 🧪 testspace (#955)

* [hybrid planning] Add action abortion and test; improve the existing test (#980)

* Add action abortion and test; improve the existing test

* Add controller run-dependency

* Fix the clearing of robot trajectory when a collision would occur

* Fix replanning if local planner is stuck

* Lambda function everything

* Thread safety for stop_hybrid_planning_

* Thread-safe state_

* Clang tidy

* Update the planning scene properly

* Update Servo test initial_positions.yaml

Co-authored-by: Tyler Weaver <tyler@picknik.ai>

* Improve loading of planning pipelines (#3036)

Ensure that planning pipelines considered in `~/planning_pipelines/*` actually have a `planning_plugin` parameter defined.
Otherwise, issue an error message.

* Avoid downgrading default C++ standard (#3043)

* 1.1.8

* Disable hybrid planning test, don't cache ci docker at all and don't cache upstream_ws if repos file is changed (#1051)

* Don't cache docker builds

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

* Don't cache upstream ws

* Use new action for getting the latest timestamp .repos file has been edited

* Debug

* Fix repos path

* Disable hybrid planning test

* Use more verbose name

Co-authored-by: Tyler Weaver <tylerjw@gmail.com>

* Fix use of std::bind (#3048)

Fixup for ab42a1d (#2967)

* Fix missing include (#3051)

* Explicitly set is_primary_planning_scene_monitor in Servo example config (#1060)

* Fix race condition in SynchronizedStringParameter::waitForMessage (#1050)

Co-authored-by: Tyler Weaver <squirrel428@protonmail.com>

* Misc fixes for time and transforms (#768)

* Fix setting shape_transform_cache_lookup_wait_time from seconds
* Fix setting last_update_time from seconds
* Check the return value of canTransform

* Fix object interactive marker in wrong pose after changing the fixed frame (#680)

* Get parameter on initialize (rebased version of #893) (#996)

Get parameter `trajectory_execution.execution_duration_monitoring` in
initialize().


Co-authored-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* Add cleaning action (#641)

* [hybrid planning] Use a map of expected feedback codes (#1065)

* Use a map of expected feedback codes

* Use a constexpr function instead of unordered_map

* Don't need this #include

* Minor function renaming

* Fix docker cleanup action (#1071)

* [Hybrid Planning] configurable planning scene topics (#1052)

* Fix Python versioned dependency (#3063)

* Use galactic in main CI (#1077)

* Do not automatically load robot description in move_group.launch (#3065)

MoveIt should not overwrite a previously uploaded robot description.
It should only provide it optionally in demo mode.

* fix move_group_commander.go(tuple(...)) (#3066)

* commander: add test for go() method interfaces

> self.assertTrue(self.group.go(tuple(current_joints)))
is currently broken and fixed in the second commit

* fix move_group_commander.go(tuple(...))

Passing a tuple of doubles triggers KeyError instead of TypeError.

Co-authored-by: Hongzhuo Liang <lianghongzhuo@126.com>

* Fix mixed-up implementations in TfSubscription creation (#1073)

Co-authored-by: Jean-Christophe Ruel <jeanchristophe.ruel@elmec.ca>

* [hybrid planning] Adjust planning scene locking (#1087)

* Create a copy of the planning scene. const robot state.

* Use LockedPlanningSceneRO over lockSceneRead()

* Use lambda function

* Add option to use simulation time for rviz trajectory display (#3055)

* MSA: boost::bind -> std::bind (#3039)

* Compilation fixes for Jammy and bring back Rolling CI (#1095)

* Use jammy dockers and clang-format-12
* Fix unused depend, and move to python3-lxml
* add ompl to repos, fix versions and ogre
* Remove ogre keys
* Fix boolean node operator
* Stop building dockers on branch and fix servo null pointer
* update pre-commit to clang-format-12 and pre-commit fixes
* clang-format workaround and more pre-commit fixes

* Fix collisions_updater's set comparison (#3076)

Use operator< of std::pair(string,string) for comparing two link pairs.

* Use GLEW::GLEW link target (#3079)

While ${GLEW_LIBRARIES} works on Ubuntu, it fails on newer installs of GLEW.

* Add Ptr definitions for TimeParameterization classes (#3078)

Follow up on #3021.

* Add special case for sphere bodies in sphere decomposition (#3056)

* moveit joy: add PS3 dual shock model (#3025)

* Added PS3 dual shock
* Simplified if-else statements with as one-liners

* Avoid creating duplicate transmission tags

Only add Gazebo transmission tags for joints if they are not yet present.

* 1.1.9

* fixup: fix variable name: transitions_elements -> transmission_elements

* fixup: avoid segfaults if expected XML elements are missing

* fixup: (slightly) improve comment

* static_cast<std::string>(*) -> std::string(*)

* Use more specific check for correct tag

* Allow (over)writing the Gazebo-compatible URDF

* fixup: avoid code duplication

* fixup: Improve message boxes

* fixup: Drop hidden_func_ from ConfigurationFilesWidget

but (re)create the list of to-be-generated files each time
entering the widget to allow for dynamic adaption of the file list

* fixup: Simplify saving

- new_gazebo_urdf_ -> save_gazebo_urdf_
- directly save content, avoid extra parsing
- hide overwrite button if doc is empty
- disable overwrite button if saving is not possible due to xacro

* fixup: config_path_ -> static const CONFIG_PATH

* getGazeboCompatibleURDF(): Skip catching YAML exceptions

There is no YAML involved!

* getGazeboCompatibleURDF(): Compare original and final XML

Instead of manually keeping track of changes, compare the two docs.
This is much more robust.

* Simplify getGazeboCompatibleURDF()

Use a new utility function uniqueInsert() to avoid code duplication
when inserting XML elements uniquely.

* Replace manual highlighting with a SyntaxHighlighter

* XmlSyntaxHighlighter: allow nested highlighting

* Use focusGiven() + focusLost() to generate and validate Gazebo URDF

* Fix widget layout

* Provide button to open original URDF file

* Move getGazeboCompatibleURDF() from MoveItConfigData to SimulationWidget

This is a function specific to the SimulationWidget

* Filter more invalid values in moveit_benchmark_statistics.py (#3084)

Count "-nan" and "-inf" as null value in the database.

* Temporarily add galactic CI (#1107)

* Add galactic CI
* Comment out rolling
* panda_ros_controllers -> panda_ros2_controllers
* Ignore flake8 tests

* [moveit_configs_utils] Minor fixes (#1103)

* Make lockSceneRead() and lockSceneWrite() protected member functions (#1100)

* No lock in planning_component.cpp

* Make lockSceneRead(), lockSceneWrite() protected

* Add a migration note

* Fix missing boost::ref -> std::ref

* Fix copyright notice in README script (#1115)

* Off by one in getAverageSegmentDuration (#1079)

* Off by one in getAverageSegmentDuration

* Case for one waypoint

Co-authored-by: AndyZe <andyz@utexas.edu>

* Warn if too few waypoints to get duration

Co-authored-by: AndyZe <andyz@utexas.edu>

* Discount first duration_from_previous from average duration if it is 0

* Restore empty duration from previous check as per Andy's suggestion

* Changed warning message for case with 1 segment with 0 duration to be distinct from empty durations

Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
Co-authored-by: AndyZe <zelenak@picknik.ai>

* Remove include of OgrePrerequisites header (#1099)

* Remove OgrePrerequisites include

* octomap_render.h includes itself

* Include OgrePrerequisites.h

* New Launch Files using moveit_configs_utils (#1113)

* Add Package Path
* Launch Utils
* Some Basic Launch Files
* Remove circular dependencies

* [moveit_cpp] Fix double param declaration (#1097)

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* [moveit_cpp] Fix config of multiple pipelines (#1096)

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* Remove launch_param_builder (#1133)

* Simply MoveItCpp::getPlanningPipelineNames() (#1114)

* Don't overwrite existing attributes

* RDFLoader Broken with Xacro Files (#1132)

* A broken RDFLoader test

* Bugfix: Add space between executable and path (if no arguments)

* Add inertial/origin tag

* Consider eef's parent group when creating eef markers (#3095)

If we have end-effector(s) defined, a corresponding rviz marker for IK
should be created only if the eef's group matches the considered JMG.

* ACM: Consider default entries when packing a ROS message (#3096)

Previously, getAllEntryNames() just returned names occurring in the collision pair list.
Now, also consider names in `default_entries_`.

* Remove new operators (#1135)

replace new operator with make_shared

* Update black version, formatting changes (#1148)

* Set controller status before it is checked on trajectory execution (#1014)

* Return `ExecutionStatus` from `MoveItCpp::execute()` (#1147)

Return an `ExecutionStatus` from `MoveItCpp::execute()`, which is
convertible to a bool in the caller code.
This change is forward compatible.

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* [ompl] Small code refactor (#1138)

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* Enable rolling / jammy CI (again) (#1134)

* Use ros2_control binaries
* Use output screen instead of explicitly stating stderr

* Use a steady clock for timeout for IK (#795)

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* Suppress clang tidy warning performance-no-int-to-ptr

* [moveit_ros_benchmarks] Add missing moveit_core dependency (#1157)

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* Fix prbt_ikfast win compilation (#1161)

* Revert OMPL parameter loading

* Comment failing rdf integration test

* Rename panda controllers

* Fix failing test

* Remove moveit_msgs and ompl from moveit2.repos (#1166)

* Replace num_dof and idx variables with JointGroup API (#1152)

* Fix Moveit Configs Utils Bug (#1174)

* Ruckig smoothing cleanup (#1111)

* Simplify. Use Ruckig as a check if duration needs extension.

* Add a test for a single waypoint

* Add a unit test that actually requires math

* Use a traj deep copy. Right-const everywhere. Minor cleanup.

* Fix the test timestep

* Read jerk from the robot model

* Revert to right-const everywhere

* Simplify jerk limit expression

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>

* Add a helpful hint if no vel/accel/jerk limits are defined.

* Update test conditions since I changed the jerk limit

* Rebase cleanup

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>

* [Servo] Add override parameter to set constant velocity scaling in Servo (#1169)

* Remove sbpl planner (#1173)

* RCLCPP Upgrade Bugfixes (#1181)

* Add launch file configurations for static tfs and spawning controllers (#1176)

* Delete an unused variable and a redundant log message (#1179)

Co-authored-by: Vatan Aksoy Tezer <vatan@picknik.ai>

* move_group launch for moveit_configs_utils (#1131)

* Add a warning for TOTG if vel/accel limits aren't specified. (#1186)

Co-authored-by: Vatan Aksoy Tezer <vatan@picknik.ai>

* Disable separate TransformListener thread in OccupancyMapServer (#1130)

* Demo launch for moveit_configs_utils (#1189)

* Demo launch for moveit_configs_utils

* Don't respawn rviz

* Fix CI permission errors (#1206)

* Fix CI permission errors

* Loosen the Ruckig test tolerance

* Comment fixup

* Clamp inputs to Ruckig. Use current waypoint as input for next iteration (#1202)

* Clamp inputs to Ruckig. Use the current waypoint as input for next iteration.

* Fix the usage of std::clamp()

* Use orocos_kdl_vendor package (#1207)

* simplify distance field method binding

source: moveit/moveit@0322d63

* drop unused arguments not needed for lambda binding

source: moveit/moveit@6805b7e ; I have also had to update how moveit_msgs is referenced (movit_msgs:: -> moveit_msgs::msg:: ) and I  added the changes to this commit that correspond to tests for the constraint samplers package.

* migrate PRA internals to lambdas

source: moveit/moveit@6436597; in addition to the original commit I updated logging to support ros2 logging standards.

* remove unused arguments from kinematics test

source moveit/moveit@ddb68b6; I also had to amend moveit_msgs to moveit_msgs::msg in this commit, otherwise everything remains the same as source commit. When I ran the kinematics plugin test locally it threw an error both before and after this change. Hopefully we can revisit this point as part of the code review, the error related to the robot description.

* planning_context_manager: rename protected methods

sources: moveit/moveit@a183bc1, moveit/moveit@c07be63;

* various: prefer object and references over pointers

source: moveit/moveit@1a8e571; this commit contains minor changes when compared to the source commit which it is based on, these changes are limited to ensuring compatibility with ROS2.

* banish bind()

source:moveit/moveit@a2911c8; required minor updates compared to original source commit in order to ensure compatibility with ROS2

* add check for bind() to clang-tidy

* Fix clang-tidy warning (#1208)

* CI: More generically fix permission errors (#1215)

* CI: More generically fix permission errors

Disable git security check for any folder.
We are running a docker container on GHA. Chances are low that this gets compromised.

* Revert changes to source Dockerfile

These are inherited from ci.

* Fix double delete in PILZ CIRC generation (#1229)

* Fix deprecated namespace (#1228)

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

* Remove unused includes for boost::bind (#1220)

* Make moveit_common a 'depend' rather than 'build_depend' (#1226)

* Change condition for loading default OMPL config (#1222)

* Fix reading joint weights in KDLKinematicsPlugin (#1216)

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>

* Make TOTG the default time-parameterization algorithm everywhere (#1218)

Co-authored-by: Jafar <cafer.abdi@gmail.com>

* Enable cppcheck (#1224)

Co-authored-by: jeoseo <jeongwooseo2012@gmail.com>

* Allow custom velocity/acceleration limits for TOTG time-parameterization algorithm (#1195)

* A new function for common time-param calculations

* Flesh out the second computeTimeStamps function

* Reading of the custom limits

* Add a unit test

* Revert comment changes about exceptions

* Small efficiency improvements

* Add computeTimeStamps() overload to the base class

Co-authored-by: Jafar <cafer.abdi@gmail.com>

* Allow custom velocity/accel/jerk limits for Ruckig smoothing (#1221)

* Move common functionality to utility functions
* Add a utility function to read RobotModel bounds
* Add a unit test for the new version of computeTimeStamps()
* Clean up Doxygen
* Use ruckig::DynamicDOFs everywhere

* Tidy up the mess (#1243)

* Alternate Package Name Specification (#1244)

* Declare the default_planning_pipeline parameter (#1227)

Co-authored-by: AndyZe <zelenak@picknik.ai>

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Co-authored-by: Jonathan Grebe <grebej@uwindsor.ca>
Co-authored-by: Tom Noble <tom@additiveautomations.com>
Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>
Co-authored-by: Tim Redick <tim.redick@rwth-aachen.de>
Co-authored-by: Rick Staa <rick.staa@outlook.com>
Co-authored-by: AndyZe <zelenak@picknik.ai>
Co-authored-by: Colin Kohler <72100281+Colin-Kohler@users.noreply.github.com>
Co-authored-by: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Co-authored-by: Michael Görner <me@v4hn.de>
Co-authored-by: Simon Schmeisser <simon.schmeisser@optonic.com>
Co-authored-by: Wolfgang Merkt <wxmerkt@users.noreply.github.com>
Co-authored-by: Tyler Weaver <tyler@picknik.ai>
Co-authored-by: Wolf Vollprecht <w.vollprecht@gmail.com>
Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>
Co-authored-by: Tomislav Bazina <tbazina@gmail.com>
Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
Co-authored-by: Captain Yoshi <captain.yoshisaur@gmail.com>
Co-authored-by: pvanlaar <22322118+pvanlaar@users.noreply.github.com>
Co-authored-by: andreas-botbuilt <94128674+andreas-botbuilt@users.noreply.github.com>
Co-authored-by: Jeroen <jeroendemaeyer@live.be>
Co-authored-by: Martin Oehler <oehler@sim.tu-darmstadt.de>
Co-authored-by: Vatan Aksoy Tezer <vatan@picknik.ai>
Co-authored-by: Tyler Weaver <tylerjw@gmail.com>
Co-authored-by: Tobias Fischer <info@tobiasfischer.info>
Co-authored-by: Joseph Schornak <joe.schornak@gmail.com>
Co-authored-by: Abishalini <abi.gpuram@gmail.com>
Co-authored-by: Cory Crean <cory.crean@picknik.ai>
Co-authored-by: Tyler Weaver <squirrel428@protonmail.com>
Co-authored-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
Co-authored-by: Hongzhuo Liang <lianghongzhuo@126.com>
Co-authored-by: Jean-Christophe Ruel <ruelj2@gmail.com>
Co-authored-by: Jean-Christophe Ruel <jeanchristophe.ruel@elmec.ca>
Co-authored-by: Loy van Beek <LoyVanBeek@users.noreply.github.com>
Co-authored-by: Job van Dieten <job-1994@users.noreply.github.com>
Co-authored-by: Hugal31 <hugo.laloge@gmail.com>
Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: Sencer Yazıcı <senceryazici@gmail.com>
Co-authored-by: Denis Štogl <denis@stogl.de>
Co-authored-by: Burak Payzun <brkpyzn@gmail.com>
Co-authored-by: Marq Rasmussen <marq.razz@gmail.com>
Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
Co-authored-by: jeoseo <50963152+jeoseo@users.noreply.github.com>
Co-authored-by: jeoseo <jeongwooseo2012@gmail.com>
v4hn pushed a commit to moveit/moveit_msgs that referenced this pull request May 10, 2023
sjahr pushed a commit to sjahr/moveit that referenced this pull request Jun 21, 2024
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

10 participants