Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Collision Checking between collision object and object attached to the robot

Hi,

I have writen a node for attaching a collision object to the planning scene. I am using the Apply Planning Scene service for that. Here is my node:

        Vector3d b(0.001, 0.001, 0.001);
        moveit_msgs::AttachedCollisionObject attached_object;   // to attach the object to the robot
        moveit_msgs::CollisionObject remove_object; // to remove the object from the world
        geometry_msgs::Pose pose;  
        moveit_msgs::PlanningScene planning_scene;

        attached_object.link_name = "tool0";    //the link to which the object will be attached
        attached_object.object.header.frame_id = "tool0";     // w.r.t. the link to which it is attached
        attached_object.object.operation = attached_object.object.ADD;
        std::string touch_links = "motor";
      attached_object.touch_links.push_back(touch_links);  

     ROS_INFO("Attaching kinematic_e1 to motor and removing it from the world.");
          attached_object.object.id = "kinematic_e1";
          shapes::Mesh* m = shapes::createMeshFromResource("package://mitsubishi_rv6sd_support/meshes/objects/kinematic_e1.stl",b);
          shape_msgs::Mesh mesh;
          shapes::ShapeMsg mesh_msg;  
          shapes::constructMsgFromShape(m, mesh_msg);
          mesh = boost::get<shape_msgs::Mesh>(mesh_msg);  
          pose.orientation.w = 1.0;
          attached_object.object.meshes.push_back(mesh);
          attached_object.object.mesh_poses.push_back(pose);
          remove_object.id = "kinematic_e1";
          remove_object.header.frame_id = "base_link";
          remove_object.operation = remove_object.REMOVE;

  planning_scene.world.collision_objects.clear();
  planning_scene.world.collision_objects.push_back(remove_object);  //This step removes the object from the world 
  planning_scene.robot_state.attached_collision_objects.push_back(attached_object); //This step attaches the object to the robot
  planning_scene.is_diff = true;    //Update the planning scene as a diff
  planning_scene.robot_state.is_diff = true;    //Update the robot state as a diff

  //Using services to make changes to the planning scene
  ros::ServiceClient planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
  planning_scene_diff_client.waitForExistence();

  moveit_msgs::ApplyPlanningScene srv;
  srv.request.scene = planning_scene;
  planning_scene_diff_client.call(srv);

Then I try to check the AllowedCollisionMatrix for the attached object (kinematic_e1 in this case). I use the GetPlanningScene service to get a copy of the allowed collision matrix. But when I query the entry names in the matrix, I cannot find the name of the attached object in the list.

    moveit_msgs::PlanningScene currentScene;
   moveit_msgs::GetPlanningScene scene_srv;
    scene_srv.request.components.components = scene_srv.request.components.ALLOWED_COLLISION_MATRIX;
    if(!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());
    for (int i = 0; i < currentACM.entry_names.size(); i++)
      ROS_ERROR_STREAM(" Name =  " << currentACM.entry_names[i]);
        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());

How can I find the attached object in the allowed collision matrix?

I dont know if this is related but when I give a pose goal where a definite collision between the robot and the robot attached link wil occur, the planning fails. This kind of tells me that MoveIt! plans taking into consideration the attached object. It appears correctly in RViz as well. Then why does the name of the attached object not show up in the allowed collision matrix?