Fix new_320 Moveit joint motion jitter problems

mycobot_320/new_mycobot_320/scripts/mycobot_320_slider.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 # -*- coding:utf-8 -*-
 """[summary]
 This file obtains the joint angle of the manipulator in ROS,
@@ -8,7 +8,8 @@ Passable parameters:
     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,11 +23,11 @@ def callback(data):
     
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
-  
-    # time.sleep(0.5)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
 
 def listener():
     global mc
@@ -38,7 +39,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
- 
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
     # spin() simply keeps python from exiting until this node is stopped
     print("spin ...")
     rospy.spin()

mycobot_320/new_mycobot_320_gripper_moveit/launch/moveit.rviz

@@ -42,7 +42,7 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Acceleration_Scaling_Factor: 0.1
+    - Acceleration_Scaling_Factor: 0.5000000000000001
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
       Move Group Namespace: ""
@@ -263,7 +263,7 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: true
       Value: true
-      Velocity_Scaling_Factor: 0.1
+      Velocity_Scaling_Factor: 0.5000000000000001
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

mycobot_320/new_mycobot_320_gripper_moveit/scripts/sync_plan.py

@@ -28,12 +28,12 @@ def callback(data):
 
     data_list = data_list[:7]
     print("radians:%s"%data_list[:6])
-    mc.send_radians(data_list[:6], 80)
+    mc.send_radians(data_list[:6], 25)
     gripper_value = int(abs(-0.7-data_list[6])* 117)
     print("gripper_value:%s"%gripper_value)
     # mc.set_gripper_mode(0)
     # time.sleep(1)
-    mc.set_gripper_value(gripper_value, 100)
+    mc.set_gripper_value(gripper_value, 90)
     
 def listener():
     global mc
@@ -46,10 +46,15 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
- 
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
     # spin() simply keeps python from exiting until this node is stopped
     print("spin ...")
-    rospy.spin()
+    # rospy.spin()
+    rate = rospy.Rate(1)
+    while not rospy.is_shutdown():
+        rate.sleep()
 
 
 if __name__ == "__main__":

mycobot_320/new_mycobot_320_moveit/launch/moveit.rviz

@@ -11,7 +11,7 @@ Panels:
         - /MotionPlanning1/Planned Path1
         - /TF1
       Splitter Ratio: 0.7425600290298462
-    Tree Height: 134
+    Tree Height: 95
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -44,7 +44,7 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Acceleration_Scaling_Factor: 1
+    - Acceleration_Scaling_Factor: 0.5000000000000001
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
       Move Group Namespace: ""
@@ -195,7 +195,7 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: false
       Value: true
-      Velocity_Scaling_Factor: 1
+      Velocity_Scaling_Factor: 0.5000000000000001
     - Class: rviz/TF
       Enabled: false
       Filter (blacklist): ""
@@ -252,7 +252,7 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 954
+  Height: 906
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -261,7 +261,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002ad00000360fc0200000007fb000000100044006900730070006c006100790073010000003d00000117000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007200000001860000004a0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000015a000002430000017d00ffffff000004850000036000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f300000330fc0200000007fb000000100044006900730070006c006100790073010000003d000000f0000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000133000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017a000001f30000017d00ffffff0000053f0000033000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1848

mycobot_320/new_mycobot_320_moveit/scripts/sync_plan.py

@@ -1,6 +1,7 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # -*- coding:utf-8 -*-
 import time
+import math
 import subprocess
 import rospy
 from sensor_msgs.msg import JointState
@@ -12,29 +13,34 @@ mc = None
 
 
 def callback(data):
-    rospy.loginfo(rospy.get_caller_id() + "%s", data)
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
-        #     value *= -1
-        data_list.append(value)
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
     global mc
     rospy.init_node("mycobot_reciver", anonymous=True)
 
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    port = rospy.get_param("~port", "/dev/ttyACM0")
     baud = rospy.get_param("~baud", 115200)
-    # 1000000
-    mc = MyCobot(port, baud)
 
+    mc = MyCobot(port, baud)
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped
-    rospy.spin()
+    # rospy.spin()
+    rate = rospy.Rate(1)
+    while not rospy.is_shutdown():
+        rate.sleep()
 
 
 if __name__ == "__main__":

mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_slider.py

@@ -9,7 +9,8 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyAMA0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import math
+import time
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -24,12 +25,11 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
-
-    print('data_list:', data_list)
-    # time.sleep(0.5)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -41,6 +41,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
 
     # spin() simply keeps python from exiting until this node is stopped
     print("spin ...")

mycobot_320/new_mycobot_320_pi_moveit/launch/moveit.rviz

@@ -43,7 +43,7 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Acceleration_Scaling_Factor: 1
+    - Acceleration_Scaling_Factor: 0.5000000000000001
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
       Move Group Namespace: ""
@@ -194,7 +194,7 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: false
       Value: true
-      Velocity_Scaling_Factor: 1
+      Velocity_Scaling_Factor: 0.5000000000000001
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py

@@ -18,9 +18,9 @@ def callback(data):
     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.send_radians(data_list, 80)
-    mc.send_angles(data_list, 60)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -31,7 +31,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     # 115200
     mc = MyCobot(port, baud)
-
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped