0
$\begingroup$

Rosanswers logo

Hello fellow ROS users,

I've never had problems using collision objects (unattached); collision checking always works perfectly with Moveit or Descartes. The issue arise when I try to attach a collision object (in this example, a tool) to the robot and use the move_group api to move the robot into collision (another collision object, which is a table). I can see visually that the collision object is attached (orange to purple in rViz and I can move the robot with the tool), but collision checking is ignored between the attached tool and the environment when using the move_group api.

Using the moveit motion planning directly in rViz, collision between the attached tool and the table is detected correctly. This is the following message I get when I plan to move the attached tool into it :

[ INFO] [1568823161.942566491]: Found a contact between 'table' (type 'Object') and 'tool' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.

Planning failed like it was supposed too. I can't seem to find why it doesn't fail when using the move_group api. I've tried using different link_name (since tool0 has no geometry), but to no avail. I've noticed that there doesn't seem to be any attached collision objects (or even collision objects) in the planning scene when I use getAttachedCollisionObjectMsgs() or getCollisionObjectMsgs(), is the planning scene not updated? Yet, it fails if any robot link (BUT the tool) touches the table, so the table is in the planning scene.

A similar question: #q324672.

**EDIT : I've noticed that in rViz, the looping animation doesn't have the attached tool while using the move_group api, but does, when using motion planning plugin on rViz directly. By looking at the topic move_group/display_planned_path, I can see that there is no attached collision object when using the move_group api. **

Here's my code :

int main(int argc, char** argv)
{
// Ros init
ros::init(argc, argv, "moveit_node");
ros::NodeHandle nh;

// Async spinner
ros::AsyncSpinner async_spinner(1);
async_spinner.start();

// Planning scene interface for adding collision objects
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

/*/ Create table collision object /*/

// Collisions objects and colors vector
std::vector<moveit_msgs::CollisionObject> collision_objects;
std::vector<moveit_msgs::ObjectColor> object_colors;

// Create collision object (table)
moveit_msgs::CollisionObject table;
table.header.frame_id = "world";
table.id = "table";

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

geometry_msgs::Pose table_pose;
table_pose.orientation.w = 1.0;
table_pose.position.x = 0;
table_pose.position.y = 0;
table_pose.position.z = -0.01;

table.primitives.push_back(table_primitive);
table.primitive_poses.push_back(table_pose);
table.operation = table.ADD;

collision_objects.push_back(table);

moveit_msgs::ObjectColor table_color;
table_color.id = "table";
table_color.color.a = 1;
table_color.color.r = 0.7;
table_color.color.g = 0.7;
table_color.color.b = 0.8;

object_colors.push_back(table_color);

// Create collision object (tool)
moveit_msgs::CollisionObject tool_object;
tool_object.id = "tool";
tool_object.header.frame_id = "tool0";

shape_msgs::SolidPrimitive tool_primitive;
tool_primitive.type = table_primitive.BOX;
tool_primitive.dimensions.resize(3);
tool_primitive.dimensions[0] = 0.05;
tool_primitive.dimensions[1] = 0.05;
tool_primitive.dimensions[2] = 0.4;

geometry_msgs::Pose tool_pose;
tool_pose.orientation.w = 1.0;
tool_pose.position.x = 0;
tool_pose.position.y = 0;
tool_pose.position.z = 0.2;

tool_object.primitives.push_back(tool_primitive);
tool_object.primitive_poses.push_back(tool_pose);
tool_object.operation = tool_object.ADD;

collision_objects.push_back(tool_object);

moveit_msgs::ObjectColor tool_color;
tool_color.id = "tool";
tool_color.color.a = 1;
tool_color.color.r = 1;
tool_color.color.g = 1;
tool_color.color.b = 1;

object_colors.push_back(tool_color);

planning_scene_interface.applyCollisionObjects(collision_objects, object_colors);

// Attach tool to robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.object.id = "tool";
attached_object.link_name = "tool0";
attached_object.object.operation = attached_object.object.ADD;
planning_scene_interface.applyAttachedCollisionObject(attached_object);

/*/--- Create move group and move robot
 *  where there is collision with new attached object ----/*/

// Planning move group
moveit::planning_interface::MoveGroupInterface move_group("manipulator");

// Attach tool (second method to attach collision object)
// move_group.attachObject("tool", "wrist_3_link");

// Get planning scene to see if the attached collision object is in the planning scene
planning_scene::PlanningScene planning_scene( move_group.getRobotModel());

// Print attached collision objects (DOESN'T PRINT ANY OBJECT)
std::vector<moveit_msgs::AttachedCollisionObject> objects;
planning_scene.getAttachedCollisionObjectMsgs(objects);
for (moveit_msgs::AttachedCollisionObject object : objects){
    ROS_ERROR_STREAM(object.object.id);
}
if (objects.size() == 0){
    ROS_ERROR_STREAM("No attached collision object");
}

// Print collision objects  (DOESN'T PRINT ANY OBJECT)
std::vector<moveit_msgs::CollisionObject> objects2;
planning_scene.getCollisionObjectMsgs(objects2);
for (moveit_msgs::CollisionObject object : objects2){
    ROS_ERROR_STREAM(object.id);
}
if (objects2.size() == 0){
    ROS_ERROR_STREAM("No collision object");
}

// Plan for robot to move into collision
move_group.setPoseReferenceFrame("/world");
move_group.setMaxVelocityScalingFactor(1);

robot_state::RobotState start_state(*move_group.getCurrentState());
move_group.setStartState(start_state);

move_group.setJointValueTarget(collision_robot_position);

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
move_group.plan(my_plan);
move_group.execute(my_plan);

ros::waitForShutdown();
}

I use kinetic.

Thank you for you time!

Julien Audet


Originally posted by Julien Audet on ROS Answers with karma: 76 on 2019-09-18

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I've found the problem, I had to get the "locked" planning scene from a planning scene monitor to correctly initialize my robot state. Now, everything works.

Here's the relevant code :

planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ptr =
    std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
planning_scene_monitor_ptr->requestPlanningSceneState();
planning_scene_monitor::LockedPlanningSceneRO lps(planning_scene_monitor_ptr);
robot_state::RobotState robot_state(lps->getCurrentState());

Originally posted by Julien Audet with karma: 76 on 2019-09-18

This answer was ACCEPTED on the original site

Post score: 4


Original comments

Comment by georgiablanco on 2021-02-16:
Thank you for this, I was wondering if you had to delete any objects after adding this code? As I am now getting:

SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.