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

  <!-- Arguments -->
  <xacro:arg name="use_sim" default="false"/>
  <xacro:arg name="use_fake_hardware" default="false"/>
  <xacro:arg name="use_nav" default="false"/>
  <xacro:arg name="exclude_world_joint" default="false"/>

  <!-- Include the base URDF -->
  <xacro:include filename="$(find a2_description)/xacro/a2.urdf"/>

  <!-- ros2_control hardware interface -->
  <ros2_control name="a2_ros2_control" type="system">
    <xacro:if value="$(arg use_sim)">
      <hardware>
        <plugin>gz_ros2_control/GazeboSimSystem</plugin>
      </hardware>
    </xacro:if>
    <xacro:unless value="$(arg use_sim)">
      <xacro:if value="$(arg use_fake_hardware)">
        <hardware>
          <plugin>mock_components/GenericSystem</plugin>
        </hardware>
      </xacro:if>
      <xacro:unless value="$(arg use_fake_hardware)">
        <hardware>
          <plugin>a2_platform/A2HardwareInterface</plugin>
        </hardware>
      </xacro:unless>
    </xacro:unless>

    <!-- Left leg joints -->
    <joint name="idx01_left_hip_roll"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx02_left_hip_yaw"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx03_left_hip_pitch"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx04_left_tarsus"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx05_left_toe_pitch"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx06_left_toe_roll"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

    <!-- Right leg joints -->
    <joint name="idx07_right_hip_roll"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx08_right_hip_yaw"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx09_right_hip_pitch"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx10_right_tarsus"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx11_right_toe_pitch"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx12_right_toe_roll"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

    <!-- Left arm joints -->
    <joint name="idx13_left_arm_joint1"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx14_left_arm_joint2"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx15_left_arm_joint3"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx16_left_arm_joint4"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx17_left_arm_joint5"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx18_left_arm_joint6"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx19_left_arm_joint7"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

    <!-- Right arm joints -->
    <joint name="idx20_right_arm_joint1"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx21_right_arm_joint2"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx22_right_arm_joint3"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx23_right_arm_joint4"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx24_right_arm_joint5"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx25_right_arm_joint6"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx26_right_arm_joint7"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

    <!-- Head joints -->
    <joint name="idx27_head_joint1"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="idx28_head_joint2"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

    <!-- Left hand finger joints -->
    <joint name="L_thumb_swing_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_thumb_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_thumb_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_thumb_3_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_index_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_index_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_middle_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_middle_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_ring_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_ring_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_pinky_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="L_pinky_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

    <!-- Right hand finger joints -->
    <joint name="R_thumb_swing_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_thumb_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_thumb_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_thumb_3_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_index_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_index_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_middle_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_middle_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_ring_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_ring_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_pinky_1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>
    <joint name="R_pinky_2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint>

  </ros2_control>

  <!-- Gazebo ros2_control plugin (sim only) -->
  <xacro:if value="$(arg use_sim)">
    <gazebo>
      <plugin filename="gz_ros2_control-system"
              name="gz_ros2_control::GazeboSimROS2ControlPlugin">
        <parameters>$(find a2_description)/config/ros2_controllers.yaml</parameters>
      </plugin>
    </gazebo>
  </xacro:if>

  <!-- Gazebo JointPositionController plugins for nav joints (bypasses ros2_control) -->
  <xacro:if value="$(arg use_sim)">
    <xacro:if value="$(arg use_nav)">
      <xacro:unless value="$(arg exclude_world_joint)">
        <gazebo>
          <plugin filename="ignition-gazebo-joint-position-controller-system"
                  name="ignition::gazebo::systems::JointPositionController">
            <joint_name>nav_x</joint_name>
            <p_gain>50000</p_gain>
            <i_gain>500</i_gain>
            <d_gain>5000</d_gain>
            <cmd_max>500000</cmd_max>
            <cmd_min>-500000</cmd_min>
          </plugin>
          <plugin filename="ignition-gazebo-joint-position-controller-system"
                  name="ignition::gazebo::systems::JointPositionController">
            <joint_name>nav_y</joint_name>
            <p_gain>50000</p_gain>
            <i_gain>500</i_gain>
            <d_gain>5000</d_gain>
            <cmd_max>500000</cmd_max>
            <cmd_min>-500000</cmd_min>
          </plugin>
          <plugin filename="ignition-gazebo-joint-position-controller-system"
                  name="ignition::gazebo::systems::JointPositionController">
            <joint_name>nav_yaw</joint_name>
            <p_gain>20000</p_gain>
            <i_gain>200</i_gain>
            <d_gain>3000</d_gain>
            <cmd_max>200000</cmd_max>
            <cmd_min>-200000</cmd_min>
          </plugin>
        </gazebo>
      </xacro:unless>
    </xacro:if>
  </xacro:if>

  <!-- Head camera sensor (sim only) -->
  <xacro:if value="$(arg use_sim)">
    <gazebo reference="head_link02">
      <sensor name="head_camera" type="camera">
        <always_on>1</always_on>
        <update_rate>12</update_rate>
        <camera>
          <horizontal_fov>1.658</horizontal_fov>
          <image><width>640</width><height>360</height><format>R8G8B8</format></image>
          <clip><near>0.05</near><far>50.0</far></clip>
        </camera>
        <topic>/head_camera</topic>
      </sensor>
    </gazebo>
  </xacro:if>

  <!-- Fix robot in world for sim (no walking physics).
       exclude_world_joint=true omits this for RSP in nav mode (avoids TF conflict
       with odom→base_link from nav_sim_bridge). -->
  <xacro:if value="$(arg use_sim)">
    <xacro:unless value="$(arg exclude_world_joint)">

      <!-- Non-nav mode: fixed joint (robot stands at table) -->
      <xacro:unless value="$(arg use_nav)">
        <link name="world"/>
        <joint name="world_to_base" type="fixed">
          <parent link="world"/>
          <child link="base_link"/>
          <origin xyz="-1.0 0 1.05" rpy="0 0 0"/>
        </joint>
      </xacro:unless>

      <!-- Nav mode: prismatic X/Y + revolute yaw joints so the robot can
           be moved around in Gazebo via JointPositionController plugins.
           NOTE: root link MUST NOT be named "world" — the URDF→SDF converter
           treats "world" as the SDF world frame and pins joints to it. -->
      <xacro:if value="$(arg use_nav)">
        <link name="world"/>
        <link name="nav_anchor">
          <inertial><mass value="0.01"/><inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/></inertial>
        </link>
        <joint name="world_to_nav_anchor" type="fixed">
          <parent link="world"/>
          <child link="nav_anchor"/>
        </joint>
        <link name="nav_slide_x">
          <inertial><mass value="0.01"/><inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/></inertial>
        </link>
        <link name="nav_slide_y">
          <inertial><mass value="0.01"/><inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/></inertial>
        </link>
        <joint name="nav_x" type="prismatic">
          <parent link="nav_anchor"/>
          <child link="nav_slide_x"/>
          <axis xyz="1 0 0"/>
          <limit lower="-10" upper="10" effort="500000" velocity="10"/>
          <dynamics damping="100" friction="0"/>
        </joint>
        <joint name="nav_y" type="prismatic">
          <parent link="nav_slide_x"/>
          <child link="nav_slide_y"/>
          <axis xyz="0 1 0"/>
          <limit lower="-10" upper="10" effort="500000" velocity="10"/>
          <dynamics damping="100" friction="0"/>
        </joint>
        <joint name="nav_yaw" type="continuous">
          <parent link="nav_slide_y"/>
          <child link="base_link"/>
          <origin xyz="0 0 1.05"/>
          <axis xyz="0 0 1"/>
          <dynamics damping="50" friction="0"/>
        </joint>
      </xacro:if>

    </xacro:unless>
  </xacro:if>

  <!-- LiDAR sensor (sim only) -->
  <xacro:if value="$(arg use_sim)">
    <!-- Keep lidar_link in URDF for TF tree (base_link -> lidar_link) -->
    <link name="lidar_link">
      <visual>
        <geometry><cylinder radius="0.03" length="0.04"/></geometry>
        <material name="lidar_grey">
          <color rgba="0.3 0.3 0.3 1"/>
        </material>
      </visual>
    </link>
    <joint name="lidar_joint" type="fixed">
      <parent link="base_link"/>
      <child link="lidar_link"/>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
    </joint>
    <!-- Note: gpu_lidar sensor is defined in the SDF world file (a2_room.sdf)
         as a standalone model "a2_lidar" because URDF→SDF conversion drops sensors
         on the canonical link in Gazebo Fortress. nav_sim_bridge teleports it
         alongside the robot. -->
  </xacro:if>

</robot>
