From 30b42a14f066b2b003bf886bcf7ccb01aeec654f Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Fri, 13 Dec 2019 09:20:46 -0800
Subject: [PATCH] add single-link arm urdf files for debugging

---
 .../pybullet/gym/pybullet_data/xarm/base.urdf | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/base_com.urdf      | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link1.urdf         | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link1_com.urdf     | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link2.urdf         | 70 +++++++++++++++++++
 .../gym/pybullet_data/xarm/link2_com.urdf     | 70 +++++++++++++++++++
 .../gym/pybullet_data/xarm/link3.urdf         | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link3_com.urdf     | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link4.urdf         | 70 +++++++++++++++++++
 .../gym/pybullet_data/xarm/link4_com.urdf     | 70 +++++++++++++++++++
 .../gym/pybullet_data/xarm/link5.urdf         | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link5_com.urdf     | 69 ++++++++++++++++++
 .../gym/pybullet_data/xarm/link6.urdf         | 70 +++++++++++++++++++
 .../gym/pybullet_data/xarm/link6_com.urdf     | 70 +++++++++++++++++++
 14 files changed, 972 insertions(+)
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/base.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/base_com.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link1.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link1_com.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link2.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link2_com.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link3.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link3_com.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link4.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link4_com.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link5.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link5_com.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link6.urdf
 create mode 100644 examples/pybullet/gym/pybullet_data/xarm/link6_com.urdf

diff --git a/examples/pybullet/gym/pybullet_data/xarm/base.urdf b/examples/pybullet/gym/pybullet_data/xarm/base.urdf
new file mode 100644
index 000000000..5d6d8cdd3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/base.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_base">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </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>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/base.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/base_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.09103"/>
+      <mass value="2.7"/>
+      <inertia ixx="0.00494875" ixy="-3.5E-06" ixz="1.25E-05" iyy="0.00494174" iyz="1.67E-06" izz="0.002219"/>
+    </inertial>
+  </link>
+  
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/base_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/base_com.urdf
new file mode 100644
index 000000000..829626252
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/base_com.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_base">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </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>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/base.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.09103"/>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/base_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.09103"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="2.7"/>
+      <inertia ixx="0.00494875" ixy="-3.5E-06" ixz="1.25E-05" iyy="0.00494174" iyz="1.67E-06" izz="0.002219"/>
+    </inertial>
+  </link>
+  
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link1.urdf b/examples/pybullet/gym/pybullet_data/xarm/link1.urdf
new file mode 100644
index 000000000..990825f80
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link1.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link1">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link1">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link1.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link1_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.002 0.02692 -0.01332"/>
+      <mass value="2.16"/>
+      <inertia ixx="0.00539427" ixy="1.095E-05" ixz="1.635E-06" iyy="0.0048979" iyz="0.000793" izz="0.00311573"/>
+    </inertial>
+  </link>
+
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link1_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/link1_com.urdf
new file mode 100644
index 000000000..497cc5e84
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link1_com.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link1">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link1">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link1.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0.002 -0.02692 0.01332"/>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link1_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0.002 -0.02692 0.01332"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="2.16"/>
+      <inertia ixx="0.00539427" ixy="1.095E-05" ixz="1.635E-06" iyy="0.0048979" iyz="0.000793" izz="0.00311573"/>
+    </inertial>
+  </link>
+
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link2.urdf b/examples/pybullet/gym/pybullet_data/xarm/link2.urdf
new file mode 100644
index 000000000..b5bff838a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link2.urdf
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link2">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link2">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link2.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Yellow"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link2_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.03531 -0.21398 0.03386"/>
+      <mass value="1.71"/>
+      <inertia ixx="0.0248674" ixy="-0.00430651" ixz="-0.00067797" iyy="0.00485548" iyz="0.00457245" izz="0.02387827"/>
+    </inertial>
+  </link>
+ 
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link2_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/link2_com.urdf
new file mode 100644
index 000000000..40a065525
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link2_com.urdf
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link2">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link2">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link2.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.03531 0.21398 -0.03386"/>
+      <material name="Yellow"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link2_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.03531 0.21398 -0.03386"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="1.71"/>
+      <inertia ixx="0.0248674" ixy="-0.00430651" ixz="-0.00067797" iyy="0.00485548" iyz="0.00457245" izz="0.02387827"/>
+    </inertial>
+  </link>
+ 
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link3.urdf b/examples/pybullet/gym/pybullet_data/xarm/link3.urdf
new file mode 100644
index 000000000..2749d2666
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link3.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link3">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link3"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link3">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link3.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link3_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.06781 0.10749 0.01457"/>
+      <mass value="1.384"/>
+      <inertia ixx="0.0053694" ixy="0.0014185" ixz="-0.00092094" iyy="0.0032423" iyz="-0.00169178" izz="0.00501731"/>
+    </inertial>
+  </link>
+
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link3_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/link3_com.urdf
new file mode 100644
index 000000000..3bc0991a1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link3_com.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link3">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link3"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link3">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link3.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.06781 -0.10749 -0.01457"/>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link3_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.06781 -0.10749 -0.01457"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="1.384"/>
+      <inertia ixx="0.0053694" ixy="0.0014185" ixz="-0.00092094" iyy="0.0032423" iyz="-0.00169178" izz="0.00501731"/>
+    </inertial>
+  </link>
+
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link4.urdf b/examples/pybullet/gym/pybullet_data/xarm/link4.urdf
new file mode 100644
index 000000000..e2e6d11ab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link4.urdf
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link4">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link4"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link4">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link4.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link4_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00021 0.02578 -0.02538"/>
+      <mass value="1.115"/>
+      <inertia ixx="0.00439263" ixy="5.028E-05" ixz="1.374E-05" iyy="0.0040077" iyz="0.00045338" izz="0.00110321"/>
+    </inertial>
+  </link>
+  
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link4_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/link4_com.urdf
new file mode 100644
index 000000000..25183bb4f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link4_com.urdf
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link4">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link4"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link4">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link4.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0.00021 -0.02578 0.02538"/>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link4_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0.00021 -0.02578 0.02538"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="1.115"/>
+      <inertia ixx="0.00439263" ixy="5.028E-05" ixz="1.374E-05" iyy="0.0040077" iyz="0.00045338" izz="0.00110321"/>
+    </inertial>
+  </link>
+  
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link5.urdf b/examples/pybullet/gym/pybullet_data/xarm/link5.urdf
new file mode 100644
index 000000000..0bc4f787b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link5.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link5">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link5"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link5">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link5.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link5_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.05428 0.01781 0.00543"/>
+      <mass value="1.275"/>
+      <inertia ixx="0.001202758" ixy="0.000492428" ixz="-0.00039147" iyy="0.0022876" iyz="-1.235E-04" izz="0.0026866"/>
+    </inertial>
+  </link>
+ 
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link5_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/link5_com.urdf
new file mode 100644
index 000000000..c1b4616c1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link5_com.urdf
@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link5">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link5"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link5">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link5.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.05428 -0.01781 -0.00543"/>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link5_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.05428 -0.01781 -0.00543"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="1.275"/>
+      <inertia ixx="0.001202758" ixy="0.000492428" ixz="-0.00039147" iyy="0.0022876" iyz="-1.235E-04" izz="0.0026866"/>
+    </inertial>
+  </link>
+ 
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link6.urdf b/examples/pybullet/gym/pybullet_data/xarm/link6.urdf
new file mode 100644
index 000000000..0ce9a887d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link6.urdf
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link6">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link6"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link6">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link6.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="Silver"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link6_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.00064 -0.00952"/>
+      <mass value="0.1096"/>
+      <inertia ixx="4.5293E-05" ixy="0" ixz="0" iyy="4.8111E-05" iyz="0" izz="7.9715E-05"/>
+    </inertial>
+  </link>
+
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/link6_com.urdf b/examples/pybullet/gym/pybullet_data/xarm/link6_com.urdf
new file mode 100644
index 000000000..4cb707430
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/link6_com.urdf
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from xarm6_robot.urdf.xacro         | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6_link6">
+  <!--
+  Author: Jason Peng <jason@ufactory.cc>
+  Contributers:
+-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/xarm</robotNamespace>
+      <!-- <controlPeriod>0.0001</controlPeriod> -->
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+      <!-- <preserveWorldVelocity>true</preserveWorldVelocity> -->
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link6"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.85 0.19  0.21 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.28 0.52 0.92 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.23 0.72 0.32 1.0"/>
+  </material>
+  <material name="Yellow">
+    <color rgba="0.95 0.76 0.05 1.0"/>
+  </material>
+  <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="link6">
+    <visual>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/visual/link6.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 -0.00064 0.00952"/>
+      <material name="Silver"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://xarm_description/meshes/xarm6/collision/link6_vhacd.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 -0.00064 0.00952"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.1096"/>
+      <inertia ixx="4.5293E-05" ixy="0" ixz="0" iyy="4.8111E-05" iyz="0" izz="7.9715E-05"/>
+    </inertial>
+  </link>
+
+</robot>
+