add YuMi SRDF w/ xacro build

Author: ClintLiddick

yumi_description/CMakeLists.txt

@@ -12,31 +12,42 @@ if(NOT YUMI_HARDWARE_INTERFACE)
   set(YUMI_HARDWARE_INTERFACE "EffortJointInterface")
 endif()
 
-set(YUMI_XACRO ${CMAKE_CURRENT_SOURCE_DIR}/urdf/yumi.urdf.xacro)
 set(YUMI_URDF_DIR ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf)
+set(YUMI_URDF_XACRO ${CMAKE_CURRENT_SOURCE_DIR}/urdf/yumi.urdf.xacro)
 set(YUMI_URDF ${YUMI_URDF_DIR}/yumi.urdf)
+set(YUMI_SRDF_XACRO ${CMAKE_CURRENT_SOURCE_DIR}/urdf/yumi.srdf.xacro)
+set(YUMI_SRDF ${YUMI_URDF_DIR}/yumi.srdf)
 
 file(MAKE_DIRECTORY "${YUMI_URDF_DIR}")
 
 # Call out to xacro to get dependencies
-execute_process(COMMAND ${CATKIN_ENV} rosrun xacro xacro --deps ${YUMI_XACRO}
-  ERROR_VARIABLE _xacro_err_ignore
-  OUTPUT_VARIABLE _xacro_deps_result
-  OUTPUT_STRIP_TRAILING_WHITESPACE)
+macro(build_xacro infile outfile argstring)
+    execute_process(COMMAND ${CATKIN_ENV} rosrun xacro xacro --deps ${infile}
+    ERROR_VARIABLE _xacro_err_ignore
+    OUTPUT_VARIABLE _xacro_deps_result
+    OUTPUT_STRIP_TRAILING_WHITESPACE)
 
-separate_arguments(_xacro_deps_result)
+    separate_arguments(_xacro_deps_result)
 
-add_custom_command(OUTPUT ${YUMI_URDF}
-  COMMENT "Generating YuMi URDF"
-  COMMAND ${CATKIN_ENV} rosrun xacro xacro
-  ARGS -o ${YUMI_URDF} ${YUMI_XACRO} ${_xacro_deps_result} prefix:=${YUMI_HARDWARE_INTERFACE}
-  DEPENDS ${YUMI_XACRO} ${_xacro_deps_result})
+    add_custom_command(OUTPUT ${outfile}
+    COMMENT "Generating ${outfile}"
+    COMMAND ${CATKIN_ENV} rosrun xacro xacro
+    ARGS -o ${outfile} ${infile} ${_xacro_deps_result} ${argstring}
+    DEPENDS ${infile} ${_xacro_deps_result})
+endmacro(build_xacro)
 
-# add target to actually cause generation
-add_custom_target(description_files ALL DEPENDS ${YUMI_URDF})
+build_xacro(${YUMI_URDF_XACRO} ${YUMI_URDF} "prefix:=${YUMI_HARDWARE_INTERFACE}")
+build_xacro(${YUMI_SRDF_XACRO} ${YUMI_SRDF} "")
 
-install(FILES ${YUMI_URDF}
-	DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf/)
+# add target to actually cause generation
+add_custom_target(description_files ALL DEPENDS
+  ${YUMI_URDF}
+  ${YUMI_SRDF})
+
+install(FILES
+  ${YUMI_URDF}
+  ${YUMI_SRDF}
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf/)
 
 install(
 	DIRECTORY gazebo launch meshes

yumi_description/urdf/yumi.srdf.xacro

@@ -0,0 +1,28 @@
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="yumi">
+
+  <xacro:macro name="yumi_arm" params="side postfix">
+    <group name="${side}">
+      <chain base_link="yumi_body" tip_link="yumi_link_7_${postfix}"/>
+    </group>
+
+    <group name="${side}_hand">
+      <link name="gripper_${postfix}_base"/>
+      <link name="gripper_${postfix}_finger_r"/>
+      <link name="gripper_${postfix}_finger_l"/>
+    </group>
+
+    <end_effector name="${side}_hand" group="${side}_hand"
+                  parent_link="yumi_link_7_${postfix}" parent_group="${side}"/>
+
+    <!-- Disable collisions between hand links -->
+    <disable_collisions link1="gripper_${postfix}_base" link2="gripper_${postfix}_finger_r"/>
+    <disable_collisions link1="gripper_${postfix}_base" link2="gripper_${postfix}_finger_l"/>
+    <disable_collisions link1="gripper_${postfix}_finger_r" link2="gripper_${postfix}_finger_l"/>
+
+    <!-- Disable arm self-collisions -->
+    <!-- TODO -->
+  </xacro:macro>
+
+  <xacro:yumi_arm side="left" postfix="l"/>
+  <xacro:yumi_arm side="right" postfix="r"/>
+</robot>