Dynamically Reconfigure Robot/Scene

[mamoll]

I am reposting an issue that @ndantam had opened on the old moveit_core issue tracker. There is still no scene graph support in MoveIt! AFAIK. This would be extremely useful to task and motion planning.

@ndantam's original bug report:

It would be useful to able to dynamically change the kinematics of the robot. For example, picking a tray containing several objects should incorporate the child objects of the tray into the robot. This is roughly possible by attaching as collision objects all children of the picked object; however, this is an additional step for something which could be managed based on the kinematic tree of the robot and scene.

It seems that this may be difficult to implement since moveit separates the RobotModel and the PlanningScene. Are the underlying data structures similar? Does the PlanningScene consider relative transforms or are all object poses in the global frame?

I think the simplest implementation would be to treat both robot and scene as kinematic trees using the same data structure.

EDIT: AFAICT, scene geometry is contained in collision_detection::World::Object instances (https://github.com/ros-planning/moveit_core/blob/0.6.15/collision_detection/include/moveit/collision_detection/world.h#L72) and robot geometry is contained in moveit::core::LinkModel instances (https://github.com/ros-planning/moveit_core/blob/0.6.15/robot_model/include/moveit/robot_model/link_model.h#L70). This dichotomy seems unnecessary. Is there an advantage that I'm overlooking?

[davetcoleman]

I think this is very needed improvement. The tesseract fork actually implemented this because of this MoveIt! shortcomings. It would be ideal to get this improvement ported back in.

[felixvd]

This also came up at the MoveIt workshop and should be a WMD issue.

[rhaschke]

Although this feature is very desirable, I don't think this is an easy task that can be implemented within a day - even for a highly skilled MoveIt maintainer.

[felixvd]

I don't think it is a topic to be finished in one day either, but I figured drafting or starting it would be a good work packet. This topic was on my mind a few times today, and I wondered if it came up in any other discussions at the workshop. I guess the commands Tesseract used are a good place to start thinking about it: 1, 2

[felixvd]

@rhaschke @v4hn

I brought up adding scene graph support as a GSoC project this year, since it came up at the workshop and during this PR. This is the oldest mention on the current Github repo that I could find, so I'd like to use this thread to discuss how to implement this feature and scope a potential GSoC project.

I have these thoughts and would appreciate input from the more experienced maintainers:

• When implementing a scene graph, does it make a lot of sense / is it practically required to merge robot_state and collision_world? If yes, does it make sense to do it in a similar way as tesseract? If yes, can we port features, and which ones? @Levi-Armstrong may have input on how to go about this from a high-level view.
• Do we need to code a kinematic tree from scratch, or should we use an external tool, like TF, KDL::Tree or tesseract's scene_graph? What are the downsides/upsides, and is there something simpler or more appropriate?
• If we implement a kinematic tree, should we keep the FrameTransformsMap as it works now and keep updating it in the background? It may work as a lookup table and avoid some redundant calculations, but it is unclear to me if that would be premature optimization, carrying old baggage, or an easy and convenient extension. I can't tell for sure without reading the code.
• What are the reasons why users may want to change the structure of their robot, and which interfaces will they want to use? Would the robot_state_publisher node have to be updated, and if yes, how? Tagging @gavanderhoorn here because we discussed the example of attaching grippers to a robot at the workshop.
• @mamoll suggested that scene graph support would "provide the necessary hook for connecting with high-level task planners". Which task planners do you mean? Are any interfaces defined on their end? Is there anything else to keep in mind regarding this?
• What other use cases do you foresee for a scene-graph-like representation?
• What pitfalls do you see for certain implementations, and which do you think are feasible?

I'd suggest the draft below as a project description, and would be happy to hear your input. Thanks!

---

On-the-fly robot reconfiguration and scene graph support List of prerequisites: ROS

Description of programming skills: C++

Difficulty level: Medium to High

List of potential mentors: Felix von Drigalski, X, Y

Detailed description: The planning scene in MoveIt is currently represented as a flat list of transforms from the origin. In this project, the planning scene should be extended with a scene graph or kinematic tree, so that it allows e.g. the dynamic attachment or removal of a hand to a robot arm, or picking up a board with other items attached to it.

This feature was implemented in Tesseract, which can serve as a blueprint for parts of this project.

[ndantam]

Since making this feature request quite some time ago, I ended up implementing reconfigurable scene graphs outside of MoveIt. Please let me share my experience/thoughts with some hope to come back together.

The main design differences from the MoveIt data structures (AFAICT) are:

• The "robot" and "environment" are combined into a single scene graph
• Geometry/meshes are associated with local coordinate frames in the scene graph structure
• Kinematics uses the scene graph directly rather than (necessarily) generating an additional data structure (e.g., a KDL Tree)

When implementing a scene graph, does it make a lot of sense / is it practically required to merge robot_state and collision_world?

I do not think this is necessary if you define geometry in the scene graph.

Do we need to code a kinematic tree from scratch, or should we use an external tool, like TF, KDL::Tree or tesseract's scene_graph? What are the downsides/upsides, and is there something simpler or more appropriate?

You may also consider the Amino scene graphs (since they exist exactly to implement this feature and have some quite fast and robust FK/IK).

What are the reasons why users may want to change the structure of their robot, and which interfaces will they want to use?

The robot picks up a tray of objects. The robot assembles a birdhouse/sandwich/stack-of-blocks and then moves it.

The general interface is to change the parent frame and relative transform of an object that is grasped/placed.

@mamoll suggested that scene graph support would "provide the necessary hook for connecting with high-level task planners". Which task planners do you mean?

http://tmkit.kavrakilab.org/

What other use cases do you foresee for a scene-graph-like representation?

Possibly changing the root robot frame for planning/kinematics (Robonaut moving between handrails is one case where this could be convenient).

Also, there may be some code simplification from combining robot and environment into one data structure.

List of prerequisites: ROS

Some solid understanding of linear algebra and robot kinematics may also be necessary to implement this well.

[mamoll]

Just to expand a little on Neil's comments, I did have TMKit in mind as a candidate for needing scene graph support, but general task planners that abstract the continuous world into some discrete representation would also benefit. The scene graph provides a rich representation for a task planner to reason about. It might be possible to integrate support for ROSPlan at some point in the future.

[felixvd]

Any opinions from other maintainers on the data structure? I am not familiar with Amino or many of the other implementations.

[tejasdubhir]

Hi, I am Tejas Dubhir, and I am really interested in working for this project. Can I get some suggestions about any reading material/ books where I can get to learn the concepts required for it? As soon as I get the necessary basics, I will start contributing to the project.

[davetcoleman]

I think we should add this to MoveIt's 2021 roadmap, assuming @felixvd wants to lead the charge. We've been chatting internally at PickNik (@mamoll @henningkayser @AndyZe) after talking with Felix and weren't interested in supporting @felixvd .

[AndyZe]

were interested!

[felixvd]

were interested!

Thanks for the clarification :)

Yes, I'm happy to do what I can. I updated the future projects here. Maybe a potential GSoC student finds it in time.

[rhaschke]

I want to revive the idea of reworking the PlanningScene representation. Let's first collect a list of desirable features:

• Not only robots but also objects (or tools) should have a tree structure: a tray of objects on a table, a multi-fingered robot hand, etc. This suggests using URDF descriptions for objects and tools as well. Thus the scene graph/tree could be composed from several subtrees that originate from separate URDF descriptions.
• Considering independent URDF descriptions for the arms, tools, and objects would nicely play with joint_state_publisher, robot_state_publisher, and rviz: The would independently operate on different robot_description parameters, but publish to common joint_states and tf topics. rviz could display individual parts by separate RobotModelDisplays.
• Pure transforms, e.g. a virtual grasp or drilling frame, should be handled in the same hierarchical fashion, either coming from virtual URDF links (i.e. links w/o visual or collision geometries) or from tf.
• It should be possible to colorize objects and links to indicate semantic meaning, e.g. graspable objects, place regions, etc. This would either override the original URDF-specified color or add a tint.
• Attaching and detaching subtrees could be realized by relinking the tree, e.g. defining new relative transforms via tf.
• Removing a subtree requires a separate API as tf cannot remove a transform.
• I would like to see the possibility to define JointModelGroups on the fly, at least programmatically, but maybe also via a ROS interface.

Now, let's turn to some implementation details.

• Obviously, we need to combine robots, objects, and pure transforms within a single scene graph data structure, unifying existing RobotModel/RobotState, World, and FrameTransformsMap. At the core of this should be a tree data structure (I think all scene graphs are actually trees, aren't they?)
• Do we want to maintain URDF subtrees as (named) logical units to facilitate their handling?
• Maybe we also want to forbid that users can tear a robot subtree apart, e.g. relinking the middle of a robot chain somewhere else or removing parts of a robot. This would require maintaining the parts of a corresponding subtree.
• I like the idea of keeping RobotState separate from the scene graph. While the latter defines the kinematic structure (much like the RobotModel before), the former defines the robot's configuration state in a compact fashion - based on a given scene graph. Thus, the actual transforms (and their dirty state) would be stored here.
• We need a new PlanningScene message to announce the whole scene graph to the ROS ecosystem. Do we want to include the full (robot) URDF descriptions in there or just reference their corresponding ROS params? Considering planning scene diff messages, maybe this doesn't really matter.

I'm curious about any comments on this (coarse) proposal, particularly from @davetcoleman, @Levi-Armstrong, @mamoll, @felixvd, @v4hn, @gavanderhoorn, @guihomework.

[felixvd]

Great initiative, I will respond later. Just want to tag @captain-yoshi who has related prior work I believe.

[v4hn]

I'm excited to see this initiative gain traction again!

Not only robots but also objects (or tools) should have a tree structure: a tray of objects on a table, a multi-fingered robot hand, etc. This suggests using URDF descriptions for objects and tools as well. Thus the scene graph/tree could be composed from several subtrees that originate from separate URDF descriptions.

I strongly support the idea of having multiple URDF structures in the PlanningScene! It's an overdue change that would nicely support articulated objects as well as multiple independent robots.

• I advocated for the planning scene to support a set of RobotModel before and I believe that would actually be the easiest way to implement it. If the user can designate a "default" model, we can even keep the getCurrentState API. We might want to change the PlanningScene message to a set of RobotModel's, but we could also keep the current one and just add an optional vector of others.
• We can allow these models to change the transform of their base link, and it would work nicely for dynamic changes as well. I think this would be an ideal application for the otherwise mostly unused fixed_frame_transforms entry.
• If we additionally support an API to add/remove these models at runtime, either explicitly or implicitly through received transform messages, it should account for almost all use-cases.
• I don't think we should copy the model descriptions around with the scene. We don't do that with the meshes either. I have played around with the idea to publish robot_descriptions as latched topics instead of ROS parameters, to make them recordable in bags and to get callbacks when any description changes. But that would entail rather drastic API changes through the whole ROS ecosystem and ROS2 has better support for dynamic parameters anyway. (I don't think they can be recorded there either though...)
• In our own setups we usually publish joint_states separately in different namespaces and then forward them all to /joint_states. In many situations this has advantages, e.g., when working with a Shadow hand attached to the arm, the hand can still be considered in isolation as required.

EDIT: provide more pointers above (see github comment history for details)

[rhaschke]

@v4hn, how would you communicate the robot models across different PlanningScene consumers? We cannot rely on a single robot_description anymore.

[v4hn]

how would you communicate the robot models across different PlanningScene consumers? We cannot rely on a single robot_description anymore.

In theory I think you could define multiple robots in the parameter as well (multiple <robot> roots), but that's not extendible. You are right, we could lift something like the robot_model_name attribute from the planning scene to identify a <robot_name>_description parameter, but we will have to modify the messages further anyway to allow for multiple parameter names.