-
Notifications
You must be signed in to change notification settings - Fork 81
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
2 changed files
with
174 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |