Merge branch 'master' into feat/demo_refactoring

2026-09-09 11:25:10 +08:00 · 2019-08-17 14:21:55 +09:00
parent 2127ec5330 055d97661e
commit 4c300129d6
15 changed files with 745 additions and 24 deletions
@@ -254,4 +254,7 @@ wandb
 save

 # pycharm
-.idea
+.idea
+
+# vscode
+.vscode
@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- gazebo related args -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" 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/open_manipulator_cam.urdf.xacro"/>
+  <arg name="load_robot_description" default="false"/>
+
+  <!-- gazebo related -->
+  <rosparam file="$(find open_manipulator_gazebo)/config/gazebo_controller.yaml" command="load" />
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find open_manipulator_gazebo)/worlds/empty.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+  <!--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"/>
+
+  <!-- Load the URDF into the ROS Parameter Server -->
+    <param name="robot_description"
+    command="$(find xacro)/xacro --inorder '$(find kair_algorithms)/urdf/open_manipulator_cam.urdf.xacro'"/>
+
+
+  <!-- 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 -->
+    <include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch"/>   
+</launch>
@@ -0,0 +1,83 @@
+# -*- coding: utf-8 -*-
+"""Abstract class used for Hindsight Experience Replay.
+- Author: Kh Kim
+- Contact: kh.kim@medipixel.io
+- Paper: https://arxiv.org/pdf/1707.01495.pdf
+"""
+
+from abc import ABCMeta, abstractmethod
+
+import numpy as np
+
+
+class HER(object):
+    """Abstract class for HER (final strategy).
+    Attributes:
+        reward_func (Callable): returns reward from state, action, next_state
+    """
+
+    __metaclass__ = ABCMeta
+
+    def __init__(self, reward_func):
+        """Initialization.
+
+        Args:
+            reward_func (Callable): returns reward from state, action, next_state
+        """
+        self.reward_func = reward_func()
+
+    @abstractmethod
+    def fetch_desired_states_from_demo(self, demo):
+        pass
+
+    @abstractmethod
+    def get_desired_state(self, *args):
+        pass
+
+    @abstractmethod
+    def generate_demo_transitions(self, demo):
+        pass
+
+    @abstractmethod
+    def _get_final_state(self, transition):
+        pass
+
+    def _append_origin_transitions(self, origin_transitions, transition, desired_state):
+        """Append original transitions adding goal state for training."""
+        origin_transitions.append(self._get_transition(transition, desired_state))
+
+    def _append_new_transitions(self, new_transitions, transition, final_state):
+        """Append new transitions made by HER strategy (final) for training."""
+        new_transitions.append(self._get_transition(transition, final_state))
+
+    def _get_transition(self, transition, goal_state):
+        """Get a single transition concatenated with a goal state."""
+        state, action, _, next_state, done = transition
+
+        done = np.array_equal(next_state, goal_state)
+        reward = self.reward_func(transition, goal_state)
+        state = np.concatenate((state, goal_state), axis=-1)
+        next_state = np.concatenate((next_state, goal_state), axis=-1)
+
+        return state, action, reward, next_state, done
+
+    def generate_transitions(
+        self, transitions, desired_state, success_score, is_demo=False
+    ):
+        """Generate new transitions concatenated with desired states."""
+        origin_transitions = list()
+        new_transitions = list()
+        final_state = self._get_final_state(transitions[-1])
+        score = np.sum(np.array(transitions), axis=0)[2]
+
+        for transition in transitions:
+            # process transitions with the initial goal state
+            self._append_origin_transitions(
+                origin_transitions, transition, desired_state
+            )
+
+            # do not need to append new transitions if sum of reward is big enough
+            if not is_demo and score <= success_score:
+                self._append_new_transitions(new_transitions, transition, final_state)
+
+        return origin_transitions + new_transitions
@@ -0,0 +1,19 @@
+# -*- coding: utf-8 -*-
+"""Abstract class for computing reward.
+- Author: Kh Kim
+- Contact: kh.kim@medipixel.io
+"""
+
+from abc import ABCMeta, abstractmethod
+
+
+class RewardFn(object):
+    """Abstract class for computing reward.
+       New compute_reward class should redefine __call__()
+    """
+
+    __metaclass__ = ABCMeta
+
+    @abstractmethod
+    def __call__(self, transition, goal_state):
+        pass
@@ -8,6 +8,7 @@
 """

 import os
+import pickle

 import numpy as np
 import torch
@@ -44,10 +45,11 @@ class Agent(AbstractAgent):
        total_step (int): total step numbers
        episode_step (int): step number of the current episode
        i_episode (int): current episode number
+        her (HER): hinsight experience replay

    """

-    def __init__(self, env, args, hyper_params, models, optims, target_entropy):
+    def __init__(self, env, args, hyper_params, models, optims, target_entropy, her):
        """Initialization.

        Args:
@@ -57,6 +59,7 @@ class Agent(AbstractAgent):
            models (tuple): models including actor and critic
            optims (tuple): optimizers for actor and critic
            target_entropy (float): target entropy for the inequality constraint
+            her (HER): hinsight experience replay

        """
        AbstractAgent.__init__(self, env, args)
@@ -69,6 +72,7 @@ class Agent(AbstractAgent):
        self.total_step = 0
        self.episode_step = 0
        self.i_episode = 0
+        self.her = her

        # automatic entropy tuning
        if self.hyper_params["AUTO_ENTROPY_TUNING"]:
@@ -92,6 +96,50 @@ class Agent(AbstractAgent):
                self.hyper_params["BUFFER_SIZE"], self.hyper_params["BATCH_SIZE"]
            )

+        # HER
+        if self.hyper_params["USE_HER"]:
+            # load demo replay memory
+            with open(self.args.demo_path, "rb") as f:
+                demo = pickle.load(f)
+
+            if self.hyper_params["DESIRED_STATES_FROM_DEMO"]:
+                self.her.fetch_desired_states_from_demo(demo)
+
+            self.transitions_epi = list()
+            self.desired_state = np.zeros((1,))
+            demo = self.her.generate_demo_transitions(demo)
+
+        if not self.args.test:
+            # Replay buffers
+            self.memory = ReplayBuffer(
+                self.hyper_params["BUFFER_SIZE"], self.hyper_params["BATCH_SIZE"]
+            )
+
+    def _preprocess_state(self, state):
+        """Preprocess state so that actor selects an action."""
+        if self.hyper_params["USE_HER"]:
+            self.desired_state = self.her.get_desired_state()
+            state = np.concatenate((state, self.desired_state), axis=-1)
+        state = torch.FloatTensor(state).to(device)
+        return state
+
+    def _add_transition_to_memory(self, transition):
+        """Add 1 step and n step transitions to memory."""
+        if self.hyper_params["USE_HER"]:
+            self.transitions_epi.append(transition)
+            done = transition[-1] or self.episode_step == self.args.max_episode_steps
+            if done:
+                # insert generated transitions if the episode is done
+                transitions = self.her.generate_transitions(
+                    self.transitions_epi,
+                    self.desired_state,
+                    self.hyper_params["SUCCESS_SCORE"],
+                )
+                self.memory.extend(transitions)
+                self.transitions_epi = list()
+        else:
+            self.memory.add(*transition)
+
    def select_action(self, state):
        """Select an action from the input space."""
        self.curr_state = state
@@ -111,11 +159,6 @@ class Agent(AbstractAgent):

        return selected_action.detach().cpu().numpy()

-    def _preprocess_state(self, state):
-        """Preprocess state so that actor selects an action."""
-        state = torch.FloatTensor(state).to(device)
-        return state
-
    def step(self, action):
        """Take an action and return the response of the env."""
        self.total_step += 1
@@ -133,10 +176,6 @@ class Agent(AbstractAgent):

        return next_state, reward, done

-    def _add_transition_to_memory(self, transition):
-        """Add 1 step and n step transitions to memory."""
-        self.memory.add(*transition)
-
    def update_model(self, experiences):
        """Train the model after each episode."""
        states, actions, rewards, next_states, dones = experiences
@@ -304,6 +343,7 @@ class Agent(AbstractAgent):
        if self.args.log:
            wandb.init()
            wandb.config.update(self.hyper_params)
+            wandb.config.update(vars(self.args))
            wandb.watch([self.actor, self.vf, self.qf_1, self.qf_2], log="parameters")

        for self.i_episode in range(1, self.args.episode_num + 1):
@@ -234,6 +234,7 @@ class Agent(AbstractAgent):
        if self.args.log:
            wandb.init()
            wandb.config.update(self.hyper_params)
+            wandb.config.update(vars(self.args))
            wandb.watch([self.actor, self.critic1, self.critic2], log="parameters")

        for i_episode in range(1, self.args.episode_num + 1):
@@ -8,6 +8,7 @@
 import numpy as np
 import torch
 import torch.optim as optim
+from config.agent.lunarlander_continuous_v2.utils import LunarLanderContinuousHER

 from algorithms.common.networks.mlp import MLP, FlattenMLP, TanhGaussianDistParams
 from algorithms.sac.agent import Agent
@@ -38,6 +39,10 @@ hyper_params = {
        "VF_HIDDEN_SIZES": [256, 256],
        "QF_HIDDEN_SIZES": [256, 256],
    },
+    # HER
+    "USE_HER": True,
+    "SUCCESS_SCORE": 250.0,
+    "DESIRED_STATES_FROM_DEMO": True,
 }


@@ -52,6 +57,9 @@ def get(env, args):
    state_dim = env.observation_space.shape[0]
    action_dim = env.action_space.shape[0]

+    if hyper_params["USE_HER"]:
+        state_dim *= 2
+
    hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
    hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
    hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
@@ -107,5 +115,8 @@ def get(env, args):
    models = (actor, vf, vf_target, qf_1, qf_2)
    optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)

+    # HER
+    her = LunarLanderContinuousHER() if hyper_params["USE_HER"] else None
+
    # create an agent
-    return Agent(env, args, hyper_params, models, optims, target_entropy)
+    return Agent(env, args, hyper_params, models, optims, target_entropy, her)
@@ -0,0 +1,67 @@
+# -*- coding: utf-8 -*-
+"""Utils for examples on LunarLanderContinuous-v2.
+- Author: Kh Kim
+- Contact: kh.kim@medipixel.io
+"""
+
+import numpy as np
+
+from algorithms.common.abstract.her import HER
+from algorithms.common.abstract.reward_fn import RewardFn
+
+
+class L1DistanceRewardFn(RewardFn):
+    def __call__(self, transition, goal_state):
+        """L1 Distance reward function."""
+        next_state = transition[3]
+        eps = 1e-6
+        if np.abs(next_state - goal_state).sum() < eps:
+            return np.float64(0.0)
+        else:
+            return np.float64(-1.0)
+
+
+class LunarLanderContinuousHER(HER):
+    """HER for LunarLanderContinuous-v2 environment.
+    Attributes:
+        demo_goal_indices (np.ndarray): indices about goal of demo list
+        desired_states (np.ndarray): desired states from demonstration
+    """
+
+    def __init__(self, reward_func=L1DistanceRewardFn):
+        """Initialization."""
+        HER.__init__(self, reward_func=reward_func)
+
+    # pylint: disable=attribute-defined-outside-init
+    def fetch_desired_states_from_demo(self, demo):
+        """Return desired goal states from demonstration data."""
+        np_demo = np.array(demo)
+        self.demo_goal_indices = np.where(np_demo[:, 4])[0]
+        self.desired_states = np_demo[self.demo_goal_indices][:, 0]
+
+    def get_desired_state(self, *args):
+        """Sample one of the desired states."""
+        return np.random.choice(self.desired_states, 1).item()
+
+    def _get_final_state(self, transition):
+        """Get final state from transitions for making HER transitions."""
+        return transition[0]
+
+    def generate_demo_transitions(self, demo):
+        """Return generated demo transitions for HER."""
+        new_demo = list()
+
+        # generate demo transitions
+        prev_idx = 0
+        for idx in self.demo_goal_indices:
+            demo_final_state = self._get_final_state(demo[idx])
+            transitions = [demo[i] for i in range(prev_idx, idx + 1)]
+            prev_idx = idx + 1
+
+            transitions = self.generate_transitions(
+                transitions, demo_final_state, 0, is_demo=True
+            )
+
+            new_demo.extend(transitions)
+
+        return new_demo
@@ -2,7 +2,6 @@ from math import pi

 from geometry_msgs.msg import Quaternion

-
 config = {
    "ENV_NAME": "OpenManipulatorReacher",
    "MAX_EPISODE_STEPS": 100,
@@ -1,9 +1,9 @@
 #! usr/bin/env python

-import numpy as np
-
 import gym
+import numpy as np
 from gym.utils import seeding
+
 from ros_interface import (
    OpenManipulatorRosGazeboInterface,
    OpenManipulatorRosRealInterface,
@@ -78,16 +78,15 @@ class OpenManipulatorReacherEnv(gym.Env):
            # TODO: Add termination condition
            # if self.ros_interface.check_for_termination():
            #     self.done = True
-            if self.ros_interface.check_for_success():
+            if (
+                self.ros_interface.check_for_success()
+                or self.episode_steps == self._max_episode_steps
+            ):
                self.done = True
                self.episode_steps = 0

        obs = self.ros_interface.get_observation()

-        if self.episode_steps == self._max_episode_steps:
-            self.done = False
-            self.episode_steps = 0
-
        return obs, self.reward_rescale_ratio * self.reward, self.done, None

    def reset(self):
@@ -6,6 +6,8 @@ from math import cos, sin

 import gym
 import numpy as np
+
+import rospkg  # noqa
 import rospy  # noqa
 import tf  # noqa
 import tf.transformations as tr  # noqa
@@ -16,8 +18,6 @@ from sensor_msgs.msg import JointState
 from std_msgs.msg import Float64
 from urdf_parser_py.urdf import URDF  # noqa

-import rospkg  # noqa
-

 class OpenManipulatorRosBaseInterface(object):
    """Open Manipulator Interface based on ROS."""
@@ -3,9 +3,9 @@
 from math import cos, pi, sin

 import numpy as np
+from config.environment.open_manipulator import config as cfg

 import rospy
-from config.environment.open_manipulator import config as cfg
 from envs.open_manipulator import OpenManipulatorReacherEnv
 from geometry_msgs.msg import Pose, Quaternion
 from open_manipulator_msgs.msg import JointPosition, KinematicsPose
@@ -10,8 +10,9 @@
 import argparse
 import importlib

-import algorithms.common.helper_functions as common_utils
 from config.environment.open_manipulator import config as env_cfg
+
+import algorithms.common.helper_functions as common_utils
 from envs.open_manipulator.open_manipulator_reacher_env import OpenManipulatorReacherEnv

 # configurations
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
+  
+    <xacro:property name="pi" value="3.1415926535897931"/>
+
+        
+    <xacro:macro name="camera_sensor" params="xyz rpy parent">
+        <joint name="camera_sensor_joint" type="fixed">
+            <axis xyz="0 1 0" />
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+            <parent link="${parent}"/>
+            <child link="camera_link"/>
+        </joint>
+        
+        <link name="camera_link">
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <box size="0.02 0.08 0.05"/>
+                </geometry>
+            </collision>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <box size="0.02 0.08 0.05"/>
+                </geometry>
+                <material name="iRobot/Green"/>
+            </visual>
+            <inertial>
+                <mass value="0.0001" />
+                <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+                <inertia ixx="0.0000001" ixy="0" ixz="0" iyy="0.0000001" iyz="0" izz="0.0000001" />
+            </inertial>
+        </link>
+        
+        
+        
+        <gazebo reference="camera_link">
+            <sensor type="camera" name="camera">
+                <update_rate>30.0</update_rate>
+                <camera name="head">
+                    <horizontal_fov>1.3962634</horizontal_fov>
+                    <image>
+                        <width>800</width>
+                        <height>600</height>
+                        <format>R8G8B8</format>
+                    </image>
+                    <clip>
+                        <near>0.02</near>
+                        <far>300</far>
+                    </clip>
+                    <noise>
+                        <type>gaussian</type>
+                        <mean>0.0</mean>
+                        <stddev>0.007</stddev>
+                    </noise>
+                </camera>
+                <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+                    <alwaysOn>true</alwaysOn>
+                    <updateRate>0.0</updateRate>
+                    <cameraName>camera</cameraName>
+                    <imageTopicName>image_raw</imageTopicName>
+                    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+                    <frameName>camera_link</frameName>
+                    <hackBaseline>0.07</hackBaseline>
+                    <distortionK1>0.0</distortionK1>
+                    <distortionK2>0.0</distortionK2>
+                    <distortionK3>0.0</distortionK3>
+                    <distortionT1>0.0</distortionT1>
+                    <distortionT2>0.0</distortionT2>
+                </plugin>
+            </sensor>
+    </gazebo>
+    
+    </xacro:macro>
+    
+</robot>
@@ -0,0 +1,380 @@
+<?xml version="1.0"?>
+<!-- Open_Manipulator Chain -->
+<robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
+  <xacro:include filename="$(find open_manipulator_description)/urdf/open_manipulator.gazebo.xacro" />
+  <!-- Import Rviz colors -->
+  <xacro:include filename="$(find open_manipulator_description)/urdf/materials.xacro" />
+  <xacro:include filename="$(find kair_algorithms)/urdf/camera.urdf.xacro"/>
+
+
+  <!-- Transmission macro -->
+  <xacro:macro name="SimpleTransmission" params="joint n">
+    <transmission name="tran${n}">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${joint}">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="motor${n}">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+  <!-- World -->
+  <link name="world">
+  </link>
+
+  <!-- World fixed joint-->
+  <joint name="world_fixed" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="world"/>
+    <child link="link1"/>
+  </joint>
+
+  <!-- Link 1 -->
+  <link name="link1">
+    <visual>
+      <origin xyz="0 0 0" rpy="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 xyz="0 0 0" rpy="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 xyz="0.012 0.0 0.017" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.9}" upper="${pi*0.9}" />
+  </joint>
+
+  <!-- Transmission 1 -->
+  <xacro:SimpleTransmission n="1" joint="joint1" />
+
+  <!--  Link 2 -->
+  <link name="link2">
+    <visual>
+      <origin xyz="0 0 0.019" rpy="0 0 0"/>
+      <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 xyz="0 0 0.019" rpy="0 0 0"/>
+      <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.018 + 2.9433464e-02}" />
+      <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 xyz="0.0 0.0 0.0595" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.5}" />
+  </joint>
+
+  <!-- Transmission 2 -->
+  <xacro:SimpleTransmission n="2" joint="joint2" />
+
+  <!--  Link 3 -->
+  <link name="link3">
+    <visual>
+      <origin xyz="0 0 0" rpy="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 xyz="0 0 0" rpy="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 xyz="0.024 0 0.128" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.3}" upper="${pi*0.44}" />
+  </joint>
+
+  <!-- Transmission 3 -->
+  <xacro:SimpleTransmission n="3" joint="joint3" />
+
+  <!--  Link 4 -->
+  <link name="link4">
+    <visual>
+      <origin xyz="0 0 0" rpy="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 xyz="0 0 0" rpy="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 xyz="0.124 0.0 0.0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.65}" />
+  </joint>
+
+  <!-- Transmission 4 -->
+  <xacro:SimpleTransmission n="4" joint="joint4" />
+
+  <!--  Link 5 -->
+  <link name="link5">
+    <visual>
+      <origin xyz="0 0 0" rpy="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 xyz="0 0 0" rpy="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 xyz="0.0 0.0 0" rpy="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 xyz="0.0 0.0 0" rpy="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 xyz="0.0817 0.021 0.0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
+  </joint>
+
+  <!-- Transmission 5 -->
+  <xacro:SimpleTransmission n="5" joint="gripper" />
+
+  <!--  Gripper link sub -->
+  <link name="gripper_link_sub">
+    <visual>
+      <origin xyz="0.0 -0.0 0" rpy="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 xyz="0.0 -0.0 0" rpy="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 xyz="0.0817 -0.021 0" rpy="0 0 0"/>
+    <axis xyz="0 -1 0"/>
+    <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
+    <mimic joint="gripper" multiplier="1"/>
+  </joint>
+
+  <!-- Transmission 6 -->
+  <xacro:SimpleTransmission n="6" joint="gripper_sub" />
+
+  <!-- end effector joint -->
+  <joint name="end_effector_joint" type="fixed">
+    <origin xyz="0.126 0.0 0.0" rpy="0 0 0"/>
+    <parent link="link5"/>
+    <child link="end_effector_link"/>
+  </joint>
+
+  <!-- end effector link -->
+  <link name="end_effector_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01" />
+      </geometry>
+      <material name="red"/>
+     </visual>
+
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="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>
+
+    <!--Camera link and plugin-->
+    <xacro:camera_sensor xyz="0.2 0 ${0.4 + 0.2}" 
+                  rpy="0 1.57 0"
+                  parent="world">
+    </xacro:camera_sensor>
+
+
+
+</robot>