Update SRDF for collision model

[fwalch]

franka_description got updated with a more coarse collision model [1] matching the internal
self-collision detection, so the SRDF needs to be adapted.

Closes #18. Resolves moveit/moveit#1210, resolves frankaemika/franka_ros#39.

[1] frankaemika/franka_ros@e52c03a

---

The changes in franka_description were not released yet, but I wanted to open this PR already to get feedback. When this is merged, I guess we should coordinate to get both packages into the ROS repos at the same time (or maybe panda_moveit_config can already be released beforehand, not completely sure if this SRDF would cause problems with the old URDF).

[rhaschke]

Could you provide more evidence that the ACM should be modified?
I hope you don't just rely on the random checks by MSA.

These changes can be merged independently of franka_description's URDF changes.

[rhaschke]

Is there a logical reason that collision of fingers with link3 became possible now and with link8 became impossible?

[v4hn]

As MoveIt's master branch points out,

ros.moveit_core.robot_model.empty_collision_geometry: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
ros.moveit_core.robot_model.empty_collision_geometry: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.

For some reason you added the overly coarse collision spheres to the hand link instead of adding them to the fingers.
If you want to keep the model like that, you should either disable all collisions with these links or not disable anything.
Until recently, MoveIt would have checked against the visual geometry for these links, so disabling them simplifies matters a lot.
But at least for the (unreleased) master branch, no collision geometry is added, so these disable_collision statements are all redundant.

[rhaschke]

Is there a logical reason why link2 and link6, link3 and link5, link3 and link7 can collide now, but couldn't before?
You mentioned, that you use a coarser model now. The ACM should reflect actually/physically impossible collisions.

[rhaschke]

Again, is there a reason for this?

[fwalch]

A general comment before I answer the individual questions:

The robot does self-collision checking itself, which complicates this somewhat. The internally used collisions models are equal to the new, coarser collision models. So MoveIt should do the same collision checks as the internal self-collision check, otherwise issues such as moveit/moveit#1210 can still occur.

[rhaschke]

I guess, with coarser you mean that collisions are detected earlier (than necessary).

[rhaschke]

As far as I understand, Panda's URDF model is now parameterized with a safety_distance parameter. I guess that the padding internally used by Panda's self-collision avoidance is parameterized as well - otherwise having a parameterizable safety_distance parameter wouldn't make sense. How do you ensure that both parameters are exactly the same?

MoveIt has a similar parameter, called padding, which even can be set individually for different links and which can be changed dynamically for different planning attempts.
The ACM marks certain link pairs as "never-colliding", such that explicit collision checks can be omitted. For all other links, the URDF collision model is used (which already got the safety_distance padding) plus extra padding from MoveIt.
As the safety_distance parameter can change between user setups, the ACM should only mark links as never-colliding for the least conservative choice of this value, i.e. safety_distance=0.

Another issue is the FCL-based collision checking. As far as I remember, spheres and cylinders are approximated with rather coarse models. If Panda's internal collision avoidance uses real spheres and cylinders (which I assume for performance reasons), this will again be more strict than MoveIt's checking (which should be avoided).

So summarizing, the ACM should be designed for a safety_distance of zero. In general, I would expect disabled pairs to be removed, not added.

[rhaschke]

@fwalch, friendly ping.

[christoph-jaehne]

Sorry for the long silence from our side. We got viciously caught up other things ;).
As far as I understand @rhaschke we should replace the effect that the safety distance from the urdf has with moveits built-in solution padding. Maybe it is sufficient to configure a default padding of 0.03 (e.g. in the launch files) and work with a safety distance of 0. If I understand this correctly that would have the same effect in planning and would achieve our goal of getting guaranteed libfranka-feasible trajectories from moveit by default. Right?

[rhaschke]

Yes, great summary.

[Le-Li-17]

@rhaschke I have another look at the MoveIt! tutorial, see the page of Planning Scene. Regarding the self-collision checking, it is written that Self-collision checking uses an unpadded version of the robot, i.e. it directly uses the collision meshes provided in the URDF with no extra padding added on. I was wondering if we can still configure the padding parameter for self-collision checking.

[rhaschke]

I'm not deep into this matter. @v4hn @j-petit can you answer this question?

[j-petit]

I think you have to create a new planning scene instance and specify the padding with it in the message (see LinkPadding entry).

[Le-Li-Franka]

I think you have to create a new planning scene instance and specify the padding with it in the message (see LinkPadding entry).

@j-petit Does it mean that the only way to configure this padding parameter is to use the planning scene C++ API? But what we are looking for is that the users can easily plan and execute the trajectory with MoveIt! through the RViz plugin.

[Le-Li-Franka]

@j-petit @rhaschke friendly ping. ;)

[v4hn]

There is no nice way to specify a default padding, the padding is not used in self-collision checks as @Le-Li-17 pointed out already, and there have been a number of patches proposed/partially merged recently to fix issues with it. Also, there is no visualization for it.

To resolve the problems discussed, adjusting the collision geometry and the srdf seem to be the best way (leaving out whole new interfaces in MoveIt and introspection capabilities for the internal padding) .

However, I just looked at the current default collision geometry when launching the demo.launch file and it looks like this:

More potential collisions do not surprise me with such a coarse representation.
Worse, the collision geometry does not include the whole visual geometry, as you can see in the white clipping area. Is that really the internal model used on the controller?

Indeed, the default startup pose of the demo.launch, depicted above, reports collisions between links panda_link7 and panda_link8, this disabled collision should have been in the config in the first place and is added in this patch...

But even with this proposed changeset, the default pose still collides in links panda_link6 and panda_link8:

I just visually verified all changes to the ACM and the do seem reasonable.
Nonetheless, there are still allowed collisions that make the default pose unreachable.
Please fix that.
Also, please revisit the defined collision geometry. It is insufficient, adds collision geometry to wrong (quasi statically-connected) links and I would expect a lot of relevant poses (similar to the default pose) are unreachable unless you want to disable collision checking altogether...

I approve this request as it improves the current state, but it leaves the robot model in a broken state still.

[v4hn]

As MoveIt's master branch points out,

ros.moveit_core.robot_model.empty_collision_geometry: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
ros.moveit_core.robot_model.empty_collision_geometry: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.

For some reason you added the overly coarse collision spheres to the hand link instead of adding them to the fingers.
If you want to keep the model like that, you should either disable all collisions with these links or not disable anything.
Until recently, MoveIt would have checked against the visual geometry for these links, so disabling them simplifies matters a lot.
But at least for the (unreleased) master branch, no collision geometry is added, so these disable_collision statements are all redundant.

[Le-Li-Franka]

As @v4hn pointed out, panda_link6 and panda_link8 are in collision with the default pose. This pull request is updated with one new commit, where the collision checking between panda_link6 and panda_link8 is disabled. Friendly ping @rhaschke @j-petit @v4hn.

[v4hn]

Thank you for updating. I still approve, as this improves the support w.r.t. the upstream robot model which changed the collision geometry.

As a side note: Let me point out, that all of this is independent of moveit_resources where we maintain a separate robot model with collision meshes.

[Le-Li-Franka]

Friendly ping @rhaschke @j-petit It will be greatly appreciated if we could close this pull request when you have nothing against it.

[v4hn]

Quoting myself:

Also, please revisit the defined collision geometry. It is insufficient, adds collision geometry to wrong (quasi statically-connected) links and I would expect a lot of relevant poses (similar to the default pose) are unreachable unless you want to disable collision checking altogether...

Are you sure you really want the geometry like that? @Le-Li-Franka , If you reply "yes" to that question, we can really just merge it. I would propose to fix at least the inconsistently attached static spheres for the gripper to avoid the mentioned warnings in MoveIt master/noetic.

[rhaschke]

Note that the franka_ros changes, @v4hn referred to in #35 (review), were not yet released into Kinetic or Melodic (latest release is 0.6 from 8/2018, while frankaemika/franka_ros@e52c03a was merged 11/2018).
As this PR targets Melodic, the corresponding changes in franka_ros should be released in a synced fashion.
What are your plans for that at Franka? We should actually release those changes in sync!

[Le-Li-Franka]

Quoting myself:

Also, please revisit the defined collision geometry. It is insufficient, adds collision geometry to wrong (quasi statically-connected) links and I would expect a lot of relevant poses (similar to the default pose) are unreachable unless you want to disable collision checking altogether...

Are you sure you really want the geometry like that? @Le-Li-Franka , If you reply "yes" to that question, we can really just merge it. I would propose to fix at least the inconsistently attached static spheres for the gripper to avoid the mentioned warnings in MoveIt master/noetic.

Long story short, the answer to your question is "yes". We are aware of the collision geometry is coarse. However, it is the collision geometry being used internally by the master controller. To make the planner and the controller behavior consistent, i.e. all plans from MoveIt! can be actually executed on the physical robot, the collision geometry in the planner has to be the same as that in the controller. Otherwise, the controller may reject the valid plan generated by the planner.

[Le-Li-Franka]

Note that the franka_ros changes, @v4hn referred to in #35 (review), were not yet released into Kinetic or Melodic (latest release is 0.6 from 8/2018, while frankaemika/franka_ros@e52c03a was merged 11/2018).
As this PR targets Melodic, the corresponding changes in franka_ros should be released in a synced fashion.
What are your plans for that at Franka? We should actually release those changes in sync!

The latest release of franka_ros is blocked by this PR. As soon as we close this PR, we will move on to the release of franka_ros with the updated collision geometry.

[v4hn]

I still don't like the current state of the model for use in MoveIt, but let's leave it to follow-up patches to improve things further.

[rhaschke]

I'm afraid that we will get many issues reported in moveit_tutorials, given this coarser model. But I agree that the model should be consistent with the controller's internal model.

[v4hn]

I'm afraid that we will get many issues reported in moveit_tutorials, given this coarser model.

Possibly. Someone will have to go through the tutorials and adjust things if they broke because of this. Before, everyone actually trying the tutorials with builds from source found a broken configuration with even more unwanted collisions... As a side-note: We deliberately kept the meshes for moveit_resources for our own tests for now.

[rhaschke]

@Le-Li-17, please notify us when you prepare a new release of franka_ros.