<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from urdf/xarm_pen.urdf.xacro       | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="xarm6">
  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
      <parameters>/root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml</parameters>
    </plugin>
  </gazebo>
  <link name="world"/>
  <joint name="world_joint" type="fixed">
    <parent link="world"/>
    <child link="link_base"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <ros2_control name="UFRobotSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      <param name="hw_ns">xarm</param>
      <param name="velocity_control">False</param>
      <param name="prefix">P</param>
      <param name="robot_ip">R</param>
      <param name="report_type">normal</param>
      <param name="dof">6</param>
      <param name="baud_checkset">True</param>
      <param name="default_gripper_baud">2000000</param>
      <param name="robot_type">lite</param>
      <param name="add_gripper">False</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-2.61799</param>
        <param name="max">2.61799</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>
    <joint name="joint3">
      <command_interface name="position">
        <param name="min">-0.061087</param>
        <param name="max">5.235988</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>
    <joint name="joint4">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>
    <joint name="joint5">
      <command_interface name="position">
        <param name="min">-2.1642</param>
        <param name="max">2.1642</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>
    <joint name="joint6">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>
  </ros2_control>
  <material name="White">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <material name="Silver">
    <color rgba="0.753 0.753 0.753 1.0"/>
  </material>
  <link name="link_base">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"/>
      <mass value="1.65393501783165"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="link1">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00036 0.03788 -0.0027"/>
      <mass value="1.169"/>
      <inertia ixx="1.45164E-03" ixy="1.24E-05" ixz="-6.7E-06" iyy="8.873E-04" iyz="1.255E-04" izz="1.31993E-03"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.2435"/>
    <parent link="link_base"/>
    <child link="link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="50.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>
  <link name="link2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.178 0.0 0.0576"/>
      <mass value="1.192"/>
      <inertia ixx="1.5854E-03" ixy="-6.766E-06" ixz="-1.15136E-03" iyy="5.6097E-03" iyz="1.14E-06" izz="4.85E-03"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint2" type="revolute">
    <origin rpy="1.5708 -1.5708 3.1416" xyz="0 0 0"/>
    <parent link="link1"/>
    <child link="link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="50.0" lower="-2.61799" upper="2.61799" velocity="3.14"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>
  <link name="link3">
    <inertial>
      <origin rpy="0 0 0" xyz="0.07285 -0.030 -0.0009"/>
      <mass value="0.930"/>
      <inertia ixx="8.861E-04" ixy="-3.9287E-04" ixz="7.066E-05" iyy="1.5785E-03" iyz="-2.445E-05" izz="1.84677E-03"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint3" type="revolute">
    <origin rpy="-3.1416 0 1.5708" xyz="0.2002 0 0"/>
    <parent link="link2"/>
    <child link="link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="32.0" lower="-0.061087" upper="5.235988" velocity="3.14"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>
  <link name="link4">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0004 -0.0275 -0.0817"/>
      <mass value="1.31"/>
      <inertia ixx="3.705E-03" ixy="-2.0E-06" ixz="7.17E-06" iyy="3.0455E-03" iyz="-9.3188E-04" izz="1.5413E-03"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint4" type="revolute">
    <origin rpy="1.5708 0 0" xyz="0.087 -0.22761 0"/>
    <parent link="link3"/>
    <child link="link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="32.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>
  <link name="link5">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.010 0.0019"/>
      <mass value="0.784"/>
      <inertia ixx="5.668E-04" ixy="6E-07" ixz="-5.3E-06" iyy="5.077E-04" iyz="-4.8E-07" izz="5.3E-04"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint5" type="revolute">
    <origin rpy="1.5708 0 0" xyz="0 0 0"/>
    <parent link="link4"/>
    <child link="link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="32.0" lower="-2.1642" upper="2.1642" velocity="3.14"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>
  <link name="link6">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 -0.00194 -0.0102"/>
      <mass value="0.180"/>
      <inertia ixx="7.726E-05" ixy="1E-06" ixz="4E-07" iyy="8.5665E-05" iyz="-6E-07" izz="1.4814E-04"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
      </geometry>
      <material name="Silver"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint6" type="revolute">
    <origin rpy="-1.5708 0 0" xyz="0 0.0625 0"/>
    <parent link="link5"/>
    <child link="link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="20.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>
  <link name="link_eef"/>
  <joint name="joint_eef" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="link6"/>
    <child link="link_eef"/>
  </joint>
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>100</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>100</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>100</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>100</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran5">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>100</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran6">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>100</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="link_base">
    <selfCollide>true</selfCollide>
  </gazebo>
  <gazebo reference="link1">
    <selfCollide>true</selfCollide>
  </gazebo>
  <gazebo reference="link2">
    <selfCollide>true</selfCollide>
  </gazebo>
  <gazebo reference="link3">
    <selfCollide>true</selfCollide>
  </gazebo>
  <gazebo reference="link4">
    <selfCollide>true</selfCollide>
  </gazebo>
  <gazebo reference="link5">
    <selfCollide>true</selfCollide>
  </gazebo>
  <gazebo reference="link6">
    <selfCollide>true</selfCollide>
  </gazebo>
  <joint name="eef_joint" type="fixed">
    <parent link="link_eef"/>
    <child link="pen_link"/>
  </joint>
  <link name="pen_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.005"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0 0 0"/>
      </geometry>
    </collision>
  </link>
  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="pen_position">
      <!-- <alwaysOn>true</alwaysOn> -->
      <ros>
        <remapping>odom:=pen_position</remapping>
      </ros>
      <frame_name>world</frame_name>
      <!-- <body_name>pen_link</body_name> -->
      <body_name>link6</body_name>
      <!--       topic name is always /odom -->
      <!-- <topic_name>pen_position</topic_name> -->
      <!--       Update rate in Hz -->
      <update_rate>10</update_rate>
      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
    </plugin>
  </gazebo>
</robot>
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
     <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
     <xacro:lite6_urdf prefix=''/> -->
<!--
     </robot> -->
