Ask Your Question
0

Collision Checking between collision object and object attached to the robot

asked 2017-03-24 09:57:17 -0600

bhavyadoshi26 gravatar image

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-03-27 02:57:49 -0600

angeltop gravatar image

I think that you have to modify the planning scene's allowed collision matrix and then call the applyPlanningScene service.

The allowed collision matrix is manually updated. In my experience(from indigo version) the planning scene contains only the allowed collsions from the SRDF model and then it is up to you to update it accordingly.

edit flag offensive delete link more

Comments

1

This is correct. If you want to allow collisions with attached objects, you have to add them to the ACM explicitly. If there is no entry for an object in there, MoveIt will always take collisions with them into account. You can use collision_detection::AllowedCollisionMatrix to simplify the update

v4hn gravatar imagev4hn ( 2017-03-27 04:51:41 -0600 )edit

If this is the case, could you please elaborate on the difference between touch_links and entries in the ACM?

BrettHemes gravatar imageBrettHemes ( 2017-05-17 12:05:37 -0600 )edit

I am not sure what the exact difference is, but I know that in order to allow collisions between two links, one has to update manually ACM. Maybe touch_links is used only for attaching an object on the robot and not for planning.

angeltop gravatar imageangeltop ( 2017-05-18 01:30:12 -0600 )edit

@v4hn Doesn't that mean that every time you attach an object to a gripper, the attached object and the gripper link are automatically in collision?

fvd gravatar imagefvd ( 2018-08-07 21:44:35 -0600 )edit

Sorry, I never noticed the replies to my comment. :/ touch_links is an independent mechanism for allowing collisions with attached objects. The vector is supposed to contain all the links of the gripper that actually touch the object.

However, this will not add the object to the ACM.

v4hn gravatar imagev4hn ( 2018-08-20 06:43:33 -0600 )edit

So if you wish to add additional allowed collisions later on, you have to manually add the object to the ACM.

@fvd: it depends on whether you filled in the touch_links. The link_name is always allowed to collide with the object.

v4hn gravatar imagev4hn ( 2018-08-20 06:45:11 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-03-24 09:57:17 -0600

Seen: 1,225 times

Last updated: Mar 27 '17