From ce430fb0f3113c224a2451e8d367ced17595f5bd Mon Sep 17 00:00:00 2001
From: Cheolhui <mch5048@korea.ac.kr>
Date: Sat, 4 May 2019 19:48:36 +0900
Subject: [PATCH] Replace the fk solver with kdl kinematics

---
 .dockerignore                                 |   0
 .flake8                                       |   0
 .gitignore                                    |   0
 .pre-commit-config.yaml                       |   0
 .travis.yml                                   |   0
 CMakeLists.txt                                |   0
 Dockerfile                                    |   0
 Makefile                                      |   0
 README.md                                     |   0
 __init__.py                                   |   0
 docker_train.sh                               |   0
 launch/open_manipulator_env.launch            |  33 +-
 msg/GoalObs.msg                               |   0
 package.xml                                   |   0
 scripts/.isort.cfg                            |   0
 scripts/__init__.py                           |   0
 scripts/algorithms/__init__.py                |   0
 scripts/algorithms/common/__init__.py         |   0
 .../algorithms/common/abstract/__init__.py    |   0
 scripts/algorithms/common/buffer/__init__.py  |   0
 .../common/buffer/priortized_replay_buffer.py |   0
 .../algorithms/common/buffer/replay_buffer.py |   0
 .../algorithms/common/buffer/segment_tree.py  |   0
 scripts/algorithms/common/helper_functions.py |   0
 .../algorithms/common/networks/__init__.py    |   0
 scripts/algorithms/common/networks/lstm.py    |   0
 scripts/algorithms/common/networks/mlp.py     |   0
 scripts/algorithms/common/noise.py            |   0
 scripts/algorithms/fd/__init__.py             |   0
 scripts/algorithms/fd/sac_agent.py            |   0
 scripts/algorithms/fd/td3_agent.py            |   0
 scripts/algorithms/sac/__init__.py            |   0
 scripts/algorithms/sac/agent.py               |   0
 scripts/algorithms/td3/__init__.py            |   0
 scripts/algorithms/td3/agent.py               |   0
 scripts/config/__init__.py                    |   0
 scripts/config/agent/__init__.py              |   0
 .../lunarlander_continuous_v2/__init__.py     |   0
 .../agent/lunarlander_continuous_v2/sac.py    |   0
 .../agent/lunarlander_continuous_v2/sacfd.py  |   0
 .../agent/lunarlander_continuous_v2/td3.py    |   0
 .../agent/lunarlander_continuous_v2/td3fd.py  |   0
 scripts/config/agent/reacher-v1/__init__.py   |   0
 scripts/config/agent/reacher-v1/sac.py        |   0
 scripts/config/agent/reacher-v1/sacfd.py      |   0
 scripts/config/agent/reacher-v1/td3.py        |   0
 scripts/config/agent/reacher-v1/td3fd.py      |   0
 scripts/config/environment/__init__.py        |   0
 scripts/data/lunarlander_continuous_demo.pkl  | Bin
 scripts/data/reacher_demo.pkl                 | Bin
 scripts/envs/__init__.py                      |   0
 scripts/envs/open_manipulator/__init__.py     |   0
 .../envs/open_manipulator/ros_interface.py    |  34 +-
 scripts/requirements-dev.txt                  |   0
 scripts/requirements.txt                      |   0
 scripts/run_lunarlander_continuous.py         |   0
 scripts/run_reacher_v1.py                     |   0
 scripts/test_lstm.py                          |   0
 urdf/om.urdf                                  | 308 ++++++++++++++++++
 59 files changed, 341 insertions(+), 34 deletions(-)
 mode change 100644 => 100755 .dockerignore
 mode change 100644 => 100755 .flake8
 mode change 100644 => 100755 .gitignore
 mode change 100644 => 100755 .pre-commit-config.yaml
 mode change 100644 => 100755 .travis.yml
 mode change 100644 => 100755 CMakeLists.txt
 mode change 100644 => 100755 Dockerfile
 mode change 100644 => 100755 Makefile
 mode change 100644 => 100755 README.md
 mode change 100644 => 100755 __init__.py
 mode change 100644 => 100755 docker_train.sh
 mode change 100644 => 100755 msg/GoalObs.msg
 mode change 100644 => 100755 package.xml
 mode change 100644 => 100755 scripts/.isort.cfg
 mode change 100644 => 100755 scripts/__init__.py
 mode change 100644 => 100755 scripts/algorithms/__init__.py
 mode change 100644 => 100755 scripts/algorithms/common/__init__.py
 mode change 100644 => 100755 scripts/algorithms/common/abstract/__init__.py
 mode change 100644 => 100755 scripts/algorithms/common/buffer/__init__.py
 mode change 100644 => 100755 scripts/algorithms/common/buffer/priortized_replay_buffer.py
 mode change 100644 => 100755 scripts/algorithms/common/buffer/replay_buffer.py
 mode change 100644 => 100755 scripts/algorithms/common/buffer/segment_tree.py
 mode change 100644 => 100755 scripts/algorithms/common/helper_functions.py
 mode change 100644 => 100755 scripts/algorithms/common/networks/__init__.py
 mode change 100644 => 100755 scripts/algorithms/common/networks/lstm.py
 mode change 100644 => 100755 scripts/algorithms/common/networks/mlp.py
 mode change 100644 => 100755 scripts/algorithms/common/noise.py
 mode change 100644 => 100755 scripts/algorithms/fd/__init__.py
 mode change 100644 => 100755 scripts/algorithms/fd/sac_agent.py
 mode change 100644 => 100755 scripts/algorithms/fd/td3_agent.py
 mode change 100644 => 100755 scripts/algorithms/sac/__init__.py
 mode change 100644 => 100755 scripts/algorithms/sac/agent.py
 mode change 100644 => 100755 scripts/algorithms/td3/__init__.py
 mode change 100644 => 100755 scripts/algorithms/td3/agent.py
 mode change 100644 => 100755 scripts/config/__init__.py
 mode change 100644 => 100755 scripts/config/agent/__init__.py
 mode change 100644 => 100755 scripts/config/agent/lunarlander_continuous_v2/__init__.py
 mode change 100644 => 100755 scripts/config/agent/lunarlander_continuous_v2/sac.py
 mode change 100644 => 100755 scripts/config/agent/lunarlander_continuous_v2/sacfd.py
 mode change 100644 => 100755 scripts/config/agent/lunarlander_continuous_v2/td3.py
 mode change 100644 => 100755 scripts/config/agent/lunarlander_continuous_v2/td3fd.py
 mode change 100644 => 100755 scripts/config/agent/reacher-v1/__init__.py
 mode change 100644 => 100755 scripts/config/agent/reacher-v1/sac.py
 mode change 100644 => 100755 scripts/config/agent/reacher-v1/sacfd.py
 mode change 100644 => 100755 scripts/config/agent/reacher-v1/td3.py
 mode change 100644 => 100755 scripts/config/agent/reacher-v1/td3fd.py
 mode change 100644 => 100755 scripts/config/environment/__init__.py
 mode change 100644 => 100755 scripts/data/lunarlander_continuous_demo.pkl
 mode change 100644 => 100755 scripts/data/reacher_demo.pkl
 mode change 100644 => 100755 scripts/envs/__init__.py
 mode change 100644 => 100755 scripts/envs/open_manipulator/__init__.py
 mode change 100644 => 100755 scripts/requirements-dev.txt
 mode change 100644 => 100755 scripts/requirements.txt
 mode change 100644 => 100755 scripts/run_lunarlander_continuous.py
 mode change 100644 => 100755 scripts/run_reacher_v1.py
 mode change 100644 => 100755 scripts/test_lstm.py
 create mode 100755 urdf/om.urdf

diff --git a/.dockerignore b/.dockerignore
old mode 100644
new mode 100755
diff --git a/.flake8 b/.flake8
old mode 100644
new mode 100755
diff --git a/.gitignore b/.gitignore
old mode 100644
new mode 100755
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
old mode 100644
new mode 100755
diff --git a/.travis.yml b/.travis.yml
old mode 100644
new mode 100755
diff --git a/CMakeLists.txt b/CMakeLists.txt
old mode 100644
new mode 100755
diff --git a/Dockerfile b/Dockerfile
old mode 100644
new mode 100755
diff --git a/Makefile b/Makefile
old mode 100644
new mode 100755
diff --git a/README.md b/README.md
old mode 100644
new mode 100755
diff --git a/__init__.py b/__init__.py
old mode 100644
new mode 100755
diff --git a/docker_train.sh b/docker_train.sh
old mode 100644
new mode 100755
diff --git a/launch/open_manipulator_env.launch b/launch/open_manipulator_env.launch
index 038b450..26c0ef3 100755
--- a/launch/open_manipulator_env.launch
+++ b/launch/open_manipulator_env.launch
@@ -7,10 +7,12 @@
   <arg name="headless" default="false"/>
   <arg name="debug" default="false"/>
 
-  <!-- rviz & tf related args -->
-  <arg name="robot_name"     default="open_manipulator"/>
-  <arg name="open_rviz"      default="false" />
-  <arg name="use_gui"        default="false" />
+  <!-- robot URDF parse args -->
+  <arg name="om_urdf" value="robot_description"/>
+  <arg name="urdf_param" default="/robot_description"/>
+  <param name="$(arg om_urdf)" textfile="$(find kair_algorithms)/urdf/om.urdf"/>
+  <arg name="load_robot_description" default="false"/>
+
 
   <!-- gazebo related -->
   <rosparam file="$(find open_manipulator_gazebo)/config/gazebo_controller.yaml" command="load" />
@@ -23,28 +25,21 @@
     <arg name="headless" value="$(arg headless)"/>
   </include>
 
-  <!-- rviz related -->
-    <!-- Send joint values -->
-    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
-      <param name="/use_gui" value="$(arg use_gui)"/>
-      <rosparam param="source_list" subst_value="true">["$(arg robot_name)/joint_states"]</rosparam>
-    </node>
-    <!-- Combine joint values to TF-->
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
+  <!--KDL chain related args-->
+  <param if="$(arg load_robot_description)" name="$(arg urdf_param)" command="$(find xacro)/xacro --inorder $(find kair_algorithms)/urdf/om.urdf"/>
 
-    <!-- Show in Rviz -->
-    <group if="$(arg open_rviz)">
-      <node name="rviz" pkg="rviz" type="rviz" args="-d $(find open_manipulator_description)/rviz/open_manipulator.rviz"/>
-    </group>
 
-    <!-- Load the URDF into the ROS Parameter Server -->
+
+
+
+  <!-- Load the URDF into the ROS Parameter Server -->
     <param name="robot_description"
     command="$(find xacro)/xacro --inorder '$(find open_manipulator_description)/urdf/open_manipulator.urdf.xacro'"/>
 
-    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
     <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
      args="-urdf -model open_manipulator -z 0.0 -param robot_description"/>
 
-    <!-- ros_control robotis manipulator launch file -->
+  <!-- ros_control robotis manipulator launch file -->
     <include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch"/>   
 </launch>
diff --git a/msg/GoalObs.msg b/msg/GoalObs.msg
old mode 100644
new mode 100755
diff --git a/package.xml b/package.xml
old mode 100644
new mode 100755
diff --git a/scripts/.isort.cfg b/scripts/.isort.cfg
old mode 100644
new mode 100755
diff --git a/scripts/__init__.py b/scripts/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/__init__.py b/scripts/algorithms/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/__init__.py b/scripts/algorithms/common/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/abstract/__init__.py b/scripts/algorithms/common/abstract/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/__init__.py b/scripts/algorithms/common/buffer/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/priortized_replay_buffer.py b/scripts/algorithms/common/buffer/priortized_replay_buffer.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/replay_buffer.py b/scripts/algorithms/common/buffer/replay_buffer.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/segment_tree.py b/scripts/algorithms/common/buffer/segment_tree.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/helper_functions.py b/scripts/algorithms/common/helper_functions.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/networks/__init__.py b/scripts/algorithms/common/networks/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/networks/lstm.py b/scripts/algorithms/common/networks/lstm.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/networks/mlp.py b/scripts/algorithms/common/networks/mlp.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/noise.py b/scripts/algorithms/common/noise.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/fd/__init__.py b/scripts/algorithms/fd/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/fd/sac_agent.py b/scripts/algorithms/fd/sac_agent.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/fd/td3_agent.py b/scripts/algorithms/fd/td3_agent.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/sac/__init__.py b/scripts/algorithms/sac/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/sac/agent.py b/scripts/algorithms/sac/agent.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/td3/__init__.py b/scripts/algorithms/td3/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/td3/agent.py b/scripts/algorithms/td3/agent.py
old mode 100644
new mode 100755
diff --git a/scripts/config/__init__.py b/scripts/config/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/__init__.py b/scripts/config/agent/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/__init__.py b/scripts/config/agent/lunarlander_continuous_v2/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/sac.py b/scripts/config/agent/lunarlander_continuous_v2/sac.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/sacfd.py b/scripts/config/agent/lunarlander_continuous_v2/sacfd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/td3.py b/scripts/config/agent/lunarlander_continuous_v2/td3.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/td3fd.py b/scripts/config/agent/lunarlander_continuous_v2/td3fd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/__init__.py b/scripts/config/agent/reacher-v1/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/sac.py b/scripts/config/agent/reacher-v1/sac.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/sacfd.py b/scripts/config/agent/reacher-v1/sacfd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/td3.py b/scripts/config/agent/reacher-v1/td3.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/td3fd.py b/scripts/config/agent/reacher-v1/td3fd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/environment/__init__.py b/scripts/config/environment/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/data/lunarlander_continuous_demo.pkl b/scripts/data/lunarlander_continuous_demo.pkl
old mode 100644
new mode 100755
diff --git a/scripts/data/reacher_demo.pkl b/scripts/data/reacher_demo.pkl
old mode 100644
new mode 100755
diff --git a/scripts/envs/__init__.py b/scripts/envs/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/envs/open_manipulator/__init__.py b/scripts/envs/open_manipulator/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py
index cd3978f..34909f7 100755
--- a/scripts/envs/open_manipulator/ros_interface.py
+++ b/scripts/envs/open_manipulator/ros_interface.py
@@ -11,7 +11,9 @@ import rospkg  # noqa
 import rospy  # noqa
 import tf  # noqa
 import tf.transformations as tr  # noqa
-from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel
+from urdf_parser_py.urdf import URDF  # noqa
+from pykdl_utils.kdl_kinematics import KDLKinematics
+from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel # noqa
 from geometry_msgs.msg import Pose
 from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState
 from sensor_msgs.msg import JointState
@@ -37,6 +39,7 @@ class OpenManipulatorRosBaseInterface(object):
         self.termination_count = 0
         self.success_count = 0
 
+        self.init_fk_solver()
         self.init_tf_transformer()
         self.init_publish_node()
         self.init_subscribe_node()
@@ -116,6 +119,10 @@ class OpenManipulatorRosBaseInterface(object):
         self.pub_joint3_position.publish(np.random.uniform(0.0, 0.0))
         self.pub_joint4_position.publish(np.random.uniform(0.0, 0.0))
 
+    def init_fk_solver(self):
+        self.robot = URDF.from_parameter_server()
+        self.solve_fk = KDLKinematics(self.robot, "link1", "end_effector_link")
+
     def joint_state_callback(self, msg):
         """Callback function of joint states subscriber.
 
@@ -129,19 +136,16 @@ class OpenManipulatorRosBaseInterface(object):
         self.joint_efforts = joints_states.effort
         # penalize jerky motion in reward for shaped reward setting.
         self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
-        try:
-            (
-                self._gripper_position,
-                self._gripper_orientation,
-            ) = self.tf_listenser.lookupTransform(
-                "/world", "/end_effector_link", rospy.Time(0)
-            )
-        except (
-            tf.LookupException,
-            tf.ConnectivityException,
-            tf.ExtrapolationException,
-        ):
-            pass
+        _fk_mat = np.array(self.solve_fk.forward(self.joint_positions[:4]))
+        self._gripper_position = _fk_mat[0:3, 3]
+        self._gripper_orientation[3] = (1 + _fk_mat[0, 0] +
+                                        _fk_mat[1, 1] + _fk_mat[2, 2])**0.5
+        self._gripper_orientation[0] = (
+            _fk_mat[2, 1] - _fk_mat[1, 2]) / (4 * self._gripper_orientation[3])
+        self._gripper_orientation[1] = (
+            _fk_mat[0, 2] - _fk_mat[2, 0]) / (4 * self._gripper_orientation[3])
+        self._gripper_orientation[2] = (
+            _fk_mat[1, 0] - _fk_mat[0, 1]) / (4 * self._gripper_orientation[3])                                       
 
     def kinematics_pose_callback(self, msg):
         """Callback function of gripper kinematic pose subscriber.
@@ -258,7 +262,7 @@ class OpenManipulatorRosBaseInterface(object):
             )
         else:
             raise ValueError("Control mode %s is not known!" % control_mode)
-        print (lower_bounds, upper_bounds, self.cfg["ACTION_DIM"])
+        print(lower_bounds, upper_bounds, self.cfg["ACTION_DIM"])
         return gym.spaces.Box(low=lower_bounds, high=upper_bounds, dtype=np.float32)
 
     def get_observation_space(self):
diff --git a/scripts/requirements-dev.txt b/scripts/requirements-dev.txt
old mode 100644
new mode 100755
diff --git a/scripts/requirements.txt b/scripts/requirements.txt
old mode 100644
new mode 100755
diff --git a/scripts/run_lunarlander_continuous.py b/scripts/run_lunarlander_continuous.py
old mode 100644
new mode 100755
diff --git a/scripts/run_reacher_v1.py b/scripts/run_reacher_v1.py
old mode 100644
new mode 100755
diff --git a/scripts/test_lstm.py b/scripts/test_lstm.py
old mode 100644
new mode 100755
diff --git a/urdf/om.urdf b/urdf/om.urdf
new file mode 100755
index 0000000..6367e64
--- /dev/null
+++ b/urdf/om.urdf
@@ -0,0 +1,308 @@
+<?xml version="1.0" ?>
+  <robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
+    <material name="black">
+      <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+    <material name="white">
+      <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+    <material name="red">
+      <color rgba="0.8 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="grey">
+      <color rgba="0.5 0.5 0.5 1.0"/>
+    </material>
+    <material name="orange">
+      <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+    </material>
+    <material name="brown">
+      <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+    </material>
+    <!-- World -->
+    <link name="world">
+    </link>
+    <!-- World fixed joint-->
+    <joint name="world_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <parent link="world"/>
+      <child link="link1"/>
+    </joint>
+    <!-- Link 1 -->
+    <link name="link1">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.082" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04"/>
+        <mass value="7.9119962e-02"/>
+        <inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07" iyy="2.1898364e-05" iyz="0.0" izz="1.9267361e-05"/>
+      </inertial>
+    </link>
+    <!-- Joint 1 -->
+    <joint name="joint1" type="revolute">
+      <parent link="link1"/>
+      <child link="link2"/>
+      <origin rpy="0 0 0" xyz="0.012 0.0 0.017"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="1" lower="-2.82743338823" upper="2.82743338823" velocity="4.8"/>
+    </joint>
+    <!--  Link 2 -->
+    <link name="link2">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0.019"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0.019"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.098" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="-3.0184870e-04 5.4043684e-04 0.047433464"/>
+        <mass value="9.8406837e-02"/>
+        <inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07" iyy="3.2689329e-05" iyz="2.8511935e-08" izz="1.8850320e-05"/>
+      </inertial>
+    </link>
+    <!--  Joint 2 -->
+    <joint name="joint2" type="revolute">
+      <parent link="link2"/>
+      <child link="link3"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0595"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-1.79070781255" upper="1.57079632679" velocity="4.8"/>
+    </joint>
+    <!--  Link 3 -->
+    <link name="link3">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.136" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01"/>
+        <mass value="1.3850917e-01"/>
+        <inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05" iyy="3.4290447e-04" iyz="-1.5717516e-06" izz="6.0346498e-05"/>
+      </inertial>
+    </link>
+    <!--  Joint 3 -->
+    <joint name="joint3" type="revolute">
+      <parent link="link3"/>
+      <child link="link4"/>
+      <origin rpy="0 0 0" xyz="0.024 0 0.128"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-0.942477796077" upper="1.38230076758" velocity="4.8"/>
+    </joint>
+
+    <!--  Link 4 -->
+    <link name="link4">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.131" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04"/>
+        <mass value="1.3274562e-01"/>
+        <inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07" iyy="2.4230292e-04" iyz="1.1559550e-08" izz="2.5155057e-04"/>
+      </inertial>
+    </link>
+    <!--  Joint 4 -->
+    <joint name="joint4" type="revolute">
+      <parent link="link4"/>
+      <child link="link5"/>
+      <origin rpy="0 0 0" xyz="0.124 0.0 0.0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-1.79070781255" upper="2.04203522483" velocity="4.8"/>
+    </joint>
+
+    <!--  Link 5 -->
+    <link name="link5">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.141" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03"/>
+        <mass value="1.4327573e-01"/>
+        <inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06" iyy="7.5980465e-05" iyz="0.0" izz="9.3127351e-05"/>
+      </inertial>
+    </link>
+    <!--  Gripper link -->
+    <link name="gripper_link">
+      <visual>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0 0 0"/>
+        <mass value="0.017"/>
+        <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03"/>
+      </inertial>
+      <!--     <inertial>
+        <origin xyz="${0.028 + 8.3720668e-03} ${0.0246 + 9.9696160e-03} -4.2836895e-07" />
+        <mass value="3.2218127e-02" />
+        <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+                 iyy="2.2552871e-05" iyz="-3.1463634e-10"
+                 izz="1.7605306e-05" />
+      </inertial>-->
+    </link>
+    <!--  Gripper joint -->
+    <joint name="gripper" type="prismatic">
+      <parent link="link5"/>
+      <child link="gripper_link"/>
+      <origin rpy="0 0 0" xyz="0.0817 0.021 0.0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-0.010" upper="0.019" velocity="4.8"/>
+    </joint>
+
+    <!--  Gripper link sub -->
+    <link name="gripper_link_sub">
+      <visual>
+        <origin rpy="0 0 0" xyz="0.0 -0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0 -0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0 0 0"/>
+        <mass value="0.017"/>
+        <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03"/>
+      </inertial>
+      <!--     <inertial>
+        <origin xyz="${0.028 + 8.3720668e-03} ${-0.0246 - 9.9696160e-03} -4.2836895e-07" />
+        <mass value="3.2218127e-02" />
+        <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+                 iyy="2.2552871e-05" iyz="-3.1463634e-10"
+                 izz="1.7605306e-05" />
+      </inertial>-->
+    </link>
+    <!--  Gripper joint sub -->
+    <joint name="gripper_sub" type="prismatic">
+      <parent link="link5"/>
+      <child link="gripper_link_sub"/>
+      <origin rpy="0 0 0" xyz="0.0817 -0.021 0"/>
+      <axis xyz="0 -1 0"/>
+      <limit effort="1" lower="-0.010" upper="0.019" velocity="4.8"/>
+      <mimic joint="gripper" multiplier="1"/>
+    </joint>
+
+    <!-- end effector joint -->
+    <joint name="end_effector_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0.126 0.0 0.0"/>
+      <parent link="link5"/>
+      <child link="end_effector_link"/>
+    </joint>
+    <!-- end effector link -->
+    <link name="end_effector_link">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <box size="0.01 0.01 0.01"/>
+        </geometry>
+        <material name="red"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+        <mass value="0.001"/>
+        <inertia ixx="1.0e-06" ixy="0.0" ixz="0.0" iyy="1.0e-06" iyz="0.0" izz="1.0e-06"/>
+      </inertial>
+    </link>
+  </robot>
+  
+