Ask Your Question
0

Disabling collision in AllowedCollisionMatrix removes attached object from the scene

asked 2020-05-18 14:17:22 -0500

cv_ros_user gravatar image

I am updating the AllowedCollisionMatrix in order to disable collision between an object attached to the robot end-effector and a collision object in the world for a specific robot movement. It works but it also removes the attached object from the scene. So it seems that it is not possible to disable temporary collision for an attached object and enable it afterward.

What would have been useful:

  • to be able to update the ACM to disable collision between an attached object and another object in the scene
  • use MoveIt to move the end-effector
  • update the ACM to enable collision
  • the attached object is still present in the scene with its position updated since the robot has moved

Unfortunately, it looks like it just removes the attached object from the scene.


A minimal code looks like this (tested on ROS Kinetic). Run with roslaunch panda_moveit_config demo.launch and rosrun <> <>.

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

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

  static const std::string PLANNING_GROUP = "panda_arm";
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(new robot_model_loader::RobotModelLoader("robot_description"));
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));

  planning_scene_monitor->startSceneMonitor();
  planning_scene_monitor->startWorldGeometryMonitor();
  planning_scene_monitor->startStateMonitor();

  //Add collision object
  {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box1";

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

    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.3;
    box_pose.position.y = 0;
    box_pose.position.z = 0;

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

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    planning_scene_interface.applyCollisionObjects(collision_objects);
  }

  //Add collision object and attach it
  {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box_attach";

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

    geometry_msgs::Pose box_pose = move_group.getCurrentPose().pose;
    box_pose.position.z -= 0.175;

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

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);

    planning_scene_interface.applyCollisionObjects(collision_objects);

    moveit_msgs::AttachedCollisionObject aco;
    aco.object.id = collision_object.id;
    aco.link_name = move_group.getEndEffectorLink();
    aco.touch_links.push_back(move_group.getEndEffectorLink());
    aco.touch_links.push_back("panda_hand");
    aco.touch_links.push_back("panda_leftfinger");
    aco.touch_links.push_back("panda_rightfinger");
    aco.object.operation = moveit_msgs::CollisionObject::ADD;
    planning_scene_interface.applyAttachedCollisionObject(aco);

    std::map<std::string, moveit_msgs::AttachedCollisionObject> attached_co = planning_scene_interface.getAttachedObjects();
    std::cout << "\nAttachedCollisionObject after applyAttachedCollisionObject:" << std::endl;
    for (const auto& kv : attached_co) {
      std::cout << kv.first << " ; " << kv.second.object.id << std::endl;
    }
  }

  //Wait a little
  const int wait_time = 2;
  ros::Duration(wait_time).sleep();

  //Ignore collision between attached object and collision object in the world
  {
    planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
    collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
    acm.setEntry("box1", "box_attach", true);
    std::cout << "\nAllowedCollisionMatrix:\n";
    acm.print(std ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-05-18 15:00:01 -0500

v4hn gravatar image

Most likely ls->getPlanningSceneDiffMsg(diff_scene); fills in a full robot state but does not know about the attached object (yet?). Adding diff_scene.robot_state.is_diff = true; after that statement should keep the attached object around.

Feel free to comment on the need for an incremental change approach for the ACM here or even provide a pull-request with the proposed changes.

edit flag offensive delete link more

Comments

Thank you! Adding diff_scene.robot_state.is_diff = true; solves indeed the issue.

About the proposal, maybe adding a bool is_diff field in the AllowedCollisionMatrix message? +1 for the proposal If it can avoid the need to use planning_scene_monitor::PlanningSceneMonitorPtr, planning_scene_monitor::LockedPlanningSceneRW, etc. for updating the ACM.

cv_ros_user gravatar image cv_ros_user  ( 2020-05-18 15:54:52 -0500 )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: 2020-05-18 14:17:22 -0500

Seen: 17 times

Last updated: May 18