def check_reachable(good_bodies, env, obj_to_grasp, manip, grasping_poses, only_reachable = False):
    """Check if the robot can reach at least one pose 

    Parameters:
    manip: a Manipulator instance
    grasping_poses: a list of (4x4 matrix). 
    only_reachable: do not check for collisions
    """
    
    # opening gripper
    pr2 = env.GetRobots()[0]
    v = pr2.GetActiveDOFValues()
    v[pr2.GetJoint('r_gripper_l_finger_joint').GetDOFIndex()]=0.54
    pr2.SetDOFValues(v)

    env.Remove(obj_to_grasp)
    if len(grasping_poses) == 0:
        return None, []
    if only_reachable:
        options = (openravepy.IkFilterOptions.IgnoreEndEffectorCollisions)
    else:
#        options = (openravepy.IkFilterOptions.IgnoreEndEffectorCollisions |
#                   openravepy.IkFilterOptions.CheckEnvCollisions)
        options = (openravepy.IkFilterOptions.CheckEnvCollisions)


    for pose in grasping_poses:
        sol = manip.FindIKSolution(pose, options)

        if sol is None:
            continue

        # if sol has collisions with unmovable objects, continue
        pr2.SetDOFValues(sol, pr2.GetActiveManipulator().GetArmIndices());                    
        collisions =  utils.get_all_collisions(pr2, env)
        # unmovable = lambda b: not (b.GetName().startswith("random") or\
        #                            b.GetName().startswith('object'))
        # unmovable_bodies = filter(unmovable, collisions)
        # if len(unmovable_bodies) == 0:
        #     pass
        #     #continue

        bad_body_exists = False
        for body in collisions:
            if body not in good_bodies:
                bad_body_exists = True
                break
        if bad_body_exists:
            continue

        env.AddKinBody(obj_to_grasp)
        return sol, collisions
    env.AddKinBody(obj_to_grasp)
    return None, []
  def get_collisions(self, traj, n=100):
    orig_values = self.robot.GetDOFValues(self.manip.GetArmIndices());

    collisions = set()
    traj_up = mu.interp2d(np.linspace(0,1,n), np.linspace(0,1,len(traj)), traj)
    with self.env:
      for joint_values in traj_up:
        self.robot.SetDOFValues(joint_values, self.manip.GetArmIndices())
        for obj in utils.get_all_collisions(self.robot, self.env):
          collisions.add(obj)

      self.robot.SetDOFValues(orig_values, self.manip.GetArmIndices())
    return collisions
def get_occluding_objects(robot, 
                             object_to_grasp, 
                             max_trials = 100,
                              just_one_attempt = False,
                              return_pose = False
                             ):
    """Generates a list of all the objects that prevent the robot from reaching
    a target object. Several (up to max_trials) attempts are performed to grasp

    Parameters:
    robot: a openravepy.Robot instance
    object_to_grasp: a openravepy.KinBody instance representing the object to grasp    
    
    Returns:
    a list of sets of objects
    """
    if just_one_attempt != return_pose:
        raise ValueError("If asking for a return poes then set just_one_attempt to True")
    
    env = robot.GetEnv()
    robot_pose = robot.GetTransform()
    manip = robot.GetActiveManipulator()
    
    ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
            robot,iktype=openravepy.IkParameterization.Type.Transform6D)
    if not ikmodel.load():
        ikmodel.autogenerate()    
    min_torso, max_torso = utils.get_pr2_torso_limit(robot)

    num_trial = 0
    collisions_list = []
    with robot:
        while num_trial < max_trials:
            num_trial +=1

            # sample a base pose
            torso_angle = generate_reaching_poses.move_random_torso(robot, 
                                                                    min_torso, 
                                                                    max_torso)
            robot_pose = generate_reaching_poses.generate_random_pos(robot, 
                                                                     object_to_grasp)
            
            robot.SetTransform(robot_pose) 
            report = openravepy.CollisionReport()
            
            collision = env.CheckCollision(robot, report=report)
            
            if not collision:                
                openravepy.raveLogInfo("Got a position not in collision")
                
                #sample a gripper pose
                grasping_poses = generate_reaching_poses.generate_grasping_pose(robot,
                                                        object_to_grasp,
                                                        use_general_grasps = True,
                                                        checkik=False) 
                openravepy.raveLogInfo("Got %d grasping poses" % len(grasping_poses))
                #check if gripper pose is reachable from base pose
                #use robot's base pose to transform precomputed
                #gripper poses into the robot's frame of reference
                sol = generate_reaching_poses.check_reachable(manip, 
                                                           grasping_poses, 
                                                              only_reachable = True)
                if sol is None:
                    print "Trial {0} No sol from base pose to gripper pose".\
                        format(num_trial)
                
                if sol is not None:                  
                    print "Sol from base pose to gripper pose found in trial {0}".\
                        format(num_trial)
                    openravepy.raveLogInfo("Getting the list of collisions")
                    with robot:
                        robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices());                    
                        collisions_list.append(utils.get_all_collisions(robot, env))
                        if just_one_attempt:
                            if return_pose:
                                return (robot_pose, sol, torso_angle, collisions_list)
                            else:
                                return collisions_list
        if num_trial == max_trials:
            print "No gripper pose reachable from collision free base pose found",
            print "after {0} trials".format(num_trial)
            

    return collisions_list