Gazebo/SDF (vs URDF) and MoveIt/PlanningScene

[andreas.bihlmaier]

Hello MoveIt developers and users,

I'd much appreciate your thoughts on how to solve the following issue:

Context:

Robotics lab with multiple ROSified industrial robots that is simulated in Gazebo. Currently, each robot is modeled as .xacro (by xacro), which is converted to .urdf that in turn becomes an .sdf (through gzsdf). The relevant inventory in the lab is modeled as multiple .sdf models (using .dae meshes). SDFs <include> feature is used to combine "ground" .sdf models into larger ones, i.e. a camera SDF together with a camera holder SDF. This is also done in order to attach different items to the robot (cameras, grippers, etc). Finally, a .world file brings together a couple of independent, i.e. not connected by joints, models to make up the whole lab.

This works out quite well, e.g. to change the current gripper I only have to modify one of the "composition" .sdf files - I have a *hierarchical* set of models that is easy to maintain and adjust to a changed situation in the lab.

Solved issues:

 - Visualize current gazebo world together with simulated sensors in rviz. Use .sdf to contextualize real sensor data with static objects in lab. -> http://wiki.ros.org/gazebo2rviz

**Open issue:**

 - MoveIt integration: How to use the current gazebo world as PlanningScene for MoveIt?<br>

What I want to achieve is the following:

   - Maintain only *one* description for each entity (automatically converted to other formats if necessary).

   - Have a *hierarchy* of models, i.e. adding a gripper should not require to redefine the robot it is attached to.

   - Use the *same* description as basis for MoveIt for the simulated and the real lab.<br>

 - I see the following **obstacles** - any hint at solving them is greatly appreciated:

   - Where to attach end-effectors? In .xacro, .sdf or add them through custom to the PlanningScene at runtime?

   - Write sdf2moveit and gazebo2moveit (or *2planning_scene) myself? If so, how to properly combine the URDF necessary for MoveIt and the SDF necessary - and to be honest preferred - for gazebo?

   - What to push to TF common to simulation and reality, for simulation only and for real robots only?

To recap: How can we achieve a better integration in different parts (here: Visual and collision models for Gazebo, Rviz and MoveIt) of ROS?

NOTE: This is a cross-post from Gazebo answers (http://answers.gazebosim.org/question/5789/gazebosdf-vs-urdf-and-moveitplanningscene/), feel free to answer here or there.

Kind regards,

ahb

[Dave Hershberger]

Hi Andreas,

I think we don't currently have a great solution in MoveIt to the problem you describe.

When you describe making a "2planning_scene" or "2moveit" program, do you mean something that would output a string-ified planning scene like what PlanningScene::loadGeometryFromStream() http://docs.ros.org/api/moveit_core/html/classplanning__scene_1_1PlanningScene.html#a82035a2556f936a857cf108add4a8e2d wants? That's the only saved representation I can think of right now for MoveIt scenes.

Seems to me like you should probably keep your basic xacro->urdf->sdf data flow for the robot description(s).

For grippers, it is probably simplest to combine grippers with arms in a top-level .xacro file that includes the current gripper(s) with a robot.  That is what ROS and MoveIt will be expecting.

Question: are you talking about changing grippers during execution?  Or just while editing config files between executions?  Changing robot descriptions at run-time is not well supported in ROS or MoveIt.  (It is a bunch of work to chase down all processes that have loaded the URDF and get them to reload it.)

For non-robot objects in the world, either static or movable (but not self-moving), I think the best approach is to post-process the .sdf file and convert all the relevant objects into collision objects for MoveIt.  It might be easiest for you to make a standalone executable that reads the .sdf file and outputs a .scene file (like loadGeometryFromScene() wants).  That is nice and modular.  Possibly we would like to add a new function to PlanningScene like loadGeometryFromSDF() which would eliminate the intermediate file, but we would have to discuss.  Certainly once the converter program was written, a PlanningScene::loadGeometryFromSDF() would be easy to write.

I don't understand what you mean by "what to push to TF common".

Dave

[andreas.bihlmaier]

Hi Dave,

thanks for your detailed answer, exactly what I was hoping for.

After pondering the issue further, I have to agree that the only viable option is to (re!)model:

 - everything that is part of an articulated object (robot arm and attachments[1]) as xacro

 - all unarticulated objects as sdf

 - the lab scene as sdf

I will then write two nodes (analogous to the gazebo2rviz ones):

 - gazebo2planningscene: gazebo ROS topics -> continuous PlanningScene diffs

 - sdf2planningscene: sdf -> PlanningScene diff or .scene file

both talking to the move_group by moveit_msgs::PlanningScene messages (http://docs.ros.org/api/pr2_moveit_tutorials/html/planning/src/doc/planning_scene_ros_api_tutorial.html).

Do you think I should rather generate a .scene or add collision object by collision object through the ROS api?

The whole situation is quite unsatisfactory because in the near future I need to create visually highly accurate models (perhaps with their own material shaders) for gazebo.

Gazebo allows to directly specify OGRE materials for such purposes, while urdf does not (and why should it).

I have to find some way to put everything into the xacro file, generate a valid urdf and a sdf with the additional information for gazebo (perhaps the <gazebo> tag will suffice).

However, one issue remains for which I cannot come up with a clean solution:

Finding out if (and which) URDF to load as robot, when it is part of a larger SDF. [2]

In other words: Distinguishing between unarticulated native SDF objects - to be added as collision objects - and articulated objects that are actually defined through URDF (actually by xacro) - not to be added because already part of their own move_group. 

I hope the ROS future will bring better integration compared to the current state regarding modeling (all SDF?). But I guess for now there is no silver bullet and one has to fiddle around with different incompatible formats.

<sarcasm>Perhaps I should define my own XML-based super format from which both URDF and SDF can be generated ...</sarcasm>

Regards,

ahb

[1] The reason I tried to avoid going down that road are the many combinations of spacers, marker rings, cameras and optics in use. Therefore, I'll have to define many robots, although in reality, there are only a few robots with many attachments.
[2] For example, I have an SDF in which two robots (stemming then from different xacro->urdfs because of different attachments) are rigidly fixed to a table (using SDF "fixed" joints)

[andreas.bihlmaier]

One further question:

Do I have to generate (as in what moveit_setup_assistant does) a corresponding moveit configuration for each "different" - as in with another attachment - robot?

[Dave Hershberger]

Hi Andreas,

Responses inline...

On 04/01/2014 01:55 AM, andreas.bihlmaier wrote:

One further question:

Do I have to generate (as in what moveit_setup_assistant does) a corresponding moveit configuration for each "different" - as in with another attachment - robot?

The simple answer is yes, you do.  However, if you look through the files generated by moveit_setup_assistant, most are probably pretty easy to automate the process of adding an attachment to a robot, since most will be a matter of parsing the files and appending the various list elements.  However, one that might be tricky is the allowed-collision matrix.  That is generated automatically in the GUI, and considers how each link in a URDF model touches all other links.  So you might need to pull out that automatic collision-checking code from the GUI and call it from your scripts.

On Tuesday, April 1, 2014 10:46:10 AM UTC+2, andreas.bihlmaier wrote:

Hi Dave,

thanks for your detailed answer, exactly what I was hoping for.

After pondering the issue further, I have to agree that the only viable option is to (re!)model:

 - everything that is part of an articulated object (robot arm and attachments[1]) as xacro

 - all unarticulated objects as sdf

 - the lab scene as sdf

I will then write two nodes (analogous to the gazebo2rviz ones):

 - gazebo2planningscene: gazebo ROS topics -> continuous PlanningScene diffs

 - sdf2planningscene: sdf -> PlanningScene diff or .scene file

both talking to the move_group by moveit_msgs::PlanningScene messages (http://docs.ros.org/api/pr2_moveit_tutorials/html/planning/src/doc/planning_scene_ros_api_tutorial.html).

Do you think I should rather generate a .scene or add collision object by collision object through the ROS api?

Depends how you want to use it.  If it's an on-line thing where sdf stuff is being generated on the fly and you have to translate it into moveit objects on the fly, then you likely want collision object messages.  If instead your SDF processing happens all at once at the start, then I would choose the .scene file method.  I feel more in control with the .scene file myself, because all the object info is in one place and I call a function to read it.  I don't need to publish messages and hope they are received.

The whole situation is quite unsatisfactory because in the near future I need to create visually highly accurate models (perhaps with their own material shaders) for gazebo.

Gazebo allows to directly specify OGRE materials for such purposes, while urdf does not (and why should it).

I have to find some way to put everything into the xacro file, generate a valid urdf and a sdf with the additional information for gazebo (perhaps the <gazebo> tag will suffice).

However, one issue remains for which I cannot come up with a clean solution:

Finding out if (and which) URDF to load as robot, when it is part of a larger SDF. [2]

In other words: Distinguishing between unarticulated native SDF objects - to be added as collision objects - and articulated objects that are actually defined through URDF (actually by xacro) - not to be added because already part of their own move_group.

Well, I haven't tried this in Gazebo specifically, but generally with programs that read XML, they just look through and read the tags they are expecting, and ignore tags they are not expecting.  That means you can often add your own tags anywhere in the XML document that you want.  So you could add a special "<IAmARobot/>" tag to your robot, or you could add "<NotARobot/>" tags to all your non-robot objects.  Since your robot is already going from URDF to SDF, I'm not sure that the converter would pass through the tags it isn't expecting.  The "not-a-robot" tags on the objects should be fine though, since they are defined first in SDF (by you) and then processed by Gazebo on the one hand and by your SDF-to-moveit translator on the other.

This reduces the general applicability of your SDF-to-moveit translator of course.  If you can find a way to distinguish robots from non-robots, that's probably better.  How does Gazebo know what is a robot and what is not?

I hope the ROS future will bring better integration compared to the current state regarding modeling (all SDF?). But I guess for now there is no silver bullet and one has to fiddle around with different incompatible formats.

I hope so too.  Would you like to pay us to work on it?

<sarcasm>Perhaps I should define my own XML-based super format from which both URDF and SDF can be generated ...</sarcasm>

... And then there would be yet another format for people to write converters for. :-/

Regards,

ahb

[1] The reason I tried to avoid going down that road are the many combinations of spacers, marker rings, cameras and optics in use. Therefore, I'll have to define many robots, although in reality, there are only a few robots with many attachments.
[2] For example, I have an SDF in which two robots (stemming then from different xacro->urdfs because of different attachments) are rigidly fixed to a table (using SDF "fixed" joints)

I hope you have a plan for handling the multiple robots simultaneously, because MoveIt does not natively handle that.  If you merge all robots together into one big URDF, it can work.  Multiple robots handled by multiple MoveIt instances should in principle work, but you have to get all the namespacing of topics and parameters worked out carefully.  I think that use case has not gotten much attention.

Dave

[andreas.bihlmaier]

Hi Dave,

I greatly appreciate your comments.

I decided - in part due to stuff other people in our lab are doing - to write an SDF to URDF converter. The good news is that it already works for all our models (also those with <include> tags).

My current plan is as follows:

- Model everything that could exist on its own as separate SDF

- Use <include> to build larger entities (e.g. robots with their attachments)

- Convert everything that is "part" of the robot (i.e. the kinematic chain from the robot base onward) from SDF to URDF

-> Automate generating moveit config (based on manual configs for each bare robot)

- Add every non-robot object in SDF/world scenes as moveit collision element (either converting to .scene or using the ROS API)

- Use TF to correctly position collision elements and robots

- Try to use multiple move_groups (and corresponding rviz MotionPlanning plugins) at the same time

=> Have a unified description as basis for path planning and collision checking/avoidance in Gazebo simulation and real lab :)

Btw. perhaps my previous post sounded a bit harsh - which I did not intent, therefore I want to emphasize how great it is that the Open-Source frameworks ROS/MoveIt/Gazebo have already come so far and thank all contributors for greatly easing my research.

I actually just intend to invest my time in a way that it will benefit the whole community instead of writing code that just works for my use case (and will never be used again once my PhD is finished).

Kind regards,

ahb

[andreas.bihlmaier]

Just a short follow up:

The SDF -> URDF/TF part is finished:

 - http://wiki.ros.org/pysdf

 - http://wiki.ros.org/gazebo2rviz (sdf2*.py especially sdf2extract_tfstatic_node.py)

Now I'm currently working on two things remaining:

 - parts not attached to robot in SDF -> planning scene collision objects

 - node to segment single trajectory_action for use with multiple independent robot controllers; together with a joint_state merger (and renamer) node

Once all of this is finished, I'll write down the complete workflow of creating complete scenes and "meta-robots" for use in simulation and reality by fusing multiple SDFs through <include>.

Regards,

ahb