Add benchmark with magnet construction set.

CMakeLists.txt

@@ -51,6 +51,16 @@ SET(SRDF_UR_BENCHMARK_FILES
   srdf/ur_benchmark/pokeball.srdf
   )
 
+SET(URDF_CONSTRUCTION_SET_FILES
+  urdf/construction_set/ground.urdf
+  urdf/construction_set/sphere.urdf
+  )
+
+SET(SRDF_CONSTRUCTION_SET_FILES
+  srdf/construction_set/ground.srdf
+  srdf/construction_set/sphere.srdf
+  )
+
 SET(SRDF_FILES
     srdf/buggy.srdf
     srdf/placard.srdf
@@ -78,6 +88,12 @@ INSTALL(FILES ${URDF_UR_BENCHMARK_FILES}
 INSTALL(FILES ${SRDF_UR_BENCHMARK_FILES}
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf/ur_benchmark)
 
+INSTALL(FILES ${URDF_CONSTRUCTION_SET_FILES}
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf/construction_set)
+
+INSTALL(FILES ${SRDF_CONSTRUCTION_SET_FILES}
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf/construction_set)
+
 INSTALL(FILES ${SRDF_FILES}
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf)
 

srdf/construction_set/ground.srdf

+<?xml version="1.0"?>
+<robot name="ground">
+  <gripper name="slot1" clearance = "0.05">
+    <position> -0.35 -0.025 0.025 0 -0.707 0 0.707 </position>
+    <link name="ground_base" />
+  </gripper>
+  <gripper name="slot2" clearance = "0.05">
+    <position> -0.40 -0.025 0.025 0 -0.707 0 0.707 </position>
+    <link name="ground_base" />
+  </gripper>
+  <gripper name="slot3" clearance = "0.05">
+    <position> -0.35 0.025 0.025 0 -0.707 0 0.707 </position>
+    <link name="ground_base" />
+  </gripper>
+  <gripper name="slot4" clearance = "0.05">
+    <position> -0.40 0.025 0.025 0 -0.707 0 0.707 </position>
+    <link name="ground_base" />
+  </gripper>
+</robot>

srdf/construction_set/sphere.srdf

+<?xml version="1.0"?>
+<robot name="sphere">
+  <handle name="handle" clearance="0.025">
+    <position> 0 0 0 0.7071067811865476 0 0.7071067811865476 0 </position>
+    <link name="base_link"/>
+  </handle>
+</robot>

urdf/construction_set/cylinder.srdf

+<robot name="cylinder">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      <mass value="0.6"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+	       iyy="0.001" iyz="0.0"
+	       izz="0.001" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+	<box size="0.025 0.025 0.025"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+	<box size="0.025 0.025 0.025"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

urdf/construction_set/cylinder.urdf

+<robot name="cylinder">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      <mass value="0.6"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+	       iyy="0.001" iyz="0.0"
+	       izz="0.001" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+	<cylinder radius="0.005" length="0.08"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+	<box size="0.08 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

urdf/construction_set/ground.urdf

+<?xml version="1.0"?>
+<robot name="ground"> 
+  <link name="ground_base">
+    <visual>
+      <geometry>
+        <box size="2 1 0.02"/>
+      </geometry>
+      <origin xyz="0 0 -0.01"/>
+      <material name="Orange">
+        <color rgba="1 0.4 0.3 0.6"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="2 1 0.02"/>
+      </geometry>
+      <origin xyz="0 0 -0.02"/>
+    </collision>
+  </link>
+</robot>

urdf/construction_set/sphere.urdf

+<robot name="sphere">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      <mass value="0.6"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+	       iyy="0.001" iyz="0.0"
+	       izz="0.001" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+	<sphere radius="0.02"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+	<sphere radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>