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

<!-- Robot shape -->
<xacro:property name="W_weight" value="0.06"/>
<xacro:property name="W_diam" value="0.065"/>

<xacro:property name="base_w" value="0.15"/>
<xacro:property name="base_h" value="0.2"/>
<xacro:property name="base_l" value=".04"/>
<xacro:property name="base_weight" value="0.3"/>

<xacro:property name="Mot_l" value="0.07"/>
<xacro:property name="Mot_diam" value="0.025"/>
<xacro:property name="Mot_weight" value="0.1"/>

<!-- IMU -->
<xacro:property name="noise" value="0.0"/>

<!-- DIFFERENTIAL DRIVE -->
<xacro:property name="update_rate" value="20"/>
<xacro:property name="torque" value="0.1"/>


<!-- WHEEL -->
  <xacro:macro name="wheel" params="side parent x y z">
    <joint name="wheel_${side}_joint" type="continuous">
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
      <parent link="${parent}"/>
      <child link="wheel_${side}_link" />
      <axis xyz="0 1 0"/>
    </joint>

    <link name="wheel_${side}_link">
      <inertial>
        <mass value="${W_weight}" />
        <origin xyz="0 0 0" rpy="1.57075 0 0" />
        <inertia ixx="0.0" ixy="0.0" ixz="0.0"
          iyy="${W_weight/8.0 * W_diam * W_diam}" iyz="0.0"
          izz="0.0" />
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="1.57075 0 0" />
        <geometry>
             <cylinder length="0.027" radius="${W_diam/2}"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="1.57075 0 0" />
        <geometry>
          <cylinder length="0.027" radius="${W_diam/2}"/>
        </geometry>
      </collision>
    </link>

  <gazebo reference="wheel_${side}_link">
    <mu1>0.7</mu1>
    <mu2>0.7</mu2>
    <kp>10000</kp>
    <kd>10000</kd>
    <material>Gazebo/Black</material>
  </gazebo>
  </xacro:macro>


<!-- MOTOR -->
  <xacro:macro name="motor" params="side parent x y z">
    <joint name="motor_${side}_joint" type="fixed">
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
      <parent link="${parent}"/>
      <child link="motor_${side}_link" />
    </joint>

    <link name="motor_${side}_link">
      <inertial>
        <mass value="${Mot_weight}" />
        <origin xyz="0 0 0" rpy="1.57075 0 0" />
        <inertia ixx="0.0" ixy="0.0" ixz="0.0"
          iyy="${Mot_weight/8.0 * Mot_diam * Mot_diam}" iyz="0.0"
          izz="0.0" />
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="1.57075 0 0" />
        <geometry>
          <cylinder length="${Mot_l}" radius="${Mot_diam/2}"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="1.57075 0 0" />
        <geometry>
          <cylinder length="${  Mot_l}" radius="${Mot_diam/2}"/>
        </geometry>
      </collision>
    </link>

  <gazebo reference="motor_${side}_link">
    <mu1>0.7</mu1>
    <mu2>0.7</mu2>
    <kp>10000</kp>
    <kd>10000</kd>
    <material>Gazebo/Gray</material>
  </gazebo>
  </xacro:macro>



    <link name="base_link">
      <inertial>
        <mass value="${base_weight}" />
        <origin xyz="0 0 0.0325" />
        <inertia ixx="${(base_w * base_w + base_h * base_h) * base_weight /12.0}" ixy="0.0" ixz="0.0"
          iyy="${(base_h * base_h + base_l * base_l) * base_weight /12.0}" iyz="0.0"
          izz="${(base_w * base_w + base_l * base_l) * base_weight /12.0}" />
      </inertial>

      <visual>
        <origin xyz="0 0 0.0325" />
        <geometry>
             <box size="${base_l} ${base_w} ${base_h}"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0 0 0.0325" />
        <geometry>
             <box size="${base_l} ${base_w} ${base_h}"/>
        </geometry>
      </collision>
    </link>

  <gazebo reference="base_link">
    <mu1>0.3</mu1>
    <mu2>0.3</mu2>
    <kp>10000</kp>
    <kd>10000</kd>
    <material>Gazebo/White</material>
  </gazebo>

    <xacro:motor parent="base_link" side="dx" x="0" y="0.04" z="-${base_h/2-Mot_diam/2}"/>
    <xacro:motor parent="base_link" side="sx" x="0" y="-0.04" z="-${base_h/2-Mot_diam/2}"/>
    <xacro:wheel parent="motor_dx_link" side="dx" x="0" y="0.05" z="0"/>
    <xacro:wheel parent="motor_sx_link" side="sx" x="0" y="-0.05" z="0"/>



<!--  SENSORS -->
  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>base_link</bodyName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>${noise}</gaussianNoise>
      <updateRate>20.0</updateRate>
    </plugin>
  </gazebo>


<!-- DIFFERENTIAL DRIVE -->
<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <legacyMode>false</legacyMode>
    <alwaysOn>true</alwaysOn>
    <updateRate>${update_rate}</updateRate>
    <leftJoint>wheel_dx_joint</leftJoint>
    <rightJoint>wheel_sx_joint</rightJoint>
    <wheelSeparation>0.2</wheelSeparation>
    <wheelDiameter>${W_diam}</wheelDiameter>
    <torque>${torque}</torque>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>base_linkS</robotBaseFrame>

    <wheelTorque>${torque}</wheelTorque>
    <wheelAcceleration>0</wheelAcceleration>
    <publishWheelJointState>true</publishWheelJointState>
    <publishOdomTF>true</publishOdomTF>
    <publishWheelTF>true</publishWheelTF>


  </plugin>
</gazebo>


</robot>