使用 OMPL 约束规划
本教程介绍如何在 MoveIt 中使用 OMPL 的约束规划功能。为了展示该规划器的能力，我们用不同类型的路径约束解决了四个规划问题。如果你已经通读过 Move Group 接口教程 中的示例，理解起来会更加容易。
何时使用此规划器
Section titled “何时使用此规划器”该接口目前只支持对机器人任意连杆施加单个位置约束或方向约束。这种规划方法为 enforce_joint_model_state_space 选项提供了一种替代方案。它在笛卡尔空间中约束区域较小（甚至为零）的场景中尤为有用，因为在这些场景下，简单的拒绝采样并不总是有效。例如，可以用该规划器将机器人末端执行器约束在平面内或沿直线运动。
如何配置 OMPL 使用约束规划
Section titled “如何配置 OMPL 使用约束规划”OMPL 从名为 ompl_planning.yaml 的文件读取配置参数。本指南直接在 launch 文件中添加所需参数，但这些参数同样可以在 yaml 文件中设置。本指南使用 Panda 机器人，该文件可以在此处找到。如果你恰好有一个位置约束或一个方向约束，OMPL 将默认使用约束规划。要在其他场景中使用受约束的状态空间，请在机器人的 ompl_planning.yaml 中添加一个参数——将 enforce_constrained_state_space 设置为 true——以告知 OMPL 在受约束的状态空间中进行规划。此外，如果尚未指定 projection_evaluator 参数，还需要添加该参数。投影评估器通过将状态空间投影到低维欧几里得空间来帮助离散化高维状态空间。你可以在此处了解更多。一般来说，选取前几个关节效果就很好。
panda_arm: enforce_constrained_state_space: true projection_evaluator: joints(panda_joint1,panda_joint2)本教程将参数添加到launch 文件中。该 launch 文件使用 moveit_configs_utils 来简化 launch 文件的编写。moveit_config 使用 moveit_resources 的 Panda MoveIt 配置进行初始化。我们通过以下几行将 OMPL 参数添加到 planning_pipelines：
moveit_config.planning_pipelines["ompl"]["panda_arm"]["enforce_constrained_state_space"] = Truemoveit_config.planning_pipelines["ompl"]["panda_arm"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)"moveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["enforce_constrained_state_space"] = Truemoveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)"运行以下命令启动示例：
ros2 launch moveit2_tutorials ompl_constrained_planning.launch.pyPanda 机器人应该会出现，RViz Visual Tools 和 Trajectory Slider 面板位于左下角。你还应该在终端中看到以下提示：
Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to start with the box constraint example点击 Next 按钮开始第一个示例。
第一个示例展示了带有盒子约束的规划。RViz 中应该会出现一个红色球体和一个绿色球体，分别表示起始状态和目标状态。此外，还会出现一个灰色盒子，表示施加在连杆 panda_link8 上的位置约束。如果规划成功，你应该会看到规划轨迹的预览。你可以使用 Trajectory Slider 面板来检查轨迹。
终端中会出现以下消息：
Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to continue to the planar constraint example按下 Next 后，将进入下一个规划问题。此示例使用等式约束，规划一条将末端执行器约束在平面内的轨迹。
同样，如果规划成功，轨迹会在 RViz 中以动画形式显示。再次按下 Next 进入第三个规划问题，这次使用等式约束沿直线进行规划。
如果规划成功，你可以看到轨迹动画。最后，按下 Next 进入方向约束示例。
此示例可能需要更长的规划时间。如果规划失败，你可以回到本节开头重试。按下 Next 尝试混合约束。
按下 Next 移除所有标记并结束示例。
如何设置约束
Section titled “如何设置约束”本节介绍如何使用 Move Group 接口设置位置约束、等式约束和方向约束。我们将逐步讲解示例代码。为简洁起见，省略了部分代码行。
请务必在 ompl_planning.yaml 中添加 enforce_constrained_state_space 和 projection_evaluator！
首先，我们设置 MoveGroupInterface，方式与 Move Group 接口教程 中的示例类似。这里假定你已经设置好了节点。
moveit::planning_interface::MoveGroupInterface move_group_interface(node, "panda_arm");接下来，我们创建一个 lambda 辅助函数，用于在当前位姿的基础上按给定的相对偏移创建新的位姿。
auto current_pose = move_group_interface.getCurrentPose();
// Creates a pose at a given positional offset from the current poseauto get_relative_pose = [current_pose, &moveit_visual_tools](double x, double y, double z) { auto target_pose = current_pose; target_pose.pose.position.x += x; target_pose.pose.position.y += y; target_pose.pose.position.z += z; moveit_visual_tools.publishSphere(current_pose.pose, rviz_visual_tools::RED, 0.05); moveit_visual_tools.publishSphere(target_pose.pose, rviz_visual_tools::GREEN, 0.05); moveit_visual_tools.trigger(); return target_pose;};现在，我们准备设置一些约束，先从盒子约束开始。
首先使用 lambda 函数创建一个相对于当前位姿偏移的目标位姿。该位姿应位于我们创建的盒子内。在本示例中，我们创建一个尺寸为 (0.1, 0.4, 0.4) 的盒子，因此目标位姿应在约束区域内。
auto target_pose = get_relative_pose(0.0, 0.3, -0.3);接下来设置约束。盒子本质上是一个 PositionConstraint——完整消息定义请参见此处。我们在 header 中设置 frame_id，以及要约束的连杆的 link_name（本例中为末端执行器）。然后用 shape_msgs 创建一个盒子并设置其尺寸，接着将该盒子放入 box_constraint。
// Let's try the simple box constraints first!moveit_msgs::msg::PositionConstraint box_constraint;box_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();box_constraint.link_name = move_group_interface.getEndEffectorLink();shape_msgs::msg::SolidPrimitive box;box.type = shape_msgs::msg::SolidPrimitive::BOX;box.dimensions = { 0.1, 0.4, 0.4 };box_constraint.constraint_region.primitives.emplace_back(box);然后设置盒子约束的位姿。这里使用 geometry_msgs 设置盒子的位置和方向，并将该位姿添加到 box_constraint。
geometry_msgs::msg::Pose box_pose;box_pose.position.x = current_pose.pose.position.x;box_pose.position.y = 0.15;box_pose.position.z = 0.45;box_pose.orientation.w = 1.0;box_constraint.constraint_region.primitive_poses.emplace_back(box_pose);box_constraint.weight = 1.0;最后，创建一个通用的 Constraints 消息，并将 box_constraint 添加到 position_constraints 中。
moveit_msgs::msg::Constraints box_constraints;box_constraints.position_constraints.emplace_back(box_constraint);约束创建完成后，通过 Move Group 接口设置路径约束并进行规划。建议适当增加默认规划时间，因为带约束的规划可能更慢。
moveit::planning_interface::MoveGroupInterface::Plan plan;move_group_interface.setPathConstraints(box_constraints);move_group_interface.setPoseTarget(target_pose);move_group_interface.setPlanningTime(10.0);move_group_interface.plan(plan);我们可以使用等式约束将末端执行器约束在平面或直线上进行规划。首先介绍平面约束的情况。
我们需要创建一个位于该平面内的位姿目标。该平面倾斜了 45 度，因此在 y 和 z 方向移动相同的距离即可保持在平面上。请确保目标状态和起始状态都满足路径约束，否则规划一定会失败。
target_pose = get_relative_pose(0.0, 0.3, -0.3);我们创建一个垂直于 y 轴的平面并将其倾斜 45 度，做法是创建一个盒子并将其中一个维度设为 0.0005。这是一个关键数字，稍后会详细说明。
moveit_msgs::msg::PositionConstraint plane_constraint;plane_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();plane_constraint.link_name = move_group_interface.getEndEffectorLink();shape_msgs::msg::SolidPrimitive plane;plane.type = shape_msgs::msg::SolidPrimitive::BOX;plane.dimensions = { 1.0, 0.0005, 1.0 };plane_constraint.constraint_region.primitives.emplace_back(plane);
geometry_msgs::msg::Pose plane_pose;plane_pose.position.x = current_pose.pose.position.x;plane_pose.position.y = current_pose.pose.position.y;plane_pose.position.z = current_pose.pose.position.z;plane_pose.orientation.x = sin(M_PI_4 / 2);plane_pose.orientation.y = 0.0;plane_pose.orientation.z = 0.0;plane_pose.orientation.w = cos(M_PI_4 / 2);plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose);plane_constraint.weight = 1.0;使用等式约束求解时情况更为复杂。我们需要明确告诉规划器，对较小的尺寸使用等式约束。具体做法是将约束的名称设置为 "use_equality_constraints"。此外，盒子中任何低于 0.001 阈值的维度都会被视为等式约束。然而，如果尺寸设得过小，盒子会比 OMPL 用于评估约束的容差（默认为 1e-4）更薄。MoveIt 会使用更严格的容差（即盒子宽度）来检查约束，许多状态因此会被判定为无效。这就是神奇数字 0.0005 的由来——它介于 0.00001 和 0.001 之间。
moveit_msgs::msg::Constraints plane_constraints;plane_constraints.position_constraints.emplace_back(plane_constraint);plane_constraints.name = "use_equality_constraints";move_group_interface.setPathConstraints(plane_constraints);和之前一样，设置目标并规划。
move_group_interface.setPoseTarget(target_pose);move_group_interface.setPlanningTime(10.0);move_group_interface.plan(plan);在上一个约束的基础上，我们还可以将盒子在 x 方向的维度也缩小，从而将其变为一条直线。
moveit_msgs::msg::PositionConstraint line_constraint;line_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();line_constraint.link_name = move_group_interface.getEndEffectorLink();shape_msgs::msg::SolidPrimitive line;line.type = shape_msgs::msg::SolidPrimitive::BOX;line.dimensions = { 0.0005, 0.0005, 1.0 };line_constraint.constraint_region.primitives.emplace_back(line);我们可以对方向施加约束。由于不再受位置约束，我们将目标位姿设置到机器人的另一侧，以进行更大幅度的运动。
target_pose = get_relative_pose(-0.6, 0.1, 0);这次创建一个 OrientationConstraint 而非 PositionConstraint，但与之前类似，我们需要设置 header.frame_id 和 link_name。
moveit_msgs::msg::OrientationConstraint orientation_constraint;orientation_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();orientation_constraint.link_name = move_group_interface.getEndEffectorLink();然后使用 geometry_msgs::msg::Quaternion 来设置方向约束。在本例中，我们直接约束方向使其相对于 current_pose 保持不变。
orientation_constraint.orientation = current_pose.pose.orientation;orientation_constraint.absolute_x_axis_tolerance = 0.4;orientation_constraint.absolute_y_axis_tolerance = 0.4;orientation_constraint.absolute_z_axis_tolerance = 0.4;orientation_constraint.weight = 1.0;与位置约束类似，我们需要使用一个通用的 Constraints 消息，但这次将其添加到 orientation_constraints 中。
moveit_msgs::msg::Constraints orientation_constraints;orientation_constraints.orientation_constraints.emplace_back(orientation_constraint);和之前一样设置规划问题并规划。
move_group_interface.setPathConstraints(orientation_constraints);move_group_interface.setPoseTarget(target_pose);move_group_interface.setPlanningTime(10.0);move_group_interface.plan(plan);最后，我们可以同时设置位置约束和方向约束。这里我们复用与方向约束相同的目标位姿。
target_pose = get_relative_pose(-0.6, 0.1, 0);我们同样复用方向约束——但这一次，原来的盒子约束将无法使用，因为目标位姿位于原来盒子之外。让我们修改盒子的位姿和尺寸，使目标位姿可达。请注意，同时施加位置约束和方向约束会大幅缩小可达区域——目标位姿不仅需要位于盒子约束内（如前所述），还需要在满足方向约束的前提下可达，这使得可视化变得更加困难。
box.dimensions = { 1.0, 0.6, 1.0 };box_constraint.constraint_region.primitives[0] = box;
box_pose.position.x = 0;box_pose.position.y = -0.1;box_pose.position.z = current_pose.pose.position.z;box_constraint.constraint_region.primitive_poses[0] = box_pose;box_constraint.weight = 1.0;和之前一样，创建一个通用的约束消息，这次同时添加位置约束和方向约束。
moveit_msgs::msg::Constraints mixed_constraints;mixed_constraints.position_constraints.emplace_back(box_constraint);mixed_constraints.orientation_constraints.emplace_back(orientation_constraint);
move_group_interface.setPathConstraints(mixed_constraints);move_group_interface.setPoseTarget(target_pose);move_group_interface.setPlanningTime(10.0);move_group_interface.plan(plan);