【MoveIt 2】示例：直接通过 C++ API 使用 MoveIt -规划场景

最新推荐文章于 2024-08-03 09:19:20 发布

十年一梦实验室

最新推荐文章于 2024-08-03 09:19:20 发布

阅读量275

点赞数 2

文章标签： c++ 开发语言

本文链接：https://blog.csdn.net/cxyhjl/article/details/140928072

版权

规划场景

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

入门

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

整个代码

整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/planning_scene

#include <rclcpp/rclcpp.hpp> // 包含 ROS 2 的核心库


// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h> // 包含 MoveIt 的机器人模型加载器
#include <moveit/planning_scene/planning_scene.h> // 包含 MoveIt 的规划场景


#include <moveit/kinematic_constraints/utils.h> // 包含 MoveIt 的运动学约束工具


// BEGIN_SUB_TUTORIAL stateFeasibilityTestExample
//
// 用户定义的约束也可以指定给 PlanningScene 类。
// 这是通过使用 setStateFeasibilityPredicate 函数指定回调来完成的。
// 下面是一个简单的用户定义回调示例，它检查 Panda 机器人的 "panda_joint1" 关节是否处于正角度或负角度：
bool stateFeasibilityTestExample(const moveit::core::RobotState& robot_state, bool /*verbose*/)
{
  const double* joint_values = robot_state.getJointPositions("panda_joint1"); // 获取 "panda_joint1" 关节的位置
  return (joint_values[0] > 0.0); // 检查关节角度是否大于 0
}
// END_SUB_TUTORIAL


static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial"); // 定义日志记录器


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv); // 初始化 ROS 2
  rclcpp::NodeOptions node_options; // 创建节点选项
  node_options.automatically_declare_parameters_from_overrides(true); // 自动声明参数
  auto planning_scene_tutorial_node = rclcpp::Node::make_shared("planning_scene_tutorial", node_options); // 创建共享指针节点


  rclcpp::executors::SingleThreadedExecutor executor; // 创建单线程执行器
  executor.add_node(planning_scene_tutorial_node); // 将节点添加到执行器
  std::thread([&executor]() { executor.spin(); }).detach();// 创建并分离线程以运行执行器


  // BEGIN_TUTORIAL
  //
  // 设置
  // ^^^^
  //
  // 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 对象，并将它们传递给碰撞检测函数。
  // 请注意，机器人是否处于自碰撞状态的结果包含在结果中。自碰撞检测使用机器人的未填充版本，即直接使用 URDF 中提供的碰撞网格，没有添加额外的填充。


  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"); // 输出自碰撞检测结果


  // 改变状态
  // ~~~~~~~~
  //
  // 现在，让我们改变机器人的当前状态。规划场景内部维护当前状态。
  // 我们可以获取它的引用并进行更改，然后检查新机器人配置的碰撞情况。
  // 特别注意，在进行新的碰撞检测请求之前，我们需要清除碰撞结果。


  moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst(); // 获取当前状态的非 const 引用
  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"; // 设置碰撞请求的组名为 "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()); // 输出接触信息
  }


  // 修改允许的碰撞矩阵
  // ~~~~~~~~~~~~~~~~~~
  //
  // 允许的碰撞矩阵（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") << " 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; // 设置位置 x 坐标
  desired_pose.pose.position.y = -0.185; // 设置位置 y 坐标
  desired_pose.pose.position.z = 0.5; // 设置位置 z 坐标
  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 约束消息并将其设置为快速处理。


  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")); // 输出约束评估结果


  // 用户定义的约束
  // ~~~~~~~~~~~~~~
  //
  // CALL_SUB_TUTORIAL stateFeasibilityTestExample


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


  rclcpp::shutdown(); // 关闭 ROS 2
  return 0; // 返回 0
}

设置

PlanningScene 类可以使用 RobotModel 或 URDF 和 SRDF 轻松设置和配置。然而，这不是实例化 PlanningScene 的推荐方法。PlanningSceneMonitor https://github.com/moveit/moveit2/blob/main/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h 是创建和维护当前规划场景的推荐方法（在下一个教程中详细讨论），使用来自机器人关节和传感器的数据。在本教程中，我们将直接实例化一个 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, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // 输出自碰撞检测结果

更改状态

现在，让我们改变机器人的当前状态。规划场景在内部维护当前状态。我们可以获取对其的引用并进行更改，然后检查新机器人配置的碰撞情况。特别注意，在进行新的碰撞检查请求之前，我们需要清除 collision_result。

moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst(); // 获取当前状态的非 const 引用
  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"); // 输出自碰撞检测结果

检查组

现在，我们将只对熊猫的手进行碰撞检查，即我们将检查手与机器人身体的其他部分之间是否有碰撞。我们可以通过在碰撞请求中添加组名“hand”来专门要求这一点。

collision_request.group_name = "hand"; // 设置碰撞请求的组名为 "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) https://github.com/moveit/moveit2/blob/main/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h 提供了一种机制，告诉碰撞世界忽略某些对象之间的碰撞：机器人和世界中的对象的各个部分。我们可以告诉碰撞检查器忽略上述连杆之间的所有碰撞，即使这些连杆实际上发生了碰撞，碰撞检查器也会忽略这些碰撞，并返回机器人此特定状态下未发生碰撞。

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

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")
                                                         << " collision");
  // 输出当前状态是否处于碰撞

约束检查

PlanningScene 类还包括用于检查约束的易于使用的函数调用。约束可以是两种类型：(a) 从 KinematicConstraint 集 https://github.com/moveit/moveit2/blob/main/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h 中选择的约束：即 JointConstraint、PositionConstraint、OrientationConstraint 和 VisibilityConstraint，以及 (b) 通过回调指定的用户定义约束。我们首先来看一个简单的 KinematicConstraint 示例。

检查运动学约束

我们将首先在 Panda 机器人 panda_arm 组的末端执行器上定义一个简单的位置和方向约束。请注意使用方便的函数来填充约束（这些函数可以在 moveit_core 的 kinematic_constraints 目录中的 utils.h 文件中 https://github.com/moveit/moveit2/blob/main/moveit_core/kinematic_constraints/include/moveit/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 约束消息并设置为快速处理。

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");
  // 获取“panda_joint1”关节的位置
  return (joint_values[0] > 0.0);
  // 如果关节角度大于0，则返回true
}

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

启动文件

整个启动文件 https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/planning_scene/launch/planning_scene_tutorial.launch.py 在 GitHub 上。本教程中的所有代码都可以从 moveit_tutorials 包中编译和运行。

from launch import LaunchDescription
# 从 launch 模块导入 LaunchDescription 类
from launch_ros.actions import Node
# 从 launch_ros.actions 模块导入 Node 类
from moveit_configs_utils import MoveItConfigsBuilder
# 从 moveit_configs_utils 模块导入 MoveItConfigsBuilder 类


def generate_launch_description():
    # 定义一个名为 generate_launch_description 的函数
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()
    # 使用 MoveItConfigsBuilder 创建一个 MoveIt 配置对象，参数为 "moveit_resources_panda"


    # Planning Scene Tutorial executable
    # 定义 Planning Scene 教程的可执行节点
    planning_scene_tutorial = Node(
        name="planning_scene_tutorial",
        # 节点名称为 "planning_scene_tutorial"
        package="moveit2_tutorials",
        # 节点所属包为 "moveit2_tutorials"
        executable="planning_scene_tutorial",
        # 可执行文件为 "planning_scene_tutorial"
        output="screen",
        # 输出方式为屏幕
        parameters=[
            moveit_config.robot_description,
            # 使用 moveit_config 的 robot_description 参数
            moveit_config.robot_description_semantic,
            # 使用 moveit_config 的 robot_description_semantic 参数
            moveit_config.robot_description_kinematics,
            # 使用 moveit_config 的 robot_description_kinematics 参数
        ],
    )


    return LaunchDescription([planning_scene_tutorial])
    # 返回一个包含 planning_scene_tutorial 节点的 LaunchDescription 对象

运行代码

直接从 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 控制台格式，请不要担心。您可以按照本教程 https://docs.ros.org/en/rolling/Tutorials/Demos/Logging-and-logger-configuration.html 自定义您的 ROS 控制台记录器。

十年一梦实验室

关注

2
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
【MoveIt 2】示例：直接通过 C++ API 使用 MoveIt -规划场景

规划场景PlanningScene 类提供了用于碰撞检查和约束检查的主要接口。在本教程中，我们将探讨该类的 C++ 接口。入门如果您还没有这样做，请确保您已完成入门中的步骤。整个代码整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/planning...
复制链接

扫一扫