MoveIt!入門教程-規劃場景(Planning Scene)--碰撞檢測

轉載自創客製造，https://www.ncnynl.com/archives/201610/1035.html，

同時可以參考planning_scene_tutorial.cpp

位於<node name="planning_scene_tutorial" pkg="moveit_tutorials" type="planning_scene_tutorial" respawn="false" output="screen">

MoveIt!入門教程-規劃場景(Planning Scene)

說明

planningscene類提供的主要的接口，可使用碰撞檢測和約束檢測。
在本教程中，我們將探討C++接口類。

使用

安裝

planningscene類可以方便地安裝和配置使用一個RobotModel或URDF和SRDF
不推薦直接實例化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);

碰撞檢測

自碰撞檢測
- 首先檢查機器人的狀態是否在自碰撞。例如是否會左手碰右手。
- 我們會實例化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");

檢查一個組

目前衝突檢測只針對PR2機器人的右臂。例如我們檢測右臂跟機器人其他部位是否會發生碰撞。
可以通過增加組名 “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");

獲得聯繫信息

首先，手動設置正確的手臂到一個位置，我們知道內部（自我）碰撞發生。
注意，這個狀態現在實際上不在PR2的關節限制範圍，我們也可以直接檢查。

std::vector joint_values;
const robot_model::JointModelGroup* joint_model_group =
current_state.getJointModelGroup("right_arm");
current_state.copyJointGroupPositions(joint_model_group, joint_values);
joint_values[
現在，我們可以得到任何可能發生在一個給定的配置的右手臂的碰撞的聯繫信息。我們可以要求通過填充在適當的字段中的碰撞請求的聯繫信息，並指定要返回的聯繫人的最大數量。

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

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

約束檢測

PlanningScene類同樣提供便利的函數用於約束檢測。
兩種約束類型：
- 來自KinematicConstraint集合的約束選擇，例如：JointConstraint, PositionConstraint, OrientationConstraint 和VisibilityConstraint
- 用戶定義的約束，可以通過回調函數來調用。
- 下面看看簡單的KinematicConstraint

檢查運動學約束
- 首先定義PR2機器人右臂的末端執行器的一個位置和方向約束
- 注意：一些便利的函數用於填充約束（這些函數在moveit_core的kinematic_constraints目錄的utils.h文件找到）

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

有很多方便的方法檢測約束(當你在規劃裏反覆檢測同類約束)。首先構建已經在ROS約束信息預處理的KinematicConstraintSet和設置它，可以快速處理。

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函數來使用回調函數。
這是簡單的回調例子，檢查PR2機器人“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);
}

現在，無論isStateFeasible是否調用，自定回調函數都會被調用。

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是否給調用，三個檢測會被實施：(a) 衝突檢測 (b) 約束檢測 (c) 用戶自定義回調函數的可行性檢測

 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來執行衝突檢測，約束檢測和用戶自定義回調函數的可行性檢測。

完整代碼

下載完整代碼

編譯代碼

編譯方法

運行代碼

roslaunch moveit_tutorials planning_scene_tutorial.launch

期望的輸出

輸出效果如下，使用隨機關節值，數字可能有差異。

[ INFO] [1385487628.853237681]: Test 1: Current state is not in self collision
[ INFO] [1385487628.857680844]: Test 2: Current state is in self collision
[ INFO] [1385487628.861798756]: Test 3: Current state is not in self collision
[ INFO] [1385487628.861876838]: Current state is not valid
[ INFO] [1385487628.866177315]: Test 4: Current state is in self collision
[ INFO] [1385487628.866228020]: Contact between: l_shoulder_pan_link and r_forearm_link
[ INFO] [1385487628.866259030]: Contact between: l_shoulder_pan_link and r_shoulder_lift_link
[ INFO] [1385487628.866305963]: Contact between: l_shoulder_pan_link and r_shoulder_pan_link
[ INFO] [1385487628.866331036]: Contact between: l_shoulder_pan_link and r_upper_arm_link
[ INFO] [1385487628.866358135]: Contact between: l_shoulder_pan_link and r_upper_arm_roll_link
[ INFO] [1385487628.870629418]: Test 5: Current state is not in self collision
[ INFO] [1385487628.877406467]: Test 6: Current state is not in self collision
[ INFO] [1385487628.879610797]: Test 7: Random state is not constrained
[ INFO] [1385487628.880027331]: Test 8: Random state is not constrained
[ INFO] [1385487628.880315077]: Test 9: Random state is not constrained
[ INFO] [1385487628.880377445]: Test 10: Random state is feasible
[ INFO] [1385487628.887157707]: Test 10: Random state is not valid

MoveIt!入門教程-規劃場景(Planning Scene)--碰撞檢測

MoveIt!入門教程-規劃場景(Planning Scene)--碰撞檢測

ubuntu 18.04 安裝opencv3.2.0的坑

odeint的runge_kutta4與Matlab的ode45的比較

ubuntu 查看當前插入的顯卡是否正確識別：

ubuntu 18.04 使用gazebo的問題

MoveIt!入門教程-規劃場景(Planning Scene)--碰撞檢測

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結