MoveIt!规划场景

参考Planning Scene Tutorial
主要用到PlanningScene类。提供了碰撞检测和约束检测用到的主要接口。

设置

使用 PlanningScene可以设置碰撞场景，但是建议使用PlanningSceneMonitor对机器人的关节和传感器建立规划场景。此文方法只用于演示。

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);

碰撞检测

自我碰撞检测

就是首先要检测自己跟自己的其他部分会不会发生碰撞。需要构建一个碰撞请求（CollisionRequest)对象和一个碰撞结果(CollisionResult)对象，再将它们传递给碰撞检测函数。检测结果保存在碰撞结果对象中。

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

修改状态后的碰撞检测

可以从规划场景中得到机器人的状态，然后改变机器人的状态，设置一个随机位置，然后进行碰撞检测。在检测前要清楚上次碰撞的结果。

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

以组为单位的碰撞检测

下面的代码检查”right_arm”与机器人其他部分的碰撞。可以把”right_arm”加入碰撞请求。

collision_request.group_name = "right_arm";
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");

获取碰撞信息

首先，手动设置right_arm的位置，让内部碰撞发生。肯定超出了关节约束的范围。

std::vector<double> joint_values;
const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("right_arm");
current_state.copyJointGroupPositions(joint_model_group, joint_values);
joint_values[0] = 1.57;  // hard-coded since we know collisions will happen here
current_state.setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM("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);
ROS_INFO_STREAM("Test 4: 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();
robot_state::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 5: 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 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

约束检查

约束有两种类型，a) 从运行学约束中选择的约束，比如关节约束，位置约束，方向约束和VisibilityConstraint；b) 用户通过回调函数定义的约束。具体看下面的例子。

运动学约束检测

首先在right_arm的末端定义一个简单的位置方向约束。

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.75;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 1.3;
desired_pose.header.frame_id = "base_footprint";
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 7: 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);
ROS_INFO_STREAM("Test 8: 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 9: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained"));

用户定义的约束

用户自定义的函数可以在PlanningScene类中使用setStateFeasibilityPredicate函数调用自定义的回调函数来实现。下面的例子用于判断“r_shoulder_pan”的角度是否大于零。

bool userCallback(const robot_state::RobotState& kinematic_state, bool verbose)
{
  const double* joint_values = kinematic_state.getJointPositions("r_shoulder_pan_joint");
  return (joint_values[0] > 0.0);
}

planning_scene.setStateFeasibilityPredicate(userCallback);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
ROS_INFO_STREAM("Test 10: Random state is " << (state_feasible ? "feasible" : "not feasible"));

当 isStateValid被调用时，将执行三个检查：1. 碰撞检查 2. 约束检查 3. 使用自定义回调函数的可行性检查。

bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "right_arm");
ROS_INFO_STREAM("Test 10: Random state is " << (state_valid ? "valid" : "not valid"));

需要注意的是，使用MoveIt!和OMPL的所有规划器都会执行碰撞检测、约束检测和使用自定义回调函数的可行性检查。
完整的代码可以参考：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/pr2_tutorials/planning

启动命令是

roslaunch moveit_tutorials planning_scene_tutorial.launch

<launch>

    <arg name="kinect" default="true"/>
    <!-- send pr2 urdf to param server -->
    <group if="$(arg kinect)">
      <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'" />
    </group>
    <group unless="$(arg kinect)">
      <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2_no_kinect.urdf.xacro'" />
    </group>

  <include file="$(find pr2_moveit_config)/launch/planning_context.launch"/>

  <node name="planning_scene_tutorial" pkg="moveit_tutorials" type="planning_scene_tutorial" respawn="false" output="screen">
    <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
  </node>
</launch>

执行结果

---