运动规划 Python API

本教程介绍 moveit_py 运动规划 API 的基础知识，分为以下几个部分：

• 快速入门： 教程环境搭建要求概述。
• 理解规划参数： 为受支持的规划器设置参数概述。
• 单管道规划（默认配置）： 使用预定义的机器人配置进行规划。
• 单管道规划（机器人状态）： 使用机器人状态实例进行规划。
• 单管道规划（位姿目标）： 使用位姿目标进行规划。
• 单管道规划（自定义约束）： 使用自定义约束进行规划。
• 多管道规划： 并行运行多个规划管道。
• 使用规划场景： 添加和移除碰撞物体以及碰撞检测。

moveitcpp 与 move_group_interface

本教程的代码可以在 moveit2_tutorials GitHub 项目中找到。

快速入门

要完成本教程，你需要搭建一个包含 MoveIt 2 及其对应教程的工作空间（workspace）。关于如何搭建此类工作空间的概述见 Getting Started Guide，更多信息请参阅该指南。

搭建好工作空间后，运行以下命令来执行本教程的代码：

ros2 launch moveit2_tutorials motion_planning_python_api_tutorial.launch.py

理解规划参数

MoveIt 开箱即用支持多种规划库，因此为所使用的规划器提供合适的参数设置非常重要。

为此，我们指定一个 yaml 配置文件来定义与 moveit_py 节点关联的参数。

此类配置文件的一个示例如下：

planning_scene_monitor_options:
        name: "planning_scene_monitor"
        robot_description: "robot_description"
        joint_state_topic: "/joint_states"
        attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
        publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
        monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
        wait_for_initial_state_timeout: 10.0

planning_pipelines:
        pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"]

plan_request_params:
        planning_attempts: 1
        planning_pipeline: ompl
        max_velocity_scaling_factor: 1.0
        max_acceleration_scaling_factor: 1.0

ompl_rrtc:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: ompl
                planner_id: "RRTConnectkConfigDefault"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.0

ompl_rrt_star:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: ompl_rrt_star # Different OMPL pipeline name!
                planner_id: "RRTstarkConfigDefault"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.5

pilz_lin:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: pilz_industrial_motion_planner
                planner_id: "PTP"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 0.8

chomp:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: chomp
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.5

配置文件的第一块设置了规划场景监视器（planning scene monitor）选项，例如它所订阅的话题（如果你不熟悉规划场景监视器，可参阅本教程）：

planning_scene_monitor_options:
        name: "planning_scene_monitor"
        robot_description: "robot_description"
        joint_state_topic: "/joint_states"
        attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
        publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
        monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
        wait_for_initial_state_timeout: 10.0

配置文件的第二块设置了我们要使用的规划管道。MoveIt 支持多种运动规划库，包括 OMPL、Pilz Industrial Motion Planner（Pilz 工业运动规划器）、Stochastic Trajectory Optimization for Motion Planning（STOMP，运动规划随机轨迹优化）、Search-Based Planning Library（SBPL，基于搜索的规划库）、Covariant Hamiltonian Optimization for Motion Planning（CHOMP，运动规划协变哈密顿优化）等。配置 moveit_py 节点时，我们需要指定要使用的规划管道：

planning_pipelines:
        pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"]

对于这些命名管道中的每一个，都必须提供一个配置，通过 planner_id 标识要使用的规划器，并提供其他设置，如规划尝试次数：

ompl_rrtc:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: ompl
                planner_id: "RRTConnectkConfigDefault"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 0.5

ompl_rrt_star:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: ompl_rrt_star
                planner_id: "RRTstarkConfigDefault"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.5

pilz_lin:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: pilz_industrial_motion_planner
                planner_id: "PTP"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 0.8

chomp:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: chomp
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.5

这些参数将作为 moveit_py 节点参数提供，并在执行规划时于运行时使用。这正是我们接下来要探讨的内容。

实例化 moveit_py 与规划组件

在规划运动之前，我们需要实例化一个 moveit_py 节点及其派生的规划组件。同时还会实例化一个 rclpy 日志记录器（logger）对象：

rclpy.init()
logger = rclpy.logging.get_logger("moveit_py.pose_goal")

# instantiate MoveItPy instance and get planning component
panda = MoveItPy(node_name="moveit_py")
panda_arm = panda.get_planning_component("panda_arm")
logger.info("MoveItPy instance created")

使用由 panda_arm 变量表示的规划组件，我们就可以开始执行运动规划了。我们首先定义一个用于规划和执行运动的辅助函数：

def plan_and_execute(
        robot,
        planning_component,
        logger,
        single_plan_parameters=None,
        multi_plan_parameters=None,
        ):
        """A helper function to plan and execute a motion."""
        # plan to goal
        logger.info("Planning trajectory")
        if multi_plan_parameters is not None:
                plan_result = planning_component.plan(
                        multi_plan_parameters=multi_plan_parameters
                )
        elif single_plan_parameters is not None:
                plan_result = planning_component.plan(
                        single_plan_parameters=single_plan_parameters
                )
        else:
                plan_result = planning_component.plan()

        # execute the plan
        if plan_result:
                logger.info("Executing plan")
                robot_trajectory = plan_result.trajectory
                robot.execute(robot_trajectory, controllers=[])
        else:
                logger.error("Planning failed")

单管道规划——默认配置

我们从执行单个规划管道开始探索 moveit_py 运动规划 API，该管道将规划到预定义的机器人配置（在 srdf 文件中定义）：

# set plan start state using predefined state
panda_arm.set_start_state(configuration_name="ready")

# set pose goal using predefined state
panda_arm.set_goal_state(configuration_name="extended")

# plan to goal
plan_and_execute(panda, panda_arm, logger)

单管道规划——机器人状态

接下来，我们将规划到某个机器人状态。这种方法非常灵活，因为我们可以随意更改机器人状态的配置（例如通过设置关节值）。这里我们使用 set_start_state_to_current_state 方法将机器人的起始状态设置为当前状态，并使用 set_goal_state 方法将目标状态设置为随机配置，然后规划到目标状态并执行：

# instantiate a RobotState instance using the current robot model
robot_model = panda.get_robot_model()
robot_state = RobotState(robot_model)

# randomize the robot state
robot_state.set_to_random_positions()

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set goal state to the initialized robot state
logger.info("Set goal state to the initialized robot state")
panda_arm.set_goal_state(robot_state=robot_state)

# plan to goal
plan_and_execute(panda, panda_arm, logger)

单管道规划——位姿目标

指定目标状态的另一种常见方式是通过表示位姿目标的 ROS 消息。这里演示如何为机器人的末端执行器设置位姿目标：

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set pose goal with PoseStamped message
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="panda_link8")

# plan to goal
plan_and_execute(panda, panda_arm, logger)

单管道规划——自定义约束

你还可以通过自定义约束来控制运动规划的输出。这里演示规划到一个满足一组关节约束的配置：

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set constraints message
joint_values = {
        "panda_joint1": -1.0,
        "panda_joint2": 0.7,
        "panda_joint3": 0.7,
        "panda_joint4": -1.5,
        "panda_joint5": -0.7,
        "panda_joint6": 2.0,
        "panda_joint7": 0.0,
}
robot_state.joint_positions = joint_values
joint_constraint = construct_joint_constraint(
        robot_state=robot_state,
        joint_model_group=panda.get_robot_model().get_joint_model_group("panda_arm"),
)
panda_arm.set_goal_state(motion_plan_constraints=[joint_constraint])

# plan to goal
plan_and_execute(panda, panda_arm, logger)

多管道规划

moveit_cpp 和 moveit_py 最近新增的功能是并行执行多个规划管道，并在所有生成的运动规划结果中选出最能满足你任务需求的那一个。在前面的章节中，我们定义了一组规划管道。下面看看如何并行使用其中几个管道进行规划：

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set pose goal with PoseStamped message
panda_arm.set_goal_state(configuration_name="ready")

# initialise multi-pipeline plan request parameters
multi_pipeline_plan_request_params = MultiPipelinePlanRequestParameters(
        panda, ["ompl_rrtc", "pilz_lin", "chomp", "ompl_rrt_star"]
)

# plan to goal
plan_and_execute(
        panda,
        panda_arm,
        logger,
        multi_plan_parameters=multi_pipeline_plan_request_params,
)

# execute the plan
if plan_result:
        logger.info("Executing plan")
        panda_arm.execute()

使用规划场景

本小节的代码要求运行另一个 Python 文件，你可以按如下方式指定：

ros2 launch moveit2_tutorials motion_planning_python_api_tutorial.launch.py example_file:=motion_planning_python_api_planning_scene.py

与规划场景交互需要创建一个规划场景监视器：

panda = MoveItPy(node_name="moveit_py_planning_scene")
panda_arm = panda.get_planning_component("panda_arm")
planning_scene_monitor = panda.get_planning_scene_monitor()

然后使用规划场景监视器的 read_write 上下文向规划场景中添加碰撞物体：

with planning_scene_monitor.read_write() as scene:
        collision_object = CollisionObject()
        collision_object.header.frame_id = "panda_link0"
        collision_object.id = "boxes"

        box_pose = Pose()
        box_pose.position.x = 0.15
        box_pose.position.y = 0.1
        box_pose.position.z = 0.6

        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions = dimensions

        collision_object.primitives.append(box)
        collision_object.primitive_poses.append(box_pose)
        collision_object.operation = CollisionObject.ADD

        scene.apply_collision_object(collision_object)
        scene.current_state.update()  # Important to ensure the scene is updated

类似地，可以使用 CollisionObject.REMOVE 操作移除物体，或移除场景中的所有物体：

with planning_scene_monitor.read_write() as scene:
        scene.remove_all_collision_objects()
        scene.current_state.update()

对于不需要修改场景的任务（例如碰撞检测），还可以使用规划场景监视器的 read_only 上下文。例如：

with planning_scene_monitor.read_only() as scene:
        robot_state = scene.current_state
        original_joint_positions = robot_state.get_joint_group_positions("panda_arm")

        # Set the pose goal
        pose_goal = Pose()
        pose_goal.position.x = 0.25
        pose_goal.position.y = 0.25
        pose_goal.position.z = 0.5
        pose_goal.orientation.w = 1.0

        # Set the robot state and check collisions
        robot_state.set_from_ik("panda_arm", pose_goal, "panda_hand")
        robot_state.update()  # required to update transforms
        robot_collision_status = scene.is_state_colliding(
                robot_state=robot_state, joint_model_group_name="panda_arm", verbose=True
        )
        logger.info(f"\nRobot is in collision: {robot_collision_status}\n")

        # Restore the original state
        robot_state.set_joint_group_positions(
                "panda_arm",
                original_joint_positions,
        )
        robot_state.update()  # required to update transforms