diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9bfc7fe5e1a4b4028c6169b143628f2d7f7f92d4..5c955d4ce306c6f6d5b1e22782efae758501771b 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -69,6 +69,7 @@ install(FILES
data/urdf/hrp2_lleg_rom.urdf
data/urdf/hrp2_rleg_rom.urdf
data/urdf/hyq/hyq_trunk.urdf
+ data/urdf/hyq/hyq_trunk_large.urdf
data/urdf/hyq/hyq_rhleg_rom.urdf
data/urdf/hyq/hyq_rfleg_rom.urdf
data/urdf/hyq/hyq_lhleg_rom.urdf
@@ -97,6 +98,7 @@ install(FILES
data/srdf/hrp2_lleg_rom.srdf
data/srdf/hrp2_rleg_rom.srdf
data/srdf/hyq/hyq_trunk.srdf
+ data/srdf/hyq/hyq_trunk_large.srdf
data/srdf/hyq/hyq_rhleg_rom.srdf
data/srdf/hyq/hyq_rfleg_rom.srdf
data/srdf/hyq/hyq_lhleg_rom.srdf
@@ -148,6 +150,7 @@ install(FILES
install(FILES
data/meshes/hyq/hyq_all.stl
data/meshes/hyq/hyq_trunk.stl
+ data/meshes/hyq/hyq_trunk_large.stl
data/meshes/hyq/hyq_rom.stl
data/meshes/hyq/hyq_rhleg_rom.stl
data/meshes/hyq/hyq_rfleg_rom.stl
@@ -156,5 +159,9 @@ install(FILES
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
)
+install(DIRECTORY data/hyq_description
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/../
+ )
+
SETUP_PROJECT_FINALIZE()
diff --git a/data/blender/darpa.blend b/data/blender/darpa.blend
new file mode 100644
index 0000000000000000000000000000000000000000..b2075b55a79244b8f36cf6b9b5220ef8afb405b7
Binary files /dev/null and b/data/blender/darpa.blend differ
diff --git a/data/blender/groundcrouch.blend b/data/blender/groundcrouch.blend
new file mode 100644
index 0000000000000000000000000000000000000000..2a23f90a125a31076e983ab79fb206e93ac0773e
Binary files /dev/null and b/data/blender/groundcrouch.blend differ
diff --git a/data/blender/hyq.blend b/data/blender/hyq.blend
new file mode 100644
index 0000000000000000000000000000000000000000..c1b74061f569128215f7bc86e0de6fc4fa768222
Binary files /dev/null and b/data/blender/hyq.blend differ
diff --git a/data/blender/stair_bauzil.blend b/data/blender/stair_bauzil.blend
index afa523a04285e659f3148abac75fbc908771f4d6..5c77bf7fedf68a8cd0c057886bbf122f15e1bdc7 100644
Binary files a/data/blender/stair_bauzil.blend and b/data/blender/stair_bauzil.blend differ
diff --git a/data/hyq_description/meshes/leg/lowerlegreduced.dae b/data/hyq_description/meshes/leg/lowerlegreduced.dae
new file mode 100644
index 0000000000000000000000000000000000000000..9829098ec951754d759d67e522481c9b88b84009
--- /dev/null
+++ b/data/hyq_description/meshes/leg/lowerlegreduced.dae
@@ -0,0 +1,98 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+ <asset>
+ <contributor>
+ <author>Blender User</author>
+ <authoring_tool>Blender 2.75.0 commit date:2015-07-07, commit time:14:56, hash:c27589e</authoring_tool>
+ </contributor>
+ <created>2015-10-19T13:54:27</created>
+ <modified>2015-10-19T13:54:27</modified>
+ <unit name="meter" meter="1"/>
+ <up_axis>Z_UP</up_axis>
+ </asset>
+ <library_images/>
+ <library_geometries>
+ <geometry id="Cube_001-mesh" name="Cube.001">
+ <mesh>
+ <source id="Cube_001-mesh-positions">
+ <float_array id="Cube_001-mesh-positions-array" count="24">-3.202436 -0.9731922 -0.9147082 -3.202436 -0.9731922 1.085292 -3.202436 1.026808 -0.9147082 -3.202436 1.026808 1.085292 -1.202436 -0.9731922 -0.9147082 -1.202436 -0.9731922 1.085292 -1.202436 1.026808 -0.9147082 -1.202436 1.026808 1.085292</float_array>
+ <technique_common>
+ <accessor source="#Cube_001-mesh-positions-array" count="8" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="Cube_001-mesh-normals">
+ <float_array id="Cube_001-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
+ <technique_common>
+ <accessor source="#Cube_001-mesh-normals-array" count="6" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="Cube_001-mesh-vertices">
+ <input semantic="POSITION" source="#Cube_001-mesh-positions"/>
+ </vertices>
+ <polylist count="12">
+ <input semantic="VERTEX" source="#Cube_001-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#Cube_001-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>3 0 2 0 0 0 7 1 6 1 2 1 5 2 4 2 6 2 1 3 0 3 4 3 2 4 6 4 4 4 7 5 3 5 1 5 1 0 3 0 0 0 3 1 7 1 2 1 7 2 5 2 6 2 5 3 1 3 4 3 0 4 2 4 4 4 5 5 7 5 1 5</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="Cube-mesh" name="Cube">
+ <mesh>
+ <source id="Cube-mesh-positions">
+ <float_array id="Cube-mesh-positions-array" count="24">-3.222331 -0.9820953 -0.9556123 -3.222331 -0.9820953 1.044388 -3.222331 1.017905 -0.9556123 -3.222331 1.017905 1.044388 -1.222331 -0.9820953 -0.9556123 -1.222331 -0.9820953 1.044388 -1.222331 1.017905 -0.9556123 -1.222331 1.017905 1.044388</float_array>
+ <technique_common>
+ <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="Cube-mesh-normals">
+ <float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
+ <technique_common>
+ <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="Cube-mesh-vertices">
+ <input semantic="POSITION" source="#Cube-mesh-positions"/>
+ </vertices>
+ <polylist count="12">
+ <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>3 0 2 0 0 0 7 1 6 1 2 1 5 2 4 2 6 2 1 3 0 3 4 3 2 4 6 4 4 4 7 5 3 5 1 5 1 0 3 0 0 0 3 1 7 1 2 1 7 2 5 2 6 2 5 3 1 3 4 3 0 4 2 4 4 4 5 5 7 5 1 5</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ </library_geometries>
+ <library_controllers/>
+ <library_visual_scenes>
+ <visual_scene id="Scene" name="Scene">
+ <node id="Cube_001" name="Cube_001" type="NODE">
+ <matrix sid="transform">-0.1032963 0 0 9.31323e-10 0 -0.01948144 0 0 0 0 -0.02072426 0 0 0 0 1</matrix>
+ <instance_geometry url="#Cube_001-mesh" name="Cube_001"/>
+ </node>
+ <node id="Cube" name="Cube" type="NODE">
+ <matrix sid="transform">-0.03529432 0 0 -3.72529e-9 0 -0.02916849 0 0 0 0 -0.03982198 0 0 0 0 1</matrix>
+ <instance_geometry url="#Cube-mesh" name="Cube"/>
+ </node>
+ </visual_scene>
+ </library_visual_scenes>
+ <scene>
+ <instance_visual_scene url="#Scene"/>
+ </scene>
+</COLLADA>
\ No newline at end of file
diff --git a/data/hyq_description/srdf/hyq.srdf b/data/hyq_description/srdf/hyq.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..07f07a745ea1f06c6aee93f66c811cdbc8c14afa
--- /dev/null
+++ b/data/hyq_description/srdf/hyq.srdf
@@ -0,0 +1,36 @@
+<?xml version="1.0" ?>
+<!--This does not replace URDF, and is not an extension of URDF.
+ This is a format for representing semantic information about the robot structure.
+ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="hyq">
+ <!--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="base_link" link2="lf_foot" reason="Adjacent" />
+ <disable_collisions link1="base_link" link2="lh_foot" reason="Adjacent" />
+ <disable_collisions link1="base_link" link2="rf_foot" reason="Adjacent" />
+ <disable_collisions link1="base_link" link2="rh_foot" reason="Adjacent" />
+ <disable_collisions link1="lf_foot" link2="lh_foot" reason="Adjacent" />
+ <disable_collisions link1="lf_foot" link2="rf_foot" reason="Adjacent" />
+ <disable_collisions link1="lf_foot" link2="rh_foot" reason="Adjacent" />
+ <disable_collisions link1="lh_foot" link2="rf_foot" reason="Adjacent" />
+ <disable_collisions link1="lh_foot" link2="rh_foot" reason="Adjacent" />
+ <disable_collisions link1="rf_foot" link2="rh_foot" reason="Adjacent" />
+-->
+ <disable_collisions link1="lf_hipassembly" link2="trunk" reason="Adjacent" />
+ <disable_collisions link1="lh_hipassembly" link2="trunk" reason="Adjacent" />
+ <disable_collisions link1="rf_hipassembly" link2="trunk" reason="Adjacent" />
+ <disable_collisions link1="rh_hipassembly" link2="trunk" reason="Adjacent" />
+ <disable_collisions link1="lf_hipassembly" link2="lf_upperleg" reason="Adjacent" />
+ <disable_collisions link1="lh_hipassembly" link2="lh_upperleg" reason="Adjacent" />
+ <disable_collisions link1="rf_hipassembly" link2="rf_upperleg" reason="Adjacent" />
+ <disable_collisions link1="rh_hipassembly" link2="rh_upperleg" reason="Adjacent" />
+ <disable_collisions link1="lf_lowerleg" link2="lf_upperleg" reason="Adjacent" />
+ <disable_collisions link1="lh_lowerleg" link2="lh_upperleg" reason="Adjacent" />
+ <disable_collisions link1="rf_lowerleg" link2="rf_upperleg" reason="Adjacent" />
+ <disable_collisions link1="rh_lowerleg" link2="rh_upperleg" reason="Adjacent" />
+ <disable_collisions link1="lf_lowerleg" link2="lf_foot" reason="Adjacent" />
+ <disable_collisions link1="lh_lowerleg" link2="lh_foot" reason="Adjacent" />
+ <disable_collisions link1="rf_lowerleg" link2="rf_foot" reason="Adjacent" />
+ <disable_collisions link1="rh_lowerleg" link2="rh_foot" reason="Adjacent" />
+</robot>
diff --git a/data/hyq_description/urdf/hyq.urdf b/data/hyq_description/urdf/hyq.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..2dd4aa38ea4f11a6e4cd88496ac4384588d00067
--- /dev/null
+++ b/data/hyq_description/urdf/hyq.urdf
@@ -0,0 +1,706 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from ../robots/hyq.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="hyq" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <!-- The following included files set up definitions of parts of the robot body -->
+ <!-- HyQ trunk -->
+ <!-- HyQ legs -->
+ <!-- HyQ sensors -->
+ <!--
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/microstrain_3dmgx325_imu.urdf.xacro"/>
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/asus_xtion.urdf.xacro"/>
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/velodyne.urdf.xacro"/>
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/kinect.urdf.xacro"/>
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/ptu_d46.urdf.xacro"/>
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/bumblebee.urdf.xacro"/>
+ <xacro:include filename="$(find hyq_description)/urdfs/sensors/vislab_3dve.urdf.xacro"/>
+-->
+ <!-- generic simulator_gazebo plugins for starting mechanism control, ros time -->
+ <!--
+ <xacro:include filename="$(find hyq_description)/gazebo/gazebo.urdf.xacro"/>
+-->
+ <!-- Now we can start using the macros included above to define the actual HyQ -->
+ <!-- The first use of a macro. This one was defined in trunk.urdf.xacro above.
+A macro like this will expand to a set of link and joint definitions, and to additional
+Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
+that equals "base", and uses it to generate names for its component links and joints
+(e.g., base_link). The included origin block is also an argument to the macro. By convention,
+the origin block defines where the component is w.r.t its parent (in this case the parent
+is the world frame). For more, see http://www.ros.org/wiki/xacro -->
+ <!-- body -->
+ <!-- trunk -->
+ <joint name="floating_base" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="base_link"/>
+ <child link="trunk"/>
+ </joint>
+ <link name="base_link">
+ <visual>
+ <geometry>
+ <cylinder length="0.01" radius="0.01"/>
+ </geometry>
+ </visual>
+ </link>
+ <link name="trunk">
+ <inertial>
+ <origin xyz="-0.02758 -2.3319e-05 0.03103"/>
+ <mass value="50.24"/>
+ <inertia ixx="0.97322" ixy="-0.0067362" ixz="0.1681" iyy="5.0487" iyz="-0.0060307" izz="5.283"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/trunk/trunk.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/trunk/trunk.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="trunk">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <!-- LF leg -->
+ <joint name="lf_haa_joint" type="revolute">
+ <origin rpy="0 1.57079632679 3.14159265359" xyz="0.3735 0.207 0"/>
+ <parent link="trunk"/>
+ <child link="lf_hipassembly"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-1.2217304764" upper="0.523598775598" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="lf_hfe_joint" type="revolute">
+ <origin rpy="1.57079632679 0 0" xyz="0.082 0 0"/>
+ <parent link="lf_hipassembly"/>
+ <child link="lf_upperleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-0.872664625997" upper="1.2217304764" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="lf_kfe_joint" type="revolute">
+ <origin rpy="0 0 0" xyz="0.35 0 0"/>
+ <parent link="lf_upperleg"/>
+ <child link="lf_lowerleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-2.44346095279" upper="-0.349065850399" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ <!-- Foot joint -->
+ </joint>
+ <joint name="lf_foot_joint" type="fixed">
+ <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.35 0 0"/>
+ <parent link="lf_lowerleg"/>
+ <child link="lf_foot"/>
+ </joint>
+ <link name="lf_hipassembly">
+ <inertial>
+ <origin xyz="0.04263 0.0 0.16931"/>
+ <mass value="2.93"/>
+ <inertia ixx="0.134705" ixy="3.6e-05" ixz="0.022734" iyy="0.144171" iyz="5.1e-05" izz="0.011033"/>
+ </inertial>
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="lf_upperleg">
+ <inertial>
+ <origin xyz="0.15074 0.02625 0.0"/>
+ <mass value="2.638"/>
+ <inertia ixx="0.005495" ixy="-0.007418" ixz="-0.000102" iyy="0.087136" iyz="-2.1e-05" izz="0.089871"/>
+ </inertial>
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="lf_lowerleg">
+ <inertial>
+ <origin xyz="0.1254 5e-05 -0.0001"/>
+ <mass value="0.881"/>
+ <inertia ixx="0.000468" ixy="0.0" ixz="0.0" iyy="0.026409" iyz="0.0" izz="0.026181"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerlegreduced.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="lf_foot">
+ <inertial>
+ <origin xyz="0 0 0"/>
+ <mass value="0.01"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+ </inertial>
+ <visual>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+ <geometry>
+ <sphere radius="0.021"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.02175"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="lf_haa_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="lf_haa_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="lf_haa_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="lf_hfe_upper_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="lf_hfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="lf_hfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="lf_kfe_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="lf_kfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="lf_kfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="lf_lowerleg">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>1.5</mu1>
+ <mu2>1.5</mu2>
+ <fdir1>1 0 0</fdir1>
+ <maxVel>1.0</maxVel>
+ <minDepth>0.00</minDepth>
+ </gazebo>
+ <!-- RF leg -->
+ <joint name="rf_haa_joint" type="revolute">
+ <origin rpy="0 1.57079632679 0" xyz="0.3735 -0.207 0"/>
+ <parent link="trunk"/>
+ <child link="rf_hipassembly"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-1.2217304764" upper="0.523598775598" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="rf_hfe_joint" type="revolute">
+ <origin rpy="-1.57079632679 0 0" xyz="0.082 0 0"/>
+ <parent link="rf_hipassembly"/>
+ <child link="rf_upperleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-0.872664625997" upper="1.2217304764" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="rf_kfe_joint" type="revolute">
+ <origin rpy="0 0 0" xyz="0.35 0 0"/>
+ <parent link="rf_upperleg"/>
+ <child link="rf_lowerleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-2.44346095279" upper="-0.349065850399" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ <!-- Foot joint -->
+ </joint>
+ <joint name="rf_foot_joint" type="fixed">
+ <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.35 0 0"/>
+ <parent link="rf_lowerleg"/>
+ <child link="rf_foot"/>
+ </joint>
+ <link name="rf_hipassembly">
+ <inertial>
+ <origin xyz="0.04263 -0.0 -0.16931"/>
+ <mass value="2.93"/>
+ <inertia ixx="0.134705" ixy="-3.6e-05" ixz="-0.022734" iyy="0.144171" iyz="5.1e-05" izz="0.011033"/>
+ </inertial>
+ <visual>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="rf_upperleg">
+ <inertial>
+ <origin xyz="0.15074 -0.02625 0.0"/>
+ <mass value="2.638"/>
+ <inertia ixx="0.005495" ixy="-0.007418" ixz="-0.000102" iyy="0.087136" iyz="-2.1e-05" izz="0.089871"/>
+ </inertial>
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="rf_lowerleg">
+ <inertial>
+ <origin xyz="0.1254 5e-05 -0.0001"/>
+ <mass value="0.881"/>
+ <inertia ixx="0.000468" ixy="0.0" ixz="0.0" iyy="0.026409" iyz="0.0" izz="0.026181"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerlegreduced.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="rf_foot">
+ <inertial>
+ <origin xyz="0 0 0"/>
+ <mass value="0.01"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+ </inertial>
+ <visual>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+ <geometry>
+ <sphere radius="0.021"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.02175"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="rf_haa_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="rf_haa_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="rf_haa_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="rf_hfe_upper_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="rf_hfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="rf_hfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="rf_kfe_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="rf_kfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="rf_kfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="rf_lowerleg">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>1.5</mu1>
+ <mu2>1.5</mu2>
+ <fdir1>1 0 0</fdir1>
+ <maxVel>1.0</maxVel>
+ <minDepth>0.00</minDepth>
+ </gazebo>
+ <!-- LH leg -->
+ <joint name="lh_haa_joint" type="revolute">
+ <origin rpy="0 1.57079632679 3.14159265359" xyz="-0.3735 0.207 0"/>
+ <parent link="trunk"/>
+ <child link="lh_hipassembly"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-1.2217304764" upper="0.523598775598" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="lh_hfe_joint" type="revolute">
+ <origin rpy="1.57079632679 0 0" xyz="0.082 0 0"/>
+ <parent link="lh_hipassembly"/>
+ <child link="lh_upperleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-1.2217304764" upper="0.872664625997" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="lh_kfe_joint" type="revolute">
+ <origin rpy="0 0 0" xyz="0.35 0 0"/>
+ <parent link="lh_upperleg"/>
+ <child link="lh_lowerleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="0.349065850399" upper="2.44346095279" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ <!-- Foot joint -->
+ </joint>
+ <joint name="lh_foot_joint" type="fixed">
+ <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.35 0 0"/>
+ <parent link="lh_lowerleg"/>
+ <child link="lh_foot"/>
+ </joint>
+ <link name="lh_hipassembly">
+ <inertial>
+ <origin xyz="0.04263 -0.0 -0.16931"/>
+ <mass value="2.93"/>
+ <inertia ixx="0.134705" ixy="3.6e-05" ixz="0.022734" iyy="0.144171" iyz="5.1e-05" izz="0.011033"/>
+ </inertial>
+ <visual>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="lh_upperleg">
+ <inertial>
+ <origin xyz="0.15074 0.02625 0.0"/>
+ <mass value="2.638"/>
+ <inertia ixx="0.005495" ixy="0.007418" ixz="0.000102" iyy="0.087136" iyz="-2.1e-05" izz="0.089871"/>
+ </inertial>
+ <visual>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="lh_lowerleg">
+ <inertial>
+ <origin xyz="0.1254 5e-05 -0.0001"/>
+ <mass value="0.881"/>
+ <inertia ixx="0.000468" ixy="0.0" ixz="0.0" iyy="0.026409" iyz="0.0" izz="0.026181"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerlegreduced.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="lh_foot">
+ <inertial>
+ <origin xyz="0 0 0"/>
+ <mass value="0.01"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+ </inertial>
+ <visual>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+ <geometry>
+ <sphere radius="0.021"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.02175"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="lh_haa_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="lh_haa_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="lh_haa_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="lh_hfe_upper_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="lh_hfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="lh_hfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="lh_kfe_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="lh_kfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="lh_kfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="lh_lowerleg">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>1.5</mu1>
+ <mu2>1.5</mu2>
+ <fdir1>1 0 0</fdir1>
+ <maxVel>1.0</maxVel>
+ <minDepth>0.00</minDepth>
+ </gazebo>
+ <!-- RH leg -->
+ <joint name="rh_haa_joint" type="revolute">
+ <origin rpy="0 1.57079632679 0" xyz="-0.3735 -0.207 0"/>
+ <parent link="trunk"/>
+ <child link="rh_hipassembly"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-1.2217304764" upper="0.523598775598" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="rh_hfe_joint" type="revolute">
+ <origin rpy="-1.57079632679 0 0" xyz="0.082 0 0"/>
+ <parent link="rh_hipassembly"/>
+ <child link="rh_upperleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="-1.2217304764" upper="0.872664625997" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ </joint>
+ <joint name="rh_kfe_joint" type="revolute">
+ <origin rpy="0 0 0" xyz="0.35 0 0"/>
+ <parent link="rh_upperleg"/>
+ <child link="rh_lowerleg"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="150.0" lower="0.349065850399" upper="2.44346095279" velocity="12.0"/>
+ <dynamics damping="0.1" friction="0"/>
+ <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.45"/>
+ <!-- Foot joint -->
+ </joint>
+ <joint name="rh_foot_joint" type="fixed">
+ <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.35 0 0"/>
+ <parent link="rh_lowerleg"/>
+ <child link="rh_foot"/>
+ </joint>
+ <link name="rh_hipassembly">
+ <inertial>
+ <origin xyz="0.04263 0.0 0.16931"/>
+ <mass value="2.93"/>
+ <inertia ixx="0.134705" ixy="-3.6e-05" ixz="-0.022734" iyy="0.144171" iyz="5.1e-05" izz="0.011033"/>
+ </inertial>
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="rh_upperleg">
+ <inertial>
+ <origin xyz="0.15074 -0.02625 0.0"/>
+ <mass value="2.638"/>
+ <inertia ixx="0.005495" ixy="0.007418" ixz="0.000102" iyy="0.087136" iyz="-2.1e-05" izz="0.089871"/>
+ </inertial>
+ <visual>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="rh_lowerleg">
+ <inertial>
+ <origin xyz="0.1254 5e-05 -0.0001"/>
+ <mass value="0.881"/>
+ <inertia ixx="0.000468" ixy="0.0" ixz="0.0" iyy="0.026409" iyz="0.0" izz="0.026181"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://hyq_description/meshes/leg/lowerlegreduced.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="rh_foot">
+ <inertial>
+ <origin xyz="0 0 0"/>
+ <mass value="0.01"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+ </inertial>
+ <visual>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+ <geometry>
+ <sphere radius="0.021"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.02175"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="rh_haa_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="rh_haa_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="rh_haa_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="rh_hfe_upper_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="rh_hfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="rh_hfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="rh_kfe_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="rh_kfe_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="rh_kfe_motor">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="rh_lowerleg">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>1.5</mu1>
+ <mu2>1.5</mu2>
+ <fdir1>1 0 0</fdir1>
+ <maxVel>1.0</maxVel>
+ <minDepth>0.00</minDepth>
+ </gazebo>
+ <!-- IMU trunk -->
+ <xacro:microstrain_3dmgx325_imu imu_name="3dmgx325_trunk_imu" imu_topic="trunk_imu/data" parent="trunk" stdev="0.00017" update_rate="100.0">
+ <origin rpy="0 -3.14159265359 0" xyz="0.3 0 0.095"/>
+ </xacro:microstrain_3dmgx325_imu>
+ <!-- Velodyne -->
+ <xacro:velodyne name="trunk_mount" parent="trunk">
+ <origin rpy="0 0 0" xyz="0.39 0 0.269"/>
+ </xacro:velodyne>
+ <!-- PTU unit -->
+ <xacro:ptu_d46 name="ptu" parent="trunk">
+ <origin rpy="-0.013127 0.569257 -0.007748" xyz="0.657353 0.015447 0.119533"/>
+ <!-- <origin xyz="0.667353 -0.010101 0.160544" rpy="0.043239 0.569257 0.041771"/>-->
+ </xacro:ptu_d46>
+ <!-- Asus Xtion Pro -->
+ <!--<origin xyz="0 0.005 0.036" rpy="0 0 0"/> with Bumblebee setup -->
+ <!--<origin xyz="0 0 0.04397" rpy="0 0 0"/> with Vislab setup -->
+ <!--
+ <xacro:asus_xtion_camera
+ name="head_mount"
+ parent="ptu_mount_link">
+ <origin xyz="0 0.005 0.036" rpy="0 0 0"/>
+ </xacro:asus_xtion_camera>
+-->
+ <!-- Kinect -->
+ <!-- is no more used, but we keep it for compatibilty -->
+ <!--xacro:kinect_camera
+ name="head_mount"
+ parent="trunk">
+ <origin xyz="0.63 0 0.095" rpy="0 0 0"/>
+ </xacro:kinect_camera-->
+ <!-- Bumblebee -->
+ <!--
+ <xacro:bumblebee_97
+ parent_link="ptu_mount_link"
+ xyz="0 0 0.018"
+ rpy="0 0 0"
+ ns="bumblebee"
+ frame_prefix="bumblebee_"
+ high_res="false"/>
+-->
+ <!-- <xacro:vislab_3dve_72
+ name="vislab_3dve"
+ parent="ptu_mount_link"
+ xyz="0 0 0.025"
+ rpy="0 0 0"
+ high_res="false"/>-->
+</robot>
+
diff --git a/data/meshes/groundcrouch.stl b/data/meshes/groundcrouch.stl
index 9651e17582c0af8e2b98113b13360a363ccf1901..d2a76e58c45807bf59d822be3a66caac3d74a1a4 100644
Binary files a/data/meshes/groundcrouch.stl and b/data/meshes/groundcrouch.stl differ
diff --git a/data/meshes/hyq/hyq_trunk.stl b/data/meshes/hyq/hyq_trunk.stl
index a7035061e18988dea93cd4c3f545268ed400cb79..e704ace9a563fe8b53432c16871acd51443295db 100644
Binary files a/data/meshes/hyq/hyq_trunk.stl and b/data/meshes/hyq/hyq_trunk.stl differ
diff --git a/data/meshes/hyq/hyq_trunk_large.stl b/data/meshes/hyq/hyq_trunk_large.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a7035061e18988dea93cd4c3f545268ed400cb79
Binary files /dev/null and b/data/meshes/hyq/hyq_trunk_large.stl differ
diff --git a/data/srdf/hyq/hyq_trunk_large.srdf b/data/srdf/hyq/hyq_trunk_large.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..188bd7a9a567b79fde4dfd20c59df6e419304b3c
--- /dev/null
+++ b/data/srdf/hyq/hyq_trunk_large.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="hyq_trunk_large">
+</robot>
diff --git a/data/urdf/hyq/hyq_trunk_large.urdf b/data/urdf/hyq/hyq_trunk_large.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..3b11b53c1f49e5c2a5ba24b059d06279f7844f9d
--- /dev/null
+++ b/data/urdf/hyq/hyq_trunk_large.urdf
@@ -0,0 +1,19 @@
+<robot name="hyq_trunk_large">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_all.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_large.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/script/scenarios/darpa_hyq_interp.py b/script/scenarios/darpa_hyq_interp.py
index 4cf5a6589ea80afb036c0dcb70dceb81e3801354..c5ee66be9daae3b03a5ecf3c5608bac1cd75da3b 100644
--- a/script/scenarios/darpa_hyq_interp.py
+++ b/script/scenarios/darpa_hyq_interp.py
@@ -19,7 +19,7 @@ fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-nbSamples = 10000
+nbSamples = 20000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
diff --git a/script/scenarios/darpa_hyq_path.py b/script/scenarios/darpa_hyq_path.py
index 77a3b5bf52b22c32aef8dbc466fdeaa9df8e9e49..32ee1ba8c32520b41a943e71b9ad61700294bd48 100644
--- a/script/scenarios/darpa_hyq_path.py
+++ b/script/scenarios/darpa_hyq_path.py
@@ -4,7 +4,7 @@ from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'hyq_trunk'
+urdfName = 'hyq_trunk_large'
urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
@@ -29,7 +29,7 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-1.5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
@@ -41,7 +41,7 @@ ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
ps.solve ()
@@ -53,6 +53,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (2)
#~ pp (0)
-pp (1)
+#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
diff --git a/script/scenarios/ground_crouch_hyq_interp.py b/script/scenarios/ground_crouch_hyq_interp.py
index a1e95eac792b3079ea324b882fbd50c1869f1e2e..936944ed20a9a8c616609f1a5235796a985fc80e 100644
--- a/script/scenarios/ground_crouch_hyq_interp.py
+++ b/script/scenarios/ground_crouch_hyq_interp.py
@@ -19,7 +19,7 @@ fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-nbSamples = 10000
+nbSamples = 20000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
@@ -33,7 +33,7 @@ rfoot = 'rf_foot_joint'
rLegOffset = [0.,0,0.]
rLegNormal = [0,1,0]
rLegx = 0.02; rLegy = 0.02
-fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "random", 0.03)
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.03)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
@@ -41,7 +41,7 @@ lfoot = 'lh_foot_joint'
lLegOffset = [0,0,0]
lLegNormal = [0,1,0]
lLegx = 0.02; lLegy = 0.02
-fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "random", 0.03)
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.03)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
@@ -49,7 +49,7 @@ rHand = 'rh_foot_joint'
rArmOffset = [0.,0,-0.]
rArmNormal = [0,1,0]
rArmx = 0.02; rArmy = 0.02
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "random", 0.03)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.03)
larmId = 'lfleg'
larm = 'lf_haa_joint'
@@ -57,7 +57,7 @@ lHand = 'lf_foot_joint'
lArmOffset = [0.,0,-0.]
lArmNormal = [0,1,0]
lArmx = 0.02; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "random", 0.03)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.03)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
@@ -76,7 +76,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
-#~ configs = fullBody.interpolate(0.1)
+configs = fullBody.interpolate(0.2)
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
diff --git a/script/scenarios/ground_crouch_hyq_path.py b/script/scenarios/ground_crouch_hyq_path.py
index b69738ed839ddc77a2bb00060751ff1e9356e187..e85bede0c5b122be095ad57dfa6d1a444c80443a 100644
--- a/script/scenarios/ground_crouch_hyq_path.py
+++ b/script/scenarios/ground_crouch_hyq_path.py
@@ -5,20 +5,20 @@ rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hyq_trunk'
-urdfNameRom = ['hyq_lhleg_rom','hyq_lhleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,5, -1, 1, 0, 2])
-rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,5, -0.1, 0.1, 0.35, 2])
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
-rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
+rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -30,15 +30,11 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-1, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
-#~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
+
q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
-#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
-#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
@@ -51,11 +47,6 @@ ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
-#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-#~
-#~ pp (2)
-#~ pp (0)
pp (1)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
diff --git a/script/scenarios/stair_bauzil_hrp2_interp.py b/script/scenarios/stair_bauzil_hrp2_interp.py
index 316cb5b929b1da612ffaa8f7dff425e74fbd05c9..47587251fdb79bb9cd6db767449e34ed0e82a5d0 100644
--- a/script/scenarios/stair_bauzil_hrp2_interp.py
+++ b/script/scenarios/stair_bauzil_hrp2_interp.py
@@ -110,7 +110,7 @@ r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
#~ f1 = open("secondchoice","w+")
-f1 = open("new","w+")
+f1 = open("hyq_crouch_20_10_15","w+")
f1.write(str(configs))
f1.close()
diff --git a/script/scenarios/stair_bauzil_hrp2_path.py b/script/scenarios/stair_bauzil_hrp2_path.py
index 2799215d7b499005e7762c2ace57055467e549de..a337c99b27e913d95d228a7eb967733a0aef03dd 100644
--- a/script/scenarios/stair_bauzil_hrp2_path.py
+++ b/script/scenarios/stair_bauzil_hrp2_path.py
@@ -13,6 +13,12 @@ rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
+rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
+rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
diff --git a/script/scenarios/stair_bauzil_hyq_interp.py b/script/scenarios/stair_bauzil_hyq_interp.py
index f47ae0431ec078cddbf10f8c89b59a062f93b329..f9aa57893a4ce5b0b2631ecaa23ae3ab046b6766 100644
--- a/script/scenarios/stair_bauzil_hyq_interp.py
+++ b/script/scenarios/stair_bauzil_hyq_interp.py
@@ -23,7 +23,6 @@ nbSamples = 10000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
-r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
rootName = 'base_joint_xyz'
@@ -31,10 +30,10 @@ rootName = 'base_joint_xyz'
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint'
-rLegOffset = [0,0,0]
+rLegOffset = [0.,0,0.]
rLegNormal = [0,1,0]
rLegx = 0.02; rLegy = 0.02
-fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "EFORT", 0.03)
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.1)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
@@ -42,23 +41,23 @@ lfoot = 'lh_foot_joint'
lLegOffset = [0,0,0]
lLegNormal = [0,1,0]
lLegx = 0.02; lLegy = 0.02
-fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "EFORT", 0.03)
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.1)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
-rArmOffset = [0,0,0]
-rArmNormal = [1,0,0]
+rArmOffset = [0.,0,-0.]
+rArmNormal = [0,1,0]
rArmx = 0.02; rArmy = 0.02
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.03)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.1)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
-lArmOffset = [0,0,0]
-lArmNormal = [1,0,0]
+lArmOffset = [0.,0,-0.]
+lArmNormal = [0,1,0]
lArmx = 0.02; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.03)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.1)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
@@ -72,13 +71,20 @@ q_0 = fullBody.getCurrentConfig();
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
-fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
configs = fullBody.interpolate(0.1)
+r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
+
i = 0;
r (configs[i]); i=i+1; i-1
+q0 = configs[2]
+q0 = fullBody.generateContacts(q0, [0,0,1])
+r(q0)
+
+
diff --git a/script/scenarios/stair_bauzil_hyq_path.py b/script/scenarios/stair_bauzil_hyq_path.py
index ed5c8cf33555a6c56bf2e3302e7b370a3e9753da..cac46b8f9367e4379b7b8bfa4b33576d97a8b82b 100644
--- a/script/scenarios/stair_bauzil_hyq_path.py
+++ b/script/scenarios/stair_bauzil_hyq_path.py
@@ -5,14 +5,15 @@ rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hyq_trunk'
-urdfNameRom = 'hyq_rom'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lhleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-5,2, -1, 1, 0, 2.2])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,4, -1, 1, 0, 2])
+rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -23,13 +24,13 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-2, 0, 0.5]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-1, 0, 0.4]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
#~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
-q_goal [0:3] = [0, 0, 0.5]; r (q_goal)
+q_goal [0:3] = [3.6, 0, 1.1]; r (q_goal)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")