Merge pull request #125 from elephantrobotics/280-ros-pump-camera

280 ros pump camera

mycobot_280/mycobot_280/CMakeLists.txt

@@ -32,6 +32,7 @@ catkin_install_python(PROGRAMS
   scripts/simple_gui.py
   scripts/follow_display_gripper.py
   scripts/slider_control_gripper.py
+  scripts/listen_real_gripper.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

mycobot_280/mycobot_280/config/mycobot.rviz

@@ -8,9 +8,10 @@ Panels:
         - /Status1
         - /RobotModel1
         - /TF1
-        - /Marker1
+        - /TF1/Frames1
+        - /TF1/Tree1
       Splitter Ratio: 0.5
-    Tree Height: 657
+    Tree Height: 609
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -246,7 +247,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.2028908729553223
+      Distance: 1.1026314496994019
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -262,17 +263,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.32539835572242737
+      Pitch: 0.7153984904289246
       Target Frame: <Fixed Frame>
-      Yaw: 3.0853891372680664
+      Yaw: 3.8853836059570312
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 954
+  Height: 906
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000031cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000031c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c20000031cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000031c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000031c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c2000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000400000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

mycobot_280/mycobot_280/launch/simple_gui_gripper.launch

@@ -20,6 +20,10 @@
 		<arg name="port" value="$(arg port)" />	
 		<arg name="baud" value="$(arg baud)" />	
     </include>
-	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+	<!-- <node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" /> -->
+	<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" output="screen">
+        <param name="port" value="$(arg port)" />
+        <param name="baud" value="$(arg baud)" />
+    </node>
 	<node name="simple_gui" pkg="mycobot_280" type="simple_gui.py" />
 </launch>

mycobot_280/mycobot_280/launch/simple_gui_pump.launch

@@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="false" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ,将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+	<node name="simple_gui" pkg="mycobot_280" type="simple_gui.py" />
+</launch>

mycobot_280/mycobot_280/launch/slider_control_camera_flange.launch

@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_camera_flange.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280/launch/slider_control_pump.launch

@@ -0,0 +1,24 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch

@@ -21,5 +21,9 @@
 		<arg name="baud" value="$(arg baud)" />	
     </include>
 	<!-- listen and pub the real angles ，监听并发布真实角度-->
-	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+	<!-- <node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" /> -->
+	<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" output="screen">
+        <param name="port" value="$(arg port)" />
+        <param name="baud" value="$(arg baud)" />
+    </node>
 </launch>

mycobot_280/mycobot_280/launch/teleop_keyboard_pump.launch

@@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="false" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ，将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<!-- listen and pub the real angles ，监听并发布真实角度-->
+	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+</launch>

mycobot_280/mycobot_280/launch/test.launch

@@ -1,5 +1,5 @@
 <launch>
-	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
 	<arg name="gui" default="true" />
 

mycobot_280/mycobot_280/launch/test_camera_flange.launch

@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_camera_flange.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280/launch/test_gripper.launch

@@ -0,0 +1,16 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_gripper_parallel.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280/launch/test_pump.launch

@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280/scripts/listen_real_gripper.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# license removed for brevity
+from distutils.log import error
+import time
+import math
+import os
+import fcntl
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from mycobot_communication.srv import GetAngles
+from pymycobot.mycobot import MyCobot
+from rospy import ServiceException
+
+mc = None
+
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+    open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+    fd = os.open(lock_file, open_mode)
+
+    pid = os.getpid()
+    lock_file_fd = None
+    
+    timeout = 50.0
+    start_time = current_time = time.time()
+    while current_time < start_time + timeout:
+        try:
+            # The LOCK_EX means that only one process can hold the lock
+            # The LOCK_NB means that the fcntl.flock() is not blocking
+            # and we are able to implement termination of while loop,
+            # when timeout is reached.
+            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+        except (IOError, OSError):
+            pass
+        else:
+            lock_file_fd = fd
+            break
+
+        # print('pid waiting for lock:%d'% pid)
+
+
+        time.sleep(1.0)
+        current_time = time.time()
+    if lock_file_fd is None:
+        os.close(fd)
+    return lock_file_fd
+
+
+def release(lock_file_fd):
+    # Do not remove the lockfile:
+    fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+    os.close(lock_file_fd)
+    return None
+
+def talker():
+    rospy.loginfo("start ...")
+    
+    rospy.init_node("real_listener_gripper", anonymous=True)
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    baud = rospy.get_param("~baud", 115200)
+    mc = MyCobot(port, baud)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state，发布关节状态
+    joint_state_send = JointState()
+    joint_state_send.header = Header()
+
+    joint_state_send.name = [
+        "joint2_to_joint1",
+        "joint3_to_joint2",
+        "joint4_to_joint3",
+        "joint5_to_joint4",
+        "joint6_to_joint5",
+        "joint6output_to_joint6",
+        "gripper_controller",
+    ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    # waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
+    rospy.loginfo("wait service")
+    rospy.wait_for_service("get_joint_angles")
+    
+    while True:
+        try:
+            func = rospy.ServiceProxy("get_joint_angles", GetAngles)
+            break
+        except ServiceException as e:
+            # pass
+            # print(f'error:{e}')
+            print("--------------error",e)
+    
+    # rospy.loginfo("start loop ...")
+    while not rospy.is_shutdown():
+        # get real angles from server.从服务器获得真实的角度。
+        res = func()
+        if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
+            continue
+        if mc:
+            lock = acquire("/tmp/mycobot_lock")
+            gripper_value = mc.get_gripper_value()
+            release(lock)
+        if gripper_value != -1:
+            gripper_value = -0.78 + round(gripper_value / 117.0, 2)
+            # print(gripper_value)            
+            radians_list = [
+                res.joint_1 * (math.pi / 180),
+                res.joint_2 * (math.pi / 180),
+                res.joint_3 * (math.pi / 180),
+                res.joint_4 * (math.pi / 180),
+                res.joint_5 * (math.pi / 180),
+                res.joint_6 * (math.pi / 180),
+            ]
+            radians_list.append(gripper_value)
+            # rospy.loginfo("res: {}".format(radians_list))
+
+            # publish angles.发布角度
+            joint_state_send.header.stamp = rospy.Time.now()
+            joint_state_send.position = radians_list
+            pub.publish(joint_state_send)
+            rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass

mycobot_280/mycobot_280/scripts/simple_gui.py

@@ -4,7 +4,7 @@ try:
     import tkinter as tk
 except ImportError:
     import Tkinter as tk
-from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
+from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus, PumpStatus
 import rospy
 import time
 from rospy import ServiceException
@@ -36,7 +36,7 @@ class Window:
         # 计算 Tk 根窗口的 x 和 y 坐标
         x = int((self.ws / 2) - 190)
         y = int((self.hs / 2) - 250)
-        self.win.geometry("430x400+{}+{}".format(x, y))
+        self.win.geometry("440x440+{}+{}".format(x, y))
         # layout,布局
         self.set_layout()
         # input section,输入部分
@@ -56,12 +56,19 @@ class Window:
 
         # Gripper switch button,夹爪开关按钮
         tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
-            row=1, column=0, sticky="w", padx=3, pady=50
+            row=1, column=0, sticky="w", padx=3, pady=20
         )
         tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid(
             row=1, column=1, sticky="w", padx=3, pady=2
         )
         
+        tk.Button(self.frmLB, text=" 吸泵(开)", command=self.pump_open, width=5).grid(
+            row=2, column=0, sticky="w", padx=3, pady=20
+        )
+        tk.Button(self.frmLB, text="吸泵(关)", command=self.pump_close, width=5).grid(
+            row=2, column=1, sticky="w", padx=3, pady=2
+        )
+
     def connect_ser(self):
         rospy.init_node("simple_gui", anonymous=True, disable_signals=True)
 
@@ -70,6 +77,7 @@ class Window:
         rospy.wait_for_service("get_joint_coords")
         rospy.wait_for_service("set_joint_coords")
         rospy.wait_for_service("switch_gripper_status")
+        rospy.wait_for_service("switch_pump_status")
         try:
             self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
             self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
@@ -78,6 +86,9 @@ class Window:
             self.switch_gripper = rospy.ServiceProxy(
                 "switch_gripper_status", GripperStatus
             )
+            self.switch_pump = rospy.ServiceProxy(
+                "switch_pump_status", PumpStatus
+            )
         except:
             print("start error ...")
             exit(1)
@@ -373,6 +384,20 @@ class Window:
         except ServiceException:
             pass
     
+    def pump_open(self):
+        try:
+            self.switch_pump(True,2, 5)
+        except ServiceException:
+            # Probably because the method has no return value, the service throws an unhandled error
+            # 可能由于该方法没有返回值，服务抛出无法处理的错误
+            pass
+
+    def pump_close(self):
+        try:
+            self.switch_pump(False, 2, 5)
+        except ServiceException:
+            pass
+
     def get_coord_input(self):
         # Get the data input by coord and send it to the robotic arm
         # 获取 coord 输入的数据，发送给机械臂

mycobot_280/mycobot_280/scripts/slider_control_gripper.py

@@ -30,7 +30,7 @@ def callback(data):
     mc.send_radians(data_list[:6], 80)
     gripper_value = int(abs(-0.7-data_list[6])* 117)
     print("gripper_value:%s"%gripper_value)
-    mc.set_gripper_value(gripper_value, 80)
+    mc.set_gripper_value(gripper_value, 80, 1)
     
 
 def listener():

mycobot_280/mycobot_280/scripts/teleop_keyboard.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from __future__ import print_function
-from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
+from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus, PumpStatus
 import rospy
 import sys
 import select
@@ -28,6 +28,10 @@ Gripper control:
     g - open
     h - close
 
+Pump control:
+    b - open
+    m - close
+
 Other:
     1 - Go to init pose
     2 - Go to home pose
@@ -68,6 +72,7 @@ def teleop_keyboard():
     rospy.wait_for_service("get_joint_coords")
     rospy.wait_for_service("set_joint_coords")
     rospy.wait_for_service("switch_gripper_status")
+    rospy.wait_for_service("switch_pump_status")
     print("service ready.")
     try:
         get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
@@ -76,6 +81,8 @@ def teleop_keyboard():
         set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
         switch_gripper = rospy.ServiceProxy(
             "switch_gripper_status", GripperStatus)
+        switch_pump = rospy.ServiceProxy(
+            "switch_pump_status", PumpStatus)
     except:
         print("start error ...")
         exit(1)
@@ -145,6 +152,10 @@ def teleop_keyboard():
                     switch_gripper(True)
                 elif key in ["h", "H"]:
                     switch_gripper(False)
+                elif key in ["b", "B"]:
+                    switch_pump(True, 2, 5)
+                elif key in ["m", "M"]:
+                    switch_pump(False, 2, 5)
                 elif key == "1":
                     rsp = set_angles(*init_pose)
                 elif key in "2":

mycobot_280/mycobot_280pi/config/mycobot.rviz

@@ -9,7 +9,7 @@ Panels:
         - /RobotModel1
         - /TF1
       Splitter Ratio: 0.5
-    Tree Height: 642
+    Tree Height: 609
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -18,17 +18,18 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -40,7 +41,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -62,6 +63,16 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        camera_flange:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        g_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         joint1:
           Alpha: 1
           Show Axes: false
@@ -105,9 +116,15 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
+        camera_flange:
+          Value: true
+        g_base:
+          Value: true
         joint1:
           Value: true
         joint2:
@@ -122,20 +139,23 @@ Visualization Manager:
           Value: true
         joint6_flange:
           Value: true
-      Marker Scale: 0.300000012
+      Marker Alpha: 1
+      Marker Scale: 0.10000000149011612
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        joint1:
+        g_base:
-          joint2:
+          joint1:
-            joint3:
+            joint2:
-              joint4:
+              joint3:
-                joint5:
+                joint4:
-                  joint6:
+                  joint5:
-                    joint6_flange:
+                    joint6:
-                      {}
+                      joint6_flange:
+                        camera_flange:
+                          {}
       Update Interval: 0
       Value: true
   Enabled: true
@@ -153,7 +173,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -163,33 +186,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.20289087
+      Distance: 1.0585439205169678
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.0706475973
+        X: -0.07064759731292725
-        Y: -0.0814988762
+        Y: -0.0814988762140274
-        Z: 0.107583851
+        Z: 0.10758385062217712
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
+      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.085398227
+      Pitch: 0.48539817333221436
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
+      Yaw: 4.103571891784668
-      Yaw: 3.08857274
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 923
+  Height: 906
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000311fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000311000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000311fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000311000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073b0000003efc0100000002fb0000000800540069006d006501000000000000073b0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003960000031100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000018c000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000436000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -198,6 +221,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1851
+  Width: 1848
-  X: 65
+  X: 72
-  Y: 24
+  Y: 27

mycobot_280/mycobot_280pi/launch/slider_control_camera_flange.launch

@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_with_camera_flange.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280pi)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280pi/launch/slider_control_pump.launch

@@ -0,0 +1,24 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280pi)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280pi/launch/test_camera_flange.launch

@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_with_camera_flange.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280pi)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_280/mycobot_280pi/launch/test_pump.launch

@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_with_pump.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280pi)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_communication/scripts/mycobot_services.py

@@ -145,11 +145,11 @@ def toggle_pump(req):
     if mc:
         lock = acquire("/tmp/mycobot_lock")
         if req.Status:
-            mc.set_basic_output(req.Pin1, 0)
+            mc.set_basic_output(2, 0)
-            mc.set_basic_output(req.Pin2, 0)
+            mc.set_basic_output(5, 0)
         else:
-            mc.set_basic_output(req.Pin1, 1)
+            mc.set_basic_output(2, 1)
-            mc.set_basic_output(req.Pin2, 1)
+            mc.set_basic_output(5, 1)
         release(lock)
 
 

mycobot_description/urdf/mycobot/mycobot_urdf.urdf

@@ -4,6 +4,21 @@
 <xacro:property name="width" value=".2" />
 
 
+  <link name="g_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
   <link name="joint1">
     <visual>
       <geometry>
@@ -125,7 +140,13 @@
   </link>
 
 
-
+  <joint name="g_base_to_joint1" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
 
 
   <joint name="joint2_to_joint1" type="revolute">

mycobot_description/urdf/mycobot/mycobot_with_camera_flange.urdf

@@ -0,0 +1,222 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="g_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+  <link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " -1.5708 0 0"/>
+    </visual>
+    <collision>
+       <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03356 " rpy = " -1.5708 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="camera_flange">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/camera_flange.dae"/>
+      </geometry>
+    <origin xyz = "0.024 -0.008 0.0 " rpy = " 0 3.14159 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/camera_flange.dae"/>
+      </geometry>
+    <origin xyz = "0.024 -0.008 0.0 " rpy = " 0 3.14159 0"/>
+    </collision>
+  </link>
+
+
+  <joint name="g_base_to_joint1" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13156" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0  0" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_camera_flange" type="fixed">
+    <!-- <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/> -->
+    <parent link="joint6_flange"/>
+    <child link="camera_flange"/>
+    <!-- <origin xyz= "0 0.0 0" rpy = "-1.5708 0 0"/>   -->
+    <origin xyz= "0 0 0.01" rpy = "1.579 0 0"/>  
+  </joint>
+
+</robot>
+

mycobot_description/urdf/mycobot/mycobot_with_pump.urdf

@@ -0,0 +1,250 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="g_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="pump_box">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/pump_box.dae"/>
+      </geometry>
+      <origin xyz = "0.0 -0.15 -0.0" rpy = "1.5708 3.14159 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot/pump_box.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " -1.5708 0 0"/>
+    </visual>
+    <collision>
+       <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03356 " rpy = " -1.5708 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="pump_head">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/pump_head.dae"/>
+      </geometry>
+    <origin xyz = "0.0 -0.008 0.0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/pump_head.dae"/>
+      </geometry>
+    <origin xyz = "0.0 -0.008 0.0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <joint name="g_base_to_joint1" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="g_base_to_pump_box" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="pump_box"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13156" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0  0" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.879793" upper = "2.879793" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_pump_head" type="fixed">
+    <!-- <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/> -->
+    <parent link="joint6_flange"/>
+    <child link="pump_head"/>
+    <!-- <origin xyz= "0 0.0 0" rpy = "-1.5708 0 0"/>   -->
+    <origin xyz= "0 0 0.01" rpy = "1.579 0 0"/>  
+  </joint>
+
+</robot>
+

mycobot_description/urdf/mycobot/mycobot_with_vision_copy.urdf

@@ -1,29 +0,0 @@
-<?xml version="1.0"?>
-<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="mycobot_ai" >
-
-  <xacro:property name="width" value=".2" />
-  <xacro:include filename="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf" />
-
-
-  <link name="env">
-    <visual>
-      <geometry>
-	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
-       <mesh filename="package://mycobot_description/urdf/mycobot/suit_env.dae"/>
-      </geometry>
-      <origin xyz = "0 0 -0.02 " rpy = "1.5708 0 -1.5708"/>
-    </visual>
-  </link>
-
-
-
-  <joint name="vision_joint" type="fixed">
-    <axis xyz="0 0 0"/>
-    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
-    <parent link="joint1"/>
-    <child link="env"/>
-    <origin xyz= "0 0 0" rpy = "0 0 0"/>
-  </joint>
-
-</robot>
-

mycobot_description/urdf/mycobot_pi/mycobot_urdf.urdf

@@ -4,6 +4,21 @@
 <xacro:property name="width" value=".2" />
 
 
+  <link name="g_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
   <link name="joint1">
     <visual>
       <geometry>
@@ -128,10 +143,16 @@
   </link>
 
 
+  <joint name="g_base_to_joint1" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
 
 
-
+  <joint name="joint2_to_joint1" type="revolute">
-    <joint name="joint2_to_joint1" type="revolute">
     <axis xyz="0 0 1"/>
     <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
     <parent link="joint1"/>

mycobot_description/urdf/mycobot_pi/mycobot_with_camera_flange.urdf

@@ -0,0 +1,234 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="g_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+      <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+      <collision>
+        <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+
+
+<link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="camera_flange">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/camera_flange.dae"/>
+      </geometry>
+    <origin xyz = "0.024 -0.008 0.0 " rpy = " 0 3.14159 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/camera_flange.dae"/>
+      </geometry>
+    <origin xyz = "0.024 -0.008 0.0 " rpy = " 0 3.14159 0"/>
+    </collision>
+  </link>
+
+
+
+  <joint name="g_base_to_joint1" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13956" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 -0.001" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_camera_flange" type="fixed">
+    <!-- <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/> -->
+    <parent link="joint6_flange"/>
+    <child link="camera_flange"/>
+    <!-- <origin xyz= "0 0.0 0" rpy = "-1.5708 0 0"/>   -->
+    <origin xyz= "0 0 0.01" rpy = "1.579 0 0"/>  
+  </joint>
+
+
+
+</robot>
+

mycobot_description/urdf/mycobot_pi/mycobot_with_pump.urdf

@@ -0,0 +1,259 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="g_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/G_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="pump_box">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/pump_box.dae"/>
+      </geometry>
+      <origin xyz = "0.0 -0.15 -0.0" rpy = "1.5708 3.14159 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/pump_box.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03" rpy = "0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+      <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+      <collision>
+        <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+
+
+<link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="pump_head">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/pump_head.dae"/>
+      </geometry>
+    <origin xyz = "0.0 -0.008 0.0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/pump_head.dae"/>
+      </geometry>
+    <origin xyz = "0.0 -0.008 0.0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+
+  <joint name="g_base_to_joint1" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="g_base_to_pump_box" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="g_base"/>
+    <child link="pump_box"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13956" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 -0.001" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_pump_head" type="fixed">
+    <!-- <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/> -->
+    <parent link="joint6_flange"/>
+    <child link="pump_head"/>
+    <!-- <origin xyz= "0 0.0 0" rpy = "-1.5708 0 0"/>   -->
+    <origin xyz= "0 0 0.01" rpy = "1.579 0 0"/>  
+  </joint>
+
+
+
+</robot>
+