Open krigi100 opened 5 years ago
We actually recently updated the collision volumes. The changes are already on the develop
branch, can you try them? Note that the SRDF might need to be updated.
In the develop branch are no collision volumes included, they were deleted in november. It would be nice if you can share the new files.
The collision volumes are now directly defined as geometries in the URDF.
We'll try to release the develop
branch with the updated collision volumes soon, and also update panda_moveit_config
accordingly. In the meantime, here's a diff for panda_moveit_config
that should work in combination with franka_ros
develop
:
diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 53ddcdf..52a2494 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -28,19 +28,20 @@
<disable_collisions link1="panda_link0" link2="panda_link3" reason="Never" />
<disable_collisions link1="panda_link0" link2="panda_link4" reason="Never" />
<disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent" />
- <disable_collisions link1="panda_link1" link2="panda_link3" reason="Never" />
+ <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default" />
<disable_collisions link1="panda_link1" link2="panda_link4" reason="Never" />
<disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent" />
<disable_collisions link1="panda_link2" link2="panda_link4" reason="Never" />
- <disable_collisions link1="panda_link2" link2="panda_link6" reason="Never" />
<disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent" />
- <disable_collisions link1="panda_link3" link2="panda_link5" reason="Never" />
<disable_collisions link1="panda_link3" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_link3" link2="panda_link7" reason="Never" />
<disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent" />
<disable_collisions link1="panda_link4" link2="panda_link6" reason="Never" />
<disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
+ <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never" />
<disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
+ <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
<disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
+ <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default" />
+ <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
</xacro:macro>
</robot>
diff --git a/config/panda_arm_hand.srdf.xacro b/config/panda_arm_hand.srdf.xacro
index 39de54f..2ab6be8 100644
--- a/config/panda_arm_hand.srdf.xacro
+++ b/config/panda_arm_hand.srdf.xacro
@@ -30,16 +30,19 @@
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="hand" parent_link="panda_link8" group="hand" parent_group="panda_arm_hand" />
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
- <disable_collisions link1="panda_hand" link2="panda_link3" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default" />
<disable_collisions link1="panda_hand" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_hand" link2="panda_link7" reason="Adjacent" />
- <disable_collisions link1="panda_leftfinger" link2="panda_link3" reason="Never" />
+ <disable_collisions link1="panda_hand" link2="panda_link7" reason="Default" />
+ <disable_collisions link1="panda_hand" link2="panda_link8" reason="Adjacent" />
<disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_link5" reason="Default" />
<disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never" />
<disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never" />
- <disable_collisions link1="panda_link3" link2="panda_rightfinger" reason="Never" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_link8" reason="Never" />
<disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never" />
+ <disable_collisions link1="panda_link5" link2="panda_rightfinger" reason="Default" />
<disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never" />
<disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never" />
+ <disable_collisions link1="panda_link8" link2="panda_rightfinger" reason="Never" />
</robot>
Sometimes i get an self_collision_avoidance_violation, but the robot seems not to be near a collision. Does somebody can explain how the internal self collision checking of the robot-controller works?
I think that the stl-Files used for collision checking in MoveIt! should be padded to avoid this errors, or the controller must be less conservative in self collision avoidance. In the Picture you can see a position of the robot, in which the controller throws a collisions-avoidance-error, but the collision model and the real robot is far away from self collision.
Perhaps someone already has padded collision files and can provide them? With my planning scene I get in 10 % of random valid goals in MoveIt! an self collision error from the Robot.