opt code and fix joint movement jitter

Mercury/mercury_a1/scripts/slider_control.py

@@ -8,6 +8,7 @@
     port: serial prot string. Defaults is '/dev/ttyAMA1'
     baud: serial prot baudrate. Defaults is 115200.
 """
+import time
 import math
 import rospy
 from sensor_msgs.msg import JointState
@@ -25,12 +26,11 @@ def callback(data):
     rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple)
     data_list = []
     for index, value in enumerate(data.position):
-        radians_to_angles = math.degrees(value)
+        radians_to_angles = round(math.degrees(value), 2)
         data_list.append(radians_to_angles)
-
-    # mc.send_radians(data_list, 80)
-    mc.send_angles(data_list, 80)
-    # time.sleep(0.5)
+
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -42,6 +42,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = Mercury(port, baud)
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

Mercury/mercury_a1_moveit/scripts/sync_plan.py

@@ -21,17 +21,13 @@
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    # print(data.position)
-    rounded_data_tuple = tuple(round(value, 2) for value in data.position)
-    rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple)
     data_list = []
     for index, value in enumerate(data.position):
         radians_to_angles = round(math.degrees(value), 2)
         data_list.append(radians_to_angles)
-
-    # mc.send_radians(data_list, 80)
-    mc.send_angles(data_list, 80)
-    # time.sleep(0.5)
+
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
 
 
 def listener():

Mercury/mercury_b1/scripts/slider_control.py

@@ -24,8 +24,7 @@
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    rounded_data_tuple = tuple(round(value, 2) for value in data.position)
-    # print(rounded_data_tuple)
+
     data_list = []
     for index, value in enumerate(data.position):
         radians_to_angles = round(math.degrees(value), 2)
@@ -39,15 +38,15 @@ def callback(data):
     print('right_arm:', right_arm)
     print('middle_arm:', middle_arm)
 
-    l.send_angles(left_arm, 50)
+    l.send_angles(left_arm, 25)
     time.sleep(0.02)
-    r.send_angles(right_arm, 50)
+    r.send_angles(right_arm, 25)
     time.sleep(0.02)
-    r.send_angle(11, middle_arm[0], 50)
+    r.send_angle(11, middle_arm[0], 25)
     time.sleep(0.02)
-    r.send_angle(12, middle_arm[1], 50)
+    r.send_angle(12, middle_arm[1], 25)
     time.sleep(0.02)
-    r.send_angle(13, middle_arm[2], 50)
+    r.send_angle(13, middle_arm[2], 25)
     time.sleep(0.02)
 
 
@@ -63,7 +62,10 @@ def listener():
     print(port2, baud)
     l = Mercury(port1, baud)
     r = Mercury(port2, baud)
-
+    time.sleep(0.05)
+    l.set_free_mode(1)
+    r.set_free_mode(1)
+    time.sleep(0.05)
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")

Mercury/mercury_b1_moveit/scripts/sync_plan.py

@@ -17,16 +17,15 @@
 from pymycobot.mercury import Mercury
 
 # left arm port
-cx1 = None
+l = None
 
 # right arm port
-cx2 = None
+r = None
 
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    rounded_data_tuple = tuple(round(value, 2) for value in data.position)
-    # print(rounded_data_tuple)
+
     data_list = []
     for index, value in enumerate(data.position):
         radians_to_angles = round(math.degrees(value), 2)
@@ -40,23 +39,20 @@ def callback(data):
     print('right_arm:', right_arm)
     print('middle_arm:', middle_arm)
 
-    cx1.send_angles(left_arm, 50)
+    l.send_angles(left_arm, 25)
     time.sleep(0.02)
-    cx2.send_angles(right_arm, 50)
+    r.send_angles(right_arm, 25)
     time.sleep(0.02)
-    cx2.send_angle(11, middle_arm[0], 50)
+    r.send_angle(11, middle_arm[0], 25)
     time.sleep(0.02)
-    cx2.send_angle(12, middle_arm[1], 50)
+    r.send_angle(12, middle_arm[1], 25)
     time.sleep(0.02)
-    cx2.send_angle(13, middle_arm[2], 50)
+    r.send_angle(13, middle_arm[2], 25)
     time.sleep(0.02)
-    # mc.send_radians(data_list, 80)
-    # mc.send_angles(data_list, 80)
-    # time.sleep(0.5)
 
 
 def listener():
-    global cx1, cx2
+    global l, r
     rospy.init_node("control_slider", anonymous=True)
 
     rospy.Subscriber("joint_states", JointState, callback)
@@ -65,9 +61,12 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port1, baud)
     print(port2, baud)
-    cx1 = Mercury(port1, baud)
-    cx2 = Mercury(port2, baud)
-
+    l = Mercury(port1, baud)
+    r = Mercury(port2, baud)
+    time.sleep(0.05)
+    l.set_free_mode(1)
+    r.set_free_mode(1)
+    time.sleep(0.05)
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")

mecharm/mecharm/scripts/slider_control.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 # -*- coding:utf-8 -*-
 
 """[summary]
@@ -9,7 +9,8 @@
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import math
+import time
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -22,25 +23,27 @@
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
-
-    # print(data_list)
-    mc.send_radians(data_list, 80)
-    # time.sleep(0.5)
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
+        
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
 
     rospy.Subscriber("joint_states", JointState, callback)
-    port = rospy.get_param("~port", "/dev/ttyUSB0") # Select connected device. 选择连接设备
+    port = rospy.get_param("~port", "/dev/ttyUSB0") 
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin() 只是阻止python退出，直到该节点停止

mecharm/mecharm_moveit/CMakeLists.txt

@@ -13,7 +13,6 @@ catkin_package()
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
   scripts/sync_plan.py
-  scripts/path_planning_and_obstacle_avoidance_demo.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )