MoveIt 教程【5】——Planning Scene

本篇博客主要介绍 PlanningScene 类所提供的用于碰撞检测和约束检测的主要接口（C++）。【由于我个人暂时用不到该知识，后面关于代码的解释不全，待以后更新】

#include <ros/ros.h>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>

#include <moveit/kinematic_constraints/utils.h>

// 待学习... 
bool stateFeasibilityTestExample(const robot_state::RobotState& kinematic_state, bool verbose)
{
  const double* joint_values = kinematic_state.getJointPositions("panda_joint1");
  return (joint_values[0] > 0.0);
}
// END_SUB_TUTORIAL

int main(int argc, char** argv)
{
  ros::init(argc, argv, "panda_arm_kinematics");
  ros::AsyncSpinner spinner(1);
  spinner.start();
  std::size_t count = 0;

  // 1、Setup
  // 用一个RobotModel或URDF和SRDF可以方便的配置 PlanningScene 类
  // 但是，不推荐直接实例化PlanningScene类
  // 推荐的方法是使用PlanningSceneMonitor类（下一个教程中详细讨论），它利用机器人的关节和传感器数据来创建和维护目前的规划场景。
  // 在本教程中，我们将planningscene直接实例化一个类，但这种方法只用于说明实例

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

  // 2、Collision Checking
  // 2.1 Self-collision checking   
  // 首先检查机器人的状态是否在自碰撞，例如机器人的当前配置是否会导致机器人的部件彼此击中
  // 为了实现自碰撞检测，我们会实例化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");

  // 2.2 Change the state
  // 现在，让我们改变机器人的当前状态。
  // 规划场景保持当前状态。我们可以得到一个参考，并改变它，然后检查新的机器人配置的碰撞。
  // 特别注意，我们需要在发起新的碰撞检测要求前，清除之前的冲突检测的结果。

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

  // 2.3 Checking for a group
  // 目前冲突检测只针对 Panda 机器人的手。例如我们检测手跟机器人其他部位是否会发生碰撞
  // 可以通过增加组名 “right_arm”来进行冲突检测。
  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");

  // 2.4 Getting Contact Information  获得联系信息
  // 首先，手动设置正确的手臂到一个位置，我们知道自我碰撞发生。
  // 这个状态现在实际上不在 Panda 的关节限制范围，我们也可以直接检查
  std::vector<double> joint_values = { 0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0 };
  const robot_model::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"));

  // 现在，我们可以得到任何可能发生在一个给定的配置的手臂的碰撞的联系信息。
  // 我们可以要求通过填充在适当的字段中的碰撞请求的联系信息，并指定要返回的联系人的最大数量
  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());
  }

  // Modifying the Allowed Collision Matrix    修改免检碰撞矩阵
  // 待学习... 

  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 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

  // Full Collision Checking
  // 待学习... 
  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");

  // Constraint Checking
  // 待学习... 
  // Checking Kinematic Constraints
  // 待学习... 
  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);

  // 待学习... 
  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"));

  // 待学习... 

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

  // 待学习... 

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

  // User-defined constraints
  // 待学习... 

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

  // 待学习... 

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

  // 待学习... 

  ros::shutdown();
  return 0;
}