From c66380e66bf49464d8e4bbbdfee0393027bcbae1 Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 5 Aug 2021 18:55:16 +0800 Subject: [PATCH] add moveit demo script. --- mycobot_280_moveit/CMakeLists.txt | 1 + ...th_planning_and_obstacle_avoidance_demo.py | 125 ++++++++++++++++++ mycobot_280_moveit/scripts/testing.py | 93 ------------- 3 files changed, 126 insertions(+), 93 deletions(-) create mode 100644 mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_280_moveit/scripts/testing.py diff --git a/mycobot_280_moveit/CMakeLists.txt b/mycobot_280_moveit/CMakeLists.txt index 7215247..e3148b0 100644 --- a/mycobot_280_moveit/CMakeLists.txt +++ b/mycobot_280_moveit/CMakeLists.txt @@ -13,6 +13,7 @@ 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_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py new file mode 100644 index 0000000..f47c0e8 --- /dev/null +++ b/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py @@ -0,0 +1,125 @@ +#!/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): + # 初始化move_group的API + moveit_commander.roscpp_initialize(sys.argv) + + # 初始化ROS节点 + rospy.init_node("moveit_ik_demo") + + # 初始化场景对象，用来监听外部环境的变化 + self.scene = moveit_commander.PlanningSceneInterface() + rospy.sleep(1) + + # 初始化需要使用move group控制的机械臂中的self.arm group + self.arm = moveit_commander.MoveGroupCommander("arm_group") + + # 获取终端link的名称 + self.end_effector_link = self.arm.get_end_effector_link() + + # 设置目标位置所使用的参考坐标系 + self.reference_frame = "joint1" + self.arm.set_pose_reference_frame(self.reference_frame) + + # 当运动规划失败后，允许重新规划 + self.arm.allow_replanning(True) + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + self.arm.set_goal_position_tolerance(0.01) + self.arm.set_goal_orientation_tolerance(0.05) + + def moving(self): + # # 控制机械臂先回到初始化位置 + self.arm.set_named_target("init_pose") + self.arm.go() + rospy.sleep(2) + + # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， + # 姿态使用四元数描述，基于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 + + # 设置机器臂当前的状态作为运动初始状态 + self.arm.set_start_state_to_current_state() + + # 设置机械臂终端运动的目标位姿 + self.arm.set_pose_target(target_pose, self.end_effector_link) + + # 规划运动路径 + traj = self.arm.plan() + + # 按照规划的运动路径控制机械臂运动 + self.arm.execute(traj) + rospy.sleep(1) + + # 控制机械臂终端向右移动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) + + # 控制机械臂终端反向旋转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() + + # 添加环境 + 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) + + # 有环境影响后在运行一次 + self.moving() + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + + +if __name__ == "__main__": + o = MoveItPlanningDemo() + o.run() diff --git a/mycobot_280_moveit/scripts/testing.py b/mycobot_280_moveit/scripts/testing.py deleted file mode 100644 index a4b3e04..0000000 --- a/mycobot_280_moveit/scripts/testing.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, 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 MoveItIkDemo: - def __init__(self): - # 初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # 初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # 初始化需要使用move group控制的机械臂中的arm group - arm = moveit_commander.MoveGroupCommander("mycobot_arm") - - # 获取终端link的名称 - end_effector_link = arm.get_end_effector_link() - - # 设置目标位置所使用的参考坐标系 - reference_frame = "joint1" - arm.set_pose_reference_frame(reference_frame) - - # 当运动规划失败后，允许重新规划 - arm.allow_replanning(True) - - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - arm.set_goal_position_tolerance(0.01) - arm.set_goal_orientation_tolerance(0.05) - - # # 控制机械臂先回到初始化位置 - arm.set_named_target("init_pose") - arm.go() - rospy.sleep(2) - - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = 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 - - # 设置机器臂当前的状态作为运动初始状态 - arm.set_start_state_to_current_state() - - # 设置机械臂终端运动的目标位姿 - arm.set_pose_target(target_pose, end_effector_link) - - # 规划运动路径 - traj = arm.plan() - - # 按照规划的运动路径控制机械臂运动 - arm.execute(traj) - rospy.sleep(1) - - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - arm.shift_pose_target(1, 0.12, end_effector_link) - arm.go() - rospy.sleep(1) - - arm.shift_pose_target(1, 0.1, end_effector_link) - arm.go() - rospy.sleep(1) - - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # arm.shift_pose_target(3, -1.57, end_effector_link) - # arm.go() - # rospy.sleep(1) - - # 控制机械臂回到初始化位置 - # arm.set_named_target("home") - # arm.go() - - # 关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - MoveItIkDemo()