diff --git a/myArm/myarm_moveit/config/ompl_planning.yaml b/myArm/myarm_moveit/config/ompl_planning.yaml index 291935d6..c6172caa 100644 --- a/myArm/myarm_moveit/config/ompl_planning.yaml +++ b/myArm/myarm_moveit/config/ompl_planning.yaml @@ -165,7 +165,7 @@ planner_configs: use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 arm_group: - default_planner_config: RRT + default_planner_config: RRTConnect planner_configs: - AnytimePathShortening - SBL diff --git a/myArm/myarm_moveit/scripts/obstacle_test.py b/myArm/myarm_moveit/scripts/obstacle_test.py new file mode 100755 index 00000000..8df8f879 --- /dev/null +++ b/myArm/myarm_moveit/scripts/obstacle_test.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 + +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") + # 切换规划器为ompl + # self.arm_group.set_planner_id("ompl") + # planner_params = {"clearance": 0.1} # 设置障碍物清除参数为0.1米（可以根据实际情况调整值） + # arm_group.set_planner_params("ompl", planner_params) + + # 设置 OMPL 规划器的名称 + # self.arm_group.set_planner_id("RRTConnectkConfigDefault") + + # 获取末端关节的名称 + 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(5) + + + # 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示 + 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 + + # 移除所有障碍物 + # scene.remove_world_object("cylinder1") + # scene.remove_world_object("cylinder2") + + # 更新当前的位姿 + 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(5) + # 获取末端执行器的姿态 + 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