<?xml version="1.0" ?>
<robot name="hyq">

    <!-- left front leg -->
    <group name="lf_leg">
        <joint name="lf_haa_joint" />
        <joint name="lf_hfe_joint" />
        <joint name="lf_kfe_joint" />
        <chain base_link="trunk" tip_link="lf_foot" />
    </group>
    <!-- right front leg -->
    <group name="rf_leg">
        <joint name="rf_haa_joint" />
        <joint name="rf_hfe_joint" />
        <joint name="rf_kfe_joint" />
        <chain base_link="trunk" tip_link="rf_foot" />
    </group>
    <!-- left hind leg -->
    <group name="lh_leg">
        <joint name="lh_haa_joint" />
        <joint name="lh_hfe_joint" />
        <joint name="lh_kfe_joint" />
        <chain base_link="trunk" tip_link="lh_foot" />
    </group>
    <!-- right hind leg -->
    <group name="rh_leg">
        <joint name="rh_haa_joint" />
        <joint name="rh_hfe_joint" />
        <joint name="rh_kfe_joint" />
        <chain base_link="trunk" tip_link="lh_foot" />
    </group>
    <group name="all_legs">
        <group name="lf" />
        <group name="rf" />
        <group name="lh" />
        <group name="rh" />
    </group>
    <group name="r_legs">
        <group name="rf" />
        <group name="rh" />
    </group>
    <group name="l_legs">
        <group name="lf" />
        <group name="lh" />
    </group>
    <group name="f_legs">
        <group name="lf" />
        <group name="rf" />
    </group>
    <group name="h_legs">
        <group name="lh" />
        <group name="rh" />
    </group>
    <group name="ld_legs">
        <group name="lf" />
        <group name="rh" />
    </group>
    <group name="rd_legs">
        <group name="rf" />
        <group name="lh" />
    </group>

    <end_effector name="lf_foot" parent_link="lf_foot" group="lf_leg" />
    <end_effector name="rf_foot" parent_link="rf_foot" group="rf_leg" />
    <end_effector name="lh_foot" parent_link="lh_foot" group="lh_leg" />
    <end_effector name="rh_foot" parent_link="rh_foot" group="rh_leg" />

    <group_state name="standing" group="all_legs">
        <joint name="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
        <joint name="lf_haa_joint" value="-0.2" />
        <joint name="lf_hfe_joint" value="0.75" />
        <joint name="lf_kfe_joint" value="-1.5" />
        <joint name="lh_haa_joint" value="-0.2" />
        <joint name="lh_hfe_joint" value="-0.75" />
        <joint name="lh_kfe_joint" value="1.5" />
        <joint name="rf_haa_joint" value="-0.2" />
        <joint name="rf_hfe_joint" value="0.75" />
        <joint name="rf_kfe_joint" value="-1.5" />
        <joint name="rh_haa_joint" value="-0.2" />
        <joint name="rh_hfe_joint" value="-0.75" />
        <joint name="rh_kfe_joint" value="1.5" />
    </group_state>

    <group_state name="straight_standing" group="all_legs">
        <joint name="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
        <joint name="lf_haa_joint" value="0." />
        <joint name="lf_hfe_joint" value="0.75" />
        <joint name="lf_kfe_joint" value="-1.5" />
        <joint name="lh_haa_joint" value="0." />
        <joint name="lh_hfe_joint" value="-0.75" />
        <joint name="lh_kfe_joint" value="1.5" />
        <joint name="rf_haa_joint" value="0." />
        <joint name="rf_hfe_joint" value="0.75" />
        <joint name="rf_kfe_joint" value="-1.5" />
        <joint name="rh_haa_joint" value="0." />
        <joint name="rh_hfe_joint" value="-0.75" />
        <joint name="rh_kfe_joint" value="1.5" />
    </group_state>

    <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"/>

    <disable_collisions link1="lf_hipassembly" link2="rf_hipassembly" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="rf_upperleg" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="lh_hipassembly" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="lh_upperleg" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="lh_lowerleg" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="rh_hipassembly" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="rh_upperleg" reason="Never" />
    <disable_collisions link1="lf_hipassembly" link2="rh_lowerleg" reason="Never" />

    <disable_collisions link1="rf_hipassembly" link2="lf_upperleg" reason="Never" />
    <disable_collisions link1="rf_hipassembly" link2="lh_hipassembly" reason="Never" />
    <disable_collisions link1="rf_hipassembly" link2="lh_upperleg" reason="Never" />
    <disable_collisions link1="rf_hipassembly" link2="lh_lowerleg" reason="Never" />
    <disable_collisions link1="rf_hipassembly" link2="rh_hipassembly" reason="Never" />
    <disable_collisions link1="rf_hipassembly" link2="rh_upperleg" reason="Never" />
    <disable_collisions link1="rf_hipassembly" link2="rh_lowerleg" reason="Never" />

    <disable_collisions link1="lh_hipassembly" link2="rf_hipassembly" reason="Never" />
    <disable_collisions link1="lh_hipassembly" link2="rf_upperleg" reason="Never" />
    <disable_collisions link1="lh_hipassembly" link2="rf_lowerleg" reason="Never" />
    <disable_collisions link1="lh_hipassembly" link2="lf_upperleg" reason="Never" />
    <disable_collisions link1="lh_hipassembly" link2="lf_lowerleg" reason="Never" />
    <disable_collisions link1="lh_hipassembly" link2="rh_hipassembly" reason="Never" />
    <disable_collisions link1="lh_hipassembly" link2="rh_upperleg" reason="Never" />

    <disable_collisions link1="rh_hipassembly" link2="rf_hipassembly" reason="Never" />
    <disable_collisions link1="rh_hipassembly" link2="rf_upperleg" reason="Never" />
    <disable_collisions link1="rh_hipassembly" link2="rf_lowerleg" reason="Never" />
    <disable_collisions link1="rh_hipassembly" link2="lh_hipassembly" reason="Never" />
    <disable_collisions link1="rh_hipassembly" link2="lh_upperleg" reason="Never" />
    <disable_collisions link1="rh_hipassembly" link2="lf_upperleg" reason="Never" />
    <disable_collisions link1="rh_hipassembly" link2="lf_lowerleg" reason="Never" />

</robot>
