def execute_traj(traj):
   #for j, conf in enumerate(traj.path()):
   #for j, conf in enumerate([traj.end()]):
   path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
   for j, conf in enumerate(path):
     set_manipulator_conf(manipulator, conf)
     raw_input('%s/%s) Step?'%(j, len(path)))
def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan):
  def execute_traj(traj):
    #for j, conf in enumerate(traj.path()):
    #for j, conf in enumerate([traj.end()]):
    path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
    for j, conf in enumerate(path):
      set_manipulator_conf(manipulator, conf)
      raw_input('%s/%s) Step?'%(j, len(path)))

  # Resets the initial state
  set_manipulator_conf(manipulator, initial_conf.value)
  for obj, pose in problem.initial_poses.iteritems():
    set_pose(bodies[obj], pose.value)

  for i, (action, args) in enumerate(plan):
    raw_input('\n%s/%s) Next?'%(i, len(plan)))
    if action.name == 'move':
      _, _, traj = args
      execute_traj(traj)
    elif action.name == 'move_holding':
      _, _, traj, _, _ = args
      execute_traj(traj)
    elif action.name == 'pick':
      obj, _, _, _, traj = args
      execute_traj(traj.reverse())
      robot.Grab(bodies[obj])
      execute_traj(traj)
    elif action.name == 'place':
      obj, _, _, _, traj = args
      execute_traj(traj.reverse())
      robot.Release(bodies[obj])
      execute_traj(traj)
    else:
      raise ValueError(action.name)
    env.UpdatePublishedBodies()
 def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
   enable_all(all_bodies, False)
   body2.Enable(True)
   set_pose(body2, pose.value)
   for conf in traj.path():
     set_manipulator_conf(manipulator, conf)
     if env.CheckCollision(robot, body2):
       return False
   return True
 def _cfree_traj_pose(traj, pose):
     enable_all(all_bodies, False)
     body2.Enable(True)
     set_pose(body2, pose.value)
     for conf in traj.path():
         set_manipulator_conf(manipulator, conf)
         if env.CheckCollision(robot, body2):
             return False
     return True
 def sample_free_motion(conf1, conf2): # Sample motion while not holding
   if DISABLE_TRAJECTORIES:
     yield [(object(),)] # [(True,)]
     return
   enable_all(all_bodies, False)
   set_manipulator_conf(manipulator, conf1.value)
   #traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
   traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
   if not traj: return
   traj.grasp = None
   yield [(traj,)]
 def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
   enable_all(all_bodies, False)
   body1.Enable(True)
   body2.Enable(True)
   set_pose(body2, pose.value)
   for conf in traj.path():
     set_manipulator_conf(manipulator, conf)
     manip_trans = manipulator.GetTransform()
     set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
     if env.CheckCollision(body1, body2):
       return False
   return True
    def sample_free_motion(conf1, conf2):
        if DISABLE_TRAJECTORIES:
            yield [(object(), )]
            return
        enable_all(all_bodies, False)
        set_manipulator_conf(manipulator, conf1.value)

        traj = cspace_traj_helper(base_manip,
                                  cspace,
                                  conf2.value,
                                  max_iterations=10)
        if not traj:
            return
        traj.grasp = None
        yield [(traj, )]
 def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
   if DISABLE_TRAJECTORIES:
     yield [(object(),)] # [(True,)]
     return
   enable_all(all_bodies, False)
   body1.Enable(True)
   set_manipulator_conf(manipulator, conf1.value)
   manip_trans = manipulator.GetTransform()
   set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
   robot.Grab(body1)
   #traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
   traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
   robot.Release(body1)
   if not traj: return
   traj.grasp = grasp
   yield [(traj,)]
  def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
    enable_all(all_bodies, False)
    body1.Enable(True)
    set_pose(body1, pose.value)
    manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
    grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration
    if grasp_conf is None: return
    if DISABLE_TRAJECTORIES:
      yield [(Conf(grasp_conf), object())]
      return

    set_manipulator_conf(manipulator, grasp_conf)
    robot.Grab(body1)
    grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp
    #grasp_traj = workspace_traj_helper(base_manip, approach_vector)
    robot.Release(body1)
    if grasp_traj is None: return
    grasp_traj.grasp = grasp
    pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration
    yield [(pregrasp_conf, grasp_traj)]
    def sample_grasp_traj(pose, grasp):
        enable_all(all_bodies, False)
        body1.Enable(True)
        set_pose(body1, pose.value)
        manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
        grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans)
        if grasp_conf is None:
            return
        if DISABLE_TRAJECTORIES:
            yield [(Conf(grasp_conf), object())]
            return

        set_manipulator_conf(manipulator, grasp_conf)
        robot.Grab(body1)
        grasp_traj = vector_traj_helper(env, robot, approach_vector)

        robot.Release(body1)
        if grasp_traj is None:
            return
        grasp_traj.grasp = grasp
        pregrasp_conf = Conf(grasp_traj.end())
        yield [(pregrasp_conf, grasp_traj)]