<?xml version="1.0"?>
<!-- @author: Joseph Masterjohn (github: @joemasterjohn)
     @author: Robin Deits (github: @rdeits)

     Strandbeest.xacro is not intended to be invoked directly.
     Instead, use the 'strandbeest' macro contained here in a separate
     file after including Strandbeest.xacro.

     These files are an adaptation of the original Strandbeest example in
     Drake, written by Robin Deits:

     https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/examples/Strandbeest/
-->
<robot xmlns:drake="http://drake.mit.edu" xmlns:xacro="http://ros.org/wiki/xacro" name="Strandbeest">
  <xacro:macro name="strandbeest" params="with_constraints">
    <xacro:include filename="Macros.xacro"/>
    <xacro:include filename="LegPair.xacro"/>

    <!-- Materials -->
    <material name="tri_red">
      <color rgba="0.8 0 0 1"/>
    </material>
    <material name="tri_black">
      <color rgba="0 0 0 1"/>
    </material>
    <material name="tri_grey">
      <color rgba="0.6 0.6 0.6 1"/>
    </material>
    <!-- Fixed floor link for collision and walking -->
    <link name="floor">
      <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value="1"/>
        <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="10 10 1"/>
        </geometry>
        <material name="gray">
          <color rgba=".4 .4 .4 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="10 10 1"/>
        </geometry>
        <drake:proximity_properties>
          <drake:mu_static value="1"/>
          <drake:mu_dynamic value="0.6"/>
        </drake:proximity_properties>
      </collision>
    </link>
    <joint name="floor_weld" type="fixed">
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <parent link="world"/>
      <child link="floor"/>
    </joint>
    <!-- Links -->
    <link name="crossbar">
      <xacro:strandbeest_cylinder xyz="0 0 0" rpy="${pi/2} 0 0" length="1.2" radius="0.01" material="tri_black"/>
    </link>
    <link name="crank_axle">
      <xacro:strandbeest_cylinder xyz="0 0 0" rpy="${pi/2} 0 0" length="1.2" radius="0.02" material="tri_red"/>
    </link>
    <!-- Joints -->
    <joint name="joint_crossbar_crank" type="continuous">
      <axis xyz="0 1 0"/>
      <parent link="crossbar"/>
      <child link="crank_axle"/>
      <origin rpy="0 0 0" xyz="0 0 0.078"/>
    </joint>
    <transmission name="crossbar_crank_transmission" type="pr2_mechanism_model/SimpleTransmission">
      <joint name="joint_crossbar_crank"/>
      <actuator name="crossbar_crank_motor"/>
      <mechanicalReduction>1</mechanicalReduction>
    </transmission>
    <!-- Macro to define a pair of legs at an offset along the crank shaft -->
    <xacro:macro name="positioned_leg_pair" params="name offset angle">
      <xacro:strandbeest_leg_pair prefix="${name}" with_constraints="${with_constraints}"/>
      <joint name="${name}_joint_crossbar_l" type="fixed">
        <axis xyz="0 1 0"/>
        <parent link="crossbar"/>
        <child link="${name}_bar_l"/>
        <origin rpy="0 0 0" xyz="0 ${offset} 0"/>
      </joint>
      <joint name="${name}_joint_crank_axle_m" type="fixed">
        <axis xyz="0 1 0"/>
        <parent link="crank_axle"/>
        <child link="${name}_bar_m"/>
        <origin rpy="0 ${angle} 0" xyz="0 ${offset} 0"/>
      </joint>
    </xacro:macro>
    <!-- Define the legs -->
    <xacro:positioned_leg_pair name="pair01" offset="0.6" angle="0">
    </xacro:positioned_leg_pair>
    <xacro:positioned_leg_pair name="pair02" offset="0.36" angle="${2*pi/3}">
    </xacro:positioned_leg_pair>
    <xacro:positioned_leg_pair name="pair03" offset="0.12" angle="${4*pi/3}">
    </xacro:positioned_leg_pair>
    <xacro:positioned_leg_pair name="pair04" offset="-0.12" angle="${pi/3}">
    </xacro:positioned_leg_pair>
    <xacro:positioned_leg_pair name="pair05" offset="-0.36" angle="${3*pi/3}">
    </xacro:positioned_leg_pair>
    <xacro:positioned_leg_pair name="pair06" offset="-0.6" angle="${5*pi/3}">
    </xacro:positioned_leg_pair>
  </xacro:macro>
</robot>
