Collision checking for arbitrary poses using MoveIt! / PlanningSceneMonitor

edit

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.