<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from src/lekiwi_ros2/lekiwi_description/urdf/base.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="lekiwi">
  <!-- Base mesh rotation about Z (rad) -->
  <!-- LIDAR mesh rotation about X (rad) -->
  <!-- Camera mesh/joint rotation about Y (rad) -->
  <!-- Max hardware velocity in steps/s (absolute motor limit, do not exceed) -->
  <!-- [0-254] -->
  <!-- Velocity mode -->
  <!-- 120° between wheels -->
  <!-- N⋅m (30 kgf⋅cm for STS3215) -->
  <!-- Omnidirectional Kinematics Calculations -->
  <!-- For 3-wheel omni drive: ω_i = (-sin(θ_i)·v_x + cos(θ_i)·v_y + R·ω_z) / r -->
  <!-- Define custom materials for base and wheels using color properties -->
  <material name="light_grey">
    <color rgba="0.8 0.8 0.8 1.0"/>
  </material>
  <material name="charcoal">
    <color rgba="0.15 0.15 0.15 1.0"/>
  </material>
  <!-- LINKS -->
  <!-- Base footprint link (empty) -->
  <link name="base_footprint"/>
  <!-- Base link -->
  <link name="base_link">
    <visual>
      <origin rpy="0 0 4.71238898038469" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/lekiwi_base.stl"/>
      </geometry>
      <material name="light_grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 4.71238898038469" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/lekiwi_base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.02"/>
    </inertial>
  </link>
  <!-- LD06 laser link -->
  <link name="laser_link">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/ld06_lidar.stl"/>
      </geometry>
      <material name="charcoal"/>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/ld06_lidar.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
  </link>
  <!-- Laser frame -->
  <link name="laser_frame"/>
  <!-- BNO055 IMU link -->
  <link name="imu_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/bno055_imu.stl"/>
      </geometry>
      <material name="charcoal"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/bno055_imu.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- IMU frame -->
  <link name="imu_frame"/>
  <!-- Camera link -->
  <link name="camera_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/hd_webcam.stl"/>
      </geometry>
      <material name="charcoal"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/hd_webcam.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- Camera frame-->
  <link name="camera"/>
  <!-- Left wheel link -->
  <link name="left_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/omni_wheel.stl"/>
      </geometry>
      <material name="charcoal"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/omni_wheel.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <!-- Back wheel link -->
  <link name="back_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/omni_wheel.stl"/>
      </geometry>
      <material name="charcoal"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/omni_wheel.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <!-- Right wheel link -->
  <link name="right_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/omni_wheel.stl"/>
      </geometry>
      <material name="charcoal"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/omni_wheel.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <!-- JOINTS -->
  <!-- base_footprint fixed joint -->
  <joint name="base_footprint_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.040639999999999996"/>
  </joint>
  <!-- laser fixed joint -->
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin rpy="0 0 0" xyz="-0.045042 0 0.0915"/>
  </joint>
  <!-- laser_frame fixed joint -->
  <joint name="laser_frame_joint" type="fixed">
    <parent link="laser_link"/>
    <child link="laser_frame"/>
    <origin rpy="0 0 0" xyz="0 0 0.027"/>
  </joint>
  <!-- IMU fixed joint -->
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin rpy="0 0 0" xyz="-0.044 0 0.08"/>
  </joint>
  <!-- imu_frame fixed joint -->
  <joint name="imu_frame_joint" type="fixed">
    <parent link="imu_link"/>
    <child link="imu_frame"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- camera fixed joint -->
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin rpy="0 1.5707963267948966 0" xyz="0.114 0 0.026"/>
  </joint>
  <!-- camera_frame fixed joint -->
  <joint name="camera_frame_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="camera"/>
    <origin rpy="0 0 4.71238898038469" xyz="0 0 0"/>
  </joint>
  <!-- left wheel joint: 60 degrees anti-clockwise from +X at radius wheel_base_radius -->
  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin rpy="0 1.5707963267948966 1.0471975511965976" xyz="0.06611950000000001 0.11452233337105037 0.01036"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.942" velocity="4.433204476989503"/>
  </joint>
  <!-- back wheel joint: 180 degrees anti-clockwise from +X at radius base_radius -->
  <joint name="back_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="back_wheel_link"/>
    <origin rpy="0 1.5707963267948966 3.141592653589793" xyz="-0.132239 1.6194606807244685e-17 0.01036"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.942" velocity="4.433204476989503"/>
  </joint>
  <!-- right wheel joint: 300 degrees (or -60 degrees) anti-clockwise from +X at radius base_radius -->
  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <origin rpy="0 1.5707963267948966 -1.0471975511965976" xyz="0.06611950000000001 -0.11452233337105037 0.01036"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.942" velocity="4.433204476989503"/>
  </joint>
  <!-- ROS 2 CONTROL -->
  <!-- System interface -->
  <ros2_control name="lekiwi_base" type="system">
    <hardware>
      <plugin>sts_hardware_interface/STSHardwareInterface</plugin>
      <!-- Serial communication parameters -->
      <param name="serial_port">/dev/ttySERVO</param>
      <param name="baud_rate">1000000</param>
      <param name="use_sync_write">True</param>
      <param name="enable_mock_mode">False</param>
      <!-- Motor-specific parameter (adjust for different STS motor models) -->
      <param name="max_velocity_steps">3400</param>
      <!-- Proportional ACC: scale per-wheel acceleration so all wheels finish ramping in equal time -->
      <param name="proportional_acc_max">25</param>
    </hardware>
    <!-- Left wheel joint (Motor ID 7) -->
    <joint name="left_wheel_joint">
      <param name="motor_id">7</param>
      <param name="operating_mode">1</param>
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <state_interface name="voltage"/>
      <state_interface name="temperature"/>
      <state_interface name="current"/>
      <state_interface name="is_moving"/>
    </joint>
    <!-- Back wheel joint (Motor ID 8) -->
    <joint name="back_wheel_joint">
      <param name="motor_id">8</param>
      <param name="operating_mode">1</param>
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <state_interface name="voltage"/>
      <state_interface name="temperature"/>
      <state_interface name="current"/>
      <state_interface name="is_moving"/>
    </joint>
    <!-- Right wheel joint (Motor ID 9) -->
    <joint name="right_wheel_joint">
      <param name="motor_id">9</param>
      <param name="operating_mode">1</param>
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <state_interface name="voltage"/>
      <state_interface name="temperature"/>
      <state_interface name="current"/>
      <state_interface name="is_moving"/>
    </joint>
  </ros2_control>
</robot>