Robotics StackExchange | Archived questions

checkCollision in MoveIt doesn't show collision between robot and scene

Hi everyone

I have problem - function checkCollision in MoveIt doesn't show collision between robot and scene.

The purpose is to move arm to random valid poses for testing planning algorithms.

My code:

int main( int argc, char** argv )
{
ros::init(argc, argv, "moveit");
ros::AsyncSpinner spinner(1);
spinner.start();

Planner planner("left_arm", "right_arm");

addBox(planner);

for (int i=0; i< 1000; ++i)
{
    ros::Time begin = ros::Time::now();
    if (planner.moveRandomPos(1))
    {
        cout << ros::Time::now()-begin << endl;
    }
}
ros::shutdown();
return 0;
}

function addBox for left arm:

bool addBox(Planner& planner)
{
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = planner.move_group1->getPlanningFrame();
collision_object.id = "box";

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.050;
primitive.dimensions[1] = 0.50;
primitive.dimensions[2] = 2;

geometry_msgs::Pose stlPose;
stlPose.orientation.w = 1.0;
stlPose.position.x = 0.3; //left
stlPose.position.y = -0.2; //nazad
stlPose.position.z = 1.5;

collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(stlPose);
collision_object.operation = collision_object.ADD;

planner.node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 0);

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planner.planning_scene_interface.addCollisionObjects(collision_objects);

ros::Duration(1.0).sleep();

moveit_msgs::PlanningScene planning_scene;
ros::Publisher planning_scene_diff_publisher = planner.node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
planning_scene.world.collision_objects.push_back(collision_object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2.0);
}

Structure Planner with constructor, moveRandomPos and isRobotStateCollision functions

struct Planner {
ros::NodeHandle node_handle;
string packPath, planGroupName1, planGroupName2, planner_plugin_name;
std::vector<std::string> joint_names1, joint_names2, link_names1, link_names2;

moveit::planning_interface::MoveGroupInterface *move_group1, *move_group2;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene::PlanningScenePtr planning_scene;
planning_scene_monitor::PlanningSceneMonitor *planning_scene_monitor;

robot_model_loader::RobotModelLoader *robot_model_loader;
robot_model::RobotModelPtr kinematic_model;

robot_state::RobotStatePtr kinematic_state;
robot_state::JointModelGroup *joint_model_group1, *joint_model_group2;

Planner(string planGroup1,string planGroup2)
{
    planGroupName1 = planGroup1;
    planGroupName2 = planGroup2;
    packPath = ros::package::getPath("moveit")+"/";

    move_group1 = new moveit::planning_interface::MoveGroupInterface(planGroupName1);
    move_group2 = new moveit::planning_interface::MoveGroupInterface(planGroupName2);
    robot_model_loader = new robot_model_loader::RobotModelLoader("robot_description");

    //new
    planning_scene_monitor = new planning_scene_monitor::PlanningSceneMonitor("robot_description");
    planning_scene = planning_scene_monitor->getPlanningScene();

    kinematic_model = robot_model_loader->getModel();
    kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
    joint_model_group1 = kinematic_model->getJointModelGroup(planGroupName1);
    joint_model_group2 = kinematic_model->getJointModelGroup(planGroupName2);

    joint_names1 = joint_model_group1->getJointModelNames();
    joint_names2 = joint_model_group2->getJointModelNames();
    link_names1 = joint_model_group1->getLinkModelNames();
    link_names2 = joint_model_group2->getLinkModelNames();
}

 bool moveRandomPos(u_int armNumber)
 {
    moveit::planning_interface::MoveGroupInterface *move_group;
    robot_state::JointModelGroup *joint_model_group;

    switch (armNumber) {
    case 1:
        move_group = move_group1;
        joint_model_group = joint_model_group1;
        break;
    case 2:
        move_group = move_group2;
        joint_model_group = joint_model_group2;
        break;
    default:
        cout << "Bad armNumber" << endl;
        return false;
    }

    move_group->setStartStateToCurrentState();
    robot_state::RobotState goal_state = planning_scene->getCurrentState();

    for (u_int i=0; i<100; ++i)
    {
        goal_state.setToRandomPositions(joint_model_group);

        if (!isRobotStateCollision(goal_state))
        {
            break;
        }

        if (i==99)
        {
            cout << "Can't gen random pos" << endl;
            return false;
        }
    }

    Eigen::Affine3d end_effector_state = goal_state.getGlobalLinkTransform(joint_model_group->getLinkModelNames().back());

    geometry_msgs::Pose idle_pose;
    tf::poseEigenToMsg(end_effector_state, idle_pose);
    move_group->setPoseTarget(idle_pose);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    move_group->setPlanningTime(10.0);

    if (move_group->plan(my_plan))
        return true;
    else
        return false;
 }

 bool isRobotStateCollision(robot_state::RobotState& robSt)
 {
    collision_detection::CollisionRequest req;
    collision_detection::CollisionResult res;
    collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();

    acm.print(cout);
    planning_scene->checkCollision(req, res, robSt, acm);

    return res.collision;
 }
}

Planning is nice, robot avoids "box" without collisions. But I have troubles with moveRandomPos and checkCollision function. I want to generate valid poses without collisions but checkCollision doesn't show collision between robot and "box" and I often have error from planner:

[ERROR] [1493255278.226718474]: RRTConnect: Unable to sample any valid states for goal tree

acm.print(cout) shows only parts of robot. How can I update acm? I try different ways but don't have a result.

I tried function publishBox instead of addBox but result is the same:

void publishBox(Planner& planner)
{
    geometry_msgs::Pose stlPose;
    stlPose.orientation.w = 1.0;
    stlPose.position.x = 0.3; //left
    stlPose.position.y = -0.2; //nazad
    stlPose.position.z = 1.5;

    //publish collision object
    moveit_msgs::CollisionObject object;
   // object.header = handle_pose.header;
    object.header.frame_id = planner.move_group1->getPlanningFrame();
    object.id = "box";

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.050;
    primitive.dimensions[1] = 0.50;
    primitive.dimensions[2] = 2;

    object.primitives.push_back(primitive);
    object.primitive_poses.push_back(stlPose);
    object.operation = object.ADD;

    planner.node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 0);
    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(object);
    ROS_INFO_NAMED("tutorial", "Add an object into the world");
    planner.planning_scene_interface.addCollisionObjects(collision_objects);
sleep(2.0);

    moveit_msgs::PlanningScene planning_scene_add;
    planning_scene_add.world.collision_objects.push_back(object);
    planning_scene_add.is_diff = true;

    ros::Publisher planning_scene_diff_publisher = planner.node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
    planning_scene_diff_publisher.publish(planning_scene_add);
    sleep(2.0);

    moveit_msgs::GetPlanningScene srv;
    srv.request.components.components =  moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES;


    //wait until box is published (I don't think that this is really needed)
    bool object_in_world = false;

    while(!object_in_world)
    {
        ROS_ERROR("waiting for box to appear");

        if (planner.client_get_scene_.call(srv))
        {
            for (int i = 0; i < (int)srv.response.scene.world.collision_objects.size(); ++i)
            {
                if (srv.response.scene.world.collision_objects[i].id == object.id)
                    object_in_world = true;
            }
        }
    }

    moveit_msgs::PlanningScene currentScene;
    moveit_msgs::PlanningScene newSceneDiff;
    moveit_msgs::GetPlanningScene scene_srv;

    scene_srv.request.components.components = scene_srv.request.components.ALLOWED_COLLISION_MATRIX;

    if(!planner.client_get_scene_.call(scene_srv))
    {
        ROS_WARN("Failed to call service /get_planning_scene");
    }
    else
    {
        ROS_INFO_STREAM("Initial scene!");
        currentScene = scene_srv.response.scene;
        moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;

        ROS_ERROR_STREAM("size of acm_entry_names before " << currentACM.entry_names.size());
        ROS_ERROR_STREAM("size of acm_entry_values before " << currentACM.entry_values.size());
        ROS_ERROR_STREAM("size of acm_entry_values[0].entries before " << currentACM.entry_values[0].enabled.size());

        currentACM.entry_names.push_back(object.id);
        moveit_msgs::AllowedCollisionEntry entry;
        entry.enabled.resize(currentACM.entry_names.size());

        for(int i = 0; i < entry.enabled.size(); i++)
        entry.enabled[i] = false;

        //add new row to allowed collsion matrix
        currentACM.entry_values.push_back(entry);

        for(int i = 0; i < currentACM.entry_values.size(); i++)
        {
            //extend the last column of the matrix
            currentACM.entry_values[i].enabled.push_back(false);
        }

        newSceneDiff.is_diff = true;
        newSceneDiff.allowed_collision_matrix = currentACM;

        planning_scene_diff_publisher.publish(newSceneDiff);


    }

    if(!planner.client_get_scene_.call(scene_srv))
    {
        ROS_WARN("Failed to call service /get_planning_scene");
    }

    else
    {
        ROS_INFO_STREAM("Modified scene!");

        currentScene = scene_srv.response.scene;
        moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;

        ROS_ERROR_STREAM("size of acm_entry_names after " << currentACM.entry_names.size());
        ROS_ERROR_STREAM("size of acm_entry_values after " << currentACM.entry_values.size());
        ROS_ERROR_STREAM("size of acm_entry_values[0].entries after " << currentACM.entry_values[0].enabled.size());
    }
}

acmprint from isRobotStateCollision doesn't show "box" but ROSERROR_STREAM from publishBox shows it:

[ERROR] [1493257331.750786744]: size of acm_entry_names before 61
[ERROR] [1493257331.750805772]: size of acm_entry_values before 61
[ERROR] [1493257331.750815565]: size of acm_entry_values[0].entries before 61
[ERROR] [1493257331.757684566]: size of acm_entry_names after 62
[ERROR] [1493257331.757750064]: size of acm_entry_values after 62
[ERROR] [1493257331.757775842]: size of acm_entry_values[0].entries after 62

Asked by Vlad222 on 2017-04-21 16:57:09 UTC

Comments

Answers

The AllowedCollisionMatrix should be fine as it is.

how are you creating the planning_scene pointer? Are you sure it is synchronized with the one you are adding the collision objects to? I would suspect a problem in that direction.

Asked by rbbg on 2017-04-22 04:57:26 UTC

Comments

rbbg, Thank you for your answer!

I edited my question

Asked by Vlad222 on 2017-04-26 15:35:33 UTC