From 84b01b404e642a223a52e0d7aecd829439e48fbe Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Tue, 12 Dec 2023 18:33:17 +0800 Subject: [PATCH] opt code and fix joint movement jitter --- Mercury/mercury_a1/scripts/slider_control.py | 13 +- .../scripts/obstacle_avoidance_demo.py | 163 ---- .../mercury_a1_moveit/scripts/sync_plan.py | 10 +- Mercury/mercury_b1/scripts/slider_control.py | 18 +- .../mercury_b1_moveit/scripts/sync_plan.py | 31 +- mecharm/mecharm/scripts/slider_control.py | 21 +- mecharm/mecharm_moveit/CMakeLists.txt | 1 - mecharm/mecharm_moveit/launch/moveit.rviz | 4 +- ...th_planning_and_obstacle_avoidance_demo.py | 132 ---- mecharm/mecharm_moveit/scripts/sync_plan.py | 16 +- mecharm/mecharm_pi/scripts/slider_control.py | 18 +- mecharm/mecharm_pi_moveit/CMakeLists.txt | 1 - mecharm/mecharm_pi_moveit/launch/moveit.rviz | 4 +- ...th_planning_and_obstacle_avoidance_demo.py | 132 ---- .../mecharm_pi_moveit/scripts/sync_plan.py | 19 +- myArm/myarm/scripts/slider_control.py | 15 +- myArm/myarm_moveit/launch/moveit.rviz | 6 +- .../myarm_moveit/launch/mycobot_moveit.launch | 65 -- myArm/myarm_moveit/scripts/sync_plan.py | 15 +- .../mycobot_280/scripts/slider_control.py | 18 +- .../scripts/slider_control_gripper.py | 7 +- .../scripts/sync_plan.py | 16 +- .../mycobot_280_moveit/launch/moveit.rviz | 4 +- .../mycobot_280_moveit/scripts/sync_plan.py | 17 +- .../scripts/slider_control.py | 16 +- .../launch/moveit.rviz | 4 +- .../scripts/sync_plan.py | 18 +- .../mycobot_280jn/scripts/slider_control.py | 21 +- .../mycobot_280jn_moveit/launch/moveit.rviz | 4 +- .../mycobot_280jn_moveit/scripts/sync_plan.py | 20 +- .../mycobot_280pi/scripts/slider_control.py | 19 +- .../mycobot_280pi_moveit/launch/moveit.rviz | 4 +- .../mycobot_280pi_moveit/scripts/sync_plan.py | 20 +- mycobot_pro/mycobot_600/scripts/slider_600.py | 32 +- mycobot_pro/mycobot_600_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 137 ---- .../mycobot_600_moveit/scripts/sync_plan.py | 32 +- .../mycobot_600_moveit/scripts/test.py | 726 ------------------ .../scripts/slider_control.py | 19 +- .../mypalletizer_260_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 132 ---- .../scripts/sync_plan.py | 15 +- .../scripts/slider_control.py | 18 +- .../mypalletizer_260_pi_moveit/CMakeLists.txt | 1 - .../launch/moveit.rviz | 4 +- ...th_planning_and_obstacle_avoidance_demo.py | 132 ---- .../scripts/sync_plan.py | 16 +- ultraArm/ultraarm/scripts/slider_control.py | 10 +- ultraArm/ultraarm_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 132 ---- ultraArm/ultraarm_moveit/scripts/sync_plan.py | 11 +- 51 files changed, 280 insertions(+), 2012 deletions(-) delete mode 100755 Mercury/mercury_a1_moveit/scripts/obstacle_avoidance_demo.py delete mode 100755 mecharm/mecharm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mecharm/mecharm_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py mode change 100644 => 100755 mecharm/mecharm_pi_moveit/scripts/sync_plan.py delete mode 100644 myArm/myarm_moveit/launch/mycobot_moveit.launch delete mode 100755 mycobot_pro/mycobot_600_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_pro/mycobot_600_moveit/scripts/test.py delete mode 100644 mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mypalletizer_260/mypalletizer_260_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 ultraArm/ultraarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py diff --git a/Mercury/mercury_a1/scripts/slider_control.py b/Mercury/mercury_a1/scripts/slider_control.py index 6955ab2c..70511972 100755 --- a/Mercury/mercury_a1/scripts/slider_control.py +++ b/Mercury/mercury_a1/scripts/slider_control.py @@ -8,6 +8,7 @@ port: serial prot string. Defaults is '/dev/ttyAMA1' baud: serial prot baudrate. Defaults is 115200. """ +import time import math import rospy from sensor_msgs.msg import JointState @@ -25,12 +26,11 @@ def callback(data): rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple) data_list = [] for index, value in enumerate(data.position): - radians_to_angles = math.degrees(value) + radians_to_angles = round(math.degrees(value), 2) data_list.append(radians_to_angles) - - # mc.send_radians(data_list, 80) - mc.send_angles(data_list, 80) - # time.sleep(0.5) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -42,6 +42,9 @@ def listener(): baud = rospy.get_param("~baud", 115200) print(port, baud) mc = Mercury(port, baud) + time.sleep(0.05) + mc.set_free_mode(1) + time.sleep(0.05) # spin() simply keeps python from exiting until this node is stopped # spin()只是阻止python退出，直到该节点停止 diff --git a/Mercury/mercury_a1_moveit/scripts/obstacle_avoidance_demo.py b/Mercury/mercury_a1_moveit/scripts/obstacle_avoidance_demo.py deleted file mode 100755 index 813b4d19..00000000 --- a/Mercury/mercury_a1_moveit/scripts/obstacle_avoidance_demo.py +++ /dev/null @@ -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 diff --git a/Mercury/mercury_a1_moveit/scripts/sync_plan.py b/Mercury/mercury_a1_moveit/scripts/sync_plan.py index 5dc6af2e..02387f4d 100755 --- a/Mercury/mercury_a1_moveit/scripts/sync_plan.py +++ b/Mercury/mercury_a1_moveit/scripts/sync_plan.py @@ -21,17 +21,13 @@ def callback(data): # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - # print(data.position) - rounded_data_tuple = tuple(round(value, 2) for value in data.position) - rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple) data_list = [] for index, value in enumerate(data.position): radians_to_angles = round(math.degrees(value), 2) data_list.append(radians_to_angles) - - # mc.send_radians(data_list, 80) - mc.send_angles(data_list, 80) - # time.sleep(0.5) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): diff --git a/Mercury/mercury_b1/scripts/slider_control.py b/Mercury/mercury_b1/scripts/slider_control.py index c2f6027b..66a860b9 100755 --- a/Mercury/mercury_b1/scripts/slider_control.py +++ b/Mercury/mercury_b1/scripts/slider_control.py @@ -24,8 +24,7 @@ def callback(data): # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - rounded_data_tuple = tuple(round(value, 2) for value in data.position) - # print(rounded_data_tuple) + data_list = [] for index, value in enumerate(data.position): radians_to_angles = round(math.degrees(value), 2) @@ -39,15 +38,15 @@ def callback(data): print('right_arm:', right_arm) print('middle_arm:', middle_arm) - l.send_angles(left_arm, 50) + l.send_angles(left_arm, 25) time.sleep(0.02) - r.send_angles(right_arm, 50) + r.send_angles(right_arm, 25) time.sleep(0.02) - r.send_angle(11, middle_arm[0], 50) + r.send_angle(11, middle_arm[0], 25) time.sleep(0.02) - r.send_angle(12, middle_arm[1], 50) + r.send_angle(12, middle_arm[1], 25) time.sleep(0.02) - r.send_angle(13, middle_arm[2], 50) + r.send_angle(13, middle_arm[2], 25) time.sleep(0.02) @@ -63,7 +62,10 @@ def listener(): print(port2, baud) l = Mercury(port1, baud) r = Mercury(port2, baud) - + time.sleep(0.05) + l.set_free_mode(1) + r.set_free_mode(1) + time.sleep(0.05) # spin() simply keeps python from exiting until this node is stopped # spin()只是阻止python退出，直到该节点停止 print("spin ...") diff --git a/Mercury/mercury_b1_moveit/scripts/sync_plan.py b/Mercury/mercury_b1_moveit/scripts/sync_plan.py index 30f526de..6a5b66f5 100755 --- a/Mercury/mercury_b1_moveit/scripts/sync_plan.py +++ b/Mercury/mercury_b1_moveit/scripts/sync_plan.py @@ -17,16 +17,15 @@ from pymycobot.mercury import Mercury # left arm port -cx1 = None +l = None # right arm port -cx2 = None +r = None def callback(data): # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - rounded_data_tuple = tuple(round(value, 2) for value in data.position) - # print(rounded_data_tuple) + data_list = [] for index, value in enumerate(data.position): radians_to_angles = round(math.degrees(value), 2) @@ -40,23 +39,20 @@ def callback(data): print('right_arm:', right_arm) print('middle_arm:', middle_arm) - cx1.send_angles(left_arm, 50) + l.send_angles(left_arm, 25) time.sleep(0.02) - cx2.send_angles(right_arm, 50) + r.send_angles(right_arm, 25) time.sleep(0.02) - cx2.send_angle(11, middle_arm[0], 50) + r.send_angle(11, middle_arm[0], 25) time.sleep(0.02) - cx2.send_angle(12, middle_arm[1], 50) + r.send_angle(12, middle_arm[1], 25) time.sleep(0.02) - cx2.send_angle(13, middle_arm[2], 50) + r.send_angle(13, middle_arm[2], 25) time.sleep(0.02) - # mc.send_radians(data_list, 80) - # mc.send_angles(data_list, 80) - # time.sleep(0.5) def listener(): - global cx1, cx2 + global l, r rospy.init_node("control_slider", anonymous=True) rospy.Subscriber("joint_states", JointState, callback) @@ -65,9 +61,12 @@ def listener(): baud = rospy.get_param("~baud", 115200) print(port1, baud) print(port2, baud) - cx1 = Mercury(port1, baud) - cx2 = Mercury(port2, baud) - + l = Mercury(port1, baud) + r = Mercury(port2, baud) + time.sleep(0.05) + l.set_free_mode(1) + r.set_free_mode(1) + time.sleep(0.05) # spin() simply keeps python from exiting until this node is stopped # spin()只是阻止python退出，直到该节点停止 print("spin ...") diff --git a/mecharm/mecharm/scripts/slider_control.py b/mecharm/mecharm/scripts/slider_control.py index 7e3f791f..c05da26a 100755 --- a/mecharm/mecharm/scripts/slider_control.py +++ b/mecharm/mecharm/scripts/slider_control.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # -*- coding:utf-8 -*- """[summary] @@ -9,7 +9,8 @@ port: serial prot string. Defaults is '/dev/ttyUSB0' baud: serial prot baudrate. Defaults is 115200. """ - +import math +import time import rospy from sensor_msgs.msg import JointState @@ -22,14 +23,13 @@ def callback(data): # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - print(data.position) data_list = [] for index, value in enumerate(data.position): - data_list.append(value) - - # print(data_list) - mc.send_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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退出，直到该节点停止 diff --git a/mecharm/mecharm_moveit/CMakeLists.txt b/mecharm/mecharm_moveit/CMakeLists.txt index b86cf68f..92e5c250 100644 --- a/mecharm/mecharm_moveit/CMakeLists.txt +++ b/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} ) diff --git a/mecharm/mecharm_moveit/launch/moveit.rviz b/mecharm/mecharm_moveit/launch/moveit.rviz index 0be28161..811b8d8e 100644 --- a/mecharm/mecharm_moveit/launch/moveit.rviz +++ b/mecharm/mecharm_moveit/launch/moveit.rviz @@ -41,7 +41,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mecharm/mecharm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mecharm/mecharm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100755 index 3584da05..00000000 --- a/mecharm/mecharm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/mecharm/mecharm_moveit/scripts/sync_plan.py b/mecharm/mecharm_moveit/scripts/sync_plan.py index 7b59645c..0692cfc5 100755 --- a/mecharm/mecharm_moveit/scripts/sync_plan.py +++ b/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: - # value *= -1 - data_list.append(value) - # print("data_list:",data_list) - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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) diff --git a/mecharm/mecharm_pi/scripts/slider_control.py b/mecharm/mecharm_pi/scripts/slider_control.py index 1e9352fa..add56770 100644 --- a/mecharm/mecharm_pi/scripts/slider_control.py +++ b/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 @@ 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) - - # print(data_list) - mc.send_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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退出，直到该节点停止 diff --git a/mecharm/mecharm_pi_moveit/CMakeLists.txt b/mecharm/mecharm_pi_moveit/CMakeLists.txt index 869ed9ab..239e9a24 100644 --- a/mecharm/mecharm_pi_moveit/CMakeLists.txt +++ b/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} ) diff --git a/mecharm/mecharm_pi_moveit/launch/moveit.rviz b/mecharm/mecharm_pi_moveit/launch/moveit.rviz index d1c74d02..31f22ab4 100644 --- a/mecharm/mecharm_pi_moveit/launch/moveit.rviz +++ b/mecharm/mecharm_pi_moveit/launch/moveit.rviz @@ -41,7 +41,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mecharm/mecharm_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mecharm/mecharm_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index 3584da05..00000000 --- a/mecharm/mecharm_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/mecharm/mecharm_pi_moveit/scripts/sync_plan.py b/mecharm/mecharm_pi_moveit/scripts/sync_plan.py old mode 100644 new mode 100755 index 37250ae7..ac32c070 --- a/mecharm/mecharm_pi_moveit/scripts/sync_plan.py +++ b/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: - # value *= -1 - data_list.append(value) - # print("data_list:",data_list) - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 diff --git a/myArm/myarm/scripts/slider_control.py b/myArm/myarm/scripts/slider_control.py index 59b5284c..64bdecb0 100755 --- a/myArm/myarm/scripts/slider_control.py +++ b/myArm/myarm/scripts/slider_control.py @@ -8,6 +8,7 @@ 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 @@ 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) - mc.send_angles(data_list, 80) - # time.sleep(0.5) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) + def listener(): @@ -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 ...") diff --git a/myArm/myarm_moveit/launch/moveit.rviz b/myArm/myarm_moveit/launch/moveit.rviz index d57286c6..dc732e2c 100644 --- a/myArm/myarm_moveit/launch/moveit.rviz +++ b/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 diff --git a/myArm/myarm_moveit/launch/mycobot_moveit.launch b/myArm/myarm_moveit/launch/mycobot_moveit.launch deleted file mode 100644 index 438b9b6f..00000000 --- a/myArm/myarm_moveit/launch/mycobot_moveit.launch +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm_moveit/scripts/sync_plan.py b/myArm/myarm_moveit/scripts/sync_plan.py index 736425c6..b324e89f 100755 --- a/myArm/myarm_moveit/scripts/sync_plan.py +++ b/myArm/myarm_moveit/scripts/sync_plan.py @@ -9,6 +9,7 @@ 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 @@ 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) - mc.send_angles(data_list, 80) - # time.sleep(0.5) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 ...") diff --git a/mycobot_280/mycobot_280/scripts/slider_control.py b/mycobot_280/mycobot_280/scripts/slider_control.py index e3cf6a61..4b9d390b 100755 --- a/mycobot_280/mycobot_280/scripts/slider_control.py +++ b/mycobot_280/mycobot_280/scripts/slider_control.py @@ -8,7 +8,8 @@ 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 @@ 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) - - mc.send_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) + 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退出，直到该节点停止 diff --git a/mycobot_280/mycobot_280/scripts/slider_control_gripper.py b/mycobot_280/mycobot_280/scripts/slider_control_gripper.py index b0b48c7e..868cafa7 100755 --- a/mycobot_280/mycobot_280/scripts/slider_control_gripper.py +++ b/mycobot_280/mycobot_280/scripts/slider_control_gripper.py @@ -9,7 +9,7 @@ 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 ...") diff --git a/mycobot_280/mycobot_280_gripper_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280_gripper_moveit/scripts/sync_plan.py index b1462419..94ed4aef 100755 --- a/mycobot_280/mycobot_280_gripper_moveit/scripts/sync_plan.py +++ b/mycobot_280/mycobot_280_gripper_moveit/scripts/sync_plan.py @@ -7,25 +7,19 @@ 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 diff --git a/mycobot_280/mycobot_280_moveit/launch/moveit.rviz b/mycobot_280/mycobot_280_moveit/launch/moveit.rviz index 38b3736c..0f3fc2fa 100644 --- a/mycobot_280/mycobot_280_moveit/launch/moveit.rviz +++ b/mycobot_280/mycobot_280_moveit/launch/moveit.rviz @@ -41,7 +41,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mycobot_280/mycobot_280_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280_moveit/scripts/sync_plan.py index 4021613b..642cbc14 100644 --- a/mycobot_280/mycobot_280_moveit/scripts/sync_plan.py +++ b/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 @@ 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: - # value *= -1 - data_list.append(value) - - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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) diff --git a/mycobot_280/mycobot_280arduino/scripts/slider_control.py b/mycobot_280/mycobot_280arduino/scripts/slider_control.py index 9973e6a5..685a3071 100644 --- a/mycobot_280/mycobot_280arduino/scripts/slider_control.py +++ b/mycobot_280/mycobot_280arduino/scripts/slider_control.py @@ -8,7 +8,8 @@ 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 @@ 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) - - mc.send_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 ...") diff --git a/mycobot_280/mycobot_280arduino_moveit/launch/moveit.rviz b/mycobot_280/mycobot_280arduino_moveit/launch/moveit.rviz index 5d412f18..cb037cd9 100644 --- a/mycobot_280/mycobot_280arduino_moveit/launch/moveit.rviz +++ b/mycobot_280/mycobot_280arduino_moveit/launch/moveit.rviz @@ -43,7 +43,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py index 2023b2f5..b4874a63 100644 --- a/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py +++ b/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 @@ def callback(data): - rospy.loginfo(rospy.get_caller_id() + "%s", data) + # rospy.loginfo(rospy.get_caller_id() + "%s", data) data_list = [] for index, value in enumerate(data.position): - # if index != 2: - # value *= -1 - data_list.append(value) - - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 diff --git a/mycobot_280/mycobot_280jn/scripts/slider_control.py b/mycobot_280/mycobot_280jn/scripts/slider_control.py index bf58aaec..b064d9ee 100755 --- a/mycobot_280/mycobot_280jn/scripts/slider_control.py +++ b/mycobot_280/mycobot_280jn/scripts/slider_control.py @@ -8,7 +8,8 @@ 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 @@ 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) - - mc.send_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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') - # port=rospy.get_param("~port",9000) - # print(ip,port) - # ms=MyCobotSocket(ip,port) - # ms.connect() + 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退出，直到该节点停止 diff --git a/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz b/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz index 8d7460d5..c1229157 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz +++ b/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz @@ -43,7 +43,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py index ed14da68..572c13ab 100644 --- a/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py +++ b/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 @@ def callback(data): - rospy.loginfo(rospy.get_caller_id() + "%s", data) + # rospy.loginfo(rospy.get_caller_id() + "%s", data) data_list = [] for index, value in enumerate(data.position): - # if index != 2: - # value *= -1 - data_list.append(value) - - mc.send_radians(data_list, 80) - + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): global mc @@ -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 diff --git a/mycobot_280/mycobot_280pi/scripts/slider_control.py b/mycobot_280/mycobot_280pi/scripts/slider_control.py index 9f9f6b71..6b3b3ab6 100755 --- a/mycobot_280/mycobot_280pi/scripts/slider_control.py +++ b/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 @@ 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 @@ 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) - - mc.send_radians(data_list, 80) - # time.sleep(0.5) - + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): global mc @@ -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退出，直到该节点停止 diff --git a/mycobot_280/mycobot_280pi_moveit/launch/moveit.rviz b/mycobot_280/mycobot_280pi_moveit/launch/moveit.rviz index 38b3736c..0f3fc2fa 100644 --- a/mycobot_280/mycobot_280pi_moveit/launch/moveit.rviz +++ b/mycobot_280/mycobot_280pi_moveit/launch/moveit.rviz @@ -41,7 +41,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mycobot_280/mycobot_280pi_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280pi_moveit/scripts/sync_plan.py index b747e39d..e358b08d 100644 --- a/mycobot_280/mycobot_280pi_moveit/scripts/sync_plan.py +++ b/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 @@ def callback(data): - rospy.loginfo(rospy.get_caller_id() + "%s", data) + # rospy.loginfo(rospy.get_caller_id() + "%s", data) data_list = [] for index, value in enumerate(data.position): - # if index != 2: - # value *= -1 - data_list.append(value) - - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 diff --git a/mycobot_pro/mycobot_600/scripts/slider_600.py b/mycobot_pro/mycobot_600/scripts/slider_600.py index 12d50565..e9319887 100755 --- a/mycobot_pro/mycobot_600/scripts/slider_600.py +++ b/mycobot_pro/mycobot_600/scripts/slider_600.py @@ -13,42 +13,26 @@ 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 - data_list.append(value) - print ("data", data_list) - - 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) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.write_angles(data_list, 1000) def listener(): global mc rospy.init_node("control_slider", anonymous=True) ip = rospy.get_param("~ip", "192.168.1.159") - print (ip) - mc = ElephantRobot(ip, 5001) + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) # START CLIENT,启动客户端 res = mc.start_client() if res != "": diff --git a/mycobot_pro/mycobot_600_moveit/CMakeLists.txt b/mycobot_pro/mycobot_600_moveit/CMakeLists.txt index eb54d802..3a55cbf1 100644 --- a/mycobot_pro/mycobot_600_moveit/CMakeLists.txt +++ b/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} ) diff --git a/mycobot_pro/mycobot_600_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_pro/mycobot_600_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100755 index f74581c3..00000000 --- a/mycobot_pro/mycobot_600_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py b/mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py index 12d50565..3f068770 100755 --- a/mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py +++ b/mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py @@ -13,42 +13,28 @@ 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 - data_list.append(value) - print ("data", data_list) - - 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) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.write_angles(data_list, 1000) - 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) - mc = ElephantRobot(ip, 5001) + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) # START CLIENT,启动客户端 res = mc.start_client() if res != "": diff --git a/mycobot_pro/mycobot_600_moveit/scripts/test.py b/mycobot_pro/mycobot_600_moveit/scripts/test.py deleted file mode 100644 index 961f62f1..00000000 --- a/mycobot_pro/mycobot_600_moveit/scripts/test.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260/scripts/slider_control.py b/mypalletizer_260/mypalletizer_260/scripts/slider_control.py index 9232ec8c..6aa25d95 100755 --- a/mypalletizer_260/mypalletizer_260/scripts/slider_control.py +++ b/mypalletizer_260/mypalletizer_260/scripts/slider_control.py @@ -9,11 +9,11 @@ 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 @@ 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] - # print(data_list) - 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(): @@ -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 ...") diff --git a/mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt b/mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt index 67ac15bf..8e838d6a 100644 --- a/mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt +++ b/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} ) diff --git a/mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index 3584da05..00000000 --- a/mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py b/mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py index 75d59d4b..d0f90357 100644 --- a/mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py +++ b/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: - # value *= -1 - data_list.append(value) - # print("data_list:",data_list) - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + del data_list[3] + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + 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 diff --git a/mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py b/mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py index 787e4ed6..1714b519 100644 --- a/mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py +++ b/mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py @@ -9,11 +9,11 @@ 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) - # 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] - # print(data_list) - mc.send_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 ...") diff --git a/mypalletizer_260/mypalletizer_260_pi_moveit/CMakeLists.txt b/mypalletizer_260/mypalletizer_260_pi_moveit/CMakeLists.txt index 9c7ba37c..4064e830 100644 --- a/mypalletizer_260/mypalletizer_260_pi_moveit/CMakeLists.txt +++ b/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} ) diff --git a/mypalletizer_260/mypalletizer_260_pi_moveit/launch/moveit.rviz b/mypalletizer_260/mypalletizer_260_pi_moveit/launch/moveit.rviz index 0115e6cb..d40eb22d 100644 --- a/mypalletizer_260/mypalletizer_260_pi_moveit/launch/moveit.rviz +++ b/mypalletizer_260/mypalletizer_260_pi_moveit/launch/moveit.rviz @@ -41,7 +41,7 @@ Visualization Manager: Plane Cell Count: 10 Reference 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 diff --git a/mypalletizer_260/mypalletizer_260_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mypalletizer_260/mypalletizer_260_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index 3584da05..00000000 --- a/mypalletizer_260/mypalletizer_260_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/mypalletizer_260/mypalletizer_260_pi_moveit/scripts/sync_plan.py b/mypalletizer_260/mypalletizer_260_pi_moveit/scripts/sync_plan.py index 6d877613..58fcabc2 100644 --- a/mypalletizer_260/mypalletizer_260_pi_moveit/scripts/sync_plan.py +++ b/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 @@ def callback(data): - rospy.loginfo(rospy.get_caller_id() + "%s", data) + # rospy.loginfo(rospy.get_caller_id() + "%s", data) data_list = [] for index, value in enumerate(data.position): - # if index != 2: - # value *= -1 - data_list.append(value) - # print("data_list:",data_list) - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -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 diff --git a/ultraArm/ultraarm/scripts/slider_control.py b/ultraArm/ultraarm/scripts/slider_control.py index 8f0dfdc8..504c9904 100755 --- a/ultraArm/ultraarm/scripts/slider_control.py +++ b/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)) - - print('data_list:',data_list) - ua.set_radians(data_list, 80) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + ua.set_angles(data_list, 25) def listener(): diff --git a/ultraArm/ultraarm_moveit/CMakeLists.txt b/ultraArm/ultraarm_moveit/CMakeLists.txt index 8c4122b9..5566105d 100644 --- a/ultraArm/ultraarm_moveit/CMakeLists.txt +++ b/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} ) diff --git a/ultraArm/ultraarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/ultraArm/ultraarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index 3584da05..00000000 --- a/ultraArm/ultraarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/ultraArm/ultraarm_moveit/scripts/sync_plan.py b/ultraArm/ultraarm_moveit/scripts/sync_plan.py index 0ab60e8e..70026f4b 100644 --- a/ultraArm/ultraarm_moveit/scripts/sync_plan.py +++ b/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)) - - print('data_list:',data_list) - ua.set_radians(data_list, 50) - # time.sleep(0.5) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + ua.set_angles(data_list, 25) def listener():