[ROS MoveIT 系列] 規劃場景

<source : greatway9999>

一、前言

在 MoveIt 的場景規劃，會使用到 PlanningScene 這個類別，用於確認碰撞與限制。官方的完整程式碼，可直接點擊連結前往查看。

執行以下 Launch 檔，你可以得到以下結果。但你必須先完成相關的設定，請參考此篇。

roslaunch moveit_tutorials planning_scene_tutorial.launch

<source : greatway9999>

你會看到如下圖般的輸出

<source : greatway9999>

接下來我們就從拆解程式中來學習。

二、程式解析

1.PlanningScene 

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

當然，透過 RobotModel 、URDF 和 SRDF 就可以輕鬆完成 " PlanningScene "的相關設定。但是更好的方法是藉由機器人的關節和感測器資訊，再透過 PlanningSceneMonitor 來創建和維護當前的場景規劃。

2.Collision Checking (碰撞檢測)

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

第一件要確認的事為機器人的當前狀態是否處於自我碰撞。因此，可以建構 CollisionRequest 和 CollisionResult  物件，並植入碰撞檢測的函式中。不論機器人是否處於自我碰撞的狀態，函式都會回傳結果。

3. Change the state (改變狀態)

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

你會發現，當改變了機器人當前狀態，規劃的場景的內部仍保持當前狀態。因此，內部設定可作為一個依據，讓我們去做改變，接著再確認新的碰撞設定。要特別注意，在執行新的碰撞檢測要求時，我們必須清除 " collision_result " 的值。

4. Checking for a group (確認要檢測的群組)

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

現在只針對熊貓機器手臂的手進行碰撞檢測，也就是要確認手是否和其他部件發生碰撞。只要將 "hand" 加到 collision_request 的函式中即可，如上程式碼所示。

5. Getting Contact Information (取得接觸資訊)

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

首先以手動方式將熊貓手臂移動到會發生碰撞的位置。接著，我們就能直接確認熊貓手臂的關節極限的當前位置。

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

現在我們可以獲得來自熊貓手臂設定檔可能發生的任何碰撞中的接觸資訊。

我們可以透過在適當的空格中填入 collision request，藉以獲得接觸資訊，同時指定接觸的最大值，讓函式回傳當作最大值回傳。

6. Modifying the Allowed Collision Matrix (修正允許碰撞矩陣)

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

AllowedCollisionMatrix (ACM) 提供一種機制可在某些物件中忽視碰撞。也就是在某些情況下，即使發生碰撞，也會回報沒有碰撞發生。

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

這是一組在規劃中常用的碰撞檢測函式。

8. Constraint Checking (限制確認)

PlanningScene 類別也包含一個用來確認限制的簡單回傳函式。限制有 2 種形式 :

• 從 KinematicConstraint (運動學限制)選擇的限制 : JointConstraint, PositionConstraint, OrientationConstraint 和 VisibilityConstraint 。
• 使用者特別定義用來回傳的限制。

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

在熊貓機器手臂中的 "arm_group" 中的末端效應器，定義簡單的位置和傾向限制。

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

在 PlanningScene 中使用 isStateConstrained 函式，進行限制狀態的檢視。

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 ，作為ROS Constraints 訊息的前處理並設定它進行快速處理。

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

以上是直接使用 KinematicConstraintSet 類別的方法。

(2) User-defined constraints，

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

使用者定義的限制也可以被用來指定 PlanningScene 類別，只要透過使用 setStateFeasibilityPredicate 的回傳函式即可。以上是使用者定義的回傳函式，用來確定熊貓機器手臂的 "panda_joint1"是正向或是逆向角度。

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

isStateFeasible 函式被呼叫，因此，使用者定義的回傳函式也會被呼叫。

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

不論何時當 isStateValid 被呼叫，有3個透過使用者定義的回傳函式檢查會被執行 :

• collision checking (碰撞檢測)
• constraint checking (限制檢測)
• feasibility checking (可行性確認)

所有透過 MoveIt 和 OMPL的規劃都可以呈現當前透過使用者定義的回傳函式的碰撞檢測、限制檢測、可行性確認。

9. Launch 檔

請參考官網提供的連結。

最後，執行以下 Launch 檔，你可以得到以下結果。但你必須先完成相關的設定，請參考此篇。

roslaunch moveit_tutorials planning_scene_tutorial.launch

<source : greatway9999>

你會看到如下圖般的輸出

<source : greatway9999>

---

參考資料 :

MoveIt : Planning Scene