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>

執行結果

---