Ros2 Moveit2 -Planning Scene(规划场景)

规划场景

PlanningScene类提供了用于碰撞检测和检查约束的主要接口。 在本教程中， 我们将探索此类的C++接口。

入门

如果您还没有这样做，请确保您已经完成入门指南中的步骤。

完整代码

完整代码可以在 MoveIt GitHub 项目中看到。

设置

可以使用RobotModel或URDF和SRDF轻松设置和配置PlanningScene类。 但是， 这不是实例化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 对象并将它们传递给碰撞检查函数。请注意，机器人是否处于自碰撞状态的结果包含在结果中。自碰撞检查使用机器人的无填充版本，即它直接使用 URDF 中提供的碰撞网格，而不添加任何额外的填充。

collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test1 1: Current State Is" << (cillision_result.collision?"in":"not in") << "self colision");

改变状态

现在， 让我们改变机器人的当前状态。 规划场.景在内部维护当前状态。 我们可以获取对他的引用并改变他， 然后检查新的机器人配置的碰撞。 特别注意， 我们需要在发出新的碰撞检查请求之前清除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"));

现在，我们可以获取在熊猫臂的给定配置下可能发生的任何碰撞的接触信息。我们可以通过在碰撞请求中填写相应字段并指定要返回的最大接触数作为大数来请求接触信息。

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 函数来检查自碰撞和与环境（当前为空）的碰撞。这是您在规划器中最常使用的碰撞检查函数集。请注意，与环境的碰撞检查将使用机器人的填充版本。填充有助于使机器人远离环境中的障碍物。

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);//planning_scene.isStateConstrained 是 MoveIt 代码库中的一个函数，用于检查给定的机器人状态 (robot_state::RobotState) 是否满足特定的运动学约束集
RCLCPP_INFO_STREAM(LOGGER, "Test 8: Random state is " << (constrained ? "constrained" : "not constrained"));

有一种更有效的方法来检查约束（当您想要反复检查相同的约束时，例如在规划器内部）。我们首先构建一个 KinematicConstraintSet，它预处理 ROS 约束消息并将其设置为快速处理。

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 提供的所有规划器当前都将使用用户定义的回调执行碰撞检查、约束检查和可行性检查。

启动文件

整个启动文件位于GitHub上。本教程中的所有代码都可以从 moveit_tutorials 包中编译和运行。

运行代码

使用 Roslaunch 启动文件直接从 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 控制台记录器。