Moveit案例

Alphapeople

184人浏览 · 2026-10-09 16:10:48

Alphapeople · 2026-10-09 16:10:48 发布

示例
这是一个兜底页面，用于放置那些尚未针对新结构更新、或仍未从 ROS 1 移植过来的页面。

要将这些页面迁移到新结构中，请参阅 Contributing 下的相应页面。

MoveGroup - C++ 中的 ROS 封装
通过脚本使用 MoveIt 的最简单方式是使用 move_group_interface。该接口非常适合初学者，并提供了对 MoveIt 许多功能的统一访问。

Move Group C++ 接口
直接通过 C++ API 使用 MoveIt
使用 MoveIt 构建更复杂的应用程序通常需要开发者深入研究 MoveIt 的 C++ API。额外的好处是，直接使用 C++ API 会跳过许多 ROS Service/Action 层，从而显著提升性能。

机器人模型与机器人状态
规划场景
规划场景监视器
规划场景 ROS API
运动规划 API
运动规划流水线
创建 MoveIt 插件
可视化碰撞
时间参数化
使用近似约束流形进行规划
抓取与放置
MoveIt Grasps
MoveIt Deep Grasps
子坐标系
MoveItCpp 教程
使用 Bullet 进行碰撞检查
为差速驱动移动底座和机械臂进行规划
直接通过 Python API 使用 MoveIt
MoveIt Python API 绑定了 C++ API 的一个子集。Python API 可用于快速原型设计和实验，或者如果你已经在 Python 开发环境中工作，也可以使用它。

运动规划 Python API
Jupyter Notebook 原型设计
与新机器人集成
在尝试将新机器人与 MoveIt 2 集成之前，请检查你的机器人是否已经完成设置（参见运行 MoveIt 的机器人列表）。否则，请按照本节中的教程将你的机器人与 MoveIt 集成。

MoveIt Setup Assistant
URDF 和 SRDF
低级控制器
感知流水线教程
手眼标定
IKFast 运动学求解器
配置
运动学配置
自定义约束采样器
OMPL 规划器
TrajOpt 规划器
规划适配器教程
其他
使用 MoveIt 的双臂
混合规划
实时伺服
集成/单元测试

RobotModel 与 RobotState 类
RobotModel 和 RobotState 类是核心类，它们让你能够访问机器人的运动学信息。

RobotModel 类包含所有连杆和关节之间的关系，包括从 URDF 加载的关节限位属性。RobotModel 还会将机器人的连杆和关节划分为 SRDF 中定义的规划组。关于 URDF 和 SRDF 的单独教程可在此处找到：URDF 和 SRDF 教程

RobotState 包含机器人在某一时间点的信息，存储关节位置向量，以及可选的关节速度和加速度向量。这些信息可用于获取依赖于机器人当前状态的运动学信息，例如末端执行器的雅可比矩阵。

RobotState 还包含辅助函数，用于根据末端执行器位置（笛卡尔位姿）设置机械臂位置，以及计算笛卡尔轨迹。

在本示例中，我们将以 Panda 机器人为例，逐步讲解如何使用这些类。

快速开始
如果你还没有完成相关步骤，请确保已完成“快速开始”中的步骤。

运行代码
本教程中的所有代码都可以在你 MoveIt 安装环境中自带的 moveit2_tutorials 包中编译并运行。

使用 roslaunch 启动 launch 文件，直接从 moveit2_tutorials 运行代码：

ros2 launch moveit2_tutorials robot_model_and_robot_state_tutorial.launch.py
预期输出
预期输出将采用以下形式。由于我们使用的是随机关节值，因此具体数值不会一致：

... [robot_model_and_state_tutorial]: Model frame: world
... [robot_model_and_state_tutorial]: Joint panda_joint1: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint2: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint3: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint4: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint5: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint6: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint7: 0.000000
... [robot_model_and_state_tutorial]: Current state is not valid
... [robot_model_and_state_tutorial]: Current state is valid
... [robot_model_and_state_tutorial]: Translation:
-0.368232
0.645742
0.752193

... [robot_model_and_state_tutorial]: Rotation:
0.362374 -0.925408 -0.11093
0.911735 0.327259 0.248275
-0.193453 -0.191108 0.962317

... [robot_model_and_state_tutorial]: Joint panda_joint1: 2.263889
... [robot_model_and_state_tutorial]: Joint panda_joint2: 1.004608
... [robot_model_and_state_tutorial]: Joint panda_joint3: -1.125652
... [robot_model_and_state_tutorial]: Joint panda_joint4: -0.278822
... [robot_model_and_state_tutorial]: Joint panda_joint5: -2.150242
... [robot_model_and_state_tutorial]: Joint panda_joint6: 2.274891
... [robot_model_and_state_tutorial]: Joint panda_joint7: -0.774846
... [robot_model_and_state_tutorial]: Jacobian:
-0.645742 -0.26783 -0.0742358 -0.315413 0.0224927 -0.031807 -2.77556e-17
-0.368232 0.322474 0.0285092 -0.364197 0.00993438 0.072356 2.77556e-17
0 -0.732023 -0.109128 0.218716 2.9777e-05 -0.11378 -1.04083e-17
0 -0.769274 -0.539217 0.640569 -0.36792 -0.91475 -0.11093
0 -0.638919 0.64923 -0.0973283 0.831769 -0.40402 0.248275
1 4.89664e-12 0.536419 0.761708 0.415688 -0.00121099 0.962317
注意：如果你的输出中 ROS 控制台格式不同，不必担心。

完整代码
完整代码可在 MoveIt GitHub 项目中此处查看。

开始
开始使用 RobotModel 类的设置非常简单。通常你会发现，大多数更高层组件都会返回一个指向 RobotModel 的共享指针。只要可能，你应始终使用该共享指针。在本示例中，我们将从这样一个共享指针开始，并只讨论基本 API。你可以查看这些类的实际代码 API，以了解更多如何使用这些类提供的更多功能的信息。

我们首先实例化一个 RobotModelLoader 对象，它会在 ROS 参数服务器上查找机器人描述，并为我们构造一个可供使用的 RobotModel。

robot_model_loader::RobotModelLoader robot_model_loader(node);
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
RCLCPP_INFO(LOGGER, "Model frame: %s", kinematic_model->getModelFrame().c_str());
使用 RobotModel，我们可以构造一个 RobotState，用于维护机器人的配置。我们会将该状态中的所有关节设置为其默认值。然后我们可以获取一个 JointModelGroup，它表示某个特定组的机器人模型，例如 Panda 机器人的“panda_arm”。

moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(kinematic_model));
robot_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");

const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
获取关节值
我们可以检索状态中为 Panda 机械臂存储的当前关节值集合。

std::vector<double> joint_values;
robot_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
关节限位
setJointGroupPositions() 本身不会强制执行关节限位，但调用 enforceBounds() 会执行该操作。

/* Set one joint in the Panda arm outside its joint limit */
joint_values[0] = 5.57;
robot_state->setJointGroupPositions(joint_model_group, joint_values);

/* Check whether any joint is outside its joint limits */
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (robot_state->satisfiesBounds() ? "valid" : "not valid"));

/* Enforce the joint limits for this state and check again*/
robot_state->enforceBounds();
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (robot_state->satisfiesBounds() ? "valid" : "not valid"));
正运动学
现在，我们可以针对一组随机关节值计算正运动学。请注意，我们想要求解的是“panda_link8”的位姿，它是机器人“panda_arm”组中最末端的连杆。

robot_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& end_effector_state = robot_state->getGlobalLinkTransform("panda_link8");

/* Print end-effector pose. Remember that this is in the model frame */
RCLCPP_INFO_STREAM(LOGGER, "Translation: \n" << end_effector_state.translation() << "\n");
RCLCPP_INFO_STREAM(LOGGER, "Rotation: \n" << end_effector_state.rotation() << "\n");
逆运动学
现在，我们可以为 Panda 机器人求解逆运动学（IK）。要解 IK，我们需要以下内容：

末端执行器的期望位姿（默认情况下，这是“panda_arm”链中的最后一个连杆）：我们在上一步中计算得到的 end_effector_state。

超时时间：0.1 秒

double timeout = 0.1;
bool found_ik = robot_state->setFromIK(joint_model_group, end_effector_state, timeout);
现在，我们可以打印出 IK 解（如果找到）：

if (found_ik)
{
robot_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
}
else
{
RCLCPP_INFO(LOGGER, "Did not find IK solution");
}
获取雅可比矩阵
我们还可以从 RobotState 获取雅可比矩阵。

Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
Eigen::MatrixXd jacobian;
robot_state->getJacobian(joint_model_group, robot_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position, jacobian);
RCLCPP_INFO_STREAM(LOGGER, "Jacobian: \n" << jacobian << "\n");
Launch 文件
要运行代码，你需要一个 launch 文件，它要完成两件事：
将 Panda URDF 和 SRDF 加载到参数服务器上，以及

将 MoveIt Setup Assistant 生成的 kinematics_solver 配置放到 ROS 参数服务器上，位于实例化本教程中这些类的节点命名空间下。

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()

tutorial_node = Node(
package="moveit2_tutorials",
executable="robot_model_and_robot_state_tutorial",
output="screen",
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
],
)

return LaunchDescription([tutorial_node])

规划场景
PlanningScene 类提供了你将用于碰撞检查和约束检查的主要接口。在本教程中，我们将探索该类的 C++ 接口。

快速开始
如果你还没有完成相关步骤，请确保已完成“快速开始”中的步骤。

完整代码
完整代码可在 MoveIt GitHub 项目中此处查看。

设置
PlanningScene 类可以很容易地使用 RobotModel 或 URDF 和 SRDF 进行设置和配置。然而，这并不是实例化 PlanningScene 的推荐方式。PlanningSceneMonitor 是创建和维护当前规划场景的推荐方法（将在下一个教程中详细讨论），它使用来自机器人关节和机器人传感器的数据。在本教程中，我们将直接实例化 PlanningScene 类，但这种实例化方法仅用于说明目的。

robot_model_loader::RobotModelLoader robot_model_loader(planning_scene_tutorial_node, "robot_description");
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);
碰撞检查
自碰撞检查
我们要做的第一件事是检查当前状态下的机器人是否处于自碰撞状态，即机器人当前的构型是否会导致机器人的各个部分相互碰撞。为此，我们将构造一个 CollisionRequest 对象和一个 CollisionResult 对象，并将它们传入碰撞检查函数。注意，机器人是否处于自碰撞状态的结果包含在 result 中。自碰撞检查使用未加 padding 的机器人版本，即它直接使用 URDF 中提供的碰撞网格，不额外添加 padding。

collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in")
<< " self collision");
更改状态
现在，让我们更改机器人的当前状态。规划场景在内部维护当前状态。我们可以获取它的引用并更改它，然后针对新的机器人构型检查碰撞。特别要注意的是，在发起新的碰撞检查请求之前，我们需要清除 collision_result。

moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in")
<< " self collision");
针对某个组进行检查
现在，我们将仅针对 Panda 的手部进行碰撞检查，即我们将检查手部与机器人身体其他部分之间是否存在任何碰撞。我们可以通过将组名“hand”添加到碰撞请求中来专门请求这一点。

collision_request.group_name = "hand";
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in")
<< " self collision");
获取接触信息
首先，手动将 Panda 机械臂设置到我们知道确实会发生内部（自）碰撞的位置。注意，此状态实际上超出了 Panda 的关节限位，我们也可以直接检查这一点。

std::vector<double> joint_values = { 0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0 };
const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup("panda_arm");
current_state.setJointGroupPositions(joint_model_group, joint_values);
RCLCPP_INFO_STREAM(LOGGER, "Test 4: Current state is "
<< (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));
现在，我们可以获取在 Panda 机械臂给定构型下可能发生的任何碰撞的接触信息。我们可以通过在碰撞请求中填写相应字段，并将要返回的最大接触数指定为一个较大的数来请求接触信息。

collision_request.contacts = true;
collision_request.max_contacts = 1000;
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in")
<< " self collision");
collision_detection::CollisionResult::ContactMap::const_iterator it;
for (it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it)
{
RCLCPP_INFO(LOGGER, "Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str());
}
修改允许碰撞矩阵
允许碰撞矩阵（AllowedCollisionMatrix，ACM）提供了一种机制，用于告诉碰撞世界忽略某些对象之间的碰撞：既包括机器人的各个部分，也包括世界中的对象。我们可以告诉碰撞检查器忽略上述连杆之间的所有碰撞，即即使这些连杆实际上处于碰撞状态，碰撞检查器也会忽略这些碰撞，并针对该特定机器人状态返回“未碰撞”。

另请注意，在本示例中，我们如何复制允许碰撞矩阵和当前状态，并将它们传入碰撞检查函数。

collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();
moveit::core::RobotState copied_state = planning_scene.getCurrentState();

collision_detection::CollisionResult::ContactMap::const_iterator it2;
for (it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2)
{
acm.setEntry(it2->first.first, it2->first.second, true);
}
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);
RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in")
<< " self collision");
完整碰撞检查
虽然我们一直在检查自碰撞，但我们可以改用 checkCollision 函数，它将同时检查自碰撞和与环境（当前为空）的碰撞。这是你在规划器中最常使用的一组碰撞检查函数。注意，与环境的碰撞检查将使用加了 padding 的机器人版本。Padding 有助于使机器人远离环境中的障碍物。

collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
RCLCPP_INFO_STREAM(LOGGER, "Test 7: Current state is " << (collision_result.collision ? "in" : "not in")
<< " self collision");
约束检查
PlanningScene 类还包含用于检查约束的易用函数调用。约束可以有两种类型：(a) 从 KinematicConstraint 集合中选择的约束：即 JointConstraint、PositionConstraint、OrientationConstraint 和 VisibilityConstraint，以及 (b) 通过回调指定的用户自定义约束。我们将首先看一个使用简单 KinematicConstraint 的示例。

检查运动学约束
我们将首先为 Panda 机器人的 panda_arm 组的末端执行器定义一个简单的位置和姿态约束。注意这里使用了用于填充约束的便捷函数（这些函数可在 moveit_core 中 kinematic_constraints 目录下的 utils.h 文件中找到）。

std::string end_effector_name = joint_model_group->getLinkModelNames().back();

geometry_msgs::msg::PoseStamped desired_pose;
desired_pose.pose.orientation.w = 1.0;
desired_pose.pose.position.x = 0.3;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 0.5;
desired_pose.header.frame_id = "panda_link0";
moveit_msgs::msg::Constraints goal_constraint =
kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);
现在，我们可以使用 PlanningScene 类中的 isStateConstrained 函数来检查某个状态是否满足该约束。

copied_state.setToRandomPositions();
copied_state.update();
bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);
RCLCPP_INFO_STREAM(LOGGER, "Test 8: Random state is " << (constrained ? "constrained" : "not constrained"));
有一种更高效的约束检查方式（当你想要反复检查同一个约束时，例如在规划器内部）。我们首先构造一个 KinematicConstraintSet，它会预处理 ROS Constraints 消息，并为其快速处理做好准备。

kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model);
kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms());
bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set);
RCLCPP_INFO_STREAM(LOGGER, "Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained"));
还有一种使用 KinematicConstraintSet 类的直接方法。

kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =
kinematic_constraint_set.decide(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "Test 10: Random state is "
<< (constraint_eval_result.satisfied ? "constrained" : "not constrained"));
用户自定义约束
用户自定义约束也可以指定给 PlanningScene 类。这是通过使用 setStateFeasibilityPredicate 函数指定一个回调来完成的。下面是一个简单的用户自定义回调示例，它检查 Panda 机器人的“panda_joint1”是处于正角度还是负角度：

bool stateFeasibilityTestExample(const moveit::core::RobotState& robot_state, bool /*verbose*/)
{
const double* joint_values = robot_state.getJointPositions("panda_joint1");
return (joint_values[0] > 0.0);
}
现在，每当调用 isStateFeasible 时，都会调用这个用户自定义回调。

planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible"));
每当调用 isStateValid 时，都会执行三项检查：(a) 碰撞检查，(b) 约束检查，以及 (c) 使用用户自定义回调进行可行性检查。

bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "panda_arm");
RCLCPP_INFO_STREAM(LOGGER, "Test 12: Random state is " << (state_valid ? "valid" : "not valid"));
注意，目前通过 MoveIt 和 OMPL 提供的所有规划器都将执行碰撞检查、约束检查以及使用用户自定义回调进行可行性检查。

Launch 文件
完整的 launch 文件在此处的 GitHub 上。本教程中的所有代码都可以从 moveit_tutorials 包中编译并运行。

运行代码
使用 roslaunch 启动 launch 文件，直接从 moveit_tutorials 运行代码：

ros2 launch moveit2_tutorials planning_scene_tutorial.launch.py
预期输出
输出应该看起来像这样，不过由于我们使用的是随机关节值，因此某些内容可能会有所不同。

moveit2_tutorials: Test 1: Current state is in self collision
moveit2_tutorials: Test 2: Current state is not in self collision
moveit2_tutorials: Test 3: Current state is not in self collision
moveit2_tutorials: Test 4: Current state is valid
moveit2_tutorials: Test 5: Current state is in self collision
moveit2_tutorials: Contact between: panda_leftfinger and panda_link1
moveit2_tutorials: Contact between: panda_link1 and panda_rightfinger
moveit2_tutorials: Test 6: Current state is not in self collision
moveit2_tutorials: Test 7: Current state is not in self collision
moveit2_tutorials: Test 8: Random state is not constrained
moveit2_tutorials: Test 9: Random state is not constrained
moveit2_tutorials: Test 10: Random state is not constrained
moveit2_tutorials: Test 11: Random state is feasible
moveit2_tutorials: Test 12: Random state is not valid
注意：如果你的输出中 ROS 控制台格式不同，不必担心。你可以按照本教程自定义你的 ROS 控制台日志记录器。

规划场景监视器
PlanningSceneMonitor 是用于维护最新规划场景的推荐接口。RobotState、CurrentStateMonitor、PlanningScene、PlanningSceneMonitor 和 PlanningSceneInterface 之间的关系一开始可能会让人非常困惑。本教程旨在阐明这些关键概念。

RobotState
RobotState 是机器人的一个快照。它包含 RobotModel 和一组关节值。

CurrentStateMonitor
CurrentStateMonitor（CSM）可以看作是 RobotState 的 ROS 封装。它订阅一个提供的主题以获取 JointState 消息，这些消息为单自由度执行器（如旋转关节或棱柱关节）提供最新的传感器值，并用这些关节值更新其内部的 RobotState。除了单自由度关节外，机器人还可能具有多自由度关节，例如浮动关节和平面关节。为了维护与多自由度关节相连的连杆和其他坐标系的最新变换信息，CSM 存储了一个 TF2 Buffer，该 Buffer 使用 TF2 TransformListener 在其内部数据中设置这些变换。

PlanningScene
PlanningScene 是世界的一个快照，它既包含 RobotState，也包含任意数量的碰撞对象。规划场景可用于碰撞检查以及获取有关环境的信息。

PlanningSceneMonitor
PlanningSceneMonitor 用一个 PlanningScene 封装了 ROS 接口，以保持 PlanningScene 处于最新状态。要访问 PlanningSceneMonitor 底层的 PlanningScene，请使用提供的 LockedPlanningSceneRW 和 LockedPlanningSceneRO 类。

PlanningSceneMonitor 拥有以下对象，它们各自具有自己的 ROS 接口，用于保持规划场景各子组件处于最新状态：

一个 CurrentStateMonitor，用于通过 robot_state_subscriber_ 和 tf_buffer_ 跟踪 RobotState 的更新，以及一个规划场景订阅者，用于监听来自其他发布者的规划场景差异。

一个 OccupancyMapMonitor，用于通过 ROS 主题和服务跟踪 OccupancyMap 的更新。

PlanningSceneMonitor 具有以下订阅者：

collision_object_subscriber_ - 监听一个提供的主题以获取 CollisionObject 消息，这些消息可能会在规划场景中添加、移除或修改碰撞对象，并将它们传递到其监视的规划场景中

planning_scene_world_subscriber_ - 监听一个提供的主题以获取 PlanningSceneWorld 消息，这些消息可能包含碰撞对象信息和/或 octomap 信息。这对于保持规划场景监视器同步非常有用

attached_collision_object_subscriber_ - 监听一个提供的主题以获取 AttachedCollisionObject 消息，这些消息决定对象与机器人状态中连杆的附着/分离。

PlanningSceneMonitor 具有以下服务：

get_scene_service_ - 这是一个可选服务，用于获取完整的规划场景状态。

PlanningSceneMonitor 通过以下方式初始化：

startSceneMonitor - 启动 planning_scene_subscriber_，

startWorldGeometryMonitor - 启动 collision_object_subscriber_、planning_scene_world_subscriber_ 和 OccupancyMapMonitor，

startStateMonitor - 启动 CurrentStateMonitor 和 attached_collision_object_subscriber_，

startPublishingPlanningScene - 启动另一个线程，在提供的主题上发布整个规划场景，供其他 PlanningSceneMonitor 订阅，以及

providePlanningSceneService - 启动 get_scene_service_。

PlanningSceneInterface
PlanningSceneInterface 是一个有用的类，用于通过 C++ API 向 MoveGroup 的 PlanningSceneMonitor 发布更新，而无需创建自己的订阅者和服务客户端。如果没有 MoveGroup 或 MoveItCpp，它可能无法工作。

在 MoveIt 中，运动规划器是通过插件基础设施加载的。这允许 MoveIt 在运行时加载运动规划器。在本示例中，我们将逐步讲解实现这一功能所需的 C++ 代码。

快速开始
如果你还没有完成相关步骤，请确保已完成“快速开始”中的步骤。

运行演示
打开两个终端。在第一个终端中启动 RViz 并等待所有内容加载完成：

ros2 launch moveit2_tutorials move_group.launch.py
在第二个终端中，运行 launch 文件：

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py
注意：本教程使用 RvizVisualToolsGui 面板逐步完成演示。要将此面板添加到 RViz，请按照“可视化教程”中的说明操作。

片刻之后，RViz 窗口应该会出现，并且看起来类似于本页顶部的窗口。要逐步完成每个演示步骤，你可以按屏幕底部 RvizVisualToolsGui 面板中的 Next 按钮，或者在屏幕顶部 Tools 面板中选择 Key Tool，然后在 RViz 获得焦点时按键盘上的 N。

完整代码

完整代码可以在 moveit_tutorials GitHub 项目中查看。

开始

设置并开始使用规划器非常简单。规划器在 MoveIt 中作为插件设置，你可以使用 ROS pluginlib 接口来加载任何你想使用的规划器。在加载规划器之前，我们需要两个对象：一个 RobotModel 和一个 PlanningScene。我们将从实例化一个 RobotModelLoader 对象开始，它会在 ROS 参数服务器上查找机器人描述，并为我们构造一个可供使用的 RobotModel。

cpp

const std::string PLANNING_GROUP = "panda_arm";
robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_api_tutorial_node, "robot_description");
const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();
/* 创建一个 RobotState 和 JointModelGroup，用于跟踪当前机器人位姿和规划组 */
moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model));
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

使用 RobotModel，我们可以构造一个 PlanningScene，它维护着世界（包括机器人）的状态。

cpp

planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

配置一个有效的机器人状态

cpp

planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

我们现在将构造一个加载器，按名称加载规划器。注意，我们在这里使用的是 ROS pluginlib 库。

cpp

std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::vector<std::string> planner_plugin_names;

我们将从 ROS 参数服务器获取想要加载的规划插件的名称，然后加载规划器，并确保捕获所有异常。

cpp

if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names))
  RCLCPP_FATAL(LOGGER, "Could not find planner plugin names");
try
{
  planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
      "moveit_core", "planning_interface::PlannerManager"));
}
catch (pluginlib::PluginlibException& ex)
{
  RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
}

if (planner_plugin_names.empty())
{
  RCLCPP_ERROR(LOGGER,
               "No planner plugins defined. Please make sure that the planning_plugins parameter is not empty.");
  return -1;
}

const auto& planner_name = planner_plugin_names.at(0);
try
{
  planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_name));
  if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node,
                                    motion_planning_api_tutorial_node->get_namespace()))
    RCLCPP_FATAL(LOGGER, "Could not initialize planner instance");
  RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance->getDescription().c_str());
}
catch (pluginlib::PluginlibException& ex)
{
  const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
  std::stringstream ss;
  for (const auto& cls : classes)
    ss << cls << " ";
  RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_name.c_str(),
               ex.what(), ss.str().c_str());
}

moveit::planning_interface::MoveGroupInterface move_group(motion_planning_api_tutorial_node, PLANNING_GROUP);

可视化

MoveItVisualTools 软件包提供了许多在 RViz 中可视化物体、机器人和轨迹的功能，以及调试工具，例如对脚本进行逐步检视。

cpp

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(motion_planning_api_tutorial_node, "panda_link0",
                                                    "move_group_tutorial", move_group.getRobotModel());
visual_tools.enableBatchPublishing();
visual_tools.deleteAllMarkers();  // 清除所有旧标记
visual_tools.trigger();

/* 远程控制是一个检视工具，允许用户通过 RViz 中的按钮和键盘快捷键逐步执行一个高层脚本 */
visual_tools.loadRemoteControl();

/* RViz 提供多种类型的标记，在本演示中我们将使用文本、圆柱体和球体 */
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);

/* 批量发布用于减少为大型可视化发送到 RViz 的消息数量 */
visual_tools.trigger();

/* 我们还可以使用 visual_tools 等待用户输入 */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

位姿目标

我们现在将为 Panda 的机械臂创建一个运动规划请求，指定末端执行器的期望位姿作为输入。

cpp

visual_tools.trigger();
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "panda_link0";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.4;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;

位置公差指定为 0.01 米，姿态公差为 0.01 弧度

cpp

std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);

我们将使用 kinematic_constraints 软件包中可用的辅助函数，把这个请求创建为一个约束。

cpp

moveit_msgs::msg::Constraints pose_goal =
    kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);

req.group_name = PLANNING_GROUP;
req.goal_constraints.push_back(pose_goal);

定义工作空间边界

cpp

req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
    req.workspace_parameters.min_corner.z = -5.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
    req.workspace_parameters.max_corner.z = 5.0;

我们现在构造一个规划上下文，它封装了场景、请求和响应。我们使用这个规划上下文来调用规划器。

cpp

planning_interface::PlanningContextPtr context =
    planner_instance->getPlanningContext(planning_scene, req, res.error_code);

if (!context)
{
  RCLCPP_ERROR(LOGGER, "Failed to create planning context");
  return -1;
}
context->solve(res);
if (res.error_code.val != res.error_code.SUCCESS)
{
  RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
  return -1;
}

可视化结果

cpp

std::shared_ptr<rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisher =
    motion_planning_api_tutorial_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",
                                                                                             1);
moveit_msgs::msg::DisplayTrajectory display_trajectory;

/* 可视化轨迹 */
moveit_msgs::msg::MotionPlanResponse response;
res.getMessage(response);

display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* 将规划场景中的状态设置为上一次规划的最终状态 */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

显示目标状态

cpp

visual_tools.publishAxisLabeled(pose.pose, "goal_1");
visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* 我们还可以使用 visual_tools 等待用户输入 */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

关节空间目标

现在，设置一个关节空间目标

cpp

moveit::core::RobotState goal_state(robot_model);
std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::msg::Constraints joint_goal =
    kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

调用规划器并可视化轨迹

cpp

/* 重新构造规划上下文 */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code);
/* 调用规划器 */
context->solve(res);
/* 检查规划是否成功 */
if (res.error_code.val != res.error_code.SUCCESS)
{
  RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
  return -1;
}
/* 可视化轨迹 */
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);

/* 现在你应该会看到两条依次规划的轨迹 */
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* 我们将添加更多目标。但首先，将规划场景中的状态
   设置为上一次规划的最终状态 */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

显示目标状态

cpp

visual_tools.publishAxisLabeled(pose.pose, "goal_2");
visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* 等待用户输入 */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

/* 现在，我们回到第一个目标，为姿态约束规划做准备 */
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code);
context->solve(res);
res.getMessage(response);

display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* 将规划场景中的状态设置为上一次规划的最终状态 */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

显示目标状态

cpp

visual_tools.trigger();

/* 等待用户输入 */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

添加路径约束

让我们再次添加一个新的位姿目标。这次我们还会给运动添加一个路径约束。

cpp

/* 让我们创建一个新的位姿目标 */

pose.pose.position.x = 0.32;
pose.pose.position.y = -0.25;
pose.pose.position.z = 0.65;
pose.pose.orientation.w = 1.0;
moveit_msgs::msg::Constraints pose_goal_2 =
    kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);

/* 现在，让我们尝试移动到这个新的位姿目标 */
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);

/* 但是，让我们对运动施加一个路径约束。
   这里，我们要求末端执行器保持水平 */
geometry_msgs::msg::QuaternionStamped quaternion;
quaternion.header.frame_id = "panda_link0";
req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);

施加路径约束要求规划器在末端执行器可能的位置空间（机器人的工作空间）中进行推理，因此，我们还需要为允许的规划体积指定一个边界；注意：WorkspaceBounds 请求适配器会自动填充一个默认边界（它是 OMPL 流水线的一部分，但本示例中没有使用它）。我们使用一个肯定包含机械臂可达空间的边界。这没问题，因为在为机械臂规划时，采样并不是在这个体积中进行的；这些边界只用于确定采样的构型是否有效。

cpp

req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
    req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
    req.workspace_parameters.max_corner.z = 2.0;

调用规划器并可视化目前为止创建的所有规划。

cpp

context = planner_instance->getPlanningContext(planning_scene, req, res.error_code);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* 将规划场景中的状态设置为上一次规划的最终状态 */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

显示目标状态

cpp

visual_tools.publishAxisLabeled(pose.pose, "goal_3");
visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();