<?xml version="1.0"?>
<robot name="two_wheel_robot">
  <!-- Metres, radians, +X forward, +Y left, +Z up. -->
  <link name="base_footprint"/>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.0145"/>
  </joint>
  <link name="base_link">
    <visual>
      <origin xyz="-0.033 0 0.0082"/>
      <geometry><box size="0.100 0.045 0.0154"/></geometry>
    </visual>
  </link>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin xyz="0 0.0350641 0"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="left_wheel_link">
    <visual>
      <origin rpy="1.57079632679 0 0"/>
      <geometry><cylinder radius="0.0145" length="0.0138"/></geometry>
    </visual>
  </link>
  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <origin xyz="0 -0.0350641 0"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="right_wheel_link">
    <visual>
      <origin rpy="1.57079632679 0 0"/>
      <geometry><cylinder radius="0.0145" length="0.0138"/></geometry>
    </visual>
  </link>

  <solar_os version="1">
    <!-- Example gains: replace kp, ki, kd and tau with your own tuning results. -->
    <joint name="left_wheel_joint" command="velocity">
      <motor device="left_mot" polarity="1"/>
      <encoder device="left_enc" ppr="70" placement="motor" polarity="1"/>
      <transmission gear_ratio="30"/>
      <param name="kp" value="0.26"/>
      <param name="ki" value="1.72"/>
      <param name="kd" value="0.005"/>
      <param name="tau" value="0.05"/>
      <param name="velocity_tau" value="0.1"/>
      <param name="max_drive" value="0.5"/>
      <param name="velocity_limit" value="4"/>
      <param name="acceleration_limit" value="10"/>
      <param name="period_ms" value="20"/>
      <param name="command_timeout_ms" value="150"/>
      <param name="feedback_timeout_ms" value="100"/>
    </joint>
    <joint name="right_wheel_joint" command="velocity">
      <motor device="right_mot" polarity="1"/>
      <encoder device="right_enc" ppr="70" placement="motor" polarity="1"/>
      <transmission gear_ratio="30"/>
      <param name="kp" value="0.266"/>
      <param name="ki" value="1.84"/>
      <param name="kd" value="0.0045"/>
      <param name="tau" value="0.05"/>
      <param name="velocity_tau" value="0.1"/>
      <param name="max_drive" value="0.5"/>
      <param name="velocity_limit" value="4"/>
      <param name="acceleration_limit" value="10"/>
      <param name="period_ms" value="20"/>
      <param name="command_timeout_ms" value="150"/>
      <param name="feedback_timeout_ms" value="100"/>
    </joint>
    <diff_drive name="base" left_joint="left_wheel_joint" right_joint="right_wheel_joint">
      <geometry wheel_radius="0.0145" wheel_separation="0.0701282"/>
      <odometry frame="odom" base_frame="base_footprint"/>
      <param name="linear_velocity_limit" value="0.04"/>
      <param name="angular_velocity_limit" value="1"/>
      <param name="command_timeout_ms" value="300"/>
    </diff_drive>
  </solar_os>
</robot>
