Moveit代码 集合（2）Planning Scene(规划场景）

PlanningScene类提供了用于碰撞检测（checkCollision）和约束检查(checkConstraint)的主要接口。

通过RobotModel或URDF，SRDF可以轻松设置PlanningScene类，但是这种实例化PlanningScene对象的方法是不推荐的。PlanningSceneMonitor是推荐的利用来自机器人关节和传感器的数据创建和维护当前规划场景的方法。本例程只作为展示。

1.初始化ros节点，创建多线程并开始：

  ros::init(argc, argv, "panda_arm_kinematics");
  ros::AsyncSpinner spinner(1);
  spinner.start();

2.加载机器人模型，创建机器人模型对象，创建Planning_Scene类

  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);//通过机器人模型创建PlanningScene类

3.碰撞检测（Collision Checking)

3.1自碰撞检测（self-collision checking）

collision_detection::CollisionRequest collision_request;//创建碰撞请求类，内含碰撞检测所需的参数，如对机器人哪个部件进行检测等，若参数为空，则为完整机器人
collision_detection::CollisionResult collision_result;//创建碰撞结果类，内含碰撞检测所有结果，如果不作特殊说明，仅有二值化的检测结果，即是否存在碰撞
planning_scene.checkSelfCollision(collision_request, collision_result);//对planning_scene对象做自碰撞检测，检测对象（机器人）为kinematic_model
ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");//输出collision_result中的检测结果

3.2改变机器人状态再次检测

/*获取planning_scene当前机器人状态，创建机器人状态（robotstate）对象current_state（robotmodel类不能修改，robotstate类可以修改），创建的是& current_state，因此对current_state修改时，planning_scene中的机器人状态也被修改了，因此后面碰撞检测时是对设置为随机状态的机器人进行碰撞检测*/
moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();
current_state.setToRandomPositions();//将当前状态设为随机值
collision_result.clear();//重新检测碰撞之前，一般要清空碰撞检测结果
planning_scene.checkSelfCollision(collision_request, collision_result);//对设置为随机状态的机器人进行自碰撞检测，检测对象为完整的及其ir
ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");//输出检测结果

3.3对部分杆件进行碰撞检测（未实现，github上提了issuse，目前无回复#769）

collision_request.group_name = "panda_hand";//碰撞请求中group_name设置，对该杆件作碰撞检测，即该杆件与机器人其他部分的碰撞检测
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");

3.4手动设置机器人状态，并检测是否满足机器人关节限制

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");//创建joint_model_group对象，并采用当前机器人状态作为关节模型组
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"));//检测当前关节值是否满足机器人限制，并输出

3.5接触信息输出

//输出接触状态需要对以下两个碰撞请求成员属性进行修改
collision_request.contacts = true;
collision_request.max_contacts = 1000;

collision_result.clear();//检测前清除之前检测结果
//进行自碰撞检测，注意，因为之前设置了collision_request.group_name为panda_hand，因此此处的检测仅为hand与机器人的碰撞检测
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());
}

3.6修改允许碰撞矩阵（AllowedCollisionMatrix (ACM) ）

ACM提供了允许忽略此处碰撞的机制（机器人自碰撞或者和环境的碰撞）

ollision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();//创建ACM对象，对象为planning_scene下
moveit::core::RobotState copied_state = planning_scene.getCurrentState();//从当前状态，复制一个机器人状态

collision_detection::CollisionResult::ContactMap::const_iterator it2;//创建接触状态对象
//循环将当前发生碰撞的所有杆件设置为ACM成员
for (it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2)
{
  acm.setEntry(it2->first.first, it2->first.second, true);
}
collision_result.clear();//清除当前碰撞结果
//进行自碰撞检测，参数为：请求，结果，检测状态，acm
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");

3.7 完全碰撞检测（机器人自碰撞，机器人与环境碰撞），注意这要用机器人的填充版本，而非只有杆件的版本

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

4.约束检测（Constraint Checking），分为两类，（1）机器人运动学约束（kinematic constraint）：包括关节约束，位置约束，方向约束和可见性约束（？） （2）用户自定义约束

4.1 机器人运动学约束（kinematic constraint）

（1）创建目标约束

//创建表示末端执行器的字符串变量，赋值为关节模型组的最后一个元素，vector.back（）为向量组最后一个元素
std::string end_effector_name = joint_model_group->getLinkModelNames().back();
//创建目标姿态，消息类型为geometry_msgs::PoseStamped
geometry_msgs::PoseStamped desired_pose;
//给目标姿态参数赋值，要求目标位姿保持w轴旋转1.0...
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，由kinematic_constraints::constructGoalConstraints函数确定值，施加约束的目标为end_effector_name,目标位姿约束为disired_pose
kinematic_constraints::constructGoalConstraints为表示运动学约束的表示和评估的命名空间*/
moveit_msgs::Constraints goal_constraint =
    kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);

（2）检查是否满足约束

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

（3）检查约束的另一种有效方法（更高效，有预处理）

//构造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 9: Random state is " << (constrained_2 ? "constrained" : "not constrained"));

（4）通过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"));

4.2 用户定义约束（一个案例，用于检查joint1是否正角还是负角）

（1）函数定义

//定义一个函数，检查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);
}

（2）将自定义函数指定给对象

//通过setStateFeasibilityPredicate函数，将用户定义的约束函数stateFeasibilityTestExample指定给planning_scene对象
planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample);
//定义bool变量state_feasible，对状态可行性进行检测，检测的状态是copied_state
bool state_feasible = planning_scene.isStateFeasible(copied_state);
//检测，输出。若大于0，返回值为真，判断为feasible
ROS_INFO_STREAM("Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible"));

/*在将用户约束函数制定给对象后，对对象进行isStateValid检测将会进行三个检测，碰撞检测，约束检测，用户自定义约束
三个参数分别为，要检测的机器人状态，运动学约束，机器人模型*/

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