新增 mycobot600 机械臂跟随仿真运动

elephantrobotics · Aug 13, 2024 · 4388a63 · 4388a63
1 parent 4117a75
commit 4388a63
Show file tree

Hide file tree

Showing 120 changed files with 3,601 additions and 883 deletions.
diff --git a/mycobot_280/mycobot_280/scripts/follow_display.py b/mycobot_280/mycobot_280/scripts/follow_display.py
@@ -13,7 +13,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:
@@ -41,6 +41,7 @@ def talker():
     joint_state_send.header = Header()
 
     joint_state_send.name = [
+
         "joint2_to_joint1",
         "joint3_to_joint2",
         "joint4_to_joint3",

diff --git a/mycobot_320/mycobot_320_gripper_moveit/.setup_assistant b/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_email: wwj@elephantrobot.com
+    author_name: Jz
+    author_email: Jz@elephantrobot.com
     generated_timestamp: 1683628879
diff --git a/mycobot_320/mycobot_320_gripper_moveit/config/fake_controllers.yaml b/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
-  - group: arm_group
-    pose: init_pose
-  - group: gripper_group
-    pose: init_gripper
+# initial:  # Define initial robot poses per group
+#   - group: arm_group
+#     pose: init_pose
+#   - group: gripper_group
+#     pose: init_gripper
diff --git a/mycobot_320/mycobot_320_gripper_moveit/launch/moveit.rviz b/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,14 +45,15 @@ 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
       MoveIt_Allow_Replanning: false
       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
+          joint2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          joint3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link2:
+          joint4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link3:
+          joint5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link4:
+          joint6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link5:
+          tool1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link6:
+          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
+          joint2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          joint3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link2:
+          joint4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link3:
+          joint5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link4:
+          joint6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link5:
+          tool1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link6:
+          tool2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -314,25 +327,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2
+      Distance: 1.4812843799591064
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.10000000149011612
-        Y: 0.25
-        Z: 0.30000001192092896
+        X: -0.07944566011428833
+        Y: 0.29104751348495483
+        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
-  X: 454
+  Width: 1842
+  X: 72
   Y: 27
diff --git a/mycobot_320/mycobot_320_gripper_moveit/launch/planning_context.launch b/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">

diff --git a/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py b/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py
@@ -8,7 +8,16 @@
     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
@@ -38,17 +47,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()