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](http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/planning_scene_ros_api_tutorial.html)
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](https://groups.google.com/forum/#!topic/moveit-users/a3dIWP7hdqo) 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("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.
↧