Add named frames to CollisionObjects#1060
Add named frames to CollisionObjects#1060felixvd wants to merge 12 commits intomoveit:kinetic-develfrom
Conversation
This changes the processAttachedCollisionObject function somewhat. Comments added to make it easier to follow.
If a motion plan request is sent through the move group interface, and its end_effector_link is a named frame on an AttachedBody instance, the function added in this commit makes the motion plan request valid by transforming the eef_link to a robot link. It uses a MoveGroupCapability because the MoveGroupInterface only tracks the robot joints, not the attached bodies. Doing it via a request to the PlanningScene would be possible as well, but this is the first prototype. "validateConstraints" is not a good name for a function that forces the constraints to become valid, but I couldn't come up with anything better on the spot.
They appear in the scene as red dots and in the Scene Objects tab.
|
Wow, this is quite a lot of work 🥇 We shortly talked about this before and, unless I'm mistaken now, Afaics now this will leave us with two somewhat redundant mechanisms if we should ever get around to implement scene graph support (which has been requested before). |
|
Oh, one more important point. Personally I believe |
|
I agree that a full scene graph would be good. The main reason I went this way is that I wasn't sure how much effort that would be, and this seemed like a less intrusive change. It also solves most of my application's pain points with a conceptually simple change, which was an argument in favor. With the child objects/scene graph structure we discussed, I worried about additional pitfalls (recursive transforms and message processing). I think I might be able to tackle it now that I know a bit more, but I didn't want to bite off more than I could chew. And I am still not sure if I really know how the world object and AttachedBody data are communicated to different parts of the system. While the child link structure may be redundant in some ways in the future, it is readable: One physical object is contained in a single CollisionObject message, and the intention of a named frame is clear. If scene graph support is added in the way you envision and all child frames are collision objects, an object with child frames would be spread over multiple CollisionObject messages, which seems less convenient and harder to read. That said, I'm totally open to doing and arranging things differently. Lastly, this change doesn't get in the way of future work, and could coexist with a scene graph, as you said. The meat of commit If this can't be merged into |
|
Really great work! More thoughts, beyond those in the related issue #1041 you created:
This needs to be updated in the tutorial, either a new one or add to an existing one
This has some big changes to MoveIt!, adding a few tests would make it much easier to get in |
moveit_core/robot_state/include/moveit/robot_state/attached_body.h
Outdated
Show resolved
Hide resolved
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Show resolved
Hide resolved
| @@ -160,7 +160,7 @@ static QString decideStatusText(const collision_detection::CollisionWorld::Objec | |||
| { | |||
There was a problem hiding this comment.
i think changes to Rviz should be a follow-up PR to keep the size of this one manageable. feel free to create it now, though
There was a problem hiding this comment.
Sure? It would make testing this PR less convenient, and I hope the Rviz changes will stay pretty limited.
There was a problem hiding this comment.
its not a hard rule, just a would be nice. your argument is fine
There was a problem hiding this comment.
#1122 and the ToDo in the code make a good case for rethinking the Rviz changes and how to visualize these frames. Red dots are obviously not ideal. I will keep this in for testing and remove it before the merge.
|
Thanks for the review. It looks like clang introduced a number of unnecessary changes. In the meantime, I actually made the planning scene monitor in our system publish the named frames to TF for convenience. I'm not sure about that choice, and why the objects weren't in TF before that. I'll bring this up in the issue about the planning scene/collision world you asked me to make. |
|
The recommended command line command for using the correct clang format is here: |
|
Thanks, that helps. I'm in a crunchtime I had to skip ROSCon and IROS for, but I'll get back to this at the latest after 20 October and before MoveIt Day. As a quick note to myself, this is not compatible with planCartesianPath yet. |
| const Eigen::Affine3d& getTransform(const std::string& id) const; | ||
|
|
||
| /** \brief Get the object id that owns the named frame with name id*/ | ||
| std::string getParent(const std::string& id) const; |
There was a problem hiding this comment.
id should be given a more descriptive name. Maybe something like object_id. From this comment it is unclear to me if this is solely for getting the parent_object_id for a child_frame or if it can also be used more generally. For instance, I might think that getParrent("wrist_2_link") would return wrist_1_link
There was a problem hiding this comment.
I tried to keep the "id" convention, but I will have another look if there are functions that only take named frames' names.
| bool moveObject(const std::string& id, const Eigen::Affine3d& transform); | ||
|
|
||
| /** \brief Replaces all shapes in an existing object. | ||
| * If the object already exists, this call will add the shape to the object |
There was a problem hiding this comment.
The function name indicates that this would remove all shapes in an object and replace them with this new one. Please either rename the function or update the comment to match functionality.
There was a problem hiding this comment.
Looking at the implementation, I found that the documentation here is wrong, but the function name matches implementation.
There was a problem hiding this comment.
True. I changed the documentation.
| return true; | ||
| else // Then objects' named frames | ||
| { | ||
| for (auto o : objects_) |
There was a problem hiding this comment.
nit: I tend to shy away from auto unless the object that is being iterated is defined nearby. If a reader has to open up another file to find out what type of object this will be then it is probably a bad idea.
There was a problem hiding this comment.
I agreed with what you wrote, but the rest of the file seems to follow the same convention. I only changed from auto to auto const& for now, under the assumption that it's faster.
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/move_group/src/default_capabilities/constraint_validation_service_capability.cpp
Outdated
Show resolved
Hide resolved
|
As discussed with Dave on Wednesday, I had to freeze this until the week of the MoveIt Day because I am completely occupied with this. I think I can get it done and tested on Melodic by WMD though. |
rhaschke
left a comment
There was a problem hiding this comment.
Think about how to store the attached frames, locally per-object (as currently) or in a global map.
A global map, would better reflect the global name space.
I will have another look, if commits are cleaned up.
| objects_.clear(); | ||
| } | ||
|
|
||
| bool World::setNamedFramesOfObject(const std::string& id, const std::map<std::string, Eigen::Affine3d>& named_frames) |
There was a problem hiding this comment.
What about methods to add/remove/access individual frames of an object?
There was a problem hiding this comment.
I can imagine some use cases, but I don't think it's required for a first version. Do you have anything specific in mind?
moveit_core/robot_state/include/moveit/robot_state/attached_body.h
Outdated
Show resolved
Hide resolved
Includes changes: - from .count() to .find() - _NAMED logging - Use for-range loops - Clean comments - Restore knowsObject function
|
A quick update before I get on my plane. I have been on the road constantly and work from my laptop's VM is slow, but I added most of the comments during WMD. The conversations related to those are marked as resolved. The last commit changes a lot of lines, but (hopefully) very little functionally. There are some decisions to take that affect the work left to do on this PR:
Known limitations:
Open small ToDos:
Another note: I will be in Europe for 3 weeks. That's another reason I am reluctant to introduce bigger changes now. I hope I can finish the small stuff very shortly, though. Will update after my flight! |
|
Closing because it is superceded by #1439. |
Description
This PR implements #1041. It adds named frames to collision objects so that they can be used for planning, both as targets in the planning scene, as well as end effector links when the objects are attached to the robot. This allows writing commands like "Move the tip of the screwdriver to the head of the screw" or "Move the spout of the teapot to the top of the mug". It also opens up avenues for using in-hand pose estimation to update the pose of an object in the gripper (e.g. grasp a tool, confirm the object pose with a camera, then use the updated tool tip pose on the object).
Currently, the named frames are displayed in Rviz in the Scene Objects tab and as red dots on the
AttachedBodies. I also made a header file that converts URDF files tocollision_objectmessages in C++, but it's not part of this PR. I didn't test it a lot yet, so there are surely some bugs, but it should be good enough to try out.Here is a gist that you can use to create the following example images. These run for a UR5, but you have to adjust everything to your system and fill in some variables. I didn't have time to prepare a whole test package.
The result of "Move the cylinder tip to the bottom of the box":
Open ToDos:
Checklist