diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1119fd11ed792a544b9eb9495d72decbefe9a027..4ce522c483237f28fb995801a1ff8a92531ef5ed 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -68,6 +68,12 @@ install(FILES
   data/urdf/hrp2_rarm_rom.urdf
   data/urdf/hrp2_lleg_rom.urdf
   data/urdf/hrp2_rleg_rom.urdf
+  data/urdf/hyq/hyq_trunk.urdf
+  data/urdf/hyq/hyq_rhleg_rom.urdf
+  data/urdf/hyq/hyq_rfleg_rom.urdf
+  data/urdf/hyq/hyq_lhleg_rom.urdf
+  data/urdf/hyq/hyq_lfleg_rom.urdf
+  data/urdf/hyq/hyq_rom.urdf
   data/urdf/box_rom.urdf
   data/urdf/box.urdf
   data/urdf/scene.urdf
@@ -88,6 +94,12 @@ install(FILES
   data/srdf/hrp2_rarm_rom.srdf
   data/srdf/hrp2_lleg_rom.srdf
   data/srdf/hrp2_rleg_rom.srdf
+  data/srdf/hyq/hyq_trunk.srdf
+  data/srdf/hyq/hyq_rhleg_rom.srdf
+  data/srdf/hyq/hyq_rfleg_rom.srdf
+  data/srdf/hyq/hyq_lhleg_rom.srdf
+  data/srdf/hyq/hyq_lfleg_rom.srdf
+  data/srdf/hyq/hyq_rom.srdf
   data/srdf/box_rom.srdf
   data/srdf/box.srdf
   data/srdf/scene.srdf
@@ -121,14 +133,21 @@ install(FILES
 	data/meshes/truck.stl
 	data/meshes/truck_vision.stl
 	data/meshes/stair_bauzil.stl
-        data/meshes/stair_bauzil_reduced.stl
+    data/meshes/stair_bauzil_reduced.stl
 	data/meshes/climb.stl
 	data/meshes/stepladder.stl
 	data/meshes/chair.stl
-	#~ data/meshes/pedal.stl
-	#~ data/meshes/board.stl
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
   )
+install(FILES
+	data/meshes/hyq/hyq_trunk.stl
+	data/meshes/hyq/hyq_rom.stl
+	data/meshes/hyq/hyq_rhleg_rom.stl
+	data/meshes/hyq/hyq_rfleg_rom.stl
+	data/meshes/hyq/hyq_lhleg_rom.stl
+	data/meshes/hyq/hyq_lfleg_rom.stl
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
+  )
   
 SETUP_PROJECT_FINALIZE()
 
diff --git a/data/meshes/hyq/hyq_lfleg_rom.stl b/data/meshes/hyq/hyq_lfleg_rom.stl
new file mode 100644
index 0000000000000000000000000000000000000000..21491da2a743a3bbf127977a5ab94952aea60b72
Binary files /dev/null and b/data/meshes/hyq/hyq_lfleg_rom.stl differ
diff --git a/data/meshes/hyq/hyq_lhleg_rom.stl b/data/meshes/hyq/hyq_lhleg_rom.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2d67f18a5330d466c0fab417205e3a61b00f95d5
Binary files /dev/null and b/data/meshes/hyq/hyq_lhleg_rom.stl differ
diff --git a/data/meshes/hyq/hyq_rfleg_rom.stl b/data/meshes/hyq/hyq_rfleg_rom.stl
new file mode 100644
index 0000000000000000000000000000000000000000..014342bb9d25053cf19197065adfd7ae3b94645f
Binary files /dev/null and b/data/meshes/hyq/hyq_rfleg_rom.stl differ
diff --git a/data/meshes/hyq/hyq_rhleg_rom.stl b/data/meshes/hyq/hyq_rhleg_rom.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e0f2e6a49fcef34db3bd134956c13c1eb6fec9e1
Binary files /dev/null and b/data/meshes/hyq/hyq_rhleg_rom.stl differ
diff --git a/data/meshes/hyq/hyq_rom.stl b/data/meshes/hyq/hyq_rom.stl
new file mode 100644
index 0000000000000000000000000000000000000000..5c8986a63c494f952d102b9f882a86ae88c5e5cb
Binary files /dev/null and b/data/meshes/hyq/hyq_rom.stl differ
diff --git a/data/meshes/hyq/hyq_trunk.stl b/data/meshes/hyq/hyq_trunk.stl
new file mode 100644
index 0000000000000000000000000000000000000000..7a4496b679fedfafc5df37d670896a80d68d4108
Binary files /dev/null and b/data/meshes/hyq/hyq_trunk.stl differ
diff --git a/data/srdf/hyq/hrp2_trunk.srdf b/data/srdf/hyq/hrp2_trunk.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..8538107defdfb1a2a6e59454a2b6f07ca8a7e4fb
--- /dev/null
+++ b/data/srdf/hyq/hrp2_trunk.srdf
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot name="hrp2_trunk">
+  <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>
+</robot>
diff --git a/data/srdf/hyq/hyq_lfleg_rom.srdf b/data/srdf/hyq/hyq_lfleg_rom.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..fa4c185983231766a43936c29ab35f4a4c66b978
--- /dev/null
+++ b/data/srdf/hyq/hyq_lfleg_rom.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_lfleg_rom">
+</robot>
diff --git a/data/srdf/hyq/hyq_lhleg_rom.srdf b/data/srdf/hyq/hyq_lhleg_rom.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..f4dde37cbbe677dd83719270a7d6671d1e677ac6
--- /dev/null
+++ b/data/srdf/hyq/hyq_lhleg_rom.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_lhleg_rom">
+</robot>
diff --git a/data/srdf/hyq/hyq_rfleg_rom.srdf b/data/srdf/hyq/hyq_rfleg_rom.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..268ca1c4ae6d9f3f2849388782351be0cde2b795
--- /dev/null
+++ b/data/srdf/hyq/hyq_rfleg_rom.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_rfleg_rom">
+</robot>
diff --git a/data/srdf/hyq/hyq_rhleg_rom.srdf b/data/srdf/hyq/hyq_rhleg_rom.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..af6533b82377be62b086a2d811e5615b255db368
--- /dev/null
+++ b/data/srdf/hyq/hyq_rhleg_rom.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_rhleg_rom">
+</robot>
diff --git a/data/srdf/hyq/hyq_rom.srdf b/data/srdf/hyq/hyq_rom.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..a12ec4ca41cc7f59194a18ee35ff747a575729ad
--- /dev/null
+++ b/data/srdf/hyq/hyq_rom.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_rom">
+</robot>
diff --git a/data/srdf/hyq/hyq_trunk.srdf b/data/srdf/hyq/hyq_trunk.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..6a54df5cb27ed972b58493a83a1895c4c3b02b24
--- /dev/null
+++ b/data/srdf/hyq/hyq_trunk.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_trunk">
+</robot>
diff --git a/data/urdf/hyq/hyq_lfleg_rom.urdf b/data/urdf/hyq/hyq_lfleg_rom.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..6dd3d6dc3f37722581e694812eb7ce08e6f48082
--- /dev/null
+++ b/data/urdf/hyq/hyq_lfleg_rom.urdf
@@ -0,0 +1,19 @@
+<robot name="hyq_lfleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_lfleg_rom.stl"/>
+      </geometry>
+      <material name="white">
+        <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/hyq/hyq_lfleg_rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/data/urdf/hyq/hyq_lhleg_rom.urdf b/data/urdf/hyq/hyq_lhleg_rom.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..5e335b3de72e386d58e716c49974aa4522d6139e
--- /dev/null
+++ b/data/urdf/hyq/hyq_lhleg_rom.urdf
@@ -0,0 +1,19 @@
+ <robot name="hyq_lhleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_lhleg_rom.stl"/>
+      </geometry>
+      <material name="white">
+        <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/hyq/hyq_lhleg_rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/data/urdf/hyq/hyq_rfleg_rom.urdf b/data/urdf/hyq/hyq_rfleg_rom.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..dd444d2c57728c149394ba53a7def38bed85ff99
--- /dev/null
+++ b/data/urdf/hyq/hyq_rfleg_rom.urdf
@@ -0,0 +1,19 @@
+ <robot name="hyq_rfleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_rfleg_rom.stl"/>
+      </geometry>
+      <material name="white">
+        <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/hyq/hyq_rfleg_rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/data/urdf/hyq/hyq_rhleg_rom.urdf b/data/urdf/hyq/hyq_rhleg_rom.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..877a9bb535445cfbeac863e63712552bf4f5f500
--- /dev/null
+++ b/data/urdf/hyq/hyq_rhleg_rom.urdf
@@ -0,0 +1,19 @@
+ <robot name="hyq_rhleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_rhleg_rom.stl"/>
+      </geometry>
+      <material name="white">
+        <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/hyq/hyq_rhleg_rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/data/urdf/hyq/hyq_rom.urdf b/data/urdf/hyq/hyq_rom.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..501db77b08f0dda3f4a320d89eee248172b2ff36
--- /dev/null
+++ b/data/urdf/hyq/hyq_rom.urdf
@@ -0,0 +1,19 @@
+<robot name="hyq_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_rom.stl"/>
+      </geometry>
+      <material name="white">
+        <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/hyq/hyq_rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/data/urdf/hyq/hyq_trunk.urdf b/data/urdf/hyq/hyq_trunk.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..07a83bd69f97d33987150ecd0d07a3b76ad74362
--- /dev/null
+++ b/data/urdf/hyq/hyq_trunk.urdf
@@ -0,0 +1,19 @@
+<robot name="hyq_trunk">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk.stl"/>
+      </geometry>
+      <material name="white">
+        <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/hyq/hyq_trunk.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>