使用 OMPL 约束规划

../../../_images/ompl_constrained_planning_header.png

本教程向您展示如何使用 MoveIt 中的 OMPL 的“约束规划功能 <http://ompl.kavrakilab.org/constrainedPlanning.html>” 。为了说明此规划器的功能，使用不同类型的路径约束解决了四个规划问题。如果您已阅读有关如何使用 Move Group 接口 的示例，将更容易理解。

何时使用此规划器

该接口目前仅支持对机器人的任何链接进行单个位置或方向约束。规划方法为 enforce_joint_model_state_space 选项提供了替代方案。它对于在笛卡尔空间中体积较小（或为零）的约束区域最有价值，因为拒绝采样并不总是有效。例如，此规划器可用于将机器人末端执行器约束在平面或沿线。

如何配置 OMPL 以使用约束规划

OMPL 从名为 ompl_planning.yaml 的文件中读取配置参数。本指南直接在启动文件中添加所需参数，但也可以在 yaml 文件中设置它们。本指南使用 Panda 机器人，该文件位于 here。如果您只有一个位置或一个方向约束，则默认情况下将使用 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)

本教程在 启动文件 中添加参数。启动文件使用 moveit_configs_utils 来简化启动文件。moveit_config 使用 moveit_resources Panda MoveIt 配置进行配置。我们使用以下几行将 OMPL 参数添加到 planning_pipelines：

moveit_config.planning_pipelines["ompl"]["panda_arm"]["enforce_constrained_state_space"] = True
moveit_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"] = True
moveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)"

运行示例

运行以下命令启动示例：:

ros2 launch moveit2_tutorials ompl_constrained_planning.launch.py

熊猫机器人应该会出现，左下角是 RViz Visual Tools 和 Trajectory Slider 面板。您还应该在终端中看到以下文本：:

等待继续：在 RvizVisualToolsGui 窗口中按“下一步”以开始框约束示例

要启动第一个示例，请单击“下一步”按钮。

../../../_images/RVizVisualTools.png

第一个例子展示了一个带有框约束的规划。RViz 中应该会出现一个红色和绿色的球体，分别显示起始状态和目标状态。此外，还应该出现一个灰色框，表示链接 panda_link8 上的位置约束。如果规划成功，您应该会看到规划的轨迹的预览。您可以使用轨迹滑块面板检查轨迹。

终端中出现以下消息：:

等待继续：在 RvizVisualToolsGui 窗口中按“下一步”继续平面约束示例

按下一步后，下一个规划问题得到解决。此示例使用等式约束来规划轨迹，其中末端执行器受平面约束。

同样，如果规划成功，轨迹将在 RViz 中以动画形式呈现。再次按 Next 继续第三个规划问题，再次使用等式约束沿直线进行规划。

如果规划成功，您可以看到轨迹动画。最后，按“下一步”转到方向约束示例。

此示例可能需要更长时间才能规划。如果规划失败，您可以从本节开头重新尝试。按下一步尝试混合约束。

按“下一步”删除所有标记并结束示例。

如何设置约束

本节将介绍如何使用 Move Group 接口来使用位置、相等性和方向约束。本节将介绍 示例代码。为简洁起见，省略了一些行。

请务必将 enforce_constrained_state_space 和 projection_evaluator 添加到您的 ompl_planning.yaml！

初始设置

首先，我们首先设置 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 pose
auto 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”——请参阅完整的消息定义 :here。我们在标题中设置“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”而不是“Position Constraint”，但与之前类似，我们设置了“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;

与位置约束类似，我们需要使用通用的“约束”消息，但这次我们将其添加到“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);