Ask Your Question
1

Collision checking for arbitrary poses using MoveIt! / PlanningSceneMonitor

asked 2017-05-16 22:24:11 -0500

BrettHemes gravatar image

I am having trouble figuring out the best way to check a bunch of poses for collisions (e.g., I have a self-made trajectory that I want to validate using the robot_description in conjunction any attached geometries (e.g., picked item) attached via the PlanningSceneMonitor as outlined in the PR2 moveit tutorial

My initial attempt used a planning scene pointer carried around inside of my planner class like I have seen in other code:

aux_movegroup_ = new moveit::planning_interface::MoveGroupInterface("manipulator");
aux_robot_model_ = aux_model_loader_.getModel();  // "robot_description"
aux_robot_state_ = robot_state::RobotStatePtr(new robot_state::RobotState(aux_robot_model_));
aux_planning_scene_ = planning_scene::PlanningScenePtr(new planning_scene::PlanningScene(aux_robot_model_));

However, it seems the member scene doesn't update with the attached geometries until the NEXT planner instance (e.g., if cycling through a bunch of attached object geometries while replanning, my planning scene pointer somehow had the information of the previous attached object while the path I was testing was for the current...).

After some head scratching and Googling I came across this approach:

// get a fresh instance of the planning scene from move_group
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 ps(planning_scene_monitor_ptr);
for (ALL_WAYPOINTS)
{
  aux_robot_state_->setFromIK(aux_joint_model_group_, WAYPOINT_i);
  collision_detection::CollisionResult::ContactMap contacts;
  ps->getCollidingPairs(contacts, *aux_robot_state_);
}

But now my touch_links for the previously attached object are not carried over and the picked object shows to be in collision with the end effector for every waypoint...

The merit of my approaches aside, I am curious to the best way to perform collision checking for arbitrary paths/poses with collision objects that have been attached to the planning scene via MoveIt!'s planning scene monitor. How should I be doing this? Any help would be greatly appreciated.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-05-17 06:29:37 -0500

v4hn gravatar image

But now my touch_links for the previously attached object are not carried over and the picked object shows to be in collision with the end effector for every waypoint...

The touch links are part of the robot state. If you initialize your aux_robot_state_ with the current state of the planning scene, I don't see why the touch_links should not be there.

How should I be doing this?

your second approach seems feasible to me.

edit flag offensive delete link more

Comments

Allright, I have tried updating the state from the locked planning scene and still have the same issue. In fact, if I collide the planning scene with itself (i.e., ps->getCollidingPairs(contacts)) I get the same result.

BrettHemes gravatar image BrettHemes  ( 2017-05-17 12:09:45 -0500 )edit

Inspecting the planning scene as a message from /get_planning_scene looks ok (touch_links are there).

Do I need to modify the ACM for what I am trying to do?

BrettHemes gravatar image BrettHemes  ( 2017-05-17 12:10:35 -0500 )edit

OK, I decided to go ahead and modify the allowed collision matrix and now add entries for each attached object. Everything seems to be working except that, when using an updated state from the planning scene monitor all of the attached objects collide with themselves. Any idea why this might be?

BrettHemes gravatar image BrettHemes  ( 2017-05-19 12:47:49 -0500 )edit

I get a working copy of the robot_state like so:

  const robot_state::RobotState scene_state = ps->getCurrentState();
  robot_state::RobotState scene_state_cpy = scene_state;

Is this correct?

BrettHemes gravatar image BrettHemes  ( 2017-05-19 12:49:44 -0500 )edit

OK, I figured out my problem... I was carrying around a modified URDF/SRDF in the node's private namespace as a relic of a previous commit and it seems that (for reasons I can't fully explain) this description was making its way into MoveIt!'s planning_scene robot_state.

BrettHemes gravatar image BrettHemes  ( 2017-05-23 10:37:02 -0500 )edit

It so happened that my modified private robot_description had links with names colliding with my attached objects, thus resulting in the self-collisions for my attached objects. There were also some intermittent segfaults to boot.... Long story short, your answer is correct.

BrettHemes gravatar image BrettHemes  ( 2017-05-23 10:39:31 -0500 )edit

However, as an aside note: attached objects with touch links that are touching will return collision-free results from any collision checks BUT the (min) distance between bodies will be zero. If you want distances from "touching" attached objects you need to modify the ACM appropriately.

BrettHemes gravatar image BrettHemes  ( 2017-05-23 10:41:29 -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

3 followers

Stats

Asked: 2017-05-16 22:24:11 -0500

Seen: 1,021 times

Last updated: May 17 '17