Added extra virtual gripper_center end effector to panda_arm_hand.srdf

- Added virtual gripper to moveit groups

- Added virtual center gripper xacro:if statement

- Added gripper_center launch file arguments

.setup_assistant

@@ -2,9 +2,10 @@ moveit_setup_assistant_config:
   URDF:
     package: franka_description
     relative_path: robots/panda_arm_hand.urdf.xacro
+    xacro_args: gripper_center:=true
   SRDF:
-    relative_path: config/panda_arm.srdf.xacro
+    relative_path: config/panda_arm_hand.srdf.xacro
   CONFIG:
-    author_name: Mike Lautman
-    author_email: mike@picknik.ai
-    generated_timestamp: 1519152260
+    author_name: Rick Staa
+    author_email: rick.staa@outlook.com
+    generated_timestamp: 1570976258

config/hand.xacro

@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
-  <xacro:macro name="hand">
+  <xacro:macro name="hand" params="gripper_center:='false'">
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
     <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
@@ -10,6 +10,10 @@
       <link name="panda_hand" />
       <link name="panda_leftfinger" />
       <link name="panda_rightfinger" />
+      <!-- Added extra virtual joint to ease the grasp movement planning -->
+      <xacro:if value="${gripper_center}"> <!-- Only when gripper_center==True -->
+        <link name="panda_gripper_center" />
+      </xacro:if>
       <joint name="panda_finger_joint1" />
       <passive_joint name="panda_finger_joint2" />
     </group>

config/panda_arm_hand.srdf.xacro

@@ -4,10 +4,14 @@
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <!-- Initialize gripper_center properties -->
+  <xacro:arg name="gripper_center" default="false" />
+  <xacro:property name="gripper_center" value="$(arg gripper_center)"/>
+  <!-- Include other subgroup xacros -->
   <xacro:include filename="$(find panda_moveit_config)/config/panda_arm.xacro" />
   <xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
   <xacro:panda_arm />
-  <xacro:hand />
+  <xacro:hand gripper_center="${gripper_center}" /> <!-- Added extra xacro macro argument for the new virtual center gripper link -->
   <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
   <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
   <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
@@ -46,6 +50,10 @@
   </group_state>
   <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
   <end_effector name="hand" parent_link="panda_link8" group="hand" parent_group="panda_arm" />
+  <!-- Added extra virtual joint to ease the grasp movement planning -->
+  <xacro:if value="${gripper_center}"> <!-- Only when gripper_center==True -->
+    <end_effector name="gripper_center" parent_link="panda_gripper_center" group="panda_arm" parent_group="hand" />
+  </xacro:if>
   <!--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" />

launch/demo.launch

@@ -9,6 +9,10 @@
   <arg name="debug" default="false" />
   <arg name="pipeline" default="ompl" />
 
+  <!-- Allow user to specify if they want an extra gripper center frame -->
+  <arg name="gripper_center" default="false"/>
+  <arg name="gripper_center_xyz" default="0.0 0.0 0.10"/>
+
   <!--
   By default, hide joint_state_publisher's GUI
 
@@ -23,6 +27,8 @@
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
   <include file="$(find panda_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
+    <arg name="gripper_center" default="$(arg gripper_center)"/>
+    <arg name="gripper_center_xyz" default="$(arg gripper_center_xyz)"/>
   </include>
 
   <!-- If needed, broadcast static tf for robot root -->
@@ -45,6 +51,8 @@
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
     <arg name="pipeline" value="$(arg pipeline)"  />
+    <arg name="gripper_center" default="$(arg gripper_center)"/>
+    <arg name="gripper_center_xyz" default="$(arg gripper_center_xyz)"/>
   </include>
 
   <!-- Run Rviz -->

launch/move_group.launch

@@ -2,8 +2,14 @@
 
   <arg name="load_gripper" default="true" />
 
+  <!-- Allow user to specify if they want an extra gripper center frame -->
+  <arg name="gripper_center" default="false"/>
+  <arg name="gripper_center_xyz" default="0.0 0.0 0.10"/>
+
   <include file="$(find panda_moveit_config)/launch/planning_context.launch">
     <arg name="load_gripper" value="$(arg load_gripper)" />
+    <arg name="gripper_center" default="$(arg gripper_center)"/>
+    <arg name="gripper_center_xyz" default="$(arg gripper_center_xyz)"/>
   </include>
 
   <arg name="pipeline" default="ompl" />

launch/planning_context.launch

@@ -1,18 +1,22 @@
 <launch>
   <arg name="load_gripper" default="true" />
 
+  <!-- Allow user to specify if they want an extra gripper center frame -->
+  <arg name="gripper_center" default="false"/>
+  <arg name="gripper_center_xyz" default="0.0 0.0 0.10"/>
+
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
+  <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro' 'gripper_center:=$(arg gripper_center)' 'gripper_center_xyz:=$(arg gripper_center_xyz)'"/>
   <param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro' 'gripper_center:=$(arg gripper_center)'" if="$(arg load_gripper)" />
   <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
 
   <!-- Load updated joint limits (override information from URDF) -->

package.xml

@@ -4,8 +4,8 @@
   <description>
      An automatically generated package with all the configuration and launch files for using the panda with the MoveIt! Motion Planning Framework
   </description>
-  <author email="mike@picknik.ai">Mike Lautman</author>
-  <maintainer email="mike@picknik.ai">Mike Lautman</maintainer>
+  <author email="rick.staa@outlook.com">Rick Staa</author>
+  <maintainer email="rick.staa@outlook.com">Rick Staa</maintainer>
 
   <license>BSD</license>
 
@@ -23,8 +23,10 @@
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>moveit_setup_assistant</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>robot_state_publisher</run_depend>
+  <run_depend>tf2_ros</run_depend>
   <run_depend>xacro</run_depend>
   <run_depend>topic_tools</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment.