add hrp2 ROM shape with low body collision

CMakeLists.txt

@@ -68,6 +68,7 @@ install(FILES
 install(FILES
   data/urdf/hrp2_trunk.urdf
   data/urdf/hrp2_trunk_flexible.urdf
+  data/urdf/hrp2_trunk_lowBody_flexible.urdf
   data/urdf/hrp2_trunk_arms_flexible.urdf
   data/urdf/hrp2_rom.urdf
   data/urdf/hrp2_larm_rom.urdf
@@ -139,6 +140,7 @@ install(FILES
 install(FILES
   data/srdf/hrp2_trunk.srdf
   data/srdf/hrp2_trunk_flexible.srdf
+  data/srdf/hrp2_trunk_lowBody_flexible.srdf
   data/srdf/hrp2_trunk_arms_flexible.srdf
   data/srdf/hrp2_rom.srdf
   data/srdf/hrp2_larm_rom.srdf
@@ -218,9 +220,11 @@ install(FILES
 	data/meshes/ground_table.stl
 	data/meshes/hrp2_trunk.stl
 	data/meshes/hrp2_trunk_body.stl
+	data/meshes/hrp2_trunk_body_low.stl
 	data/meshes/hrp2_trunk_torso.stl
 	data/meshes/hrp2_trunk_torso_arms.stl
 	data/meshes/hrp2_trunk_body_view.dae
+	data/meshes/hrp2_trunk_body_low_view.dae
 	data/meshes/hrp2_trunk_torso_arms_view.dae
 	data/meshes/hrp2_trunk_torso_view.dae
 	data/meshes/hrp2_rom.stl

data/srdf/hrp2_trunk_lowBody_flexible.srdf

+<?xml version="1.0"?>
+<robot name="hrp2_trunk_flexible">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+  <disable_collisions link1="BODY"
+		      link2="torso" />  
+  <disable_collisions link1="torso"
+		      link2="HEAD_LINK0" />  
+  <disable_collisions link1="torso"
+		      link2="HEAD_LINK1" />  
+  <disable_collisions link1="HEAD_LINK0"
+		      link2="HEAD_LINK1" />  
+</robot>

data/urdf/hrp2_trunk_lowBody_flexible.urdf

+
+<robot name="hrp2_trunk_flexible">
+  <link name="base_link"/>
+  <joint name="WAIST" type="fixed">
+    <parent link="base_link"/>
+    <child link="BODY"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <link name="BODY">   
+  <inertial>
+     <mass value="58.0"/>
+     <inertia ixx="1." ixy="0.0" ixz="0.0" iyy="1." iyz="0.0" izz="1."/>
+  </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk_body_low_view.dae"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk_body_low.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Chest -->
+  <joint name="CHEST_JOINT0" type="revolute">
+    <parent link="BODY"/>
+    <child link="CHEST_LINK0"/>
+    <origin rpy="0 0 0" xyz="0.032 0 0.3507"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="151.1" lower="-0.785398" upper="0.785398" velocity="4.40217"/>
+  </joint>
+  <link name="CHEST_LINK0">
+  </link>
+  <joint name="CHEST_JOINT1" type="revolute">
+    <parent link="CHEST_LINK0"/>
+    <child link="torso"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="278.9" lower="-0.0872665" upper="1.0472" velocity="2.3963"/>
+  </joint>  
+  <link name="torso">  
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0.003" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk_torso_arms_view.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0.003" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk_torso_arms.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Head -->
+  <joint name="HEAD_JOINT0" type="revolute">
+    <parent link="torso"/>
+    <child link="HEAD_LINK0"/>
+    <origin rpy="0 -0 0" xyz="-0.007 0 0.2973"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="9.6" lower="-0.785398" upper="0.785398" velocity="6.87679"/>
+  </joint>
+  <link name="HEAD_LINK0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hrp2_14_description/meshes/HEAD_LINK0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hrp2_14_description/meshes/convex/HEAD_LINK0.dae"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HEAD_JOINT1" type="revolute">
+    <parent link="HEAD_LINK0"/>
+    <child link="HEAD_LINK1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="9.6" lower="-0.523599" upper="0.785398" velocity="6.87679"/>
+  </joint>
+  <!-- VRML link name="HEAD_LINK1" -->
+  <link name="HEAD_LINK1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hrp2_14_description/meshes/HEAD_LINK1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hrp2_14_description/meshes/convex/HEAD_LINK1.dae"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>