planning_scene_tutorial.cpp详解

英文解释网站

代码注释

#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
/** 
 * PlanningScene class: 提供了用于碰撞检查和约束检查的主要接口。在本教程中，我们将探索此类的C++接口。
 */
bool stateFeasibilityTextExample(const robot_state::RobotState& kinematic_state, bool verbose){
    const double *joint_values = kinematic_state.getJointPositions("panda_joint_1");
    return (joint_values[0] > 0.0);
}

int main(int argc, char **argv){
    ros::init(argc, argv, "panda_arm_kinematics");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    std::size_t count = 0;
    /* 使用RobotModel或者URDF和SRDF能够配置PlanningScene类，然而这不是推荐使用的方式，
    推荐使用的方式是通过PlanningSceneMonitor创建和维护当前的planning scene，
    该方式使用机器人的关节和机器人上的传感器(在下一教程中将详细讨论)。
    但是在本教程中我们使用第一种方式，该方法仅用于说明，因为这个更简单： */
    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. Self-collision checking: 检查初始状态的机器人是否自碰撞 ************/   
    /* 检查当前状态的机器人是否处于“自碰撞”状态，即机器人的当前配置是否会导致机器人的零件相互碰撞。 
    为此，我们创建CollisionRequest和CollisionResult对象，并将它们传递给碰撞检查函数。结果保存在CollisionResult中。
    自碰撞检查使用的是未填充版本的机器人，即它直接使用URDF中提供的碰撞网格，而没有添加额外的填充。 */
    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");

/***********************************************/
/********* 2. Change the state: 将机器人的状态改为任意位置然后检查是否自碰撞 ************/  
    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");

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

/***********************************************/
/********* 4. Getting Contact Information: 将机器人的关节位置设置为超出限制，然后获得碰撞信息 ************/  
    std::vector<double> joint_values = {0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0};
    const robot_model::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) ? "vaild" : "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());
    }

/***********************************************/
/********* 5. Modifying the Allowed Collision Matrix: 使用checkSelfCollision函数在ACM下检查是否自碰撞 ************/
    /* ACM告诉碰撞检查器忽略掉某些部件间的碰撞，这样即使这些部件间发生了碰撞，也会被忽略掉的。
    checkSelfCollision函数针对给定的ACM，检查指定状态(copied_state)是否处于自碰撞中 */  
    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){
        /* 设置对应于一对元素的条目，最后一个参数如果为true，表示两个元素之间的碰撞是可以的，并且不需要显式的碰撞计算，
        如果为false，表示必须检查两个元素之间的冲突，并且不会忽略任何冲突 */
        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");

/***********************************************/
/********* 6. Full Collision Checking: 使用checkCollision函数在ACM下同时检查是否自碰撞和与环境的碰撞 ************/
    /* checkCollision函数针对给定的ACM，检查指定状态(copied_state)是否处于碰撞中，
    需要注意的是，与环境的碰撞检查将使用机器人的填充版本，填充有助于使机器人远离环境中的障碍物。 */
    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");

/***********************************************/
/********* 7. Constraint Checking: PlanningScene class提供了检查约束的函数 ************/
    /** 
     * 约束有两种：
     *  1.从KinematicConstraint集合中选取的约束，如JointConstraint, PositionConstraint, 
     *    OrientationConstraint和VisibilityConstraint
     *  2.通过回调函数指定的用户定义的约束 
     */

    /* 7.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);
    
    /* 7.1.1 使用PlanningScene类中的isStateConstrained函数根据这个约束检查状态。 */
    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"));
    /* 7.1.2 更有效的检查约束的方法（当您想一遍又一遍地检查相同的约束时，例如在规划器内部）。 
    我们首先构造一个KinematicConstraintSet，对ROS Constraints消息进行预处理，并将其设置为快速处理。 */
    kinematic_constraints::KinematicConstraintSet kinematic_constraints_set(kinematic_model);
    kinematic_constraints_set.add(goal_constraint, planning_scene.getTransforms());
    bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraints_set);
    ROS_INFO_STREAM("Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained"));
    /* 7.1.3 使用KinematicConstraintSet类有一种直接的方法。 */
    kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = 
    kinematic_constraints_set.decide(copied_state);
    ROS_INFO_STREAM("Test 10: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained"));
    
    /* 7.2 User-defined Constraints */
    /* 指定一个谓词，以决定状态在碰撞检查和约束评估所未涵盖的情况下，被视为有效还是无效。 */
    planning_scene.setStateFeasibilityPredicate(stateFeasibilityTextExample);
    /* 现在，每当调用isStateFeasible时，都会调用此用户定义的回调。 */
    bool state_feasible = planning_scene.isStateFeasible(copied_state);
    ROS_INFO_STREAM("Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible"));
    /* 每当调用isStateValid时，都会执行三项检查: (a)冲突检查 (b)约束检查和(c)使用用户定义的回调进行可行性检查。 */
    bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraints_set, "panda_arm");
    ROS_INFO_STREAM("Test 12: Random state is " << (state_valid ? "valid" : "not valid"));

    /* 请注意，通过MoveIt!和OMPL可用的所有规划器当前都将使用用户定义的回调执行碰撞检查，约束检查和可行性检查。*/
    ros::shutdown();
    return 0;
}

运行结果

[ INFO] [1613986431.293164309]: Loading robot model 'panda'...
[ INFO] [1613986431.423661317]: Loading robot model 'panda'...
[ INFO] [1613986431.443555920]: Test 1: Current state is in self collision
[ INFO] [1613986431.443732930]: Test 2: Current state is in self collision
[ INFO] [1613986431.443845861]: Test 3: Current state is not in self collision
[ INFO] [1613986431.443870151]: Test 4: Current state is valid
[ INFO] [1613986431.443959342]: Test 5: Current state is in self collision
[ INFO] [1613986431.444000062]: Contact between: panda_leftfinger and panda_link1
[ INFO] [1613986431.444033722]: Contact between: panda_link1 and panda_rightfinger
[ INFO] [1613986431.444114823]: Test 6: Current state is not in self collision
[ INFO] [1613986431.444174623]: Test 7: Current state is not in self collision
[ INFO] [1613986431.444350274]: Test 8: Random state is not constrained
[ INFO] [1613986431.444415934]: Test 9: Random state is not constrained
[ INFO] [1613986431.444453825]: Test 10: Random state is not constrained
[ INFO] [1613986431.444484135]: Test 11: Random state is feasible
[ INFO] [1613986431.444550935]: Test 12: Random state is not valid

解释：Test6 和 Test 7添加了ACM矩阵，所以将显示没有发生自碰撞