Collision checking using the PlanningScene

[Felix Burget]

Hello everyone,

i am currently working on a motion planning algorithm for a fixed base manipulator. Currently i am encountering some problems
with the collision checking procedure of MoveIt. The collision object is correctly added to the planning scene by the following code:



   ps_obj_pub_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

   ros::WallDuration sleep_t(0.5);
   sleep_t.sleep();
   

    moveit_msgs::CollisionObject coll_object;
    coll_object.header.frame_id = "/lbr_0_link";  //"/lbr_0_link = base frame of the manipulator
    coll_object.header.stamp = ros::Time::now();
    coll_object.id = "box";

    geometry_msgs::Pose pose;
    pose.position.x = 0.0;
    pose.position.y = -0.2;
    pose.position.z = 0.6;
    pose.orientation.w = 1.0;

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 1.5; //X dim
    primitive.dimensions[1] = 0.1; //Y dim
    primitive.dimensions[2] = 0.1; //Z dim

  
    coll_object.primitives.push_back(primitive);
    coll_object.primitive_poses.push_back(pose);
    coll_object.operation = coll_object.ADD;


    //Publish on planning_scene topic
    moveit_msgs::PlanningScene planning_scene_msg;
    planning_scene_msg.name = "motion_planning_scene";
    planning_scene_msg.world.collision_objects.push_back(coll_object);
    planning_scene_msg.is_diff = true;
    ps_obj_pub_.publish(planning_scene_msg);
    sleep_time.sleep();


However, when running my planner collisions with the object are not detected by the "checkCollision" function of the PlanningScene object. Here are the
lines for collision checking:

    //Collision checking Setup (ps_ is the PlanningScene Object)
    collision_detection::CollisionRequest collision_request;
    collision_request.group_name = planning_group_;
    collision_detection::CollisionResult collision_result;
    //collision_detection::AllowedCollisionMatrix acm = ps_->getAllowedCollisionMatrix();
    collision_detection::AllowedCollisionMatrix acm = ps_->getAllowedCollisionMatrix();

    robot_state::RobotState state(ps_->getRobotModel());
    state.setToDefaultValues();


    //Set Configuration of robot state
    state.setVariablePositions(configuration);   //"configuration" is a std::map<std::string, double>

   //Apply robot state to planning scene
    ps_->setCurrentState(state);

   //Clear the collision checking result
    collision_result.clear();

    //Check for collisions
    ps_->checkCollision(collision_request, collision_result, state, acm);


For debugging i checked whether the object is added to the CollisionWorld

    bool obj_exist = ps_->getCollisionWorld()->getWorld()->hasObject("box");
   cout<<"Object exists in collision world: "<<obj_exist<<endl;
  

and it turned out that the object does not exist there. Any ideas, or sample code how to make the collision check work?

Thank you for your answer in advance! Best,
Felix

[Simon Schmeißer]

Hi Felix,

I would check if the object did exist before

ps_->setCurrentState(state);

because I think "RobotState" might contain (and therefore apply) more information than you expect

Simon

[Felix Burget]

Hi Simon,

i am actually checking for the object existence (last code snippet) right after publishing it to the planning scene (after "sleep_time.sleep();" ), i.e. before any collision check is performed. Therefore I believe that it is never added to the collision world.
Just to let you know, the object is present in RViz so it should be in the PlanningScene.

[Simon Schmeißer]

My previous answer was a quick shot before going to lunch. I'm still not 100% understanding everything, but here is what I guess where you error lies:

A PlanningScene should be thought of as a snapshot of the world at a given time. There can be many PlanningScenes, some of them not up to date. (You can play around with them, change them, plan with the changed ones). There is one PlanningSceneMonitor listening to incoming information about changes in the world (joint positions, collision objects, ...) and producing fresh PlanningScenes from time to time (ie 2 Hz or something). You probably store a PlanningScene once and assume it would update itself. That's not the case. I get a fresh copy by using this function:

const std::string PLANNING_SCENE_SERVICE = "get_planning_scene";
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ =  boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"/*, tf_listener_*/);

planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
ps->getCurrentStateNonConst().update();
//if you want to modify it
//planning_scene::PlanningScenePtr scene = ps->diff();
//scene->decoupleParent();

you should scope this function, because as long as "ps" lives, there will be no updates to any planning scenes (?).

(You maybe want to think about where the knowledge about the world actually lives. If you use MoveGroup, it's actually in a different process than your own code. The PlanningSceneMonitor you create will connect to the one in the MoveGroup process and fetch a fresh PlanningScene and then present it to you)

I'm happy to hear about any misunderstandings on my side!

[Felix Burget]

Hi Simon,

thank you for your response. I also used a planning scene monitor as follows:

    //Create planning scene monitor
    planning_scene_monitor_  = boost::shared_ptr<planning_scene_monitor::PlanningSceneMonitor>(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));

    //Start Monitors of Planning_Scene_Monitor
    bool use_octomap_monitor = false; // this prevents a /tf warning
    planning_scene_monitor_->startWorldGeometryMonitor(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC,planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC,use_octomap_monitor);
    planning_scene_monitor_->startSceneMonitor();
    planning_scene_monitor_->startStateMonitor();

but after publishing the object onto the planning scene:

    moveit_msgs::CollisionObject coll_object;
    coll_object.header.frame_id = "/lbr_0_link";

    coll_object.header.stamp = ros::Time::now();
    coll_object.id = "box";
    geometry_msgs::Pose pose;
    pose.position.x = 0.0;
    pose.position.y = -0.2;
    pose.position.z = 0.6;
    pose.orientation.w = 1.0;
    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 1.5; //X dim
    primitive.dimensions[1] = 0.1; //Y dim
    primitive.dimensions[2] = 0.1; //Z dim
    coll_object.primitives.push_back(primitive);
    coll_object.primitive_poses.push_back(pose);

    coll_object.operation = moveit_msgs::CollisionObject::ADD;

    //Publish on planning_scene topic
    moveit_msgs::PlanningScene planning_scene_msg;

    planning_scene_monitor_->getPlanningScene()->getPlanningSceneMsg(planning_scene_msg);
    planning_scene_msg.name = "kuka_motion_planning";

    planning_scene_msg.world.collision_objects.push_back(coll_object);
    planning_scene_msg.is_diff = true;
    ps_obj_pub_.publish(planning_scene_msg);
    sleep_time.sleep();

the answer to the following query is "false", so the PSM does not know about the object with ID "box"

    bool obj_exist = planning_scene_monitor_->getPlanningScene()->getCollisionWorld()->getWorld()->hasObject("box");

    cout<<"Object exists in collision world: "<<obj_exist<<endl;

[Simon Schmeißer]

Hi Felix,

you're welcome. In your code you tell your PlanningSceneMonitor to keep track of all changes ("you create a second world"), maybe you miss some topic here. What my code does is to query the PlanningSceneMonitor of the move_group process (my PlanningSceneMonitor acts as a proxy to the "central" PlanningSceneMonitor).

[Felix Burget]

So if i understand correctly, i need to launch a move_group node to keep track of the world state (as explained here http://moveit.ros.org/wiki/MoveGroup_Node )?

[Felix Burget]

Just to be sure.

Is it correct that launching the move_group node from the my moveit config package ....

roslaunch <your moveit configuration package> move_group.launch

already starts a planning_scene monitor. So the final code snippet for checking a configuration for collision should work?

    const std::string PLANNING_SCENE_SERVICE = "get_planning_scene";

    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ =  boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");

    planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
    planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
    ps->getCurrentStateNonConst().update();
    //if you want to modify it

    planning_scene::PlanningScenePtr scene = ps->diff();

    scene->decoupleParent();


    //Collision checking Setup

    collision_detection::CollisionRequest collision_request;
    collision_request.group_name = planning_group_;
    collision_detection::CollisionResult collision_result;

    collision_detection::AllowedCollisionMatrix acm = scene->getAllowedCollisionMatrix();

    //Init isValid flag to be returned by the function
    bool isValid = true;

    //Set up Map storing the configuration of manipulator
    std::map<std::string, double> configuration;
    for (int i = 0; i < num_joints_ ; i++)
    {
        configuration[joint_names_[i]] = config(i);
        //std::cout<<joint_names_[i] <<": "<<configuration[joint_names_[i]]<<std::endl;
    }

    //Assign the robot state to the Kinematic Model
    robot_state::RobotState state(scene->getRobotModel());

    state.setToDefaultValues();

    //Set Configuration of robot state
    state.setVariablePositions(configuration);

    //Apply robot state to planning scene

    scene->setCurrentState(state);

    //Clear the collision checking result
    collision_result.clear();

    //Check for self- collisions
    scene->checkSelfCollision(collision_request, collision_result, state, acm);

I didn't try to run it yet, because i am missing a moveit config package for my robot. Initially I thought that collision checking from moveit can be used without starting a move_group node (since i am using my own planner and not one of the MoveIt/ OMPL planners) 

[Simon Schmeißer]

yes, a move_group node will be launched by that roslaunch file (just read it). Also you will definitely need a moveIt config package (it defines what a group is etc)

I think you should start by getting everything to run with ompl and then later on write a plugin similar to the ompl-plugin for your own planner.

Check the tutorials from ros industrial:
http://wiki.ros.org/Industrial/Tutorials/Create_a_MoveIt_Pkg_for_an_Industrial_Robot

they also have a kuka package, so maybe you can save some work


Finally there is a function "isStateValid" that will save you quite some code

planning_scene_monitor_->getPlanningScene()->isStateValid(intermediateState, "groupname")

[Felix Burget]

Hi Simon,

the collision checking is finally working. Since I had already a planning launch file containing an urdf,srdf etc. i just had to add the following part of the move_group.launch to my own launch file.

<!-- GDB Debug Option -->
    <arg name="debug" default="false" />
    <arg unless="$(arg debug)" name="launch_prefix" value="" />
    <arg     if="$(arg debug)" name="launch_prefix"
       value="gdb -x $(find kuka_lbr_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
      
    <!-- Verbose Mode Option -->
   <arg name="info" default="$(arg debug)" />
   <arg unless="$(arg info)" name="command_args" value="" />
   <arg     if="$(arg info)" name="command_args" value="--debug" />
  
  
   <!-- move_group settings -->
   <arg name="allow_trajectory_execution" default="true"/>
   <arg name="fake_execution" default="false"/>
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
 
   
    <!-- Start the actual move_group node/action server -->
    <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
        <!-- Set the display variable, in case OpenGL code is used internally -->
        <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

        <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
        <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
        <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />

        <!-- MoveGroup capabilities to load -->
        <param name="capabilities" value="move_group/MoveGroupCartesianPathService
                          move_group/MoveGroupExecuteService
                          move_group/MoveGroupKinematicsService
                          move_group/MoveGroupMoveAction
                          move_group/MoveGroupPickPlaceAction
                          move_group/MoveGroupPlanService
                          move_group/MoveGroupQueryPlannersService
                          move_group/MoveGroupStateValidationService
                          move_group/MoveGroupGetPlanningSceneService
                          move_group/ClearOctomapService
                          " />

        <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
        <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
        <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
        <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
        <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
    </node>

with the "get_planning_scene" service of the "move_group" node the message on topic "/collision_object" is now taken into account by the PlanningSceneMonitor.

Thank you for you help once again!

Best,
Felix