< >
Home » MoveIt!入门教程 » MoveIt!入门教程-规划场景(Planning Scene)

MoveIt!入门教程-规划场景(Planning Scene)

MoveIt!入门教程-规划场景(Planning Scene)

说明

  • planningscene类提供的主要的接口，可使用碰撞检测和约束检测。
  • 在本教程中，我们将探讨C++接口类。

使用

  1. 安装
  • 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);
  1. 碰撞检测
  • 自碰撞检测
    • 首先检查机器人的状态是否在自碰撞。例如是否会左手碰右手。
    • 我们会实例化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");
  1. 改变状态
  • 现在，让我们改变机器人的当前状态。
  • 规划场景保持当前状态。我们可以得到一个参考，并改变它，然后检查新的机器人配置的碰撞。
  • 特别注意，我们需要在发起新的碰撞检测要求前，清除之前的冲突检测的结果。
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");
  1. 检查一个组
  • 目前冲突检测只针对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");
  1. 获得联系信息

  • 首先，手动设置正确的手臂到一个位置，我们知道内部（自我）碰撞发生。

  • 注意，这个状态现在实际上不在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());
}
  1. 修改免检碰撞矩阵（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");
  1. 充分的碰撞检测
  • 我们可以进行自碰撞检测，也可以使用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");
  1. 约束检测
  • PlanningScene类同样提供便利的函数用于约束检测。
  • 两种约束类型：
    • 来自KinematicConstraint集合的约束选择，例如：JointConstraint, PositionConstraint, OrientationConstraint 和VisibilityConstraint
    • 用户定义的约束，可以通过回调函数来调用。
    • 下面看看简单的KinematicConstraint
  1. 检查运动学约束
    • 首先定义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"));
  1. 自定义约束
  • 用户定义的约束也可以被指定给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来执行冲突检测，约束检测和用户自定义回调函数的可行性检测。
  1. 完整代码
  1. 编译代码
  1. 运行代码
roslaunch moveit_tutorials planning_scene_tutorial.launch
  1. 期望的输出
  • 输出效果如下，使用随机关节值，数字可能有差异。
[ 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

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: moveit!入门教程