Ubuntu20.04，ROS Neotic。Move It！（5）场景规划：碰撞检测和关节约束

规划场景Planning Scene

该PlanningScene类提供的主界面，你将用于碰撞检测和约束检查。

设置

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

robot_model_loader::RobotModelLoader robot_model_loader("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);
ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

改变状态

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

moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

检查组Checking for a group

现在，我们将仅对panda的手进行碰撞检查，即，我们将检查手与机器人身体其他部位之间是否存在任何碰撞。我们可以通过将组名称“ hand”添加到冲突请求中来具体请求。

collision_request.group_name = "hand";
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("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);
ROS_INFO_STREAM("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);
ROS_INFO_STREAM("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)
{
  ROS_INFO("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);
ROS_INFO_STREAM("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);
ROS_INFO_STREAM("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::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::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);
ROS_INFO_STREAM("Test 8: Random state is " << (constrained ? "constrained" : "not constrained"));

有一种更有效的检查约束的方法（当您想一遍又一遍地检查相同的约束时，例如在规划器planner内部）。我们首先构造一个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);
ROS_INFO_STREAM("Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained"));

有一种直接的方法可以使用KinematicConstraintSet类来实现。

kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =
    kinematic_constraint_set.decide(copied_state);
ROS_INFO_STREAM("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);
ROS_INFO_STREAM("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");
ROS_INFO_STREAM("Test 12: Random state is " << (state_valid ? "valid" : "not valid"));

请注意，可通过MoveIt和OMPL使用的所有计划程序当前都将使用用户定义的回调执行冲突检查，约束检查和可行性检查。

运行代码

roslaunch启动文件以直接从moveit_tutorials运行代码：

roslaunch moveit_tutorials planning_scene_tutorial.launch