MoveIt2——5.场景规划

最新推荐文章于 2024-01-11 23:12:34 发布

babyrtsh .

最新推荐文章于 2024-01-11 23:12:34 发布

阅读量408

点赞数

分类专栏： Moveit2 ROS2 文章标签： c++ ubuntu

本文链接：https://blog.csdn.net/Avada_C_L/article/details/125672475

版权

Moveit2 同时被 2 个专栏收录

10 篇文章 11 订阅

订阅专栏

ROS2

8 篇文章 1 订阅

订阅专栏

场景规划

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对象并将它们传递给碰撞检测函数。请注意，机器人是否自碰撞会包含在该检测结果中。自碰撞检测会使用未填充（unpadded）版本的机器人，即直接使用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();
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机器人的手臂（arm）设置到内部知道自碰撞确实会发生的某个位置。请注意，此状态现在实际上超出了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函数来进行自碰撞检测和与环境（当前为空）的碰撞检测。这是在规划器中最常使用的一组碰撞检测函数。请注意，对与环境的碰撞进行检测将会使用机器人的填充版本。填充有助于使机器人远离环境中的障碍物。

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约束消息进行预处理并设置它以进行快速处理。

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& kinematic_state, bool /*verbose*/)
{
  const double* joint_values = kinematic_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"));

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"));

请注意，MoveIt2和OMPL中所有可用的规划器当前都会执行碰撞检测、约束检测和使用用户定义的回调函数执行可行性检测。

launch文件

完整的启动文件在GitHub网站上此处。本教程中的所有代码都可以从moveit_tutorials软件包中编译和运行。

运行代码

可以使用ros2 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