MoveIt!规划场景

程序员文章站 2022-07-12 23:03:25

...

参考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>

执行结果

MoveIt!规划场景

MoveIt!规划场景

设置

碰撞检测

自我碰撞检测

修改状态后的碰撞检测

以组为单位的碰撞检测

获取碰撞信息

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

全碰撞检测

约束检查

运动学约束检测

用户定义的约束

启动命令是

执行结果

Laravel5.1框架注册中间件的三种场景详解

“后装”转“前装” 华为全场景智慧化战略打造智能生活蓝图

华为发布全场景智慧化家居战略技术与连接双轨并行

PS自带的动画场景制作gif动态图片

java工程师职业规划怎么回答（java工程师的职业发展道路）

NatureMade入驻拼多多商城，场景化营销是关键

网站建设前期如何做好规划？

HTML5之SVG 2D入门13—svg对决canvas及长处和适用场景分析

java工程师职业规划怎么回答（java工程师的职业发展道路）

photoshop合成制作冲天而起的魔法白色光束场景

MoveIt!规划场景

设置

碰撞检测

自我碰撞检测

修改状态后的碰撞检测

以组为单位的碰撞检测

获取碰撞信息

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

全碰撞检测

约束检查

运动学约束检测

用户定义的约束

启动命令是

执行结果

Laravel5.1框架注册中间件的三种场景详解

“后装”转“前装” 华为全场景智慧化战略打造智能生活蓝图

华为发布全场景智慧化家居战略 技术与连接双轨并行

PS自带的动画场景制作gif动态图片

java工程师职业规划怎么回答（java工程师的职业发展道路）

NatureMade入驻拼多多商城，场景化营销是关键

网站建设前期如何做好规划？

HTML5之SVG 2D入门13—svg对决canvas及长处和适用场景分析

java工程师职业规划怎么回答（java工程师的职业发展道路）

photoshop合成制作冲天而起的魔法白色光束场景

华为发布全场景智慧化家居战略技术与连接双轨并行