<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from xacro/robot.xacro              | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="go1">
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="deep-grey">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>
  <material name="light-grey">
    <color rgba="0.35 0.35 0.35 1.0"/>
  </material>
  <material name="silver">
    <color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
  </material>
  <material name="orange">
    <color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
  </material>
  <material name="brown">
    <color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
  </material>
  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <ros2_control name="UnitreeSystem" type="system">
    <hardware>
      <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
    </hardware>
    <joint name="FR_hip_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="FR_thigh_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="FR_calf_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="FL_hip_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="FL_thigh_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="FL_calf_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="RR_hip_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="RR_thigh_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="RR_calf_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="RL_hip_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="RL_thigh_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="RL_calf_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <sensor name="imu_sensor">
      <state_interface name="orientation.w"/>
      <state_interface name="orientation.x"/>
      <state_interface name="orientation.y"/>
      <state_interface name="orientation.z"/>
      <state_interface name="angular_velocity.x"/>
      <state_interface name="angular_velocity.y"/>
      <state_interface name="angular_velocity.z"/>
      <state_interface name="linear_acceleration.x"/>
      <state_interface name="linear_acceleration.y"/>
      <state_interface name="linear_acceleration.z"/>
    </sensor>
    <sensor name="foot_force">
      <state_interface name="FR"/>
      <state_interface name="FL"/>
      <state_interface name="RR"/>
      <state_interface name="RL"/>
    </sensor>
  </ros2_control>
  <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
  <!-- Rotor related joint and link is only for demonstrate location. -->
  <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
  <link name="base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <joint name="floating_base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base"/>
    <child link="trunk"/>
  </joint>
  <link name="trunk">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/trunk.dae" scale="1 1 1"/>
      </geometry>
      <material name="light-grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.3762 0.0935 0.114"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0223 0.002 -0.0005"/>
      <mass value="5.204"/>
      <inertia ixx="0.0168128557" ixy="-0.0002296769" ixz="-0.0002945293" iyy="0.063009565" iyz="-4.18731e-05" izz="0.0716547275"/>
    </inertial>
  </link>
  <joint name="imu_joint" type="fixed">
    <parent link="trunk"/>
    <child link="imu_link"/>
    <origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
  </joint>
  <link name="imu_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
      <!-- <material name="red"/> -->
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
  </link>
  <joint name="FR_hip_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.1881 -0.04675 0"/>
    <parent link="trunk"/>
    <child link="FR_hip"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
  </joint>
  <joint name="FR_hip_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.11215 -0.04675 0"/>
    <parent link="trunk"/>
    <child link="FR_hip_rotor"/>
  </joint>
  <link name="FR_hip">
    <visual>
      <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
      </geometry>
      <material name="deep-grey"/>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.046"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.005657 0.008752 -0.000102"/>
      <mass value="0.591"/>
      <inertia ixx="0.000334008405" ixy="1.0826066e-05" ixz="1.290732e-06" iyy="0.000619101213" iyz="-1.643194e-06" izz="0.00040057614"/>
    </inertial>
  </link>
  <link name="FR_hip_rotor">
    <visual>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="FR_thigh_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 -0.08 0"/>
    <parent link="FR_hip"/>
    <child link="FR_thigh"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
  </joint>
  <joint name="FR_thigh_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.00015 0"/>
    <parent link="FR_hip"/>
    <child link="FR_thigh_rotor"/>
  </joint>
  <link name="FR_thigh">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.0245 0.034"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003342 0.018054 -0.033451"/>
      <mass value="0.92"/>
      <inertia ixx="0.004431760472" ixy="-5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="-0.000572001265" izz="0.000740309489"/>
    </inertial>
  </link>
  <link name="FR_thigh_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="FR_calf_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="FR_thigh"/>
    <child link="FR_calf"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
  </joint>
  <joint name="FR_calf_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.03235 0"/>
    <parent link="FR_thigh"/>
    <child link="FR_calf_rotor"/>
  </joint>
  <link name="FR_calf">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
      </geometry>
      <material name="light-grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.016 0.016"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
      <mass value="0.135862"/>
      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
    </inertial>
  </link>
  <link name="FR_calf_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="FR_foot_fixed" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="FR_calf"/>
    <child link="FR_foot"/>
  </joint>
  <link name="FR_foot">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.06"/>
      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
    </inertial>
  </link>
  <gazebo reference="{name}_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="{name}_thigh">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="{name}_calf">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="{name}_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <transmission name="FR_hip_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FR_hip_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="FR_hip_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="FR_thigh_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FR_thigh_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="FR_thigh_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="FR_calf_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FR_calf_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="FR_calf_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="FL_hip_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.1881 0.04675 0"/>
    <parent link="trunk"/>
    <child link="FL_hip"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
  </joint>
  <joint name="FL_hip_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.11215 0.04675 0"/>
    <parent link="trunk"/>
    <child link="FL_hip_rotor"/>
  </joint>
  <link name="FL_hip">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
      </geometry>
      <material name="deep-grey"/>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.046"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.005657 -0.008752 -0.000102"/>
      <mass value="0.591"/>
      <inertia ixx="0.000334008405" ixy="-1.0826066e-05" ixz="1.290732e-06" iyy="0.000619101213" iyz="1.643194e-06" izz="0.00040057614"/>
    </inertial>
  </link>
  <link name="FL_hip_rotor">
    <visual>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="FL_thigh_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.08 0"/>
    <parent link="FL_hip"/>
    <child link="FL_thigh"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
  </joint>
  <joint name="FL_thigh_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.00015 0"/>
    <parent link="FL_hip"/>
    <child link="FL_thigh_rotor"/>
  </joint>
  <link name="FL_thigh">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh.dae" scale="1 1 1"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.0245 0.034"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003342 -0.018054 -0.033451"/>
      <mass value="0.92"/>
      <inertia ixx="0.004431760472" ixy="5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="0.000572001265" izz="0.000740309489"/>
    </inertial>
  </link>
  <link name="FL_thigh_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="FL_calf_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="FL_thigh"/>
    <child link="FL_calf"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
  </joint>
  <joint name="FL_calf_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.03235 0"/>
    <parent link="FL_thigh"/>
    <child link="FL_calf_rotor"/>
  </joint>
  <link name="FL_calf">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
      </geometry>
      <material name="light-grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.016 0.016"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
      <mass value="0.135862"/>
      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
    </inertial>
  </link>
  <link name="FL_calf_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="FL_foot_fixed" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="FL_calf"/>
    <child link="FL_foot"/>
  </joint>
  <link name="FL_foot">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.06"/>
      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
    </inertial>
  </link>
  <gazebo reference="{name}_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="{name}_thigh">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="{name}_calf">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="{name}_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <transmission name="FL_hip_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FL_hip_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="FL_hip_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="FL_thigh_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FL_thigh_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="FL_thigh_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="FL_calf_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FL_calf_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="FL_calf_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="RR_hip_joint" type="revolute">
    <origin rpy="0 0 0" xyz="-0.1881 -0.04675 0"/>
    <parent link="trunk"/>
    <child link="RR_hip"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
  </joint>
  <joint name="RR_hip_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.11215 -0.04675 0"/>
    <parent link="trunk"/>
    <child link="RR_hip_rotor"/>
  </joint>
  <link name="RR_hip">
    <visual>
      <origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
      </geometry>
      <material name="deep-grey"/>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.046"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.005657 0.008752 -0.000102"/>
      <mass value="0.591"/>
      <inertia ixx="0.000334008405" ixy="-1.0826066e-05" ixz="-1.290732e-06" iyy="0.000619101213" iyz="-1.643194e-06" izz="0.00040057614"/>
    </inertial>
  </link>
  <link name="RR_hip_rotor">
    <visual>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="RR_thigh_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 -0.08 0"/>
    <parent link="RR_hip"/>
    <child link="RR_thigh"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
  </joint>
  <joint name="RR_thigh_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.00015 0"/>
    <parent link="RR_hip"/>
    <child link="RR_thigh_rotor"/>
  </joint>
  <link name="RR_thigh">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.0245 0.034"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003342 0.018054 -0.033451"/>
      <mass value="0.92"/>
      <inertia ixx="0.004431760472" ixy="-5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="-0.000572001265" izz="0.000740309489"/>
    </inertial>
  </link>
  <link name="RR_thigh_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="RR_calf_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="RR_thigh"/>
    <child link="RR_calf"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
  </joint>
  <joint name="RR_calf_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.03235 0"/>
    <parent link="RR_thigh"/>
    <child link="RR_calf_rotor"/>
  </joint>
  <link name="RR_calf">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
      </geometry>
      <material name="light-grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.016 0.016"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
      <mass value="0.135862"/>
      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
    </inertial>
  </link>
  <link name="RR_calf_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="RR_foot_fixed" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="RR_calf"/>
    <child link="RR_foot"/>
  </joint>
  <link name="RR_foot">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.06"/>
      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
    </inertial>
  </link>
  <gazebo reference="{name}_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="{name}_thigh">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="{name}_calf">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="{name}_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <transmission name="RR_hip_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RR_hip_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="RR_hip_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RR_thigh_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RR_thigh_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="RR_thigh_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RR_calf_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RR_calf_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="RR_calf_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="RL_hip_joint" type="revolute">
    <origin rpy="0 0 0" xyz="-0.1881 0.04675 0"/>
    <parent link="trunk"/>
    <child link="RL_hip"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
  </joint>
  <joint name="RL_hip_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.11215 0.04675 0"/>
    <parent link="trunk"/>
    <child link="RL_hip_rotor"/>
  </joint>
  <link name="RL_hip">
    <visual>
      <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
      </geometry>
      <material name="deep-grey"/>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.046"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.005657 -0.008752 -0.000102"/>
      <mass value="0.591"/>
      <inertia ixx="0.000334008405" ixy="1.0826066e-05" ixz="-1.290732e-06" iyy="0.000619101213" iyz="1.643194e-06" izz="0.00040057614"/>
    </inertial>
  </link>
  <link name="RL_hip_rotor">
    <visual>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="RL_thigh_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.08 0"/>
    <parent link="RL_hip"/>
    <child link="RL_thigh"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
  </joint>
  <joint name="RL_thigh_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.00015 0"/>
    <parent link="RL_hip"/>
    <child link="RL_thigh_rotor"/>
  </joint>
  <link name="RL_thigh">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh.dae" scale="1 1 1"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.0245 0.034"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003342 -0.018054 -0.033451"/>
      <mass value="0.92"/>
      <inertia ixx="0.004431760472" ixy="5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="0.000572001265" izz="0.000740309489"/>
    </inertial>
  </link>
  <link name="RL_thigh_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="RL_calf_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="RL_thigh"/>
    <child link="RL_calf"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
  </joint>
  <joint name="RL_calf_rotor_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.03235 0"/>
    <parent link="RL_thigh"/>
    <child link="RL_calf_rotor"/>
  </joint>
  <link name="RL_calf">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
      </geometry>
      <material name="light-grey"/>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
      <geometry>
        <box size="0.213 0.016 0.016"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
      <mass value="0.135862"/>
      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
    </inertial>
  </link>
  <link name="RL_calf_rotor">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.035"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.089"/>
      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
    </inertial>
  </link>
  <joint name="RL_foot_fixed" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.213"/>
    <parent link="RL_calf"/>
    <child link="RL_foot"/>
  </joint>
  <link name="RL_foot">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.06"/>
      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
    </inertial>
  </link>
  <gazebo reference="{name}_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="{name}_thigh">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="{name}_calf">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="{name}_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <transmission name="RL_hip_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RL_hip_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="RL_hip_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RL_thigh_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RL_thigh_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="RL_thigh_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RL_calf_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RL_calf_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="RL_calf_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="camera_joint_face" type="fixed">
    <origin rpy="3.141592653589793 0 0" xyz="0.2785 0.0125 0.0167"/>
    <parent link="trunk"/>
    <child link="camera_face"/>
  </joint>
  <link name="camera_face">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="camera_optical_joint_face" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_face"/>
    <child link="camera_optical_face"/>
  </joint>
  <link name="camera_optical_face">
        </link>
  <!--        <gazebo reference="camera_${name}">-->
  <!--            &lt;!&ndash; <material>Gazebo/Black</material> &ndash;&gt;-->
  <!--            <sensor name="camera_${name}_camera" type="depth">-->
  <!--                <update_rate>16</update_rate>-->
  <!--                <camera>-->
  <!--                    <horizontal_fov>2.094</horizontal_fov>-->
  <!--                    <image>-->
  <!--                        <width>928</width>-->
  <!--                        <height>800</height>-->
  <!--                        <format>R8G8B8</format>-->
  <!--                    </image>-->
  <!--                    <clip>-->
  <!--                        <near>0.1</near>-->
  <!--                        <far>5</far>-->
  <!--                    </clip>-->
  <!--                </camera>-->
  <!--                <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">-->
  <!--                    <baseline>0.025</baseline>-->
  <!--                    <alwaysOn>true</alwaysOn>-->
  <!--                    <updateRate>0.0</updateRate>-->
  <!--                    <cameraName>camera_${name}_ir</cameraName>-->
  <!--                    <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>-->
  <!--                    <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>-->
  <!--                    <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>-->
  <!--                    <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>-->
  <!--                    &lt;!&ndash; <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> &ndash;&gt;-->
  <!--                    <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>-->
  <!--                    <frameName>camera_optical_${name}</frameName>-->
  <!--                    <pointCloudCutoff>0.1</pointCloudCutoff>-->
  <!--                    <pointCloudCutoffMax>1.5</pointCloudCutoffMax>-->
  <!--                    <distortionK1>0.0</distortionK1>-->
  <!--                    <distortionK2>0.0</distortionK2>-->
  <!--                    <distortionK3>0.0</distortionK3>-->
  <!--                    <distortionT1>0.0</distortionT1>-->
  <!--                    <distortionT2>0.0</distortionT2>-->
  <!--                    <CxPrime>0</CxPrime>-->
  <!--                    <Cx>0.0045</Cx>-->
  <!--                    <Cy>0.0039</Cy>-->
  <!--                    <focalLength>0</focalLength>-->
  <!--                    <hackBaseline>0</hackBaseline>-->
  <!--                </plugin>-->
  <!--            </sensor>-->
  <!--        </gazebo>-->
  <joint name="camera_joint_chin" type="fixed">
    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="0.2522 0.0125 -0.0436"/>
    <parent link="trunk"/>
    <child link="camera_chin"/>
  </joint>
  <link name="camera_chin">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="camera_optical_joint_chin" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_chin"/>
    <child link="camera_optical_chin"/>
  </joint>
  <link name="camera_optical_chin">
        </link>
  <!--        <gazebo reference="camera_${name}">-->
  <!--            &lt;!&ndash; <material>Gazebo/Black</material> &ndash;&gt;-->
  <!--            <sensor name="camera_${name}_camera" type="depth">-->
  <!--                <update_rate>16</update_rate>-->
  <!--                <camera>-->
  <!--                    <horizontal_fov>2.094</horizontal_fov>-->
  <!--                    <image>-->
  <!--                        <width>928</width>-->
  <!--                        <height>800</height>-->
  <!--                        <format>R8G8B8</format>-->
  <!--                    </image>-->
  <!--                    <clip>-->
  <!--                        <near>0.1</near>-->
  <!--                        <far>5</far>-->
  <!--                    </clip>-->
  <!--                </camera>-->
  <!--                <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">-->
  <!--                    <baseline>0.025</baseline>-->
  <!--                    <alwaysOn>true</alwaysOn>-->
  <!--                    <updateRate>0.0</updateRate>-->
  <!--                    <cameraName>camera_${name}_ir</cameraName>-->
  <!--                    <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>-->
  <!--                    <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>-->
  <!--                    <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>-->
  <!--                    <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>-->
  <!--                    &lt;!&ndash; <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> &ndash;&gt;-->
  <!--                    <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>-->
  <!--                    <frameName>camera_optical_${name}</frameName>-->
  <!--                    <pointCloudCutoff>0.1</pointCloudCutoff>-->
  <!--                    <pointCloudCutoffMax>1.5</pointCloudCutoffMax>-->
  <!--                    <distortionK1>0.0</distortionK1>-->
  <!--                    <distortionK2>0.0</distortionK2>-->
  <!--                    <distortionK3>0.0</distortionK3>-->
  <!--                    <distortionT1>0.0</distortionT1>-->
  <!--                    <distortionT2>0.0</distortionT2>-->
  <!--                    <CxPrime>0</CxPrime>-->
  <!--                    <Cx>0.0045</Cx>-->
  <!--                    <Cy>0.0039</Cy>-->
  <!--                    <focalLength>0</focalLength>-->
  <!--                    <hackBaseline>0</hackBaseline>-->
  <!--                </plugin>-->
  <!--            </sensor>-->
  <!--        </gazebo>-->
  <joint name="camera_joint_left" type="fixed">
    <origin rpy="3.141592653589793 0.2618 1.5707963267948966" xyz="-0.066 0.082 -0.0176"/>
    <parent link="trunk"/>
    <child link="camera_left"/>
  </joint>
  <link name="camera_left">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="camera_optical_joint_left" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_left"/>
    <child link="camera_optical_left"/>
  </joint>
  <link name="camera_optical_left">
        </link>
  <!--        <gazebo reference="camera_${name}">-->
  <!--            &lt;!&ndash; <material>Gazebo/Black</material> &ndash;&gt;-->
  <!--            <sensor name="camera_${name}_camera" type="depth">-->
  <!--                <update_rate>16</update_rate>-->
  <!--                <camera>-->
  <!--                    <horizontal_fov>2.094</horizontal_fov>-->
  <!--                    <image>-->
  <!--                        <width>928</width>-->
  <!--                        <height>800</height>-->
  <!--                        <format>R8G8B8</format>-->
  <!--                    </image>-->
  <!--                    <clip>-->
  <!--                        <near>0.1</near>-->
  <!--                        <far>5</far>-->
  <!--                    </clip>-->
  <!--                </camera>-->
  <!--                <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">-->
  <!--                    <baseline>0.025</baseline>-->
  <!--                    <alwaysOn>true</alwaysOn>-->
  <!--                    <updateRate>0.0</updateRate>-->
  <!--                    <cameraName>camera_${name}_ir</cameraName>-->
  <!--                    <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>-->
  <!--                    <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>-->
  <!--                    <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>-->
  <!--                    <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>-->
  <!--                    &lt;!&ndash; <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> &ndash;&gt;-->
  <!--                    <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>-->
  <!--                    <frameName>camera_optical_${name}</frameName>-->
  <!--                    <pointCloudCutoff>0.1</pointCloudCutoff>-->
  <!--                    <pointCloudCutoffMax>1.5</pointCloudCutoffMax>-->
  <!--                    <distortionK1>0.0</distortionK1>-->
  <!--                    <distortionK2>0.0</distortionK2>-->
  <!--                    <distortionK3>0.0</distortionK3>-->
  <!--                    <distortionT1>0.0</distortionT1>-->
  <!--                    <distortionT2>0.0</distortionT2>-->
  <!--                    <CxPrime>0</CxPrime>-->
  <!--                    <Cx>0.0045</Cx>-->
  <!--                    <Cy>0.0039</Cy>-->
  <!--                    <focalLength>0</focalLength>-->
  <!--                    <hackBaseline>0</hackBaseline>-->
  <!--                </plugin>-->
  <!--            </sensor>-->
  <!--        </gazebo>-->
  <joint name="camera_joint_right" type="fixed">
    <origin rpy="3.141592653589793 0.2618 -1.5707963267948966" xyz="-0.041 -0.082 -0.0176"/>
    <parent link="trunk"/>
    <child link="camera_right"/>
  </joint>
  <link name="camera_right">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="camera_optical_joint_right" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_right"/>
    <child link="camera_optical_right"/>
  </joint>
  <link name="camera_optical_right">
        </link>
  <!--        <gazebo reference="camera_${name}">-->
  <!--            &lt;!&ndash; <material>Gazebo/Black</material> &ndash;&gt;-->
  <!--            <sensor name="camera_${name}_camera" type="depth">-->
  <!--                <update_rate>16</update_rate>-->
  <!--                <camera>-->
  <!--                    <horizontal_fov>2.094</horizontal_fov>-->
  <!--                    <image>-->
  <!--                        <width>928</width>-->
  <!--                        <height>800</height>-->
  <!--                        <format>R8G8B8</format>-->
  <!--                    </image>-->
  <!--                    <clip>-->
  <!--                        <near>0.1</near>-->
  <!--                        <far>5</far>-->
  <!--                    </clip>-->
  <!--                </camera>-->
  <!--                <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">-->
  <!--                    <baseline>0.025</baseline>-->
  <!--                    <alwaysOn>true</alwaysOn>-->
  <!--                    <updateRate>0.0</updateRate>-->
  <!--                    <cameraName>camera_${name}_ir</cameraName>-->
  <!--                    <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>-->
  <!--                    <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>-->
  <!--                    <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>-->
  <!--                    <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>-->
  <!--                    &lt;!&ndash; <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> &ndash;&gt;-->
  <!--                    <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>-->
  <!--                    <frameName>camera_optical_${name}</frameName>-->
  <!--                    <pointCloudCutoff>0.1</pointCloudCutoff>-->
  <!--                    <pointCloudCutoffMax>1.5</pointCloudCutoffMax>-->
  <!--                    <distortionK1>0.0</distortionK1>-->
  <!--                    <distortionK2>0.0</distortionK2>-->
  <!--                    <distortionK3>0.0</distortionK3>-->
  <!--                    <distortionT1>0.0</distortionT1>-->
  <!--                    <distortionT2>0.0</distortionT2>-->
  <!--                    <CxPrime>0</CxPrime>-->
  <!--                    <Cx>0.0045</Cx>-->
  <!--                    <Cy>0.0039</Cy>-->
  <!--                    <focalLength>0</focalLength>-->
  <!--                    <hackBaseline>0</hackBaseline>-->
  <!--                </plugin>-->
  <!--            </sensor>-->
  <!--        </gazebo>-->
  <joint name="camera_joint_rearDown" type="fixed">
    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="-0.0825 0.0125 -0.04365"/>
    <parent link="trunk"/>
    <child link="camera_rearDown"/>
  </joint>
  <link name="camera_rearDown">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="camera_optical_joint_rearDown" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_rearDown"/>
    <child link="camera_optical_rearDown"/>
  </joint>
  <link name="camera_optical_rearDown">
        </link>
  <!--        <gazebo reference="camera_${name}">-->
  <!--            &lt;!&ndash; <material>Gazebo/Black</material> &ndash;&gt;-->
  <!--            <sensor name="camera_${name}_camera" type="depth">-->
  <!--                <update_rate>16</update_rate>-->
  <!--                <camera>-->
  <!--                    <horizontal_fov>2.094</horizontal_fov>-->
  <!--                    <image>-->
  <!--                        <width>928</width>-->
  <!--                        <height>800</height>-->
  <!--                        <format>R8G8B8</format>-->
  <!--                    </image>-->
  <!--                    <clip>-->
  <!--                        <near>0.1</near>-->
  <!--                        <far>5</far>-->
  <!--                    </clip>-->
  <!--                </camera>-->
  <!--                <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">-->
  <!--                    <baseline>0.025</baseline>-->
  <!--                    <alwaysOn>true</alwaysOn>-->
  <!--                    <updateRate>0.0</updateRate>-->
  <!--                    <cameraName>camera_${name}_ir</cameraName>-->
  <!--                    <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>-->
  <!--                    <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>-->
  <!--                    <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>-->
  <!--                    <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>-->
  <!--                    &lt;!&ndash; <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> &ndash;&gt;-->
  <!--                    <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>-->
  <!--                    <frameName>camera_optical_${name}</frameName>-->
  <!--                    <pointCloudCutoff>0.1</pointCloudCutoff>-->
  <!--                    <pointCloudCutoffMax>1.5</pointCloudCutoffMax>-->
  <!--                    <distortionK1>0.0</distortionK1>-->
  <!--                    <distortionK2>0.0</distortionK2>-->
  <!--                    <distortionK3>0.0</distortionK3>-->
  <!--                    <distortionT1>0.0</distortionT1>-->
  <!--                    <distortionT2>0.0</distortionT2>-->
  <!--                    <CxPrime>0</CxPrime>-->
  <!--                    <Cx>0.0045</Cx>-->
  <!--                    <Cy>0.0039</Cy>-->
  <!--                    <focalLength>0</focalLength>-->
  <!--                    <hackBaseline>0</hackBaseline>-->
  <!--                </plugin>-->
  <!--            </sensor>-->
  <!--        </gazebo>-->
  <joint name="camera_laserscan_joint_left" type="fixed">
    <origin rpy="0 0.2618 0" xyz="0 0 0"/>
    <parent link="camera_left"/>
    <child link="camera_laserscan_link_left"/>
  </joint>
  <link name="camera_laserscan_link_left">
    </link>
  <joint name="camera_laserscan_joint_right" type="fixed">
    <origin rpy="0 0.2618 0" xyz="0 0 0"/>
    <parent link="camera_right"/>
    <child link="camera_laserscan_link_right"/>
  </joint>
  <link name="camera_laserscan_link_right">
    </link>
  <joint name="ultraSound_joint_left" type="fixed">
    <origin rpy="0 0.2618 1.5707963267948966" xyz="-0.0535  0.0826 0.00868"/>
    <parent link="trunk"/>
    <child link="ultraSound_left"/>
  </joint>
  <link name="ultraSound_left">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
      </geometry>
      <material name="black"/>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="ultraSound_joint_right" type="fixed">
    <origin rpy="0 0.2618 -1.5707963267948966" xyz="-0.0535 -0.0826 0.00868"/>
    <parent link="trunk"/>
    <child link="ultraSound_right"/>
  </joint>
  <link name="ultraSound_right">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
      </geometry>
      <material name="black"/>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="ultraSound_joint_face" type="fixed">
    <origin rpy="0 0 0" xyz="0.2747 0.0 -0.0088"/>
    <parent link="trunk"/>
    <child link="ultraSound_face"/>
  </joint>
  <link name="ultraSound_face">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
      </geometry>
      <material name="black"/>
    </visual>
    <inertial>
      <mass value="1e-5"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
</robot>
