<?xml version="1.0" ?>
<robot name="navi_robot" xmlns:xacro="http://ros.org/wiki/xacro">


  <xacro:property name="torso_m" value="10.0"/>
  <xacro:property name="torso_x" value="0.6"/>
  <xacro:property name="torso_y" value="0.4"/>
  <xacro:property name="torso_z" value="0.15"/>

  <xacro:property name="caster_m" value="2"/>
  <xacro:property name="caster_r" value="0.1"/>

  <xacro:property name="base_m" value="2"/>
  <xacro:property name="base_x" value="0.4"/>
  <xacro:property name="base_y" value="0.1"/>
  <xacro:property name="base_z" value="0.1"/>

  <xacro:property name="wheel_m" value="0.4"/>
  <xacro:property name="wheel_rad" value="0.035"/>
  <xacro:property name="wheel_len" value="0.1"/>

  <xacro:property name="head_m" value="2"/>
  <xacro:property name="head_rad" value="0.2"/>

  <xacro:property name="box_m" value="1"/>
  <xacro:property name="box_l" value="0.08"/>

  <xacro:property name="mu1" value="0.9"/>
  <xacro:property name="mu2" value="0.9"/>
  <xacro:property name="kp" value="10000000.0"/>
  <xacro:property name="kd" value="1.0"/>

  <xacro:macro name="sphere_inertial" params="radius mass">
    <inertial>
      <mass value="${mass}" />
      <origin xyz="0 0 0" />
      <inertia ixx="${0.4 * mass * radius * radius}" ixy="0.0" ixz="0.0"
               iyy="${0.4 * mass * radius * radius}" iyz="0.0"
               izz="${0.4 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertial" params="radius length mass">
    <inertial>
      <mass value="${mass}" />
      <origin xyz="0 0 0" />
      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
               iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
               izz="${0.5 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="box_inertial" params="x y z mass">
    <inertial>
      <mass value="${mass}" />
      <origin xyz="0 0 0" />
      <inertia ixx="${0.0833333 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
               iyy="${0.0833333 * mass * (x*x + z*z)}" iyz="0.0"
               izz="${0.0833333 * mass * (x*x + y*y)}" />
    </inertial>
  </xacro:macro>

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>

  <link name="base_link">
    <visual>
      <geometry>
        <box size="${torso_x} ${torso_y} ${torso_z}"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <box size="${torso_x} ${torso_y} ${torso_z}"/>
      </geometry>
    </collision>
    <xacro:box_inertial x="${torso_x}" y="${torso_y}" z="${torso_z}" mass="${torso_m}"/>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Blue</material>
  </gazebo>

  <link name="caster">
    <visual>
      <geometry>
        <sphere radius="${caster_r}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="${caster_r}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <xacro:sphere_inertial radius="${caster_r}" mass="${caster_m}"/>
  </link>

  <joint name="base_to_caster" type="continuous">
    <parent link="base_link"/>
    <child link="caster"/>
    <origin xyz="-0.2 0 ${-torso_z*0.5}"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="base_to_caster2" type="continuous">
    <parent link="base_link"/>
    <child link="caster"/>
    <origin xyz="-0.2 0 ${-torso_z*0.5}"/>
    <axis xyz="0 1 0"/>
  </joint>
</robot>