Merge pull request #135 from elephantrobotics/mycobot600_gripper

Mycobot600 gripper

myArm/myarm_c650/scripts/debug.py

@@ -21,14 +21,15 @@ import moveit_commander
 from geometry_msgs.msg import Pose
 
 global mam
-mam = MyArmM('/dev/ttyACM1', debug=False)
+mam = MyArmC('/dev/ttyACM0', debug=False)
 angle = mam.get_joints_angle()
 print(angle)
-# mam.set_tool_led_color(255,255,255)
+# mam.set_tool_led_color(0,255,0)
 # for i in range(8):
 #     mam.set_servo_calibrate(i)
 #     time.sleep(0.5)
-
+# angle = mam.get_joints_angle()
+# print(angle)
 # def linear_transform(x):
 #     # 两个已知数据点
 #     x1, y1 = -89.5, 0.022

mycobot_280/mycobot_280/config/mycobot.rviz

@@ -11,7 +11,7 @@ Panels:
         - /TF1/Frames1
         - /TF1/Tree1
       Splitter Ratio: 0.5
-    Tree Height: 609
+    Tree Height: 637
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -65,57 +65,7 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        camera_flange:
+        base:
-          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
-          Show Trail: false
-          Value: true
-        joint2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        joint3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        joint4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        joint5:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        joint6:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        joint6_flange:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        pump_box:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        pump_head:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -132,48 +82,14 @@ Visualization Manager:
       Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
-        All Enabled: true
+        All Enabled: false
-        camera_flange:
-          Value: true
-        g_base:
-          Value: true
-        joint1:
-          Value: true
-        joint2:
-          Value: true
-        joint3:
-          Value: true
-        joint4:
-          Value: true
-        joint5:
-          Value: true
-        joint6:
-          Value: true
-        joint6_flange:
-          Value: true
-        pump_box:
-          Value: true
-        pump_head:
-          Value: true
       Marker Alpha: 1
-      Marker Scale: 0.10000000149011612
+      Marker Scale: 0.30000001192092896
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        g_base:
-          joint1:
-            joint2:
-              joint3:
-                joint4:
-                  joint5:
-                    joint6:
-                      joint6_flange:
-                        camera_flange:
-                          pump_head:
-                            {}
-          pump_box:
         {}
       Update Interval: 0
       Value: true
@@ -189,7 +105,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: joint1
+    Fixed Frame: base
     Frame Rate: 30
   Name: root
   Tools:
@@ -213,7 +129,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.0996487140655518
+      Distance: 1.204347848892212
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -221,25 +137,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.07064759731292725
+        X: -0.021931247785687447
-        Y: -0.0814988762140274
+        Y: -0.011295657604932785
-        Z: 0.10758385062217712
+        Z: -0.008990013971924782
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5753980875015259
+      Pitch: 0.6003984808921814
       Target Frame: <Fixed Frame>
-      Yaw: 3.950380325317383
+      Yaw: 1.0435491800308228
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 906
+  Height: 934
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c2000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000400000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000308fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000308000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c200000308fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000308000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003fe0000030800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -248,6 +164,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1848
+  Width: 1846
   X: 72
   Y: 27

mycobot_280/mycobot_280/launch/slider_control_gripper.launch

@@ -15,7 +15,7 @@
 	<!-- 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)" />	 -->
+		<param name="use_gui" value="$(arg gui)" />	
 		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
 	</node>
 	<!-- Open control script -->

mycobot_280/mycobot_280/launch/test.launch

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

mycobot_280/mycobot_280/scripts/follow_display.py

@@ -24,7 +24,7 @@ def talker():
     rospy.init_node("display", anonymous=True)
 
     print("Try connect real mycobot...")
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    port = rospy.get_param("~port", "/dev/ttyACM0")
     baud = rospy.get_param("~baud", 115200)
     print("port: {}, baud: {}\n".format(port, baud))
     try:
@@ -52,6 +52,7 @@ def talker():
     joint_state_send.header = Header()
 
     joint_state_send.name = [
+        
         "joint2_to_joint1",
         "joint3_to_joint2",
         "joint4_to_joint3",

mycobot_280/mycobot_280_moveit/config/firefighter.srdf

@@ -28,6 +28,7 @@
     </group_state>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="vitual_joint" type="fixed" parent_frame="basic" child_link="joint1" />
+
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="joint1" link2="joint2" reason="Adjacent" />
     <disable_collisions link1="joint1" link2="joint5" reason="Never" />
@@ -39,4 +40,6 @@
     <disable_collisions link1="joint5" link2="joint6" reason="Adjacent" />
     <disable_collisions link1="joint5" link2="joint6_flange" reason="Never" />
     <disable_collisions link1="joint6" link2="joint6_flange" reason="Adjacent" />
+
+    
 </robot>

mycobot_280/mycobot_280_moveit/launch/moveit.rviz

@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -9,8 +9,8 @@ Panels:
         - /MotionPlanning1/Scene Robot1
         - /MotionPlanning1/Planning Request1
         - /MotionPlanning1/Planned Path1
-      Splitter Ratio: 0.742560029
+      Splitter Ratio: 0.7425600290298462
-    Tree Height: 325
+    Tree Height: 204
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -18,6 +18,8 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -29,7 +31,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
@@ -44,17 +46,16 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.5
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
+      JointsTab_Use_Radians: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false
       MoveIt_Allow_Sensor_Positioning: false
-      MoveIt_Goal_Tolerance: 0
       MoveIt_Planning_Attempts: 10
       MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
       MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
       MoveIt_Workspace:
         Center:
           X: 0
@@ -74,6 +75,11 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          g_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
           joint1:
             Alpha: 1
             Show Axes: false
@@ -118,20 +124,21 @@ Visualization Manager:
         State Display Time: 0.05 s
         Trail Step Size: 1
         Trajectory Topic: move_group/display_planned_path
+        Use Sim Time: false
       Planning Metrics:
         Payload: 1
         Show Joint Torques: false
         Show Manipulability: false
         Show Manipulability Index: false
         Show Weight Limit: false
-        TextHeight: 0.0799999982
+        TextHeight: 0.07999999821186066
       Planning Request:
         Colliding Link Color: 255; 0; 0
         Goal State Alpha: 1
         Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
+        Interactive Marker Size: 0.15000000596046448
         Joint Violation Color: 255; 0; 255
-        Planning Group: mycobot_arm
+        Planning Group: arm_group
         Query Goal State: true
         Query Start State: true
         Show Workspace: false
@@ -142,7 +149,7 @@ Visualization Manager:
       Scene Geometry:
         Scene Alpha: 1
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.200000003
+        Scene Display Time: 0.20000000298023224
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
@@ -154,6 +161,11 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          g_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
           joint1:
             Alpha: 1
             Show Axes: false
@@ -210,30 +222,30 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 1.39158678
+      Distance: 1.2357749938964844
       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.113567002
+        X: 0.0016935542225837708
-        Y: 0.105920002
+        Y: -0.01711718738079071
-        Z: 2.23518001e-07
+        Z: 1.0430871100197692e-07
       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.439796686
+      Pitch: 0.2997974753379822
       Target Frame: joint1
-      Value: XYOrbit (rviz)
+      Yaw: 0.9767614603042603
-      Yaw: 2.5317657
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1028
+  Height: 824
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -242,9 +254,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003befc0200000007fb000000100044006900730070006c0061007900730100000028000001da000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002080000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002580000018e0000018300ffffff00000471000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002defc0200000007fb000000100044006900730070006c006100790073010000003d00000109000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000014c000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000193000001880000017d00ffffff0000053d000002de00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1828
+  Width: 1846
-  X: 92
+  X: 72
-  Y: 24
+  Y: 27

mycobot_280/mycobot_280_moveit/launch/planning_context.launch

@@ -7,6 +7,9 @@
 
   <!-- Load universal robot description format (URDF) -->
   <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot_280_m5/mycobot_280_m5.urdf"/>
+  <!-- <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot_pro_630/mycobot_pro_630.urdf"/> -->
+  <!-- <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot_pro_630/mybase_j3.urdf"/> -->
+
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic" textfile="$(find mycobot_280_moveit)/config/firefighter.srdf" />

mycobot_320/mycobot_320_gripper_moveit/.setup_assistant

@@ -6,6 +6,6 @@ moveit_setup_assistant_config:
   SRDF:
     relative_path: config/firefighter.srdf
   CONFIG:
-    author_name: wwj
+    author_name: Jz
-    author_email: wwj@elephantrobot.com
+    author_email: Jz@elephantrobot.com
     generated_timestamp: 1683628879

mycobot_320/mycobot_320_gripper_moveit/config/fake_controllers.yaml

@@ -2,18 +2,25 @@ controller_list:
   - name: fake_arm_group_controller
     type: $(arg fake_execution_type)
     joints:
+      # - joint2_to_joint1
+      # - joint3_to_joint2
+      # - joint4_to_joint3
+      # - joint5_to_joint4
+      # - joint6_to_joint5
+      # - joint6output_to_joint6
+      - joint1_to_base
       - joint2_to_joint1
       - joint3_to_joint2
       - joint4_to_joint3
       - joint5_to_joint4
       - joint6_to_joint5
-      - joint6output_to_joint6
+      - joint6output_to_tool1
   - name: fake_gripper_group_controller
     type: $(arg fake_execution_type)
     joints:
       - gripper_controller
-initial:  # Define initial robot poses per group
+# initial:  # Define initial robot poses per group
-  - group: arm_group
+#   - group: arm_group
-    pose: init_pose
+#     pose: init_pose
-  - group: gripper_group
+#   - group: gripper_group
-    pose: init_gripper
+#     pose: init_gripper

mycobot_320/mycobot_320_gripper_moveit/launch/moveit.rviz

@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -9,7 +9,7 @@ Panels:
         - /MotionPlanning1/Planning Request1
         - /MotionPlanning1/Planned Path1
       Splitter Ratio: 0.5
-    Tree Height: 109
+    Tree Height: 200
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -45,6 +45,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
+      JointsTab_Use_Radians: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: true
       MoveIt_Allow_External_Program: false
@@ -52,7 +53,7 @@ Visualization Manager:
       MoveIt_Allow_Sensor_Positioning: false
       MoveIt_Planning_Attempts: 10
       MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Cartesian_Path: true
       MoveIt_Use_Constraint_Aware_IK: true
       MoveIt_Workspace:
         Center:
@@ -78,10 +79,6 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           gripper_base:
             Alpha: 1
             Show Axes: false
@@ -117,32 +114,42 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          link1:
+          joint1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link2:
+          joint2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link3:
+          joint3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link4:
+          joint4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link5:
+          joint5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link6:
+          joint6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -168,11 +175,11 @@ Visualization Manager:
         Colliding Link Color: 255; 0; 0
         Goal State Alpha: 1
         Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0.15000000596046448
+        Interactive Marker Size: 0.20000000298023224
         Joint Violation Color: 255; 0; 255
         Planning Group: arm_group
         Query Goal State: true
-        Query Start State: false
+        Query Start State: true
         Show Workspace: false
         Start State Alpha: 1
         Start State Color: 0; 255; 0
@@ -198,10 +205,6 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           gripper_base:
             Alpha: 1
             Show Axes: false
@@ -237,32 +240,42 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          link1:
+          joint1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link2:
+          joint2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link3:
+          joint3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link4:
+          joint4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link5:
+          joint5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link6:
+          joint6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -314,7 +327,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2
+      Distance: 1.4812843799591064
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -322,17 +335,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.10000000149011612
+        X: -0.07944566011428833
-        Y: 0.25
+        Y: 0.29104751348495483
-        Z: 0.30000001192092896
+        Z: 0.29134249687194824
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3399999737739563
+      Pitch: 1.0449998378753662
       Target Frame: base
-      Yaw: 2.9449498653411865
+      Yaw: 6.149951934814453
     Saved:
       - Class: rviz/Orbit
         Distance: 2
@@ -357,7 +370,7 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 919
+  Height: 820
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -366,9 +379,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000033dfc0200000007fb000000100044006900730070006c006100790073010000003d000000fe000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000141000000520000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000199000001e10000017d00ffffff000003120000033d00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002dafc0200000007fb000000100044006900730070006c006100790073010000003d00000105000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000148000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000018f000001880000017d00ffffff00000539000002da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1291
+  Width: 1842
-  X: 454
+  X: 72
   Y: 27

mycobot_320/mycobot_320_gripper_moveit/launch/planning_context.launch

@@ -6,10 +6,12 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot_320_m5_2020/mycobot_pro_320_m5_2020_gripper.urdf"/>
+  <!-- <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot_320_m5_2020/mycobot_pro_320_m5_2020_gripper.urdf"/> -->
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot_pro_630/mycobot_630_gripper.urdf"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find mycobot_320_gripper_moveit)/config/firefighter.srdf" />
+  <!-- <param name="$(arg robot_description)_semantic" textfile="$(find mycobot_320_gripper_moveit)/config/firefighter.srdf" /> -->
+  <param name="$(arg robot_description)_semantic" textfile="$(find new_mycobot_630_gripper_moveit)/config/firefighter.srdf" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">

mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py

@@ -8,7 +8,16 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
+from socket import *
+import math
+import sys
+import time
+from multiprocessing import Lock
 
+import rospy
+from sensor_msgs.msg import JointState
+
+from pymycobot.elephantrobot import ElephantRobot
 import rospy
 import time
 from sensor_msgs.msg import JointState
@@ -50,17 +59,24 @@ def callback(data):
 def listener():
     global mc
     global gripper_value
-   
     rospy.init_node("control_slider", anonymous=True)
 
+    ip = rospy.get_param("~ip", "192.168.1.161")
+    port = rospy.get_param("~port", 5001)
+    print (ip, port)
+    mc = ElephantRobot(ip, int(port))
+    # START CLIENT,启动客户端
+    res = mc.start_client()
+    if res != "":
+        sys.exit(1)
+
+    mc.set_speed(90)
+
     rospy.Subscriber("joint_states", JointState, callback)
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
-    baud = rospy.get_param("~baud", 115200)
-    print(port, baud)
-    mc = MyCobot(port, baud)
 
     # spin() simply keeps python from exiting until this node is stopped
-    print("spin ...")
+    # spin()只是阻止python退出，直到该节点停止
+    print ("sping ...")
     rospy.spin()
 
 

mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan_630.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+from socket import *
+import math
+import sys
+import time
+from multiprocessing import Lock
+import numpy as np
+import rospy
+from sensor_msgs.msg import JointState
+
+from pymycobot.elephantrobot import ElephantRobot
+
+global mc
+gripper_value = []
+
+
+
+def linear_mapping(value, input_start, input_end, output_start, output_end):
+    """
+    Maps a value from one range to another using a linear transformation.
+
+    Args:
+        value (float): The input value to be mapped.
+        input_start (float): The start of the input range.
+        input_end (float): The end of the input range.
+        output_start (float): The start of the output range.
+        output_end (float): The end of the output range.
+
+    Returns:
+        float: The mapped value in the output range.
+    """
+    # Calculate the slope of the linear function
+    slope = (output_end - output_start) / (input_end - input_start)
+    # Calculate the mapped value
+    mapped_value = output_start + slope * (value - input_start)
+    return mapped_value
+
+
+def callback(data):
+    """callback function,回调函数"""
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+
+    data_list = []
+    for index, value in enumerate(data.position):
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
+
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    # mc.write_angles(data_list, 800)
+    
+    list_i = []
+    for i in data_list[:6]:
+        list_i.append(i)
+    mc.write_angles(list_i,800)
+    
+    list_j = []
+    for j in data_list[6:]:
+        list_j.append(j)
+    # list_j = [int(a) for a in list_j]
+    
+    mc.set_gripper_mode(0)
+    # Define the input and output ranges
+    input_start = 17
+    input_end = -40
+    output_start = 100
+    output_end = 0
+    num = [linear_mapping(value, input_start, input_end, output_start, output_end) for value in list_j]
+    num2 = [int(i) for i in num]
+    num2 = num2[1]
+    mc.set_gripper_value(num2,100)
+    rospy.loginfo(num2)
+    
+
+
+
+def listener():
+    global mc
+    global gripper_value
+    rospy.init_node("control_slider", anonymous=True)
+
+    ip = rospy.get_param("~ip", "192.168.1.161")
+    port = rospy.get_param("~port", 5001)
+    print (ip, port)
+    mc = ElephantRobot(ip, int(port))
+    # START CLIENT,启动客户端
+    res = mc.start_client()
+    if res != "":
+        sys.exit(1)
+
+    mc.set_speed(90)
+
+    rospy.Subscriber("joint_states", JointState, callback)
+
+    # spin() simply keeps python from exiting until this node is stopped
+    # spin()只是阻止python退出，直到该节点停止
+    print ("sping ...")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    listener()

mycobot_320/mycobot_320_gripper_moveit/scripts/test.py

@@ -0,0 +1,35 @@
+from pymycobot.elephantrobot import ElephantRobot
+import time
+
+global mc
+
+mc = ElephantRobot("192.168.1.161",5001)
+
+
+
+
+# 启动机器人必要指令
+mc.start_client()
+time.sleep(1)
+mc.set_gripper_mode(0)
+time.sleep(1)
+
+
+
+# mc.power_off()#夹爪透传换IO模式时需要先关闭机器再重启机器人一次，仅使用夹爪透传模式不必关闭机器人
+# mc.state_off()
+# time.sleep(3)
+# mc.power_on()
+# time.sleep(3)
+# mc.state_on()
+# time.sleep(3)
+# mc.set_gripper_mode(0)
+mc.set_gripper_calibrate()
+
+# #透传模式
+
+# for i in range(3):
+#     mc.set_gripper_value(46, 20)
+#     time.sleep(1)
+#     mc.set_gripper_value(96, 20)
+#     time.sleep(1)

mycobot_description/urdf/flange/flange.urdf

@@ -0,0 +1,19 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/flange/v1.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/flange/v1.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </collision>
+    </link>
+  
+</robot>

mycobot_description/urdf/mycobot_280_m5/mycobot_280_improve.urdf

@@ -0,0 +1,277 @@
+<?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_280_m5/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_280_m5/G_base.dae" />
+            </geometry>
+            <origin xyz="0.0 0 -0.03" rpy="0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint1">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04  1.5708 3.14159-->
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint1.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0 " rpy=" 0 0 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint2.dae" />
+            </geometry>
+            <origin xyz="0.0 0 -0.06096 " rpy=" 0 0 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint3.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0.03256 " rpy=" 0 -1.5708 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint4">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint4.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0.03056 " rpy=" 0 -1.5708 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint5">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint5.dae" />
+            </geometry>
+            <origin xyz="0.0 0 -0.03356 " rpy=" -1.5708 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint6">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint6.dae" />
+            </geometry>
+            <origin xyz="0 0.00 -0.038 " rpy=" 0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint6_flange">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint7.dae" />
+            </geometry>
+            <origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </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.13156" 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" 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>
+    <transmission name="trans_joint2_to_joint1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint2_to_joint1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint2_to_joint1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint3_to_joint2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint3_to_joint2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint3_to_joint2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint4_to_joint3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint4_to_joint3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint4_to_joint3_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint5_to_joint4">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint5_to_joint4">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint5_to_joint4_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6_to_joint5">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6_to_joint5">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6_to_joint5_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6output_to_joint6">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6output_to_joint6">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6output_to_joint6_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    
+</robot>
+

mycobot_description/urdf/mycobot_280_m5/mycobot_280_m5.urdf

@@ -71,14 +71,12 @@
   <link name="joint4">
     <visual>
       <geometry>
-       <!--- 0.0 0 -0.04 -->
        <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint4.dae"/>
       </geometry>
       <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
@@ -89,14 +87,12 @@
   <link name="joint5">
     <visual>
       <geometry>
-       <!--- 0.0 0 -0.04 -->
        <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint5.dae"/>
       </geometry>
       <origin xyz = "0.0 0 -0.03356 " rpy = " -1.5708 0 0"/>
@@ -107,14 +103,12 @@
   <link name="joint6">
     <visual>
       <geometry>
-       <!--- 0.0 0 -0.04 -->
        <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint6.dae"/>
       </geometry>
     <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
@@ -125,14 +119,12 @@
   <link name="joint6_flange">
     <visual>
       <geometry>
-       <!--- 0.0 0 -0.04 -->
        <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/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_280_m5/joint7.dae"/>
       </geometry>
     <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>

mycobot_description/urdf/mycobot_280_m5/mycobot_280m5_with_gripper_parallel.urdf

@@ -1,3 +1,407 @@
+<<<<<<< HEAD
+<?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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/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_280_m5/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_base">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_base.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_280_m5/gripper_base.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left1">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left1.dae"/>
+      </geometry>
+    <origin xyz = "0.039 -0.0133 -0.012 " rpy = " 0 0 0"/>
+    <!-- <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/> -->
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left1.dae"/>
+      </geometry>
+        <origin xyz = "0.039 -0.0133 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left2.dae"/>
+      </geometry>
+    <origin xyz = "0.005 -0.0195 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left2.dae"/>
+      </geometry>
+        <origin xyz = "0.005 -0.0195 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left3.dae"/>
+      </geometry>
+    <origin xyz = "0.012 0.0025 -0.012 " rpy = " 0 0 0"/>
+    <!-- <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/> -->
+
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left3.dae"/>
+      </geometry>
+    <origin xyz = "0.012 0.0025 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right1.dae"/>
+      </geometry>
+    <origin xyz = "-0.039 -0.0133 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right1.dae"/>
+      </geometry>
+        <origin xyz = "-0.039 -0.0133 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right2.dae"/>
+      </geometry>
+    <origin xyz = "-0.005 -0.0195 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right2.dae"/>
+      </geometry>
+        <origin xyz = "-0.005 -0.0195 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right3.dae"/>
+      </geometry>
+    <origin xyz = "-0.012 0.0025 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right3.dae"/>
+      </geometry>
+    <origin xyz = "-0.012 0.0025 -0.012" 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="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_gripper_base" 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="gripper_base"/>
+    <!-- <origin xyz= "0 0.0 0" rpy = "-1.5708 0 0"/>   -->
+    <origin xyz= "0 0 0.034" rpy = "1.579 0 0"/>  
+  </joint>
+
+
+<joint name="gripper_controller" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.7" upper = "0.15" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_left3"/>
+    <origin xyz= "-0.012 0.005 0" rpy = "0 0 0"/> 
+  </joint>
+
+  <joint name="gripper_base_to_gripper_left2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.8" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_left2"/>
+    <origin xyz= "-0.005 0.027 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_left3_to_gripper_left1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_left3"/>
+    <child link="gripper_left1"/>
+    <origin xyz= "-0.027 0.016 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <!-- <joint name="gripper_left3_to_gripper_left2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_left3"/>
+    <child link="gripper_left2"/>
+    <origin xyz= "-0.027 0.016 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint> -->
+
+  <!-- <joint name="gripper_left2_to_gripper_left1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_left2"/>
+    <child link="gripper_left1"/>
+    <origin xyz= "-0.040 0.050 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint> -->
+
+  <joint name="gripper_base_to_gripper_right3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.15" upper = "0.7" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_right3"/>
+    <origin xyz= "0.012 0.005 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_base_to_gripper_right2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.8" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_right2"/>
+    <origin xyz= "0.005 0.027 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_right3_to_gripper_right1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_right3"/>
+    <child link="gripper_right1"/>
+    <origin xyz= "0.027 0.016 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint>
+
+  <!-- <joint name="gripper_right2_to_gripper_right1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_right2"/>
+    <child link="gripper_right1"/>
+    <origin xyz= "0.020 0.030 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint> -->
+
+=======
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
     <xacro:property name="width" value=".2" />
@@ -535,5 +939,6 @@
             <robotNamespace>/</robotNamespace>
         </plugin>
     </gazebo>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
 </robot>
 

mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf

@@ -0,0 +1,405 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="base">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J1-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = " 1.5706 0 0"/>
+    </visual>
+    <collision>
+      <origin xyz = "0 0 0.04 " rpy = " 0 0 1.5708"/>
+      <geometry>
+        <cylinder length="0.06" radius="0.038"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J2-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0.05 -0.08 " rpy = "0 0 3.14159"/>
+    </visual>
+     <collision>
+      <origin xyz = "0.0 0.0 0.04 " rpy = " 0 0 1.5708"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <link name="link2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J3-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.092 0.044 " rpy = " 0 -1.5706 0"/>
+    </visual>
+     <collision>
+     <origin xyz = "0 0.128 -0.04 " rpy = " 1.5708 0 0"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.024"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J4-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.09 -0.01 " rpy = " 0 1.5706 0"/>
+    </visual>
+     <collision>
+      <origin xyz = "0.0 0.128 0.076 " rpy = "1.5708 0 0"/>
+      <geometry>
+        <cylinder length="0.24" radius="0.024"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+
+  <link name="link4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J5-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.6987 0 " rpy = " 0 0 0"/>
+    </visual>
+     <collision>
+      <origin xyz = "0.0 0.02 -0.05" rpy = " 1.5708 1.5708 0"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.024"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <link name="link5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.048 " rpy = " 1.5706  0 3.14159"/>
+    </visual>
+     <collision>
+      <origin xyz = "0.0 0.0 0.038 " rpy = " 0  0 -1.5708"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.028"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="link6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J7-W.dae"/>
+      </geometry>
+      <material name = "grey">
+        <color rgba = "0.5 0.5 0.5 1"/>
+      </material>
+    <origin xyz = "0.06 0 0" rpy = " 3.14159 3.14159  0"/>
+    </visual>
+     <collision>
+      <origin xyz = "0.003 0.0 -0.0 " rpy = " 0 1.5708 0"/>
+      <geometry>
+        <cylinder length="0.006" radius="0.026"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <!-- <link name="tool1">
+    <visual>
+        <geometry>
+            <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/tool1-mm.dae" />
+        </geometry>
+        <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+    </visual>
+    <collision>
+        <geometry>
+            <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/tool1-mm.dae" />
+        </geometry>
+        <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+    </collision>   
+  </link> -->
+
+  <link name="tool2">
+    <visual>
+        <geometry>
+            <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/tool2-mm.dae" />
+        </geometry>
+        <origin xyz="0.0 0 0" rpy=" 0 0  -1.5706" />
+    </visual>
+    <collision>
+        <origin xyz = "0.003 0.0 0.005 " rpy = " 0 0 -1.5708"/>
+      <geometry>
+        <cylinder length="0.006" radius="0.026"/>
+      </geometry>
+    </collision>   
+  </link>
+
+  <link name="gripper_base">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_base.dae"/>
+      </geometry>
+    <origin xyz = "-0.02 0.012 -0.018 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder radius="0.026" length="0.03" />
+      </geometry>
+      <origin xyz="0.0 0 0.01 " rpy=" 0 0 1.5708" />
+    </collision>
+  </link>
+
+  <link name="gripper_left1">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left1.dae"/>
+      </geometry>
+    <origin xyz = "0.036 -0.052 0.0 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder radius="0.004" length="0.005" />
+      </geometry>
+      <origin xyz="0.0 0.0 0.002 " rpy=" 0 0 1.5708" />
+    </collision>
+  </link>
+
+  <link name="gripper_left2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left2.dae"/>
+      </geometry>
+    <origin xyz = "0.03 0.024 -0.018 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder radius="0.003" length="0.003" />
+      </geometry>
+      <origin xyz="0.00 -0.0 -0.003 " rpy=" 0 0 1.5708" />
+    </collision>
+  </link>
+
+  <link name="gripper_left3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left3.dae"/>
+      </geometry>
+    <origin xyz = "0.034 0.0 -0.012 " rpy = " 0 3.14159 -1.5708"/>
+    <!-- <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/> -->
+
+    </visual>
+    <collision>
+      <geometry>
+       <cylinder radius="0.018" length="0.003" />
+      </geometry>
+      <origin xyz="-0.002  0 -0.016 " rpy=" 0 3.14159 -1.5708" />
+    </collision>
+  </link>
+
+  <link name="gripper_right1">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right1.dae"/>
+      </geometry>
+    <origin xyz = "-0.072 -0.05 0.0 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <cylinder radius="0.004" length="0.005" />
+      </geometry>
+      <origin xyz="0.0 0.0 0.002 " rpy=" 0 0 1.5708" />
+    </collision>
+  </link>
+
+  <link name="gripper_right2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right2.dae"/>
+      </geometry>
+    <origin xyz = "-0.066 0.023 -0.018 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <cylinder radius="0.003" length="0.003" />
+      </geometry>
+      <origin xyz="0.00 -0.0 -0.003 " rpy=" 0 0 1.5708" />
+    </collision>
+  </link>
+
+  <link name="gripper_right3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right3.dae"/>
+      </geometry>
+    <origin xyz = "0 0.0 -0.012 " rpy = " 0 3.14159 -1.5708"/>
+
+    </visual>
+    <collision>
+      <geometry>
+       <cylinder radius="0.018" length="0.003" />
+      </geometry>
+      <origin xyz="-0.002  0 -0.016 " rpy=" 0 3.14159 -1.5708" />
+    </collision>
+  </link>
+
+
+
+
+
+  <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="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0.1655" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <!-- <limit effort = "1000.0" lower = "-4.71" upper = "1.5708" velocity = "0"/>   -->
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <!-- <origin xyz= "0 0  0" rpy = "1.5708 -1.5708 0"/>   -->
+    <origin xyz= "0 0.042  0.072" rpy = "1.5708 1.5706 0"/>
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 -1"/>
+    <limit effort = "1000.0" lower = "-2.61" upper = "2.618" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "0.0 0.25 0.005   " rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 -1"/>
+    <!-- <limit effort = "1000.0" lower = "-4.53" upper = "1.3962" velocity = "0"/> -->
+    <limit effort = "1000.0" lower = "-2.97" upper = "2.97" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <!-- <origin xyz= "0.25 0  -0.1091" rpy = "0 0 1.5708"/>   -->
+    <origin xyz= "0 0.25  0.025" rpy = "0 0 -1.5706"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.93" upper = "2.9321" velocity = "0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz= "0 0.058 -0.052" rpy = "1.57080 -1.57080 3.14159"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="-1 0 0"/>
+    <limit effort = "1000.0" lower = "-3.03" upper = "3.0368" velocity = "0"/>
+    <parent link="link5"/>
+    <child link="link6"/>
+    <origin xyz= "-0.06 0 0.048" rpy = "-1.57080 0 0 "/>  
+  </joint>
+
+  <joint name="joint6output_to_tool2" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="link6" />
+    <child link="tool2" />
+    <origin xyz="-0.016 0.0 0.0" rpy="-1.5706 0 1.5706" />
+  </joint>
+
+  <!-- <joint name="tool1_to_tool2" type="fixed">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="tool1" />
+    <child link="tool2" />
+    <origin xyz="0.0 0.008 0.00" rpy="1.5708 -1.5706 3.14159" />
+  </joint> -->
+
+  <joint name="tool2_to_gripper_base" type="fixed">
+    <axis xyz="0 0 -1"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="tool2" />
+    <child link="gripper_base" />
+    <origin xyz="0.005 0.0 0.047" rpy="1.5706 0 1.5706" />
+  </joint>
+
+  <joint name="gripper_controller" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.7" upper = "0.3" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_left3"/>
+    <origin xyz= "-0.018 0.016 0" rpy = "0 0 0"/> 
+  </joint>
+
+  <joint name="gripper_base_to_gripper_left2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.8" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_left2"/>
+    <origin xyz= "-0.05 -0.01 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_left3_to_gripper_left1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_left3"/>
+    <child link="gripper_left1"/>
+    <origin xyz= "-0.0356 0.051 -0.018" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+    <joint name="gripper_base_to_gripper_right3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.3" upper = "0.7" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_right3"/>
+    <origin xyz= "0.018 0.016 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_base_to_gripper_right2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.8" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_right2"/>
+    <origin xyz= "0.046 -0.01 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_right3_to_gripper_right1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_right3"/>
+    <child link="gripper_right1"/>
+    <origin xyz= "0.033 0.051 -0.018" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint>
+
+
+
+</robot>
+

mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf

@@ -8,21 +8,44 @@
     <visual>
       <geometry>
 	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+<<<<<<< HEAD
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J1-mm.dae"/>
+=======
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/base.dae"/>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
       </geometry>
     <origin xyz = "0 0 0 " rpy = " 0 0 0"/>
     </visual>
     <collision>
+<<<<<<< HEAD
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J1-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = " 0 0 0"/>
+=======
       <origin xyz = "0 0 0.04 " rpy = " 0 0 1.5708"/>
       <geometry>
         <cylinder length="0.06" radius="0.038"/>
       </geometry>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     </collision>
   </link>
 
   <link name="link1">
     <visual>
       <geometry>
+<<<<<<< HEAD
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J2-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = "0 0 0"/>
+    </visual>
+     <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J2-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = " 0 0 0"/>
+=======
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/link1.dae"/>
       </geometry>
     <origin xyz = "0 0 0 " rpy = "0 0 3.1415926"/>
@@ -32,6 +55,7 @@
       <geometry>
         <cylinder length="0.05" radius="0.03"/>
       </geometry>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     </collision>
   </link>
 
@@ -39,6 +63,17 @@
   <link name="link2">
     <visual>
       <geometry>
+<<<<<<< HEAD
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J3-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0.0 0.0 " rpy = " 3.1415926 0 3.1415926"/>
+    </visual>
+     <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J3-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0.0 0.0 " rpy = " 3.1415926 0 3.1415926"/>
+=======
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/link2.dae"/>
       </geometry>
     <origin xyz = "0 0 -0.080 " rpy = " 3.1415926 0 3.1415926"/>
@@ -48,6 +83,7 @@
       <geometry>
         <cylinder length="0.28" radius="0.024"/>
       </geometry>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     </collision>
   </link>
 
@@ -56,6 +92,18 @@
     <visual>
       <geometry>
        <!--- 0.0 0 -0.04 -->
+<<<<<<< HEAD
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J4-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.0 " rpy = " 3.1415926 3.14159 3.1415926"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J4-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.0 " rpy = "3.1415926 03.14159 3.1415926"/>
+=======
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/link3.dae"/>
       </geometry>
     <origin xyz = "0 0 -0.0855 " rpy = " 3.1415926 0 3.1415926"/>
@@ -65,24 +113,41 @@
       <geometry>
         <cylinder length="0.24" radius="0.024"/>
       </geometry>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     </collision>
   </link>
 
 
 
+<<<<<<< HEAD
+  <link name="link4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J5-mm.dae"/>
+=======
 <link name="link4">
     <visual>
       <geometry>
        <!--- 0.0 0 -0.04 -->
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/link4.dae"/>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
       </geometry>
     <origin xyz = "0 0 0 " rpy = " 0 3.1415926 0"/>
     </visual>
      <collision>
+<<<<<<< HEAD
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J5-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = " 0 3.1415926 0"/>
+=======
       <origin xyz = "0.0 -0.01 0.007" rpy = " 1.5708 1.5708 0"/>
       <geometry>
         <cylinder length="0.05" radius="0.024"/>
       </geometry>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     </collision>
   </link>
 
@@ -91,6 +156,19 @@
     <visual>
       <geometry>
        <!--- 0.0 0 -0.04 -->
+<<<<<<< HEAD
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0  0 " rpy = " 0  0 -1.5706"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-mm.dae"/>
+      </geometry>
+    <origin xyz = "0 0  0 " rpy = " 0  0 -1.5706"/>
+    </collision>
+=======
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/link5.dae"/>
       </geometry>
     <origin xyz = "0 0  0 " rpy = " 0  0 1.5707"/>
@@ -101,17 +179,35 @@
         <cylinder length="0.05" radius="0.028"/>
       </geometry>
      </collision>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </link>
 
   <link name="link6">
     <visual>
       <geometry>
        <!--- 0.0 0 -0.04 -->
+<<<<<<< HEAD
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-tool-mm.dae"/>
+=======
        <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/link6.dae"/>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
       </geometry>
       <material name = "grey">
         <color rgba = "0.5 0.5 0.5 1"/>
       </material>
+<<<<<<< HEAD
+    <origin xyz = "0.02 0 0" rpy = " 0 -1.5707  0"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-tool-mm.dae"/>
+      </geometry>
+      <material name = "grey">
+        <color rgba = "0.5 0.5 0.5 1"/>
+      </material>
+    <origin xyz = "0.02 0 0" rpy = " 0 -1.5707  0"/>
+=======
     <origin xyz = "0.03 0 0" rpy = " 0 -1.5707  0"/>
     </visual>
      <collision>
@@ -119,27 +215,45 @@
       <geometry>
         <cylinder length="0.006" radius="0.026"/>
       </geometry>
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     </collision>
   </link>
 
 
+<<<<<<< HEAD
+
+=======
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   <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="base"/>
     <child link="link1"/>
+<<<<<<< HEAD
+    <origin xyz= "0 0 0.1655" rpy = "0 0 0"/>  
+=======
     <origin xyz= "0 0 0.19934" rpy = "0 0 0"/>  
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </joint>
 
 
   <joint name="joint3_to_joint2" type="revolute">
     <axis xyz="0 0 -1"/>
+<<<<<<< HEAD
+    <!-- <limit effort = "1000.0" lower = "-4.71" upper = "1.5708" velocity = "0"/>   -->
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <!-- <origin xyz= "0 0  0" rpy = "1.5708 -1.5708 0"/>   -->
+    <origin xyz= "0 0.04  0.085" rpy = "1.5708 0 0"/>
+=======
     <!-- <limit effort = "1000.0" lower = "-4.71" upper = "1.5708" velocity = "0"/> -->
     <limit effort = "1000.0" lower = "-3.14159" upper = "3.14159" velocity = "0"/>
     <parent link="link1"/>
     <child link="link2"/>
     <origin xyz= "0 0  0" rpy = "1.5708 -1.5708 0"/>  
     <!-- <origin xyz= "0 0  0" rpy = "1.5708 0 0"/> -->
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </joint>
 
 
@@ -148,17 +262,29 @@
     <limit effort = "1000.0" lower = "-2.61" upper = "2.618" velocity = "0"/>
     <parent link="link2"/>
     <child link="link3"/>
+<<<<<<< HEAD
+    <origin xyz= "0.0 0.25 0   " rpy = "0 0 0"/>  
+=======
     <origin xyz= "0.25 0 0   " rpy = "0 0 0"/>  
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </joint>
 
   <joint name="joint5_to_joint4" type="revolute">
     <axis xyz=" 0 0 -1"/>
     <!-- <limit effort = "1000.0" lower = "-4.53" upper = "1.3962" velocity = "0"/> -->
+<<<<<<< HEAD
+    <limit effort = "1000.0" lower = "-2.97" upper = "2.97" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <!-- <origin xyz= "0.25 0  -0.1091" rpy = "0 0 1.5708"/>   -->
+    <origin xyz= "0 0.25  0.025" rpy = "0 0 0"/>  
+=======
     <limit effort = "1000.0" lower = "-2.9670" upper = "2.9670" velocity = "0"/>
     <parent link="link3"/>
     <child link="link4"/>
     <origin xyz= "0.25 0  -0.1091" rpy = "0 0 1.5708"/>  
     <!-- <origin xyz= "0.25 0  -0.1091" rpy = "0 0 0"/>   -->
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </joint>
 
   <joint name="joint6_to_joint5" type="revolute">
@@ -166,7 +292,11 @@
     <limit effort = "1000.0" lower = "-2.93" upper = "2.9321" velocity = "0"/>
     <parent link="link4"/>
     <child link="link5"/>
+<<<<<<< HEAD
+    <origin xyz= "0 0.058 -0.05" rpy = "1.57080 -1.57080 3.14159"/>  
+=======
     <origin xyz= "0 -0.108 0" rpy = "1.57080 -1.57080 0"/>  
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </joint>
 
   <joint name="joint6output_to_joint6" type="revolute">
@@ -174,7 +304,11 @@
     <limit effort = "1000.0" lower = "-3.03" upper = "3.0368" velocity = "0"/>
     <parent link="link5"/>
     <child link="link6"/>
+<<<<<<< HEAD
+    <origin xyz= "-0.076 0 0.05" rpy = "-1.57080 0 0 "/>  
+=======
     <origin xyz= "-0.07586 0 0" rpy = "-1.57080 0 0 "/>  
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   </joint>
 
 

mycobot_description/urdf/mycobot_pro_600/new_mycobot_pro_600_moveit.urdf

@@ -0,0 +1,192 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="base">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J1-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = " 1.5706 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J1-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0 " rpy = " 1.5706 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J2-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0.05 -0.08 " rpy = "0 0 3.14159"/>
+    </visual>
+     <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J2-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0.05 -0.08 " rpy = " 0 0 3.14159"/>
+    </collision>
+  </link>
+
+
+  <link name="link2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J3-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.092 0.044 " rpy = " 0 -1.5706 0"/>
+    </visual>
+     <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J3-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.092 0.044 " rpy = " 0 -1.5706 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J4-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.09 -0.01 " rpy = " 0 1.5706 0"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J4-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.09 -0.01 " rpy = "0 1.5706 0"/>
+    </collision>
+  </link>
+
+
+
+  <link name="link4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J5-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.6987 0 " rpy = " 0 0 0"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J5-W.dae"/>
+      </geometry>
+    <origin xyz = "0 -0.6987 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.048 " rpy = " 1.5706  0 3.14159"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-W.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.048 " rpy = " 1.5706  0 3.14159"/>
+    </collision>
+  </link>
+
+  <link name="link6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J7-W.dae"/>
+      </geometry>
+      <material name = "grey">
+        <color rgba = "0.5 0.5 0.5 1"/>
+      </material>
+    <origin xyz = "0.06 0 0" rpy = " 3.14159 3.14159  0"/>
+    </visual>
+     <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J7-W.dae"/>
+      </geometry>
+      <material name = "grey">
+        <color rgba = "0.5 0.5 0.5 1"/>
+      </material>
+    <origin xyz = "0.06 0 0" rpy = " 3.14159 3.14159  0"/>
+    </collision>
+  </link>
+
+
+
+  <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="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0.1655" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <!-- <limit effort = "1000.0" lower = "-4.71" upper = "1.5708" velocity = "0"/>   -->
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <!-- <origin xyz= "0 0  0" rpy = "1.5708 -1.5708 0"/>   -->
+    <origin xyz= "0 0.042  0.072" rpy = "1.5708 1.5706 0"/>
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 -1"/>
+    <limit effort = "1000.0" lower = "-2.61" upper = "2.618" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "0.0 0.25 0.005   " rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 -1"/>
+    <!-- <limit effort = "1000.0" lower = "-4.53" upper = "1.3962" velocity = "0"/> -->
+    <limit effort = "1000.0" lower = "-2.97" upper = "2.97" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <!-- <origin xyz= "0.25 0  -0.1091" rpy = "0 0 1.5708"/>   -->
+    <origin xyz= "0 0.25  0.025" rpy = "0 0 -1.5706"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.93" upper = "2.9321" velocity = "0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz= "0 0.058 -0.052" rpy = "1.57080 -1.57080 3.14159"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="-1 0 0"/>
+    <limit effort = "1000.0" lower = "-3.03" upper = "3.0368" velocity = "0"/>
+    <parent link="link5"/>
+    <child link="link6"/>
+    <origin xyz= "-0.06 0 0.048" rpy = "-1.57080 0 0 "/>  
+  </joint>
+
+
+
+</robot>
+

mycobot_description/urdf/mycobot_pro_630/my630.urdf

@@ -0,0 +1,155 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-W.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-W.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-W.dae" />
+            </geometry>
+            <origin xyz="0 0.05 -0.07 " rpy="0 0 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-W.dae" />
+            </geometry>
+            <origin xyz="0 0.05 -0.07 " rpy=" 0 0 3.14159" />
+        </collision>
+        
+    </link>
+    <link name="joint2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.0925 -0.053 " rpy=" 0 1.5706 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.0925 -0.053 " rpy=" 0 1.5706 0" />
+        </collision>
+        
+    </link>
+    <link name="joint3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.091 0.0 " rpy=" 0 -1.5706 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.091 0.0 " rpy=" 0 -1.5706 0" />
+        </collision>
+        
+    </link>
+    <link name="joint4">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.7 0.003 " rpy=" 0 3.14159 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.7 0.003 " rpy=" 0 3.14159 0" />
+        </collision>
+        
+    </link>
+    <link name="joint5">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6W.dae" />
+            </geometry>
+            <origin xyz="0.05 0 -0.04 " rpy=" 3.14159 1.5706 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6W.dae" />
+            </geometry>
+            <origin xyz="0.05 0 -0.04 " rpy=" 3.14159 1.5706 0" />
+        </collision>
+        
+    </link>
+    <link name="joint6">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-W.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 3.14159  -1.5706" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-W.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 3.14159  -1.5706" />
+        </collision>
+        
+    </link>
+    
+
+    <joint name="joint1_to_base" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14159" velocity="0" />
+        <parent link="base" />
+        <child link="joint1" />
+        <origin xyz="0 0 0.156" 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.14" velocity="0" />
+        <parent link="joint1" />
+        <child link="joint2" />
+        <origin xyz="0 0.05  0.083 " rpy="1.5707 0 3.14159" />
+    </joint>
+    <joint name="joint3_to_joint2" type="revolute">
+        <axis xyz=" 0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint2" />
+        <child link="joint3" />
+        <origin xyz="0 0.25 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint4_to_joint3" type="revolute">
+        <axis xyz=" 0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint3" />
+        <child link="joint4" />
+        <origin xyz="0 0.25 -0.03   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint5_to_joint4" type="revolute">
+        <axis xyz="0 0 -1" />
+        <limit effort="1000.0" lower="-2.99" upper="2.99" velocity="0" />
+        <parent link="joint4" />
+        <child link="joint5" />
+        <origin xyz="0 0.055 0.055" rpy="1.5707 -1.5707 0" />
+    </joint>
+    <joint name="joint6_to_joint5" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-3.03" upper="3.0368" velocity="0" />
+        <parent link="joint5" />
+        <child link="joint6" />
+        <origin xyz="0.0495 0 -0.0405" rpy="-1.5707 0 -1.5706 " />
+    </joint>
+    
+    
+</robot>

mycobot_description/urdf/mycobot_pro_630/my630_gripper.urdf

@@ -0,0 +1,728 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0035 " rpy="0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0035 " rpy=" 0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.00 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.00 " rpy=" 0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.009 " rpy=" 0 3.14159 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.009 " rpy=" 0 3.14159 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint4">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.003 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.003 " rpy=" 0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint5">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 3.14159 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 3.14159 3.14159" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="joint6">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 1.5707  0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 1.5707  0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="tool1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool1-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool1-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="tool2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 0  -1.5706" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 0  -1.5706" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <!-- <link name="gripper_base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/gp-base-mm.dae" />
+            </geometry>
+            <origin xyz="-0.02 0.0 0.05 " rpy=" 0 -1.5706 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/gp-base-mm.dae" />
+            </geometry>
+            <origin xyz="-0.02 0.0 0.05 " rpy=" 0 -1.5706 3.14159" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left1">
+        <visual>
+            <geometry>
+               
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/L1-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0.0 0.0 " rpy=" 0 0 -0.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/L1-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0.0 0.0 " rpy=" 0 0 -0.5708" />
+        </collision>
+        
+    </link>
+    <link name="gripper_left2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/L2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0.0 0.0 " rpy=" 0 -1.5706 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/L2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0.0 0.0 " rpy=" 0 -1.5706 3.14159" />
+        </collision>
+        
+    </link>
+    <link name="gripper_left3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/L3-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 -0.0025 -0.0045 " rpy=" 0 3.14159 -1.9408" />
+            
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/L3-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 -0.0025 -0.0045 " rpy=" 0 3.14159 -1.9408" />
+        </collision>
+        
+    </link> -->
+    <!-- <link name="gripper_right1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/R1-mm.dae" />
+            </geometry>
+            <origin xyz="-0.072 -0.05 0.0 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/R1-mm.dae" />
+            </geometry>
+            <origin xyz="-0.036 -0.052 0.0 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/R2-mm.dae" />
+            </geometry>
+            <origin xyz="-0.066 0.023 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/R2-mm.dae" />
+            </geometry>
+            <origin xyz="-0.03 0.024 -0.018 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/R3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/R3-mm.dae" />
+            </geometry>
+            <origin xyz="-0.034 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link> -->
+    <link name="gripper_base">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_base.dae" />
+            </geometry>
+            <origin xyz="-0.02 0.012 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_base.dae" />
+            </geometry>
+            <origin xyz="-0.02 0.012 -0.018 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left1">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left1.dae" />
+            </geometry>
+            <origin xyz="0.036 -0.052 0.0 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left1.dae" />
+            </geometry>
+            <origin xyz="0.036 -0.052 0.0 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left2.dae" />
+            </geometry>
+            <origin xyz="0.03 0.024 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left2.dae" />
+            </geometry>
+            <origin xyz="0.03 0.024 -0.018 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left3.dae" />
+            </geometry>
+            <origin xyz="0.034 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+            <!-- <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/> -->
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left3.dae" />
+            </geometry>
+            <origin xyz="0.034 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right1">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right1.dae" />
+            </geometry>
+            <origin xyz="-0.072 -0.05 0.0 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right1.dae" />
+            </geometry>
+            <origin xyz="-0.072 -0.05 0.0 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right2.dae" />
+            </geometry>
+            <origin xyz="-0.066 0.023 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right2.dae" />
+            </geometry>
+            <origin xyz="-0.066 0.023 -0.018 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right3.dae" />
+            </geometry>
+            <origin xyz="0 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right3.dae" />
+            </geometry>
+            <origin xyz="0.0 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <joint name="joint1_to_base" type="revolute">
+        <axis xyz="0 0 -1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14159" velocity="0" />
+        <parent link="base" />
+        <child link="joint1" />
+        <origin xyz="0 0 0.16034" 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.14" velocity="0" />
+        <parent link="joint1" />
+        <child link="joint2" />
+        <origin xyz="0 0.06  0.038" rpy="1.5707 0 3.14159" />
+    </joint>
+    <joint name="joint3_to_joint2" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint2" />
+        <child link="joint3" />
+        <origin xyz="0 0.2685 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint4_to_joint3" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint3" />
+        <child link="joint4" />
+        <origin xyz="0 0.2663 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint5_to_joint4" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-2.99" upper="2.99" velocity="0" />
+        <parent link="joint4" />
+        <child link="joint5" />
+        <origin xyz="0 0.055 0.039" rpy="1.5707 -1.5707 0" />
+    </joint>
+    <joint name="joint6_to_joint5" type="revolute">
+        <axis xyz="1 0 0" />
+        <limit effort="1000.0" lower="-3.03" upper="3.0368" velocity="0" />
+        <parent link="joint5" />
+        <child link="joint6" />
+        <origin xyz="0 0.04 -0.0385" rpy="-1.5707 0 1.5706 " />
+    </joint>
+    <joint name="joint6output_to_tool1" type="revolute">
+        <axis xyz="0 1 0" />
+        <limit effort="1000.0" lower="-3.05" upper="3.05" velocity="0" />
+        <parent link="joint6" />
+        <child link="tool1" />
+        <origin xyz="0.0079 0.0 0.0" rpy="1.5708 -1.5706 3.14159" />
+    </joint>
+    <joint name="tool1_to_tool2" type="fixed">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-3.05" upper="3.05" velocity="0" />
+        <parent link="tool1" />
+        <child link="tool2" />
+        <origin xyz="0.0 0.008 0.00" rpy="1.5708 -1.5706 3.14159" />
+    </joint>
+    <joint name="tool2_to_gripper_base" type="fixed">
+        <axis xyz="0 0 -1" />
+        <limit effort="1000.0" lower="-3.05" upper="3.05" velocity="0" />
+        <parent link="tool2" />
+        <child link="gripper_base" />
+        <origin xyz="0.0 0.0 0.04" rpy="1.5706 0 1.5706" />
+    </joint>
+    <!-- <joint name="gripper_controller" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.7" upper="0.3" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left3" />
+        <origin xyz="-0.014 -0.016 0.06" rpy="0 -1.5706 0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_left2" type="revolute">
+        <axis xyz="-1 0 0" />
+        <limit effort="1000.0" lower="-0.8" upper="0.5" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left2" />
+        <origin xyz="-0.01 -0.03 0.118" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint>
+    <joint name="gripper_left3_to_gripper_left1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_left3" />
+        <child link="gripper_left1" />
+        <origin xyz="0.033 -0.051 -0.009" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint> -->
+    <!-- <joint name="gripper_base_to_gripper_right3" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.3" upper="0.7" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right3" />
+        <origin xyz="0.018 0.016 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_right2" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.8" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right2" />
+        <origin xyz="0.046 -0.01 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_right3_to_gripper_right1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_right3" />
+        <child link="gripper_right1" />
+        <origin xyz="0.033 0.051 -0.018" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint> -->
+    <joint name="gripper_controller" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.7" upper="0.3" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left3" />
+        <origin xyz="-0.018 0.016 0" rpy="0 0 0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_left2" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.8" upper="0.5" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left2" />
+        <origin xyz="-0.05 -0.01 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint>
+    <joint name="gripper_left3_to_gripper_left1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_left3" />
+        <child link="gripper_left1" />
+        <origin xyz="-0.0356 0.051 -0.018" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_right3" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.3" upper="0.7" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right3" />
+        <origin xyz="0.018 0.016 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_right2" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.8" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right2" />
+        <origin xyz="0.046 -0.01 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_right3_to_gripper_right1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_right3" />
+        <child link="gripper_right1" />
+        <origin xyz="0.033 0.051 -0.018" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint>
+    <transmission name="trans_joint1_to_base">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint1_to_base">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint1_to_base_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint2_to_joint1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint2_to_joint1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint2_to_joint1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint3_to_joint2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint3_to_joint2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint3_to_joint2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint4_to_joint3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint4_to_joint3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint4_to_joint3_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint5_to_joint4">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint5_to_joint4">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint5_to_joint4_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6_to_joint5">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6_to_joint5">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6_to_joint5_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6output_to_tool1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6output_to_tool1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6output_to_tool1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_controller">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_controller">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_controller_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_base_to_gripper_left2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_base_to_gripper_left2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_base_to_gripper_left2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_left3_to_gripper_left1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_left3_to_gripper_left1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_left3_to_gripper_left1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_base_to_gripper_right3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_base_to_gripper_right3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_base_to_gripper_right3_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_base_to_gripper_right2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_base_to_gripper_right2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_base_to_gripper_right2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_right3_to_gripper_right1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_right3_to_gripper_right1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_right3_to_gripper_right1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/</robotNamespace>
+        </plugin>
+    </gazebo>
+</robot>

mycobot_description/urdf/mycobot_pro_630/mycobot_630.urdf

@@ -0,0 +1,155 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0035 " rpy="0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0035 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.00 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.00 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.009 " rpy=" 0 3.14159 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.009 " rpy=" 0 3.14159 0" />
+        </collision>
+        
+    </link>
+    <link name="joint4">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.003 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.003 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint5">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 3.14159 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 3.14159 3.14159" />
+        </collision>
+        
+    </link>
+    <link name="joint6">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 1.5707  0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 1.5707  0" />
+        </collision>
+        
+    </link>
+    
+
+    <joint name="joint1_to_base" type="revolute">
+        <axis xyz="0 0 -1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14159" velocity="0" />
+        <parent link="base" />
+        <child link="joint1" />
+        <origin xyz="0 0 0.16034" 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.14" velocity="0" />
+        <parent link="joint1" />
+        <child link="joint2" />
+        <origin xyz="0 0.06  0.038" rpy="1.5707 0 3.14159" />
+    </joint>
+    <joint name="joint3_to_joint2" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint2" />
+        <child link="joint3" />
+        <origin xyz="0 0.2685 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint4_to_joint3" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint3" />
+        <child link="joint4" />
+        <origin xyz="0 0.2663 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint5_to_joint4" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-2.99" upper="2.99" velocity="0" />
+        <parent link="joint4" />
+        <child link="joint5" />
+        <origin xyz="0 0.055 0.039" rpy="1.5707 -1.5707 0" />
+    </joint>
+    <joint name="joint6_to_joint5" type="revolute">
+        <axis xyz="1 0 0" />
+        <limit effort="1000.0" lower="-3.03" upper="3.0368" velocity="0" />
+        <parent link="joint5" />
+        <child link="joint6" />
+        <origin xyz="0 0.04 -0.0385" rpy="-1.5707 0 1.5706 " />
+    </joint>
+    
+    
+</robot>

mycobot_description/urdf/mycobot_pro_630/mycobot_630_gripper.urdf

@@ -0,0 +1,423 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0035 " rpy="0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J1-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0035 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.00 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J2-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.00 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.009 " rpy=" 0 3.14159 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J3-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.009 " rpy=" 0 3.14159 0" />
+        </collision>
+        
+    </link>
+    <link name="joint4">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.003 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J4-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0.003 " rpy=" 0 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint5">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 3.14159 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J5-mm.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 0 3.14159 3.14159" />
+        </collision>
+        
+    </link>
+    <link name="joint6">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 1.5707  0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/J6-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 1.5707  0" />
+        </collision>
+        
+    </link>
+    <link name="tool1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool1-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool1-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+        </collision>
+        
+    </link>
+
+    <link name="tool2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 0  -1.5706" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/tool2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 0  -1.5706" />
+        </collision>
+        
+    </link>
+
+    <link name="gripper_base">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_base.dae"/>
+      </geometry>
+    <origin xyz = "-0.02 0.012 -0.018 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_base.dae"/>
+      </geometry>
+    <origin xyz = "-0.02 0.012 -0.018 " rpy = " 0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left1">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left1.dae"/>
+      </geometry>
+    <origin xyz = "0.036 -0.052 0.0 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left1.dae"/>
+      </geometry>
+        <origin xyz = "0.036 -0.052 0.0 " rpy = " 0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left2.dae"/>
+      </geometry>
+    <origin xyz = "0.03 0.024 -0.018 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left2.dae"/>
+      </geometry>
+        <origin xyz = "0.03 0.024 -0.018 " rpy = " 0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left3.dae"/>
+      </geometry>
+    <origin xyz = "0.034 0.0 -0.012 " rpy = " 0 3.14159 -1.5708"/>
+    <!-- <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/> -->
+
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left3.dae"/>
+      </geometry>
+    <origin xyz = "0.034 0.0 -0.012 " rpy = " 0 3.14159 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right1">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right1.dae"/>
+      </geometry>
+    <origin xyz = "-0.072 -0.05 0.0 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right1.dae"/>
+      </geometry>
+        <origin xyz = "-0.072 -0.05 0.0 " rpy = " 0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right2.dae"/>
+      </geometry>
+    <origin xyz = "-0.066 0.023 -0.018 " rpy = " 0 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right2.dae"/>
+      </geometry>
+        <origin xyz = "-0.066 0.023 -0.018 " rpy = " 0 0 1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right3.dae"/>
+      </geometry>
+    <origin xyz = "0 0.0 -0.012 " rpy = " 0 3.14159 -1.5708"/>
+
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0.0 -0.012 " rpy = " 0 3.14159 -1.5708"/>
+    </collision>
+  </link>
+
+
+
+
+
+    <joint name="joint1_to_base" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14159" velocity="0" />
+        <parent link="base" />
+        <child link="joint1" />
+        <origin xyz="0 0 0.16034" 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.14" velocity="0" />
+        <parent link="joint1" />
+        <child link="joint2" />
+        <origin xyz="0 0.06  0.038" rpy="1.5707 -1.5706 3.14159" />
+    </joint>
+    <joint name="joint3_to_joint2" type="revolute">
+        <axis xyz=" 0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint2" />
+        <child link="joint3" />
+        <origin xyz="0 0.2685 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint4_to_joint3" type="revolute">
+        <axis xyz=" 0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="joint3" />
+        <child link="joint4" />
+        <origin xyz="0 0.2663 0   " rpy="0 0 1.5706" />
+    </joint>
+    <joint name="joint5_to_joint4" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-2.99" upper="2.99" velocity="0" />
+        <parent link="joint4" />
+        <child link="joint5" />
+        <origin xyz="0 0.055 0.039" rpy="1.5707 0 0" />
+    </joint>
+    <joint name="joint6_to_joint5" type="revolute">
+        <axis xyz="1 0 0" />
+        <limit effort="1000.0" lower="-3.03" upper="3.0368" velocity="0" />
+        <parent link="joint5" />
+        <child link="joint6" />
+        <origin xyz="0 0.04 -0.0385" rpy="-1.5707 0 1.5706 " />
+    </joint>
+
+    <joint name="joint6output_to_tool1" type="fixed">
+        <axis xyz="0 1 0"/>
+        <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+        <parent link="joint6" />
+        <child link="tool1" />
+        <origin xyz="0.0079 0.0 0.0" rpy="1.5708 -1.5706 3.14159" />
+    </joint>
+
+    <joint name="tool1_to_tool2" type="fixed">
+        <axis xyz="0 0 1"/>
+        <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+        <parent link="tool1" />
+        <child link="tool2" />
+        <origin xyz="0.0 0.008 0.00" rpy="1.5708 -1.5706 3.14159" />
+    </joint>
+
+    <joint name="tool2_to_gripper_base" type="fixed">
+        <axis xyz="0 0 -1"/>
+        <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+        <parent link="tool2" />
+        <child link="gripper_base" />
+        <origin xyz="0.0 0.0 0.04" rpy="1.5706 0 1.5706" />
+    </joint>
+    <!-- <joint name="gripper_controller" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.7" upper="0.3" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left3" />
+        <origin xyz="-0.014 -0.016 0.06" rpy="0 -1.5706 0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_left2" type="revolute">
+        <axis xyz="-1 0 0" />
+        <limit effort="1000.0" lower="-0.8" upper="0.5" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left2" />
+        <origin xyz="-0.01 -0.03 0.118" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint>
+    <joint name="gripper_left3_to_gripper_left1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_left3" />
+        <child link="gripper_left1" />
+        <origin xyz="0.033 -0.051 -0.009" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint> -->
+    <!-- <joint name="gripper_base_to_gripper_right3" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.3" upper="0.7" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right3" />
+        <origin xyz="0.018 0.016 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_right2" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.8" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right2" />
+        <origin xyz="0.046 -0.01 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_right3_to_gripper_right1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_right3" />
+        <child link="gripper_right1" />
+        <origin xyz="0.033 0.051 -0.018" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint> -->
+  <joint name="gripper_controller" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.7" upper = "0.3" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_left3"/>
+    <origin xyz= "-0.018 0.016 0" rpy = "0 0 0"/> 
+  </joint>
+
+  <joint name="gripper_base_to_gripper_left2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.8" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_left2"/>
+    <origin xyz= "-0.05 -0.01 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_left3_to_gripper_left1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_left3"/>
+    <child link="gripper_left1"/>
+    <origin xyz= "-0.0356 0.051 -0.018" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+    <joint name="gripper_base_to_gripper_right3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.3" upper = "0.7" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_right3"/>
+    <origin xyz= "0.018 0.016 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_base_to_gripper_right2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.8" velocity = "0"/>
+    <parent link="gripper_base"/>
+    <child link="gripper_right2"/>
+    <origin xyz= "0.046 -0.01 0" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+  </joint>
+
+  <joint name="gripper_right3_to_gripper_right1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-0.5" upper = "0.5" velocity = "0"/>
+    <parent link="gripper_right3"/>
+    <child link="gripper_right1"/>
+    <origin xyz= "0.033 0.051 -0.018" rpy = "0 0 0"/> 
+    <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+  </joint>
+    
+</robot>

mycobot_description/urdf/mycobot_pro_630/mycobot_pro_630.urdf

@@ -1,3 +1,156 @@
+<<<<<<< HEAD
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/base.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </collision>
+        
+    </link>
+    <link name="joint1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link1.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.06 " rpy="0 0 3.1415926" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link1.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.06 " rpy=" 0 0 3.1415926" />
+        </collision>
+        
+    </link>
+    <link name="joint2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link2.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0446 " rpy=" 1.5707 -1.5707 -1.5707" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link2.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0446 " rpy=" 1.5707 -1.5707 -1.5707" />
+        </collision>
+        
+    </link>
+    <link name="joint3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link3.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0444 " rpy=" 0 1.5707 0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link3.dae" />
+            </geometry>
+            <origin xyz="0 0 -0.0444 " rpy="0 1.5707 0" />
+        </collision>
+        
+    </link>
+    <link name="joint4">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link4.dae" />
+            </geometry>
+            <origin xyz="0 -0.002 0.041 " rpy=" -1.5707 0 -1.5707" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link4.dae" />
+            </geometry>
+            <origin xyz="0 -0.002 0.041 " rpy=" -1.5707 0 -1.5707" />
+        </collision>
+        
+    </link>
+    <link name="joint5">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link5.dae" />
+            </geometry>
+            <origin xyz="-0.043 0  -0.08 " rpy=" 3.14159  1.5707 3.14159" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link5.dae" />
+            </geometry>
+            <origin xyz="-0.043 0  -0.08 " rpy=" 3.14159  1.5707 3.14159" />
+        </collision>
+        
+    </link>
+    <link name="joint6">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link6.dae" />
+            </geometry>
+            <origin xyz="0.01 0 0" rpy=" 0 1.5707  0" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_630/link6.dae" />
+            </geometry>
+            <origin xyz="0.01 0 0" rpy=" 0 1.5707  0" />
+        </collision>
+        
+    </link>
+    <joint name="joint1_to_base" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-3.14" upper="3.14159" velocity="0" />
+        <parent link="base" />
+        <child link="joint1" />
+        <origin xyz="0 0 0.22934" rpy="0 0 0" />
+    </joint>
+    
+    <joint name="joint2_to_joint1" type="revolute">
+        <axis xyz="0 0 -1" />
+        <limit effort="1000.0" lower="-2.26" upper="2.26" velocity="0" />
+        <parent link="joint1" />
+        <child link="joint2" />
+        <origin xyz="0 0  0" rpy="1.5707 0 0" />
+    </joint>
+    <joint name="joint3_to_joint2" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-2.61" upper="2.618" velocity="0" />
+        <parent link="joint2" />
+        <child link="joint3" />
+        <origin xyz="0 0.27 0   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint4_to_joint3" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-3.17" upper="3.17" velocity="0" />
+        <parent link="joint3" />
+        <child link="joint4" />
+        <origin xyz="0 0.267  -0.0745" rpy="0 0 0" />
+    </joint>
+    <joint name="joint5_to_joint4" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-2.99" upper="2.99" velocity="0" />
+        <parent link="joint4" />
+        <child link="joint5" />
+        <origin xyz="0 0 0.002" rpy="1.5707 -1.5707 0" />
+    </joint>
+    <joint name="joint6_to_joint5" type="revolute">
+        <axis xyz="-1 0 0" />
+        <limit effort="1000.0" lower="-3.03" upper="3.0368" velocity="0" />
+        <parent link="joint5" />
+        <child link="joint6" />
+        <origin xyz="-0.054 0 -0.08" rpy="-1.5707 0 0 " />
+    </joint>
+=======
 <?xml version="1.0"?>
 <robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
 	
@@ -172,6 +325,7 @@
     <origin xyz= "-0.054 0 0" rpy = "-1.5707 0 0 "/>  
   </joint>
 
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
 
 
 </robot>

mycobot_pro/mycobot_600/config/mycobot_600.rviz

@@ -8,6 +8,7 @@ Panels:
         - /Status1
         - /RobotModel1
         - /TF1
+        - /TF1/Frames1
       Splitter Ratio: 0.5
     Tree Height: 609
   - Class: rviz/Selection
@@ -68,6 +69,41 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        gripper_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_left1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_left2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_left3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_right1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_right2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_right3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         link1:
           Alpha: 1
           Show Axes: false
@@ -98,6 +134,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        tool2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -110,23 +151,43 @@ Visualization Manager:
       Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
-        All Enabled: true
+        All Enabled: false
         base:
+          Value: false
+        gripper_base:
           Value: true
+        gripper_left1:
+          Value: false
+        gripper_left2:
+          Value: false
+        gripper_left3:
+          Value: false
+        gripper_right1:
+          Value: false
+        gripper_right2:
+          Value: false
+        gripper_right3:
+          Value: false
         link1:
-          Value: true
+          Value: false
         link2:
-          Value: true
+          Value: false
         link3:
-          Value: true
+          Value: false
         link4:
-          Value: true
+          Value: false
         link5:
-          Value: true
+          Value: false
         link6:
+          Value: false
+        tool2:
           Value: true
       Marker Alpha: 1
+<<<<<<< HEAD
+      Marker Scale: 0.30000001192092896
+=======
       Marker Scale: 0.20000000298023224
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
       Name: TF
       Show Arrows: true
       Show Axes: true
@@ -139,6 +200,17 @@ Visualization Manager:
                 link4:
                   link5:
                     link6:
+                      tool2:
+                        gripper_base:
+                          gripper_left2:
+                            {}
+                          gripper_left3:
+                            gripper_left1:
+                              {}
+                          gripper_right2:
+                            {}
+                          gripper_right3:
+                            gripper_right1:
                               {}
       Update Interval: 0
       Value: true
@@ -170,7 +242,11 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
+<<<<<<< HEAD
+      Distance: 1.0985256433486938
+=======
       Distance: 2.0004639625549316
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -178,17 +254,29 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
+<<<<<<< HEAD
+        X: 0.3120266795158386
+        Y: 0.00957618746906519
+        Z: 0.04648329317569733
+=======
         X: -0.07064759731292725
         Y: -0.0814988762140274
         Z: 0.10758385062217712
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
+<<<<<<< HEAD
+      Pitch: 0.6053987145423889
+      Target Frame: <Fixed Frame>
+      Yaw: 2.4103732109069824
+=======
       Pitch: 0.45539799332618713
       Target Frame: <Fixed Frame>
       Yaw: 2.6153931617736816
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
     Saved: ~
 Window Geometry:
   Displays:
@@ -196,7 +284,11 @@ Window Geometry:
   Height: 906
   Hide Left Dock: false
   Hide Right Dock: false
+<<<<<<< HEAD
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000304fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000304000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005c8000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+=======
   QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000393000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+>>>>>>> 24a25596ca0e4bedd317e5ade3bb39d908910b0a
   Selection:
     collapsed: false
   Time:

mycobot_pro/mycobot_600/launch/mycobot_600_follow_display.launch

@@ -0,0 +1,11 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_600)/config/mycobot_600.rviz" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_pro/mycobot_600/launch/test.launch

@@ -1,6 +1,8 @@
 <launch>
 	<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
-	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pro_600/mycobot_pro_600.urdf"/>
+	<!-- <arg name="model" default="$(find mycobot_description)/urdf/mycobot_pro_600/mycobot_pro_600.urdf"/> -->
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf"/>
+	<!-- <arg name="model" default="$(find mycobot_description)/urdf/mycobot_pro_600/new_mycobot_pro_600_moveit.urdf"/> -->
 	<arg name="rvizconfig" default="$(find mycobot_600)/config/mycobot_600.rviz" />
 	<arg name="gui" default="true" />
 	

mycobot_pro/mycobot_600/launch/test_gripper.launch

@@ -0,0 +1,16 @@
+<launch>
+	<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_600)/config/mycobot_600.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_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
+
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_pro/mycobot_600/scripts/mycobot_600_follow_display.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python2
+# -*- coding:utf-8 -*-
+from cgi import print_environ
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+
+from pymycobot.mycobot import MyCobot
+
+
+def talker():
+    rospy.init_node("display", anonymous=True)
+
+    print("Try connect real mycobot...")
+    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    baud = rospy.get_param("~baud", 115200)
+    print("port: {}, baud: {}\n".format(port, baud))
+    try:
+        mycobot = MyCobot(port, baud)
+    except Exception as e:
+        print(e)
+        print(
+            """\
+            \rTry connect mycobot failed!
+            \rPlease check wether connected with mycobot.
+            \rPlease chckt wether the port or baud is right.
+        """
+        )
+        exit(1)
+    mycobot.release_all_servos()
+    time.sleep(0.1)
+    print("Rlease all servos over.\n")
+
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
+    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",
+    ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    marker_ = Marker()
+    marker_.header.frame_id = "/joint1"
+    marker_.ns = "my_namespace"
+
+    print("publishing ...")
+    while not rospy.is_shutdown():
+        joint_state_send.header.stamp = rospy.Time.now()
+
+        angles = mycobot.get_radians()
+        data_list = []
+        for index, value in enumerate(angles):
+            data_list.append(value)
+
+        # rospy.loginfo('{}'.format(data_list))
+        joint_state_send.position = data_list
+
+        pub.publish(joint_state_send)
+
+        coords = mycobot.get_coords()
+
+        # marker
+        marker_.header.stamp = rospy.Time.now()
+        marker_.type = marker_.SPHERE
+        marker_.action = marker_.ADD
+        marker_.scale.x = 0.04
+        marker_.scale.y = 0.04
+        marker_.scale.z = 0.04
+
+        # marker position initial
+        if not coords:
+            coords = [0, 0, 0, 0, 0, 0]
+
+            rospy.loginfo("error [101]: can not get coord values")
+
+        marker_.pose.position.x = coords[1] / 1000 * -1
+        marker_.pose.position.y = coords[0] / 1000
+        marker_.pose.position.z = coords[2] / 1000
+
+        marker_.color.a = 1.0
+        marker_.color.g = 1.0
+        pub_marker.publish(marker_)
+
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass

mycobot_pro/mycobot_600/scripts/test.py

@@ -0,0 +1,23 @@
+from pymycobot import ElephantRobot
+import time
+if __name__=='__main__':
+    # "Connect to robot server"
+    elephant_client = ElephantRobot("192.168.1.6", 5001)
+
+    # "Enable TCP communication"
+    elephant_client.start_client()
+
+    # "Setting"
+    # elephant_client.set_speed(20)   # set speed(20%)
+    # elephant_client.set_payload(2.0)   # set payload(2.0Kg)
+
+    # "Power on the robot"
+    # elephant_client.power_on()
+    
+    # "Start the system"
+    # elephant_client.state_on()
+
+    # "test move"
+    elephant_client.write_coords([-108, -140, 620, 88, -2, -37], 3000)
+    elephant_client.wait(3)
+    elephant_client.command_wait_done()

mycobot_pro/mycobot_600_gripper_moveit/.setup_assistant

@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: mycobot_description
+    relative_path: urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf
+    xacro_args: ""
+  SRDF:
+    relative_path: config/firefighter.srdf
+  CONFIG:
+    author_name: aa
+    author_email: aa@123.com
+    generated_timestamp: 1748499850

mycobot_pro/mycobot_600_gripper_moveit/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.1.3)
+project(mycobot_600_gripper_moveit)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

mycobot_pro/mycobot_600_gripper_moveit/config/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57

mycobot_pro/mycobot_600_gripper_moveit/config/chomp_planning.yaml

@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5

mycobot_pro/mycobot_600_gripper_moveit/config/fake_controllers.yaml

@@ -0,0 +1,19 @@
+controller_list:
+  - name: fake_arm_group_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - joint2_to_joint1
+      - joint3_to_joint2
+      - joint4_to_joint3
+      - joint5_to_joint4
+      - joint6_to_joint5
+      - joint6output_to_joint6
+  - name: fake_gripper_group_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - gripper_controller
+initial:  # Define initial robot poses per group
+  - group: arm_group
+    pose: init_pose
+  - group: gripper_group
+    pose: init_gripper

mycobot_pro/mycobot_600_gripper_moveit/config/firefighter.srdf

@@ -0,0 +1,207 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="firefighter">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="arm_group">
+        <chain base_link="base" tip_link="link6"/>
+    </group>
+    <group name="gripper_group">
+        <chain base_link="gripper_base" tip_link="gripper_left3"/>
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="init_pose" group="arm_group">
+        <joint name="joint2_to_joint1" value="0"/>
+        <joint name="joint3_to_joint2" value="0"/>
+        <joint name="joint4_to_joint3" value="0"/>
+        <joint name="joint5_to_joint4" value="0"/>
+        <joint name="joint6_to_joint5" value="0"/>
+        <joint name="joint6output_to_joint6" value="0"/>
+    </group_state>
+    <group_state name="init_gripper" group="gripper_group">
+        <joint name="gripper_controller" value="0"/>
+    </group_state>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="gripper" parent_link="link6" group="gripper_group" parent_group="arm_group"/>
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="base"/>
+    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
+    <passive_joint name="gripper_base_to_gripper_left2"/>
+    <passive_joint name="gripper_base_to_gripper_right2"/>
+    <passive_joint name="gripper_base_to_gripper_right3"/>
+    <passive_joint name="gripper_right3_to_gripper_right1"/>
+    <passive_joint name="gripper_left3_to_gripper_left1"/>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <!-- <disable_collisions link1="base" link2="link1" reason="Adjacent"/>
+    <disable_collisions link1="base" link2="link2" reason="Never"/>
+    <disable_collisions link1="base" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="gripper_left1" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="gripper_left2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="gripper_left3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="gripper_right2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="gripper_right3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="tool2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_left2" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_left3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_right3" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_left3" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_right3" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="gripper_right3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_right1" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="gripper_right3" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link4" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="tool2" reason="Never"/>
+    <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
+    <disable_collisions link1="link1" link2="link3" reason="Never"/>
+    <disable_collisions link1="link1" link2="link4" reason="Never"/>
+    <disable_collisions link1="link1" link2="link5" reason="Never"/>
+    <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
+    <disable_collisions link1="link2" link2="link4" reason="Never"/>
+    <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
+    <disable_collisions link1="link3" link2="link5" reason="Never"/>
+    <disable_collisions link1="link3" link2="link6" reason="Never"/>
+    <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
+    <disable_collisions link1="link4" link2="link6" reason="Never"/>
+    <disable_collisions link1="link4" link2="tool2" reason="Never"/>
+    <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
+    <disable_collisions link1="link5" link2="tool2" reason="Never"/>
+    <disable_collisions link1="link6" link2="tool2" reason="Adjacent"/> -->
+    <disable_collisions link1="base" link2="link1" reason="Adjacent"/>
+    <disable_collisions link1="base" link2="link2" reason="Never"/>
+    <disable_collisions link1="base" link2="link3" reason="Never"/>
+    <disable_collisions link1="base" link2="link4" reason="Never"/>
+    <disable_collisions link1="base" link2="link5" reason="Never"/>
+    <disable_collisions link1="base" link2="link6" reason="Never"/>
+    <disable_collisions link1="base" link2="tool2" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_base" reason="Never"/>
+    <!-- <disable_collisions link1="base" link2="gripper_left3" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_right3" reason="Never"/> -->
+    <disable_collisions link1="base" link2="gripper_left1" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_left2" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_left3" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="base" link2="gripper_right3" reason="Never"/>
+    
+    
+    <disable_collisions link1="gripper_base" link2="gripper_left1" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="gripper_left2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="gripper_left3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="gripper_right2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="gripper_right3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_base" link2="link5" reason="Never"/>
+    <!-- <disable_collisions link1="gripper_base" link2="link6" reason="Never"/> -->
+    <disable_collisions link1="gripper_base" link2="link3" reason="Never"/>
+    <disable_collisions link1="gripper_base" link2="link2" reason="Never"/>
+    <!-- <disable_collisions link1="gripper_base" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_base" link2="tool2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_left2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_left3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_right1" reason="Default"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="gripper_right3" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_left1" link2="link6" reason="Never"/>
+    
+    <!-- <disable_collisions link1="gripper_left1" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_left1" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_left3" reason="Default"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="gripper_right3" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_left2" link2="link6" reason="Never"/>
+    
+    <!-- <disable_collisions link1="gripper_left2" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_left2" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="gripper_right1" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="gripper_right2" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="gripper_right3" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_left3" link2="link6" reason="Never"/>
+    
+    <!-- <disable_collisions link1="gripper_left3" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_left3" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="gripper_right2" reason="Adjacent"/>
+    <disable_collisions link1="gripper_right1" link2="gripper_right3" reason="Adjacent"/>
+    <disable_collisions link1="gripper_right1" link2="link5" reason="Never"/>
+    <!-- <disable_collisions link1="gripper_right1" link2="link6" reason="Never"/> -->
+    <disable_collisions link1="gripper_right1" link2="link3" reason="Never"/>
+    <disable_collisions link1="gripper_right1" link2="link2" reason="Never"/>
+    
+    <!-- <disable_collisions link1="gripper_right1" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_right1" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="gripper_right3" reason="Default"/>
+    <disable_collisions link1="gripper_right2" link2="link5" reason="Never"/>
+    <!-- <disable_collisions link1="gripper_right2" link2="link6" reason="Never"/> -->
+    <disable_collisions link1="gripper_right2" link2="link3" reason="Never"/>
+    <disable_collisions link1="gripper_right2" link2="link2" reason="Never"/>
+    <!-- <disable_collisions link1="gripper_right2" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_right2" link2="tool2" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link5" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link6" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link3" reason="Never"/>
+    <disable_collisions link1="gripper_right3" link2="link2" reason="Never"/>
+    <!-- <disable_collisions link1="gripper_right3" link2="link4" reason="Never"/> -->
+    <disable_collisions link1="gripper_right3" link2="tool2" reason="Never"/>
+    <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
+    <!-- <disable_collisions link1="link1" link2="tool2" reason="Never"/> -->
+    <disable_collisions link1="link1" link2="link3" reason="Never"/>
+
+    <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
+    <!-- <disable_collisions link1="link2" link2="tool2" reason="Never"/> -->
+    <disable_collisions link1="link2" link2="link4" reason="Never"/>
+
+    <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
+    <!-- <disable_collisions link1="link3" link2="tool2" reason="Never"/> -->
+    <disable_collisions link1="link3" link2="link5" reason="Never"/>
+
+    <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
+    <!-- <disable_collisions link1="link4" link2="tool2" reason="Never"/> -->
+    <disable_collisions link1="link4" link2="link6" reason="Never"/>
+
+    <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
+    <disable_collisions link1="link5" link2="tool2" reason="Never"/>
+    <!-- <disable_collisions link1="link6" link2="tool1" reason="Adjacent"/> -->
+    <disable_collisions link1="link6" link2="tool2" reason="Adjacent"/>
+    <!-- <disable_collisions link1="tool1" link2="tool2" reason="Adjacent"/> -->
+</robot>

mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_controllers.yaml

@@ -0,0 +1,4 @@
+# Publish joint_states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50

mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_firefighter.urdf

@@ -0,0 +1,555 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
+    <xacro:property name="width" value=".2" />
+    <link name="base">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04  1.5708 3.14159-->
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J1-W.dae" />
+            </geometry>
+            <origin xyz="0 0 0 " rpy=" 1.5706 0 0" />
+        </visual>
+        <collision>
+            <origin xyz="0 0 0.04 " rpy=" 0 0 1.5708" />
+            <geometry>
+                <cylinder length="0.06" radius="0.038" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="link1">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J2-W.dae" />
+            </geometry>
+            <origin xyz="0 0.05 -0.08 " rpy="0 0 3.14159" />
+        </visual>
+        <collision>
+            <origin xyz="0.0 0.0 0.04 " rpy=" 0 0 1.5708" />
+            <geometry>
+                <cylinder length="0.05" radius="0.03" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="link2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J3-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.092 0.044 " rpy=" 0 -1.5706 0" />
+        </visual>
+        <collision>
+            <origin xyz="0 0.128 -0.04 " rpy=" 1.5708 0 0" />
+            <geometry>
+                <cylinder length="0.28" radius="0.024" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="link3">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J4-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.09 -0.01 " rpy=" 0 1.5706 0" />
+        </visual>
+        <collision>
+            <origin xyz="0.0 0.128 0.076 " rpy="1.5708 0 0" />
+            <geometry>
+                <cylinder length="0.24" radius="0.024" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="link4">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J5-W.dae" />
+            </geometry>
+            <origin xyz="0 -0.6987 0 " rpy=" 0 0 0" />
+        </visual>
+        <collision>
+            <origin xyz="0.0 0.02 -0.05" rpy=" 1.5708 1.5708 0" />
+            <geometry>
+                <cylinder length="0.05" radius="0.024" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="link5">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J6-W.dae" />
+            </geometry>
+            <origin xyz="0 0 0.048 " rpy=" 1.5706  0 3.14159" />
+        </visual>
+        <collision>
+            <origin xyz="0.0 0.0 0.038 " rpy=" 0  0 -1.5708" />
+            <geometry>
+                <cylinder length="0.05" radius="0.028" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="link6">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/J7-W.dae" />
+            </geometry>
+            <material name="grey">
+                <color rgba="0.5 0.5 0.5 1" />
+            </material>
+            <origin xyz="0.06 0 0" rpy=" 3.14159 3.14159  0" />
+        </visual>
+        <collision>
+            <origin xyz="0.003 0.0 -0.0 " rpy=" 0 1.5708 0" />
+            <geometry>
+                <cylinder length="0.006" radius="0.026" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <!-- <link name="tool1">
+    <visual>
+        <geometry>
+            <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/tool1-mm.dae" />
+        </geometry>
+        <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+    </visual>
+    <collision>
+        <geometry>
+            <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/tool1-mm.dae" />
+        </geometry>
+        <origin xyz="0.0 0 0" rpy=" -1.5706 0  0" />
+    </collision>   
+  </link> -->
+    <link name="tool2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_pro_600/tool2-mm.dae" />
+            </geometry>
+            <origin xyz="0.0 0 0" rpy=" 0 0  -1.5706" />
+        </visual>
+        <collision>
+            <origin xyz="0.003 0.0 0.005 " rpy=" 0 0 -1.5708" />
+            <geometry>
+                <cylinder length="0.006" radius="0.026" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_base">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_base.dae" />
+            </geometry>
+            <origin xyz="-0.02 0.012 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.026" length="0.03" />
+            </geometry>
+            <origin xyz="0.0 0 0.01 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left1">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left1.dae" />
+            </geometry>
+            <origin xyz="0.036 -0.052 0.0 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.004" length="0.005" />
+            </geometry>
+            <origin xyz="0.0 0.0 0.002 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left2.dae" />
+            </geometry>
+            <origin xyz="0.03 0.024 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.003" length="0.003" />
+            </geometry>
+            <origin xyz="0.00 -0.0 -0.003 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_left3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_left3.dae" />
+            </geometry>
+            <origin xyz="0.034 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+            <!-- <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/> -->
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.018" length="0.003" />
+            </geometry>
+            <origin xyz="-0.002  0 -0.016 " rpy=" 0 3.14159 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right1">
+        <visual>
+            <geometry>
+                <!--- 0.0 0 -0.04 -->
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right1.dae" />
+            </geometry>
+            <origin xyz="-0.072 -0.05 0.0 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.004" length="0.005" />
+            </geometry>
+            <origin xyz="0.0 0.0 0.002 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right2">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right2.dae" />
+            </geometry>
+            <origin xyz="-0.066 0.023 -0.018 " rpy=" 0 0 1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.003" length="0.003" />
+            </geometry>
+            <origin xyz="0.00 -0.0 -0.003 " rpy=" 0 0 1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <link name="gripper_right3">
+        <visual>
+            <geometry>
+                <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2022/gripper_right3.dae" />
+            </geometry>
+            <origin xyz="0 0.0 -0.012 " rpy=" 0 3.14159 -1.5708" />
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.018" length="0.003" />
+            </geometry>
+            <origin xyz="-0.002  0 -0.016 " rpy=" 0 3.14159 -1.5708" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
+        </inertial>
+    </link>
+    <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="base" />
+        <child link="link1" />
+        <origin xyz="0 0 0.1655" rpy="0 0 0" />
+    </joint>
+    <joint name="joint3_to_joint2" type="revolute">
+        <axis xyz="0 0 -1" />
+        <!-- <limit effort = "1000.0" lower = "-4.71" upper = "1.5708" velocity = "0"/>   -->
+        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0" />
+        <parent link="link1" />
+        <child link="link2" />
+        <!-- <origin xyz= "0 0  0" rpy = "1.5708 -1.5708 0"/>   -->
+        <origin xyz="0 0.042  0.072" rpy="1.5708 1.5706 0" />
+    </joint>
+    <joint name="joint4_to_joint3" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <limit effort="1000.0" lower="-2.61" upper="2.618" velocity="0" />
+        <parent link="link2" />
+        <child link="link3" />
+        <origin xyz="0.0 0.25 0.005   " rpy="0 0 0" />
+    </joint>
+    <joint name="joint5_to_joint4" type="revolute">
+        <axis xyz=" 0 0 -1" />
+        <!-- <limit effort = "1000.0" lower = "-4.53" upper = "1.3962" velocity = "0"/> -->
+        <limit effort="1000.0" lower="-2.97" upper="2.97" velocity="0" />
+        <parent link="link3" />
+        <child link="link4" />
+        <!-- <origin xyz= "0.25 0  -0.1091" rpy = "0 0 1.5708"/>   -->
+        <origin xyz="0 0.25  0.025" rpy="0 0 -1.5706" />
+    </joint>
+    <joint name="joint6_to_joint5" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-2.93" upper="2.9321" velocity="0" />
+        <parent link="link4" />
+        <child link="link5" />
+        <origin xyz="0 0.058 -0.052" rpy="1.57080 -1.57080 3.14159" />
+    </joint>
+    <joint name="joint6output_to_joint6" type="revolute">
+        <axis xyz="-1 0 0" />
+        <limit effort="1000.0" lower="-3.03" upper="3.0368" velocity="0" />
+        <parent link="link5" />
+        <child link="link6" />
+        <origin xyz="-0.06 0 0.048" rpy="-1.57080 0 0 " />
+    </joint>
+    <joint name="joint6output_to_tool2" type="fixed">
+        <axis xyz="0 1 0" />
+        <limit effort="1000.0" lower="-3.05" upper="3.05" velocity="0" />
+        <parent link="link6" />
+        <child link="tool2" />
+        <origin xyz="-0.016 0.0 0.0" rpy="-1.5706 0 1.5706" />
+    </joint>
+    <!-- <joint name="tool1_to_tool2" type="fixed">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="tool1" />
+    <child link="tool2" />
+    <origin xyz="0.0 0.008 0.00" rpy="1.5708 -1.5706 3.14159" />
+  </joint> -->
+    <joint name="tool2_to_gripper_base" type="fixed">
+        <axis xyz="0 0 -1" />
+        <limit effort="1000.0" lower="-3.05" upper="3.05" velocity="0" />
+        <parent link="tool2" />
+        <child link="gripper_base" />
+        <origin xyz="0.005 0.0 0.047" rpy="1.5706 0 1.5706" />
+    </joint>
+    <joint name="gripper_controller" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.7" upper="0.3" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left3" />
+        <origin xyz="-0.018 0.016 0" rpy="0 0 0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_left2" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.8" upper="0.5" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_left2" />
+        <origin xyz="-0.05 -0.01 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint>
+    <joint name="gripper_left3_to_gripper_left1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_left3" />
+        <child link="gripper_left1" />
+        <origin xyz="-0.0356 0.051 -0.018" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_right3" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.3" upper="0.7" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right3" />
+        <origin xyz="0.018 0.016 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_base_to_gripper_right2" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.8" velocity="0" />
+        <parent link="gripper_base" />
+        <child link="gripper_right2" />
+        <origin xyz="0.046 -0.01 0" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
+    </joint>
+    <joint name="gripper_right3_to_gripper_right1" type="revolute">
+        <axis xyz="0 0 1" />
+        <limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0" />
+        <parent link="gripper_right3" />
+        <child link="gripper_right1" />
+        <origin xyz="0.033 0.051 -0.018" rpy="0 0 0" />
+        <mimic joint="gripper_controller" multiplier="1.0" offset="0" />
+    </joint>
+    <transmission name="trans_joint2_to_joint1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint2_to_joint1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint2_to_joint1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint3_to_joint2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint3_to_joint2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint3_to_joint2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint4_to_joint3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint4_to_joint3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint4_to_joint3_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint5_to_joint4">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint5_to_joint4">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint5_to_joint4_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6_to_joint5">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6_to_joint5">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6_to_joint5_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6output_to_joint6">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6output_to_joint6">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6output_to_joint6_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_controller">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_controller">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_controller_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_base_to_gripper_left2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_base_to_gripper_left2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_base_to_gripper_left2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_left3_to_gripper_left1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_left3_to_gripper_left1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_left3_to_gripper_left1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_base_to_gripper_right3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_base_to_gripper_right3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_base_to_gripper_right3_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_base_to_gripper_right2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_base_to_gripper_right2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_base_to_gripper_right2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_gripper_right3_to_gripper_right1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="gripper_right3_to_gripper_right1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="gripper_right3_to_gripper_right1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <gazebo>
+        <plugin name="gazebo_ros_control">
+            <robotNamespace>/</robotNamespace>
+        </plugin>
+    </gazebo>
+</robot>
+