opt code and fix joint movement jitter

Mercury/mercury_a1/scripts/slider_control.py

@@ -8,6 +8,7 @@ Passable parameters:
     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)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_angles(data_list, 80)
+    mc.send_angles(data_list, 25)
-    # time.sleep(0.5)
 
 
 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/obstacle_avoidance_demo.py

@@ -1,163 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-
-import sys
-import rospy
-import moveit_commander
-import moveit_msgs.msg
-import geometry_msgs.msg
-import tf
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        rospy.init_node('moveit_avoid_obstacles', anonymous=True)
-        # 初始化MoveIt
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # 创建RobotCommander对象
-        self.robot = moveit_commander.RobotCommander()
-
-        # 创建PlanningSceneInterface对象
-        self.scene = moveit_commander.PlanningSceneInterface()
-
-        # 创建MoveGroupCommander对象
-        self.arm_group = moveit_commander.MoveGroupCommander("arm_group")
-        
-        # 获取末端关节的名称
-        self.end_effector_link = self.arm_group.get_end_effector_link()
-        
-        # 设置目标位置所使用的坐标参考系
-        self.reference_frame = 'base'
-        self.arm_group.set_pose_reference_frame(self.reference_frame)
-        
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm_group.set_goal_position_tolerance(0.01)
-        self.arm_group.set_goal_orientation_tolerance(0.05)
-        
-        # 当运动规划失败后，允许重新规划
-        self.arm_group.allow_replanning(True)
-        # 设置规划的最大时间为20秒
-        self.arm_group.set_planning_time(20)
-        # 设置规划尝试次数为10次（或者更大的值）
-        self.arm_group.set_num_planning_attempts(20)  
-
-        
-    def add_scene(self):
-        # 添加第一个圆柱作为障碍物（垂直于平面）
-        cylinder1_pose = geometry_msgs.msg.PoseStamped()
-        cylinder1_pose.header.frame_id = self.robot.get_planning_frame()
-        cylinder1_pose.pose.position.x = 0.15
-        cylinder1_pose.pose.position.y = 0
-        cylinder1_pose.pose.position.z = 0.30
-        cylinder1_pose.pose.orientation.w = 1.0
-        self.scene.add_cylinder("cylinder1", cylinder1_pose, height=0.6, radius=0.01)
-
-        # 添加第二个圆柱作为障碍物（水平于平面，构成十字架）
-        cylinder2_pose = geometry_msgs.msg.PoseStamped()
-        cylinder2_pose.header.frame_id = self.robot.get_planning_frame()
-        cylinder2_pose.pose.position.x = 0.15
-        cylinder2_pose.pose.position.y = 0
-        cylinder2_pose.pose.position.z = 0.40
-        cylinder2_pose.pose.orientation.w = 1.0
-        cylinder2_pose.pose.orientation.x = 0.707  # 围绕x轴旋转90度（水平方向）
-        cylinder2_pose.pose.orientation.y = 0.0
-        cylinder2_pose.pose.orientation.z = 0.0
-        cylinder2_pose.pose.orientation.w = 0.707
-        self.scene.add_cylinder("cylinder2", cylinder2_pose, height=0.6, radius=0.02)
-        # 发布当前场景信息
-        planning_scene = moveit_msgs.msg.PlanningScene()
-        planning_scene.world.collision_objects.extend(self.scene.get_objects().values())
-        planning_scene.is_diff = True
-
-        planning_scene_publisher = rospy.Publisher('/planning_scene', moveit_msgs.msg.PlanningScene, queue_size=1)
-        planning_scene_publisher.publish(planning_scene)
-        rospy.sleep(2)
-
-    def robot_move(self):
-
-        # 控制机械臂回到初始化位置
-        self.arm_group.set_named_target('init_pose')
-        self.arm_group.go()
-        rospy.sleep(3)
-
-        
-        # 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示
-        target_pose = geometry_msgs.msg.PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.142  # 设置目标点的x坐标
-        target_pose.pose.position.y = -0.140  # 设置目标点的y坐标
-        target_pose.pose.position.z = 0.075  # 设置目标点的z坐标
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-        
-        # 更新当前的位姿
-        self.arm_group.set_start_state_to_current_state()
-
-        # 获取机械臂当前的关节状态
-        current_joint_values = self.arm_group.get_current_joint_values()
-        
-        # 打印当前关节状态
-        print("Current Joint Values:", current_joint_values)
-        print("end Joint Values:", self.end_effector_link)
-        # 设置机械臂的目标姿态
-        self.arm_group.set_pose_target(target_pose, self.end_effector_link)
-        # 进行运动规划
-        plan = self.arm_group.plan()
-        # print('plan point:', plan[1])
-        # 执行运动
-        self.arm_group.execute(plan[1])
-        rospy.sleep(3)
-        # 获取末端执行器的姿态
-        end_effector_pose = self.arm_group.get_current_pose().pose
-
-        # 打印末端执行器的坐标位置
-        print("End Effector Position:", end_effector_pose.position)
-        print("End Effector Orientation:", end_effector_pose.orientation)
-        # 控制机械臂末端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        # self.arm_group.shift_pose_target(1, 0.22, self.end_effector_link)
-        # self.arm_group.go()
-        # rospy.sleep(5)
-    
-        # 设置机械臂的目标位置，使用7轴的位置数据进行描述（单位：弧度）
-        # joint_pose = [0.2967, 0, 0, -1.57000, 0, -1.3439, 0]
-        # joint_pose = [0.2967, 0, 0, 0, 0, -1.3439, 0]
-        # arm_group.set_joint_value_target(joint_pose)
-      
-        # 控制机械臂完成运动
-        # arm_group.go()
-        # rospy.sleep(10)
-        # 控制机械臂回到初始化位置
-        # arm_group.set_named_target('init_pose')
-        # arm_group.go()
-        
-    def run(self):
-        # 移除所有障碍物
-        # self.scene.remove_world_object("cylinder1")
-        # self.scene.remove_world_object("cylinder2")
-        # 没有障碍物运行一次
-        # self.robot_move()
-        
-        # 增加障碍物
-        self.add_scene()
-        rospy.sleep(3)
-        # 获取当前场景中的所有障碍物
-        current_obstacles = self.scene.get_known_object_names()
-        rospy.loginfo("Current obstacles in the scene: %s", current_obstacles)
-        rospy.sleep(2)
-        # 有障碍物后再运行一次
-        self.robot_move()
-        # 关闭MoveIt
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-        
-
-if __name__ == '__main__':
-    try:
-        obstacle = MoveItPlanningDemo()
-        obstacle.run()
-    except rospy.ROSInterruptException:
-        pass

Mercury/mercury_a1_moveit/scripts/sync_plan.py

@@ -21,17 +21,13 @@ mc = None
 
 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)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_angles(data_list, 80)
+    mc.send_angles(data_list, 25)
-    # time.sleep(0.5)
 
 
 def listener():

Mercury/mercury_b1/scripts/slider_control.py

@@ -24,8 +24,7 @@ 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)
@@ -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 sensor_msgs.msg import JointState
 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)
+    l = Mercury(port1, baud)
-    cx2 = Mercury(port2, 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 @@ 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,14 +23,13 @@ mc = None
 
 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)
+        radians_to_angles = round(math.degrees(value), 2)
-
+        data_list.append(radians_to_angles)
-    # print(data_list)
+        
-    mc.send_radians(data_list, 80)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # time.sleep(0.5)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -37,10 +37,13 @@ def listener():
     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}
 )
 

mecharm/mecharm_moveit/launch/moveit.rviz

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

mecharm/mecharm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,132 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group. 初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to listen for changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group 
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link. 获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position.
-        #  设置目标位置所使用的参考坐标系
-        self.reference_frame = "joint1"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails. 
-        # 当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian). 
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # Control the robotic arm to return to the initialization position first. 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system 
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion. 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm. 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-
-        traj = self.arm.plan()
-
-        # Control the motion of the robotic arm according to the planned motion path. 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參数1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端反向旋转90度,  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        self.moving()
-
-        # Add the environment. 添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        suit_path = (
-            roslib.packages.get_pkg_dir("mycobot_description")
-            + "/urdf/mycobot/suit_env.dae"
-        )
-        # need `pyassimp==3.3`
-        self.scene.add_mesh("suit", suit_post, suit_path)
-        rospy.sleep(2)
-
-        # Run again if there is environmental impact. 有环境影响后再运行一次
-        self.moving()
-
-        # Close and exit moveit. 关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

mecharm/mecharm_moveit/scripts/sync_plan.py

@@ -1,5 +1,6 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # encoding=utf-8
+import math
 import time
 import rospy
 from sensor_msgs.msg import JointState
@@ -15,11 +16,11 @@ def callback(data):
     rospy.loginfo(rospy.get_caller_id() + "%s", data)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+        
-    # print("data_list:",data_list)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -30,6 +31,9 @@ def listener():
     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)
 
     rospy.Subscriber("joint_states", JointState, callback)
 

mecharm/mecharm_pi/scripts/slider_control.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 # -*- coding:utf-8 -*-
 
 """[summary]
@@ -9,7 +9,8 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyAMA0'
     baud: serial prot baudrate. Defaults is 1000000.
 """
-
+import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -25,11 +26,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)
-    # print(data_list)
+        
-    mc.send_radians(data_list, 80)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # time.sleep(0.5)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -41,6 +42,9 @@ def listener():
     baud = rospy.get_param("~baud", 1000000)
     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_pi_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}
 )
 

mecharm/mecharm_pi_moveit/launch/moveit.rviz

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

mecharm/mecharm_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,132 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group. 初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to listen for changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group 
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link. 获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position.
-        #  设置目标位置所使用的参考坐标系
-        self.reference_frame = "joint1"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails. 
-        # 当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian). 
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # Control the robotic arm to return to the initialization position first. 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system 
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion. 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm. 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-
-        traj = self.arm.plan()
-
-        # Control the motion of the robotic arm according to the planned motion path. 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參数1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端反向旋转90度,  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        self.moving()
-
-        # Add the environment. 添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        suit_path = (
-            roslib.packages.get_pkg_dir("mycobot_description")
-            + "/urdf/mycobot/suit_env.dae"
-        )
-        # need `pyassimp==3.3`
-        self.scene.add_mesh("suit", suit_post, suit_path)
-        rospy.sleep(2)
-
-        # Run again if there is environmental impact. 有环境影响后再运行一次
-        self.moving()
-
-        # Close and exit moveit. 关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

mecharm/mecharm_pi_moveit/scripts/sync_plan.py

@@ -1,10 +1,10 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
+import math
 import time
 import rospy
 from sensor_msgs.msg import JointState
 
 from pymycobot.mycobot import MyCobot
-# from pymycobot.mypalletizer import MyPalletizer
 
 
 mc = None
@@ -14,11 +14,11 @@ def callback(data):
     rospy.loginfo(rospy.get_caller_id() + "%s", data)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+        
-    # print("data_list:",data_list)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -29,7 +29,10 @@ def listener():
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
     mc = MyCobot(port, baud)
-
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
+    
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped 

myArm/myarm/scripts/slider_control.py

@@ -8,6 +8,7 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
+import time
 import math
 import rospy
 from sensor_msgs.msg import JointState
@@ -20,15 +21,15 @@ mc = None
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print(data.position)
+
     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)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_angles(data_list, 80)
+    mc.send_angles(data_list, 25)
-    # time.sleep(0.5)
+
 
 
 def listener():
@@ -40,7 +41,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyArm(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退出，直到该节点停止
     print("spin ...")

myArm/myarm_moveit/launch/moveit.rviz

@@ -9,7 +9,7 @@ Panels:
         - /MotionPlanning1/Planning Metrics1
         - /MotionPlanning1/Planned Path1
       Splitter Ratio: 0.5
-    Tree Height: 215
+    Tree Height: 155
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -113,7 +113,7 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-        Loop Animation: true
+        Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
         Show Robot Collision: true
@@ -252,7 +252,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000168000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ab000001880000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d0000012c000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000016f000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1291

myArm/myarm_moveit/launch/mycobot_moveit.launch

@@ -1,65 +0,0 @@
-<launch>
-
-  <!-- By default, we do not start a database (it can be large) -->
-   <!-- 默认情况下，我们不启动数据库（它可能很大） -->
-  <arg name="db" default="false" />
-  <!-- Allow user to specify database location -->
-   <!-- 允许用户指定数据库位置 -->
-  <arg name="db_path" default="$(find myarm_moveit)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->  <!-- 默认情况下，我们不处于调试模式 -->
-  <arg name="debug" default="false" />
-
-  <!--
-  By default, hide joint_state_publisher's GUI
-
-  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
-  The latter one maintains and publishes the current joint configuration of the simulated robot.
-  It also provides a GUI to move the simulated robot around "manually".
-  This corresponds to moving around the real robot without the use of MoveIt.
-  -->
-  <arg name="use_gui" default="false" />
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-   <!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
-  <include file="$(find myarm_moveit)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-   <!-- 我们没有连接机器人，所以发布假关节状态 -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
-    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-   <!-- 给定已发布的关节状态，为机器人链接发布 tf -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
-    <!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
-  <include file="$(find myarm_moveit)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-   <!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
-  <include file="$(find myarm_moveit)/launch/moveit_rviz.launch">
-    <arg name="config" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <!-- 如果启用了数据库加载，也启动 mongodb -->
-  <include file="$(find myarm_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>

myArm/myarm_moveit/scripts/sync_plan.py

@@ -9,6 +9,7 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
+import time
 import math
 import rospy
 from sensor_msgs.msg import JointState
@@ -21,15 +22,14 @@ mc = None
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print(data.position)
+
     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)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_angles(data_list, 80)
+    mc.send_angles(data_list, 25)
-    # time.sleep(0.5)
 
 
 def listener():
@@ -41,7 +41,10 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyArm(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退出，直到该节点停止
     print("spin ...")

mycobot_280/mycobot_280/scripts/slider_control.py

@@ -8,7 +8,8 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -20,13 +21,15 @@ mc = None
 
 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)
+        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)
+    mc.get_system_version()
 
 
 def listener():
@@ -38,6 +41,9 @@ def listener():
     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退出，直到该节点停止

mycobot_280/mycobot_280/scripts/slider_control_gripper.py

@@ -9,7 +9,7 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import time
 import rospy
 from sensor_msgs.msg import JointState
 from pymycobot.mycobot import MyCobot
@@ -27,7 +27,7 @@ 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_value(gripper_value, 80, 1)
@@ -43,6 +43,9 @@ def listener():
     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退出，直到该节点停止
     print("spin ...")

mycobot_280/mycobot_280_gripper_moveit/scripts/sync_plan.py

@@ -7,25 +7,19 @@ from pymycobot.mycobot import MyCobot
 
 
 def callback(data):
-    # port = rospy.get_param("~port", "/dev/ttyUSB0")
+    
-    # baud = rospy.get_param("~baud", 115200)
-    # # print(port, baud)
-    # mc = MyCobot(port, baud)
-
     data_list = []
     for index, value in enumerate(data.position):
         data_list.append(round(value, 3))
     data_list = data_list[:7]
     print("radians:%s" % data_list[:6])
     # t1 = time.time()
-    mc.send_radians(data_list[:6], 80)
+    mc.send_radians(data_list[:6], 25)
     # time.sleep(0.02)
     gripper_value = int(abs(-0.7 - data_list[6]) * 117)
     print("gripper_value:%s\n" % gripper_value)
     mc.set_gripper_value(gripper_value, 80)
-    # t2 = time.time() - t1
+
-    # print("cost time:", t2)
-    # time.sleep(1)
 
 
 def listener():
@@ -35,6 +29,10 @@ def listener():
     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)
+    
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped

mycobot_280/mycobot_280_moveit/launch/moveit.rviz

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

mycobot_280/mycobot_280_moveit/scripts/sync_plan.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python2
 import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -10,14 +11,13 @@ mc = None
 
 
 def callback(data):
-    rospy.loginfo(rospy.get_caller_id() + "%s", data)
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-
+    mc.send_angles(data_list, 25)
-    mc.send_radians(data_list, 80)
 
 
 def listener():
@@ -25,9 +25,12 @@ def listener():
     rospy.init_node("mycobot_reciver", anonymous=True)
 
     port = rospy.get_param("~port", "/dev/ttyUSB0")
-    baud = rospy.get_param("~baud", 1000000)
+    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)
 
     rospy.Subscriber("joint_states", JointState, callback)
 

mycobot_280/mycobot_280arduino/scripts/slider_control.py

@@ -8,7 +8,8 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -20,13 +21,13 @@ mc = None
 
 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)
+        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():
@@ -38,6 +39,9 @@ def listener():
     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
     print("spin ...")

mycobot_280/mycobot_280arduino_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.5
       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.5
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python2
+import math
 import time
 import rospy
 from sensor_msgs.msg import JointState
@@ -10,14 +11,14 @@ 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:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+        
-
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -28,7 +29,10 @@ def listener():
     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)
+    
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped

mycobot_280/mycobot_280jn/scripts/slider_control.py

@@ -8,7 +8,8 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyTHS1'
     baud: serial prot baudrate. Defaults is 1000000.
 """
-
+import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -21,13 +22,13 @@ mc = None
 
 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)
+        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():
@@ -39,11 +40,9 @@ def listener():
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
     mc = MyCobot(port, baud)
-    # ip=rospy.get_param("~ip",'192.168.125.226')
+    time.sleep(0.05)
-    # port=rospy.get_param("~port",9000)
+    mc.set_free_mode(1)
-    # print(ip,port)
+    time.sleep(0.05)
-    # ms=MyCobotSocket(ip,port)
-    # ms.connect()
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

mycobot_280/mycobot_280jn_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.5
       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.5
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -10,15 +11,14 @@ 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:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+        
-
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 25)
-
 
 def listener():
     global mc
@@ -27,8 +27,12 @@ def listener():
     port = rospy.get_param("~port", "/dev/ttyTHS1")
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
+    
     mc = MyCobot(port, baud)
-
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
+    
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped

mycobot_280/mycobot_280pi/scripts/slider_control.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 # encoding=utf-8
 
 """[summary]
@@ -9,7 +9,8 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyAMA0'
     baud: serial prot baudrate. Defaults is 1000000.
 """
-
+import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -21,14 +22,13 @@ mc = None
 
 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)
+        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
@@ -39,6 +39,9 @@ def listener():
     baud = rospy.get_param("~baud", 1000000)
     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退出，直到该节点停止

mycobot_280/mycobot_280pi_moveit/launch/moveit.rviz

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

mycobot_280/mycobot_280pi_moveit/scripts/sync_plan.py

@@ -1,6 +1,7 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 # encoding=utf-8
 import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -11,14 +12,14 @@ 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:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+        
-
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -29,7 +30,10 @@ def listener():
     baud = rospy.get_param("~baud", 1000000)
     print(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

mycobot_pro/mycobot_600/scripts/slider_600.py

@@ -13,42 +13,26 @@ from pymycobot.elephantrobot import ElephantRobot
 
 global mc
 
-old_list = []
-
 
 def callback(data):
     """callback function,回调函数"""
-    satrt_time=time.time()
-    global old_list
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print("position:", data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        value = value * 180 / math.pi
+        radians_to_angles = round(math.degrees(value), 2)
-        data_list.append(value)
+        data_list.append(radians_to_angles)
-    print ("data", data_list)
+        
-
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    if not old_list:
+    mc.write_angles(data_list, 1000)
-        old_list = data_list
-        mc.write_angles(data_list, 1999)
-    elif old_list != data_list:
-        old_list = data_list
-        # if mc.check_running():
-            # mc.task_stop()
-            # time.sleep(0.05)
-            
-        mc.write_angles(data_list, 1999)
-
-        end_time=time.time()
-        print('loop_time:',end_time-satrt_time)
 
 def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
 
     ip = rospy.get_param("~ip", "192.168.1.159")
-    print (ip)
+    port = rospy.get_param("~port", 5001)
-    mc = ElephantRobot(ip, 5001)
+    print (ip, port)
+    mc = ElephantRobot(ip, int(port))
     # START CLIENT,启动客户端
     res = mc.start_client()
     if res != "":

mycobot_pro/mycobot_600_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}
 )
 

mycobot_pro/mycobot_600_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,137 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group,初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to monitor changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link,获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position
-        # 设置目标位置所使用的参考坐标系
-        self.reference_frame = "base"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian)
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # # Control the robotic arm to return to the initialization position first
-        # 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion
-        # 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm
-        # 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-
-        # Plan the movement path,规划运动路径
-        traj = self.arm.plan()
-
-        # Control the motion of the robotic arm according to the planned motion path
-        # 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy
-        # 控制机械臂终端反向旋转90度  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        # Run once without obstacles,没有障碍物运行一次
-        self.moving()
-
-        # Add environment,添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        # suit_path = (
-        #     roslib.packages.get_pkg_dir("mycobot_description")
-        #     + "/urdf/mycobot/suit_env.dae"
-        # )
-        # # need `pyassimp==3.3`
-        # self.scene.add_mesh("suit", suit_post, suit_path)
-        # rospy.sleep(2)
-
-        # Run it again if there is an environmental impact,有环境影响后再运行一次
-        self.moving()
-
-        # close and exit moveit，关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py

@@ -13,42 +13,28 @@ from pymycobot.elephantrobot import ElephantRobot
 
 global mc
 
-old_list = []
-
 
 def callback(data):
     """callback function,回调函数"""
-    satrt_time=time.time()
-    global old_list
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print("position:", data.position)
+
     data_list = []
     for index, value in enumerate(data.position):
-        value = value * 180 / math.pi
+        radians_to_angles = round(math.degrees(value), 2)
-        data_list.append(value)
+        data_list.append(radians_to_angles)
-    print ("data", data_list)
+        
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.write_angles(data_list, 1000)
 
-    if not old_list:
-        old_list = data_list
-        mc.write_angles(data_list, 1999)
-    elif old_list != data_list:
-        old_list = data_list
-        # if mc.check_running():
-            # mc.task_stop()
-            # time.sleep(0.05)
-            
-        mc.write_angles(data_list, 1999)
-
-        end_time=time.time()
-        print('loop_time:',end_time-satrt_time)
 
 def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
 
     ip = rospy.get_param("~ip", "192.168.1.159")
-    print (ip)
+    port = rospy.get_param("~port", 5001)
-    mc = ElephantRobot(ip, 5001)
+    print (ip, port)
+    mc = ElephantRobot(ip, int(port))
     # START CLIENT,启动客户端
     res = mc.start_client()
     if res != "":

mycobot_pro/mycobot_600_moveit/scripts/test.py

@@ -1,726 +0,0 @@
-#!/usr/bin/env python2
-# -*- coding: utf-8 -*-
-from socket import *
-import math
-from multiprocessing import Lock
-from operator import imod
-from tokenize import Pointfloat
-import cv2
-import numpy as np
-import time
-import json
-import os,sys
-import rospy
-from visualization_msgs.msg import Marker  
-import rospy
-from sensor_msgs.msg import JointState
-# from moving_utils import Movement
-
-IS_CV_4 = cv2.__version__[0] == '4'
-__version__ = "1.0"
-
-global mc
-mutex = Lock()
-
-class ElephantRobot(object):
-    def __init__(self, host, port):
-        # setup connection
-        # 建立连接
-        self.BUFFSIZE = 2048
-        self.ADDR = (host, port)
-        self.tcp_client = socket(AF_INET, SOCK_STREAM)
-
-    def start_client(self):
-        try:
-            self.tcp_client.connect(self.ADDR)
-            return ""
-        except error,e:
-            return e
-
-    def stop_client(self):
-        self.tcp_client.close()
-
-    def send_command(self, command):
-        with mutex:
-            self.tcp_client.send(command.encode())
-            recv_data = self.tcp_client.recv(self.BUFFSIZE).decode()
-            res_str = str(recv_data)
-            print("recv = " )+ res_str
-            res_arr = res_str.split(":")
-            if len(res_arr) == 2:
-                return res_arr[1]
-            else:
-                return ""
-
-    def string_to_coords(self, data):
-        data = data.replace("[", "")
-        data = data.replace("]", "")
-        data_arr = data.split(",")
-        if len(data_arr) == 6:
-            try:
-                coords_1 = float(data_arr[0])
-                coords_2 = float(data_arr[1])
-                coords_3 = float(data_arr[2])
-                coords_4 = float(data_arr[3])
-                coords_5 = float(data_arr[4])
-                coords_6 = float(data_arr[5])
-                coords = [coords_1, coords_2, coords_3, coords_4, coords_5, coords_6]
-                return coords
-            except:
-                return invalid_coords()
-        return invalid_coords()
-
-    def string_to_double(self, data):
-        try:
-            val = float(data)
-            return val
-        except:
-            return -9999.99
-
-    def string_to_int(self, data):
-        try:
-            val = int(data)
-            return val
-        except:
-            return -9999
-
-    def invalid_coords(self):
-        coords = [-1, -2, -3, -4, -1, -1]
-        return coords
-
-    def get_angles(self):
-        command = "get_angles()\n"
-        res = self.send_command(command)
-        return self.string_to_coords(res)
-
-    def get_coords(self):
-        command = "get_coords()\n"
-        res = self.send_command(command)
-        return self.string_to_coords(res)
-
-    def get_speed(self):
-        command = "get_speed()\n"
-        res = self.send_command(command)
-        return self.string_to_double(res)
-
-    def power_on(self):
-        command = "power_on()\n"
-        res = self.send_command(command)
-        return True
-
-    def power_off(self):
-        command = "power_off()\n"
-        res = self.send_command(command)
-        return True
-
-    def check_running(self):
-        command = "check_running()\n"
-        res = self.send_command(command)
-        return res == "1"
-
-    def state_check(self):
-        command = "state_check()\n"
-        res = self.send_command(command)
-        return res == "1"
-
-    def program_open(self, file_path):
-        command = "program_open(" + file_path + ")\n"
-        res = self.send_command(command)
-        return self.string_to_int(res)
-
-    def program_run(self, start_line):
-        """run program，运行程序"""
-        command = "program_run(" + str(start_line) + ")\n"
-        res = self.send_command(command)
-        return self.string_to_int(res)
-
-    def read_next_error(self):
-        command = "read_next_error()\n"
-        res = self.send_command(command)
-        return res
-
-    def write_coords(self, coords, speed):
-        """set coords,设置坐标"""
-        command = "set_coords("
-        for item in coords:
-            command += str(item) + ","
-        command += str(speed) + ")\n"
-        self.send_command(command)
-
-    def write_coord(self, axis, value, speed):
-        coords = self.get_coords()
-        if coords != self.invalid_coords():
-            coords[axis] = value
-            self.write_coords(coords, speed)
-
-    def write_angles(self, angles, speed):
-        """set angles,设置角度"""
-        command = "set_angles("
-        for item in angles:
-            command += str(item) + ","
-        command += str(speed) + ")\n"
-        self.send_command(command)
-
-    def write_angle(self, joint, value, speed):
-        angles = self.get_angles()
-        if angles != self.invalid_coords():
-            angles[joint] = value
-            self.write_angles(angles, speed)
-
-    def set_speed(self, percentage):
-        command = "set_speed(" + str(percentage) + ")\n"
-        self.send_command(command)
-
-    def set_carte_torque_limit(self, axis_str, value):
-        command = "set_torque_limit(" + axis_str + "," + str(value) + ")\n"
-        self.send_command(command)
-
-    def set_upside_down(self, up_down):
-        up = "1"
-        if up_down:
-            up = "0"
-        command = "set_upside_down(" + up + ")\n"
-        self.send_command(command)
-
-    def set_payload(self, payload):
-        command = "set_speed(" + str(payload) + ")\n"
-        self.send_command(command)
-
-    def state_on(self):
-        command = "state_on()\n"
-        self.send_command(command)
-
-    def state_off(self):
-        command = "state_off()\n"
-        self.send_command(command)
-
-    def task_stop(self):
-        command = "task_stop()\n"
-        self.send_command(command)
-
-    def jog_angle(self, joint_str, direction, speed):
-        command = (
-            "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n"
-        )
-        self.send_command(command)
-
-    def jog_coord(self, axis_str, direction, speed):
-        command = (
-            "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n"
-        )
-        self.send_command(command)
-
-    def get_digital_in(self, pin_number):
-        command = "get_digital_in(" + str(pin_number) + ")\n"
-        self.send_command(command)
-
-    def get_digital_out(self, pin_number):
-        command = "get_digital_out(" + str(pin_number) + ")\n"
-        self.send_command(command)
-
-    def set_digital_out(self, pin_number, pin_signal):
-        command = "set_digital_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
-        self.send_command(command)
-
-    def set_analog_out(self, pin_number, pin_signal):
-        command = "set_analog_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
-        self.send_command(command)
-
-    def get_acceleration(self):
-        command = "get_acceleration()\n"
-        res = self.send_command(command)
-        return self.string_to_int(res)
-
-    def set_acceleration(self, acceleration):
-        command = "set_acceleration(" + str(acceleration) + ")\n"
-        self.send_command(command)
-
-    def command_wait_done(self):
-        command = "wait_command_done()\n"
-        self.send_command(command)
-
-    def wait(self, seconds):
-        command = "wait(" + str(seconds) + ")\n"
-        self.send_command(command)
-
-    def assign_variable(self, var_name, var_value):
-        command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ")\n"
-        self.send_command(command)
-
-    def get_variable(self, var_name):
-        command = 'get_variable("' + str(var_name) + '")\n'
-        return self.send_command(command)
-
-class Object_detect(object):
-
-    def __init__(self, camera_x = 140, camera_y = 5): # m5
-    # def __init__(self, camera_x = 140, camera_y = -5): # pi
-        # inherit the parent class
-        super(Object_detect, self).__init__()
-        # get path of file
-        dir_path = os.path.dirname(__file__)
-
-        # declare 600
-        self.mc = None
-
-        # 移动角度
-        self.move_angles = [
-            [0, 0, 0, 0, 90, 0],  # point to grab 
-            [-33.31, 2.02, -10.72, -0.08, 95, -54.84],  # init the point
-        ]
-
-        # 移动坐标
-        self.move_coords = [
-            [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
-            [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
-            [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
-            [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket         
-        ]
-
-        # which robot: USB* is m5; ACM* is wio; AMA* is raspi
-        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
-        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
-        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
-        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
-        self.raspi = False
-        if "dev" in self.robot_m5:
-            self.Pin = [2, 5]
-            # self.Pin = [5]
-        elif "dev" in self.robot_wio:
-            self.Pin = [20, 21]
-            for i in self.move_coords:
-                i[2] -= 20
-        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
-            import RPi.GPIO as GPIO
-            GPIO.setwarnings(False)
-            self.GPIO = GPIO
-            GPIO.setmode(GPIO.BCM)
-            GPIO.setup(20, GPIO.OUT)
-            GPIO.setup(21, GPIO.OUT)
-            GPIO.output(20, 1)
-            GPIO.output(21, 1)
-            self.raspi = True
-        if self.raspi:
-            self.gpio_status(False)
-        # choose place to set cube
-        self.color = 0
-        # parameters to calculate camera clipping parameters
-        self.x1 = self.x2 = self.y1 = self.y2 = 0
-        # set cache of real coord
-        self.cache_x = self.cache_y = 0
-        # set color HSV
-        self.HSV = {
-            "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
-            "red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
-            "green": [np.array([35, 43, 46]), np.array([77, 255, 255])],
-            "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
-            "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
-        }
-        # use to calculate coord between cube and 600
-        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
-        # The coordinates of the grab center point relative to the 600
-        self.camera_x, self.camera_y = camera_x, camera_y
-        # The coordinates of the cube relative to the 600
-        self.c_x, self.c_y = 0, 0
-        # The ratio of pixels to actual values
-        self.ratio = 0
-        # Get ArUco marker dict that can be detected.
-        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
-        # Get ArUco marker params.
-        self.aruco_params = cv2.aruco.DetectorParameters_create()
-
-        # init a node and a publisher
-        rospy.init_node("marker", anonymous=True)
-        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
-        # init a Marker
-        self.marker = Marker()
-        self.marker.header.frame_id = "/joint1"
-        self.marker.ns = "cube"
-        self.marker.type = self.marker.CUBE
-        self.marker.action = self.marker.ADD
-        self.marker.scale.x = 0.04
-        self.marker.scale.y = 0.04
-        self.marker.scale.z = 0.04
-        self.marker.color.a = 1.0
-        self.marker.color.g = 1.0
-        self.marker.color.r = 1.0
-
-        # marker position initial
-        self.marker.pose.position.x = 0
-        self.marker.pose.position.y = 0
-        self.marker.pose.position.z = 0.03
-        self.marker.pose.orientation.x = 0
-        self.marker.pose.orientation.y = 0
-        self.marker.pose.orientation.z = 0
-        self.marker.pose.orientation.w = 1.0
-
-    # publish marker
-    def pub_marker(self, x, y, z=0.03):
-        self.marker.header.stamp = rospy.Time.now()
-        self.marker.pose.position.x = x
-        self.marker.pose.position.y = y
-        self.marker.pose.position.z = z
-        self.marker.color.g = self.color
-        self.pub.publish(self.marker)
-
-    # pump_control pi
-    def gpio_status(self, flag):
-        if flag:
-            self.GPIO.output(20, 0)
-            self.GPIO.output(21, 0)
-        else:
-            self.GPIO.output(20, 1)
-            self.GPIO.output(21, 1)
-    
-    # 开启吸泵 m5
-    def pump_on(self):
-        # 让2号位工作
-        self.mc.set_basic_output(2, 0)
-        # 让5号位工作
-        self.mc.set_basic_output(5, 0)
-
-    # 停止吸泵 m5
-    def pump_off(self):
-        # 让2号位停止工作
-        self.mc.set_basic_output(2, 1)
-        # 让5号位停止工作
-        self.mc.set_basic_output(5, 1)
-
-    # Grasping motion
-    def move(self, x, y, color):
-        # send Angle to move 600
-        print(color)
-        self.mc.send_angles(self.move_angles[0], 30)
-        time.sleep(4)
-
-        # send coordinates to move 600
-        self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
-        time.sleep(3)
-        self.pub_marker(x/1000.0, y/1000.0, 140/1000.0)
-
-        
-        self.mc.send_coords([x, y, 95, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
-        # self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
-        time.sleep(3)
-        self.pub_marker(x/1000.0, y/1000.0, 90/1000.0)
-
-        
-        # open pump
-        if "dev" in self.robot_m5:
-            self.pump_on()
-        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
-            self.gpio_status(True)
-        time.sleep(1.5)
-
-        tmp = []
-        while True:
-            if not tmp: 
-                tmp = self.mc.get_angles()    
-            else:
-                break
-        time.sleep(0.5)
-        
-        # print(tmp)
-        self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
-        time.sleep(3)
-
-
-
-        self.mc.send_coords(self.move_coords[color], 30, 1)
-        self.pub_marker(self.move_coords[color][0]/1000.0, 
-                        self.move_coords[color][1]/1000.0,
-                        self.move_coords[color][2]/1000.0)
-        time.sleep(3)
-       
-        # close pump
-        if "dev" in self.robot_m5:
-           self.pump_off()
-        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
-            self.gpio_status(False)
-        time.sleep(6)
-
-        if color == 1:
-            self.pub_marker(
-                self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
-        elif color == 0:
-            self.pub_marker(
-                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
-
-        self.mc.send_angles(self.move_angles[1], 30)
-        time.sleep(1.5)
-
-    # decide whether grab cube
-    def decide_move(self, x, y, color):
-        print(x, y, self.cache_x, self.cache_y)
-        # detect the cube status move or run
-        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
-            self.cache_x, self.cache_y = x, y
-            return
-        else:
-            self.cache_x = self.cache_y = 0
-            # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
-            self.move(x, y, color)
-
-    # init 600
-    def run(self):
-        if "dev" in self.robot_m5:
-            self.mc = MyCobot(self.robot_m5, 115200) 
-        elif "dev" in self.robot_raspi:
-            self.mc = MyCobot(self.robot_raspi, 1000000)
-        if not self.raspi:
-            self.pub_pump(False, self.Pin)
-        self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
-        time.sleep(3)
-
-    # draw aruco
-    def draw_marker(self, img, x, y):
-        # draw rectangle on img
-        cv2.rectangle( 
-            img,
-            (x - 20, y - 20),
-            (x + 20, y + 20),
-            (0, 255, 0),
-            thickness=2,
-            lineType=cv2.FONT_HERSHEY_COMPLEX,
-        )
-        # add text on rectangle
-        cv2.putText(img, "({},{})".format(x, y), (x, y),
-                    cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
-
-    # get points of two aruco
-    def get_calculate_params(self, img):
-        # Convert the image to a gray image
-        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
-        # Detect ArUco marker.
-        corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
-            gray, self.aruco_dict, parameters=self.aruco_params
-        )
-
-        """
-        Two Arucos must be present in the picture and in the same order.
-        There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
-        Determine the center of the aruco by the four corners of the aruco.
-        """
-        if len(corners) > 0:
-            if ids is not None:
-                if len(corners) <= 1 or ids[0] == 1:
-                    return None
-                x1 = x2 = y1 = y2 = 0
-                point_11, point_21, point_31, point_41 = corners[0][0]
-                x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
-                    (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
-                point_1, point_2, point_3, point_4 = corners[1][0]
-                x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
-                    (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
-                return x1, x2, y1, y2
-        return None
-
-    # set camera clipping parameters
-    def set_cut_params(self, x1, y1, x2, y2):
-        self.x1 = int(x1)
-        self.y1 = int(y1)
-        self.x2 = int(x2)
-        self.y2 = int(y2)
-        print(self.x1, self.y1, self.x2, self.y2)
-
-    # set parameters to calculate the coords between cube and 600
-    def set_params(self, c_x, c_y, ratio):
-        self.c_x = c_x
-        self.c_y = c_y
-        self.ratio = 220.0/ratio
-
-    # calculate the coords between cube and 600
-    def get_position(self, x, y):
-        return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
-
-    """
-    Calibrate the camera according to the calibration parameters.
-    Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
-    If two ARuco values have been calculated, clip the video.
-    """
-    def transform_frame(self, frame):
-        # enlarge the image by 1.5 times
-        fx = 1.5
-        fy = 1.5
-        frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
-                           interpolation=cv2.INTER_CUBIC)
-        if self.x1 != self.x2:
-            # the cutting ratio here is adjusted according to the actual situation
-            frame = frame[int(self.y2*0.2):int(self.y1*1.15),
-                          int(self.x1*0.7):int(self.x2*1.15)]
-        return frame
-
-    # detect cube color
-    def color_detect(self, img):
-        # set the arrangement of color'HSV
-        x = y = 0
-        for mycolor, item in self.HSV.items():
-            # print("mycolor:",mycolor)
-            redLower = np.array(item[0])
-            redUpper = np.array(item[1])
-
-            # transfrom the img to model of gray
-            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
-            # print("hsv",hsv)
-
-            # wipe off all color expect color in range
-            mask = cv2.inRange(hsv, item[0], item[1])
-
-            # a etching operation on a picture to remove edge roughness
-            erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
-
-            # the image for expansion operation, its role is to deepen the color depth in the picture
-            dilation = cv2.dilate(erosion, np.ones(
-                (1, 1), np.uint8), iterations=2)
-
-            # adds pixels to the image
-            target = cv2.bitwise_and(img, img, mask=dilation)
-
-            # the filtered image is transformed into a binary image and placed in binary
-            ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
-
-            # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
-            contours, hierarchy = cv2.findContours(
-                dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-
-            if len(contours) > 0:
-                # do something about misidentification
-                boxes = [
-                    box
-                    for box in [cv2.boundingRect(c) for c in contours]
-                    if min(img.shape[0], img.shape[1]) / 10
-                    < min(box[2], box[3])
-                    < min(img.shape[0], img.shape[1]) / 1
-                ]
-                if boxes:
-                    for box in boxes:
-                        x, y, w, h = box
-                    # find the largest object that fits the requirements
-                    c = max(contours, key=cv2.contourArea)
-                    # get the lower left and upper right points of the positioning object
-                    x, y, w, h = cv2.boundingRect(c)
-                    # locate the target by drawing rectangle
-                    cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
-                    # calculate the rectangle center
-                    x, y = (x*2+w)/2, (y*2+h)/2
-                    # calculate the real coordinates of 600 relative to the target
-                    
-                    if mycolor == "red":
-                        self.color = 0
-                    elif mycolor == "green":
-                        self.color = 1
-                    elif mycolor == "cyan":
-                        self.color = 2
-                    else:
-                        self.color = 3
-
-        if abs(x) + abs(y) > 0:
-            return x, y
-        else:
-            return None
-
-if __name__ == "__main__":
-
-    # open the camera
-    cap_num = 0
-    cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
-    if not cap.isOpened():
-        cap.open()
-    # init a class of Object_detect
-    detect = Object_detect()
-    # init 600
-    detect.run()
-
-    _init_ = 20  
-    init_num = 0
-    nparams = 0
-    num = 0
-    real_sx = real_sy = 0
-    while cv2.waitKey(1) < 0:
-       # read camera
-        _, frame = cap.read()
-        # deal img
-        frame = detect.transform_frame(frame)
-        if _init_ > 0:
-            _init_ -= 1
-            continue
-
-        # calculate the parameters of camera clipping
-        if init_num < 20:
-            if detect.get_calculate_params(frame) is None:
-                cv2.imshow("figure", frame)
-                continue
-            else:
-                x1, x2, y1, y2 = detect.get_calculate_params(frame)
-                detect.draw_marker(frame, x1, y1)
-                detect.draw_marker(frame, x2, y2)
-                detect.sum_x1 += x1
-                detect.sum_x2 += x2
-                detect.sum_y1 += y1
-                detect.sum_y2 += y2
-                init_num += 1
-                continue
-        elif init_num == 20:
-            detect.set_cut_params(
-                (detect.sum_x1)/20.0,
-                (detect.sum_y1)/20.0,
-                (detect.sum_x2)/20.0,
-                (detect.sum_y2)/20.0,
-            )
-            detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
-            init_num += 1
-            continue
-
-        # calculate params of the coords between cube and 600
-        if nparams < 10:
-            if detect.get_calculate_params(frame) is None:
-                cv2.imshow("figure", frame)
-                continue
-            else:
-                x1, x2, y1, y2 = detect.get_calculate_params(frame)
-                detect.draw_marker(frame, x1, y1)
-                detect.draw_marker(frame, x2, y2)
-                detect.sum_x1 += x1
-                detect.sum_x2 += x2
-                detect.sum_y1 += y1
-                detect.sum_y2 += y2
-                nparams += 1
-                continue
-        elif nparams == 10:
-            nparams += 1
-            # calculate and set params of calculating real coord between cube and 600
-            detect.set_params(
-                (detect.sum_x1+detect.sum_x2)/20.0,
-                (detect.sum_y1+detect.sum_y2)/20.0,
-                abs(detect.sum_x1-detect.sum_x2)/10.0 +
-                abs(detect.sum_y1-detect.sum_y2)/10.0
-            )
-            print("ok")
-            continue
-
-        # get detect result
-        detect_result = detect.color_detect(frame)
-        if detect_result is None:
-            cv2.imshow("figure", frame)
-            continue
-        else:
-            x, y = detect_result
-            # calculate real coord between cube and 600
-            real_x, real_y = detect.get_position(x, y)
-            if num == 20:
-                detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
-                detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
-                num = real_sx = real_sy = 0
-
-            else:
-                num += 1
-                real_sy += real_y
-                real_sx += real_x
-
-        cv2.imshow("figure", frame)
-
-        # close the window
-        if cv2.waitKey(1) & 0xFF == ord('q'):
-            cap.release()
-            cv2.destroyAllWindows()
-            sys.exit()

mypalletizer_260/mypalletizer_260/scripts/slider_control.py

@@ -9,11 +9,11 @@ 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
 
-# from pymycobot.mycobot import MyCobot
 from pymycobot.mypalletizer import MyPalletizer
 
 
@@ -22,15 +22,14 @@ mc = None
 
 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)
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
+        
     del data_list[3] # delete the angle of joint3 to  joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！
-    # data_list[3] = data_list[4]
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # print(data_list)
+    mc.send_angles(data_list, 25)
-    mc.send_radians(data_list, 80)
-    # time.sleep(0.5)
 
 
 def listener():
@@ -42,7 +41,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyPalletizer(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退出，直到该节点停止
     print("spin ...")

mypalletizer_260/mypalletizer_260_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}
 )
 

mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,132 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group. 初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to listen for changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group 
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link. 获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position.
-        #  设置目标位置所使用的参考坐标系
-        self.reference_frame = "joint1"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails. 
-        # 当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian). 
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # Control the robotic arm to return to the initialization position first. 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system 
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion. 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm. 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-
-        traj = self.arm.plan()
-
-        # Control the motion of the robotic arm according to the planned motion path. 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參数1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端反向旋转90度,  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        self.moving()
-
-        # Add the environment. 添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        suit_path = (
-            roslib.packages.get_pkg_dir("mycobot_description")
-            + "/urdf/mycobot/suit_env.dae"
-        )
-        # need `pyassimp==3.3`
-        self.scene.add_mesh("suit", suit_post, suit_path)
-        rospy.sleep(2)
-
-        # Run again if there is environmental impact. 有环境影响后再运行一次
-        self.moving()
-
-        # Close and exit moveit. 关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python2
+import math
 import time
 import rospy
 from sensor_msgs.msg import JointState
@@ -14,11 +15,11 @@ def callback(data):
     rospy.loginfo(rospy.get_caller_id() + "%s", data)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+    del data_list[3]
-    # print("data_list:",data_list)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 25)
 
 
 def listener():
@@ -29,7 +30,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyPalletizer(port, baud)
-
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped 

mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py

@@ -9,11 +9,11 @@ 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
 
-# from pymycobot.mycobot import MyCobot
 from pymycobot.mypalletizer import MyPalletizer
 
 
@@ -25,12 +25,10 @@ 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)
-    # del data_list[3] # delete the angle of joint3 to  joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！
+        data_list.append(radians_to_angles)
-    # data_list[3] = data_list[4]
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # print(data_list)
+    mc.send_angles(data_list, 25)
-    mc.send_radians(data_list, 80)
-    # time.sleep(0.5)
 
 
 def listener():
@@ -42,7 +40,9 @@ def listener():
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
     mc = MyPalletizer(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退出，直到该节点停止
     print("spin ...")

mypalletizer_260/mypalletizer_260_pi_moveit/CMakeLists.txt

@@ -14,7 +14,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}
 )
 

mypalletizer_260/mypalletizer_260_pi_moveit/launch/moveit.rviz

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

mypalletizer_260/mypalletizer_260_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,132 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group. 初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to listen for changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group 
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link. 获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position.
-        #  设置目标位置所使用的参考坐标系
-        self.reference_frame = "joint1"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails. 
-        # 当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian). 
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # Control the robotic arm to return to the initialization position first. 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system 
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion. 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm. 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-
-        traj = self.arm.plan()
-
-        # Control the motion of the robotic arm according to the planned motion path. 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參数1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端反向旋转90度,  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        self.moving()
-
-        # Add the environment. 添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        suit_path = (
-            roslib.packages.get_pkg_dir("mycobot_description")
-            + "/urdf/mycobot/suit_env.dae"
-        )
-        # need `pyassimp==3.3`
-        self.scene.add_mesh("suit", suit_post, suit_path)
-        rospy.sleep(2)
-
-        # Run again if there is environmental impact. 有环境影响后再运行一次
-        self.moving()
-
-        # Close and exit moveit. 关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

mypalletizer_260/mypalletizer_260_pi_moveit/scripts/sync_plan.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python2
 # -*- coding: utf-8 -*-
 import time
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -12,14 +13,13 @@ 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:
+        radians_to_angles = round(math.degrees(value), 2)
-        #     value *= -1
+        data_list.append(radians_to_angles)
-        data_list.append(value)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # print("data_list:",data_list)
+    mc.send_angles(data_list, 25)
-    mc.send_radians(data_list, 80)
 
 
 def listener():
@@ -30,7 +30,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyPalletizer(port, baud)
-
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped 

ultraArm/ultraarm/scripts/slider_control.py

@@ -24,11 +24,11 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(round(value,3))
+        radians_to_angles = round(math.degrees(value), 2)
-  
+        data_list.append(radians_to_angles)
-    print('data_list:',data_list)
+        
-    ua.set_radians(data_list, 80)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # time.sleep(0.5)
+    ua.set_angles(data_list, 25)
 
 
 def listener():

ultraArm/ultraarm_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}
 )
 

ultraArm/ultraarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,132 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group. 初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to listen for changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group 
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link. 获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position.
-        #  设置目标位置所使用的参考坐标系
-        self.reference_frame = "joint1"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails. 
-        # 当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian). 
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # Control the robotic arm to return to the initialization position first. 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system 
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion. 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm. 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-
-        traj = self.arm.plan()
-
-        # Control the motion of the robotic arm according to the planned motion path. 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參数1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端反向旋转90度,  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        self.moving()
-
-        # Add the environment. 添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        suit_path = (
-            roslib.packages.get_pkg_dir("mycobot_description")
-            + "/urdf/mycobot/suit_env.dae"
-        )
-        # need `pyassimp==3.3`
-        self.scene.add_mesh("suit", suit_post, suit_path)
-        rospy.sleep(2)
-
-        # Run again if there is environmental impact. 有环境影响后再运行一次
-        self.moving()
-
-        # Close and exit moveit. 关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

ultraArm/ultraarm_moveit/scripts/sync_plan.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import math
 import rospy
 from sensor_msgs.msg import JointState
 from pymycobot.ultraArm import ultraArm
@@ -12,11 +13,11 @@ def callback(data):
     # print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(round(value,3))
+        radians_to_angles = round(math.degrees(value), 2)
-    
+        data_list.append(radians_to_angles)
-    print('data_list:',data_list)
+        
-    ua.set_radians(data_list, 50)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    # time.sleep(0.5)
+    ua.set_angles(data_list, 25)
 
 
 def listener():