def fn(belief): problem = compile_belief(belief, goal) if FOCUSED: search = get_fast_downward( 'astar', verbose=True ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy #plan, universe = focused_planner(problem, search=search, stream_cost=0, verbose=True, optimal=True, debug=False) # stream_cost = 0 instead of None plan, universe = simple_focused(problem, search=search, stream_cost=0, max_level=0, verbose=True, optimal=True, debug=False) else: search = get_fast_downward( 'wastar1' ) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy plan, universe = incremental_planner(problem, search=search, frequency=1, optimal=True, waves=True, debug=False, max_calls=5000) print print 'Cost:', plan_cost(universe, plan) print 'Length:', plan_length(universe, plan) plan = convert_plan(plan) print 'Plan:', plan if plan is None or not plan: return None action, params = plan[0] return OPERATOR_MAP[action.name](operators, *params)
def run_test(self, problem_fn, planner, search): print SEPARATOR print problem_fn.__name__, self.planner, self.search tamp_problem = problem_fn(p=self.p) stream_problem = compile_problem(tamp_problem) print stream_problem if search == BFS: search_fn = get_bfs() elif search == FAST_DOWNWARD: # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy search_fn = get_fast_downward('eager') else: raise ValueError(search) if planner == INCREMENTAL: plan, _ = incremental_planner(stream_problem, search=search_fn, waves=True, frequency=1, verbose=False) elif planner == FOCUSED: plan, _ = simple_focused(stream_problem, search=search_fn, check_feasible=False, greedy=False, dfs=True, verbose=False) else: raise ValueError(planner) self.assertTrue(plan is not None)
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot, manipulator, base_manip, _ = initialize_openrave(env, ARM, min_delta=.01) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} open_gripper(manipulator) initial_conf = Conf( robot.GetConfigurationValues()[manipulator.GetArmIndices()]) #################### fts_problem = get_problem(env, problem, robot, manipulator, base_manip, bodies, initial_conf) print print fts_problem stream_problem = constraint_to_stripstream(fts_problem) print print stream_problem for stream in stream_problem.cond_streams: print stream, stream.max_level # TODO - why is this slower/less reliable than the other one (extra axioms, eager evaluation?) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager', max_time=10, verbose=False) #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False) env.Lock() plan, universe = solve() env.Unlock() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### if viewer and plan is not None: print SEPARATOR visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan) raw_input('Finish?')
def solve_continuous_tamp(planner, search, visualize, display, verbose=False, deterministic=False): # if deterministic: # set_deterministic() # sample_tamp_problem | sample_grasp_problem | sample_namo_problem tamp_problem = sample_tamp_problem() # stream_problem = compile_problem(tamp_problem, # streams_fn=generative_streams) # constant_streams | implicit_streams | # generative_streams stream_problem = compile_problem(tamp_problem) print stream_problem viewer = None if visualize: viewer = visualize_initial(tamp_problem, stream_problem) raw_input('Continue?') # viewer.save('initial') if search == DEFAULT: search_fn = DEFAULT_SEARCH elif search == BFS: search_fn = get_bfs() elif search == FAST_DOWNWARD: # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy search_fn = get_fast_downward('eager') else: raise ValueError(search) t0 = time() if planner == INCREMENTAL: plan, universe = incremental_planner( stream_problem, search=search_fn, frequency=1, verbose=verbose) # 1 | 20 | 100 | INF elif planner == FOCUSED: #plan, universe = focused_planner(stream_problem, search=search_fn, greedy=False, verbose=verbose) plan, universe = simple_focused(stream_problem, search=search_fn, greedy=True, optimal=False, verbose=verbose) #plan, universe = plan_focused(stream_problem, search=search_fn, greedy=True, optimal=False, verbose=verbose) else: raise ValueError(planner) print SEPARATOR print 'Planner:', planner print 'Search:', search print 'Plan:', convert_plan(plan) print 'Solved:', plan is not None print 'Length:', len(plan) if plan is not None else None print 'Time:', time() - t0 # TODO - sometimes the movement actions, especially for the lazy # algorithm, screw up in the display for some reason... if display and plan is not None: if viewer is None: viewer = visualize_initial(tamp_problem, stream_problem) print '\nExecuting' # TODO - need to return the problem states = get_states(universe, plan) for i, state in enumerate(states): viewer.clear_state() visualize_atoms(viewer, state) raw_input('%s) %s?' % (i, 'Continue' if i != len(states) - 1 else 'Finish')) elif viewer is not None: raw_input('\nFinish?')
def solve_tamp(env, use_focused): use_viewer = env.GetViewer() is not None problem = PROBLEM(env) # TODO: most of my examples have literally had straight-line motions plans robot, manipulator, base_manip, ir_model = initialize_openrave( env, ARM, min_delta=MIN_DELTA) set_manipulator_conf(ir_model.manip, CARRY_CONFIG) bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names} surfaces = {surface.name: surface for surface in problem.surfaces} open_gripper(manipulator) initial_q = Conf(robot.GetConfigurationValues()) initial_poses = { name: Pose(name, get_pose(body)) for name, body in bodies.iteritems() } # TODO: just keep track of the movable degrees of freedom # GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues saver = EnvironmentStateSaver(env) #cfree_pose = cfree_pose_fn(env, bodies) #cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies) #base_generator = base_generator_fn(ir_model) #base_values = base_values_from_full_config(initial_q.value) #goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value) #goal_conf = Conf(goal_values) #return #################### # TODO - should objects contain their geometry cond_streams = [ EasyListGenStream(inputs=[O, P, G], outputs=[Q, T], conditions=[IsPose(O, P), IsGrasp(O, G)], effects=[GraspMotion(O, P, G, Q, T)], generator=sample_grasp_traj_fn( base_manip, ir_model, bodies, CARRY_CONFIG)), EasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_base_motion_fn(base_manip, bodies), order=1, max_level=0), EasyTestStream( inputs=[O, P, O2, P2], conditions=[IsPose(O, P), IsPose(O2, P2)], effects=[CFreePose(P, P2)], test=lambda *args: True, #cfree_pose, eager=True), EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)], test=lambda *args: True), #test=cfree_traj), EasyListGenStream(inputs=[O], outputs=[G], conditions=[], effects=[IsGrasp(O, G)], generator=grasp_generator_fn(bodies, TOP_GRASPS, SIDE_GRASPS, MAX_GRASPS)), EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[], effects=[IsPose(O, P), Stable(P, S)], generator=pose_generator_fn(bodies, surfaces)), #EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)], # effects=[], generator=base_generator), ] #################### constants = [] initial_atoms = [ConfEq(initial_q), HandEmpty()] for name in initial_poses: initial_atoms += body_initial_atoms(name, initial_poses, bodies, surfaces) goal_formula = And( ConfEq(initial_q), *[ OnSurface(obj, surface) for obj, surface in problem.goal_surfaces.iteritems() ]) #goal_formula = ConfEq(goal_conf) #obj, _ = problem.goal_surfaces.items()[0] #goal_formula = And(Holding(obj)) #goal_formula = Holding(obj) # TODO: this cause a bug stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) print stream_problem handles = draw_affine_limits(robot) if use_viewer: for surface in problem.surfaces: surface.draw(env) raw_input('Start?') max_time = INF search_fn = get_fast_downward('eager', max_time=10, verbose=False) if use_focused: solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=max_time) else: solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=10, waves=False, debug=False, max_time=max_time) with env: plan, universe = solve() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### if plan is not None: commands = process_plan(robot, bodies, plan) if use_viewer: print SEPARATOR saver.Restore() visualize_solution(commands, step=False) raw_input('Finish?')
GREEDY, DFS = True, True PLANNERS = [ ('incremental', lambda p, s: incremental_planner(p, search=s, frequency=1, verbose=False) ), # Focused planners ('focused', lambda p, s: focused_planner(p, search=s, check_feasible=False, greedy=GREEDY, dfs=DFS, verbose=True)), ('simple_focused', lambda p, s: simple_focused(p, search=s, check_feasible=False, greedy=GREEDY, dfs=DFS, verbose=False)), ('plan_focused', lambda p, s: plan_focused(p, search=s, check_feasible=False, greedy=GREEDY, dfs=DFS, verbose=False)), ] def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('--planner', help='planner name.', default=None) parser.add_argument('--search', help='search name.', default=None)
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot, manipulator, base_manip, _ = initialize_openrave(env, ARM, min_delta=.01) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} all_bodies = bodies.values() assert len({body.GetKinematicsGeometryHash() for body in all_bodies }) == 1 # Assuming all objects has the same geometry body1 = all_bodies[-1] # Generic object 1 body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2 grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else [] poses = problem.known_poses if problem.known_poses else [] open_gripper(manipulator) initial_conf = Conf( robot.GetConfigurationValues()[manipulator.GetArmIndices()]) #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[Q, T], conditions=[], effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj_fn( env, manipulator, body1, all_bodies)), # Move trajectory EasyListGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion_fn( manipulator, base_manip, all_bodies), order=1, max_level=0), EasyListGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[], effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion_fn( manipulator, base_manip, body1, all_bodies), order=1, max_level=0), # Collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)], test=cfree_pose_fn(env, body1, body2), eager=True), EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)], test=cfree_traj_fn(env, manipulator, body1, body2, all_bodies)), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ConfEq(initial_conf), HandEmpty(), ] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()] goal_formula = And( ConfEq(initial_conf), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager', max_time=10, verbose=False) #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False) env.Lock() plan, universe = solve() env.Unlock() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### if viewer and plan is not None: print SEPARATOR visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan) raw_input('Finish?')
def solve_countable_tamp(planner, search, visualize, display, verbose=False): #tamp_problem = get_grasp_problem(p=3) # 10 | 1000 #tamp_problem = get_distract_problem(p=3) # 10 | 1000 #tamp_problem = get_invert_problem(p=3) tamp_problem = get_shift_problem(p=3) #tamp_problem = get_distract_place_problem(p=40) #stream_problem = compile_problem(tamp_problem, stream_fn=generative_streams) # constant_streams | implicit_streams | constant_streams stream_problem = compile_problem(tamp_problem) print stream_problem #stream_problem.operators = stream_problem.convert_axioms_to_effects() #stream_problem.replace_axioms() #for action in stream_problem.operators: # print action.pddl(True) viewer = None if visualize: from stripstream.pddl.examples.countable_tamp.countable_tamp_viewer import CountableTMPViewer viewer = CountableTMPViewer(1, NUM_POSES, title='State', height=250) visualize_atoms(viewer, stream_problem.initial_atoms) raw_input('Continue?') verbose_search = False if search == DEFAULT: search_fn = DEFAULT_SEARCH elif search == BFS: search_fn = get_bfs() elif search == FAST_DOWNWARD: search_fn = get_fast_downward( 'eager', verbose=verbose_search ) # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy elif search == FAST_FORWARD: search_fn = get_fast_forward(verbose=verbose_search) elif search == LAPKT: stream_problem.replace_axioms() # TODO - can I automate this process? search_fn = get_lakpt(verbose=verbose_search) #elif search == PYPLANNERS: # search_fn = get_pyplanners(verbose=False) else: raise ValueError(search) t0 = time() if planner == INCREMENTAL: plan, universe = incremental_planner( stream_problem, search=search_fn, frequency=1, verbose=verbose) # 1 | 20 | 100 | INF elif planner == FOCUSED: #plan, universe = focused_planner(stream_problem, search=search_fn, greedy=False, verbose=verbose) plan, universe = simple_focused(stream_problem, search=search_fn, greedy=True, dfs=True, verbose=verbose) #elif planner == 'hierarchy': # selector = first_selector # first_selector | all_selector # plan, universe = replan_hierarchy(stream_problem, selector=selector, execute=True, verbose=verbose) #elif planner == 'simultaneous': # plan, universe = simultaneous(stream_problem, frequency=frequency, max_time=max_time) #elif planner == 'progression': # plan, universe = progression(stream_problem, max_time=max_time) else: raise ValueError(planner) print SEPARATOR print 'Planner:', planner print 'Search:', search print 'Plan:', convert_plan(plan) print 'Solved:', plan is not None print 'Length:', len(plan) if plan is not None else None print 'Time:', time() - t0 #universe.print_statistics() #universe.print_domain_statistics() #print SEPARATOR #print_plan_stats(plan, universe) if display and plan is not None: if viewer is None: # TODO - unify this across the two from stripstream.pddl.examples.countable_tamp.countable_tamp_viewer import CountableTMPViewer viewer = CountableTMPViewer(1, NUM_POSES, title='State', height=250) print '\nExecuting' states = get_states(universe, plan) for i, state in enumerate(states): viewer.clear() viewer.draw_environment() visualize_atoms(viewer, state) raw_input('%s) %s?' % (i, 'Continue' if i != len(states) - 1 else 'Finish')) elif viewer is not None: raw_input('\nFinish?')
def solve_tamp(env): viewer = env.GetViewer() is not None #problem = dantam(env) problem = dantam2(env) #problem = move_several_4(env) robot = env.GetRobots()[0] set_base_values(robot, (-.75, .2, -math.pi/2)) initialize_openrave(env, 'leftarm') manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) #USE_GRASP_APPROACH = GRASP_APPROACHES.SIDE USE_GRASP_APPROACH = GRASP_APPROACHES.TOP #USE_GRASP_TYPE = GRASP_TYPES.TOUCH USE_GRASP_TYPE = GRASP_TYPES.GRASP bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()} assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry all_bodies = bodies.values() body1 = all_bodies[-1] body2 = all_bodies[-2] if len(bodies) >= 2 else body1 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1] poses = problem.known_poses if problem.known_poses else [] ################################################## def enable_all(enable): for body in all_bodies: body.Enable(enable) def collision_free(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def grasp_env_cfree(mt, g): enable_all(False) # TODO - base config? body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): return False return True def grasp_pose_cfree(mt, g, p): enable_all(False) # TODO - base config? body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): return False return True ################################################## """ class CollisionStream(Stream): # TODO - could make an initial state version of this that doesn't need pose2 def get_values(self, **kwargs): self.enumerated = True pose1, pose2 = map(get_value, self.inputs) if collision_free(pose1, pose2): return [PoseCFree(pose1, pose2)] return [] #return [Movable()] # NOTE - only make movable when it fails a collision check CheckedInitial = Pred(POSE) # How should this be handled? The planner will need to revisit on the next state anyways class EnvCollisionStream(Stream): # NOTE - I could also make an environment OBJECT which I mutate. I'm kind of doing that now movable = [] def get_values(self, **kwargs): self.enumerated = True pose, = map(get_value, self.inputs) results = [CheckedInitial(pose)] for obj, pose2 in problem.initial_poses.iteritems(): if obj not in self.movable and collision_free(pose, pose2): pass #results.append(PoseCFree(pose, pose2)) else: self.movable.append(obj) results.append(PoseEq(obj, pose2)) #results.append(Movable(obj)) if results: pass # NOTE - I could make this fail if there is a collision # I could prevent the binding by directly adding CheckedInitial to the universe # In general, I probably can just mutate the problem however I see fit here return results """ ################################################## # NOTE - can do pose, approach manip, true approach traj, motion plan # NOTE - can make something that produces approach trajectories def get_manip_vector(pose, grasp): manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) #enable_all(False) #if manipulator.CheckEndEffectorCollision(manip_trans): # return None return ManipVector(manip_trans, approach_vector) def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp enable_all(False) set_base_values(robot, base_conf.value) body1.Enable(True) set_pose(body1, pose.value) manip_vector = get_manip_vector(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions #print manipulator.CheckEndEffectorCollision(manip_trans) if grasp_config is not None: yield [Config(grasp_config)] #traj = workspace_traj_helper(base_manip, approach_vector) def sample_grasp_traj(pose, grasp, base_conf): enable_all(False) set_base_values(robot, base_conf.value) body1.Enable(True) set_pose(body1, pose.value) manip_vector = get_manip_vector(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector) if grasp_config is None: return yield [(Config(grasp_traj.end()), grasp_traj)] ################################################## # NOTE - can either include the held object in the traj or have a special condition that not colliding def sample_arm_traj(mq1, mq2, bq): # TODO - need to add holding back in yield None #enable_all(False) #with robot: # set_base_values(robot, bq.value) # pass # TODO - does it make sense to make a new stream for the biasing or to continuously just pass things # I suppose I could cache the state or full plan as context class MotionStream(Stream): # TODO - maybe make this produce the correct values num = 0 #def get_values(self, **kwargs): # self.enumerated = True # mq1, mq2, bq = map(get_value, self.inputs) # #mt = None # mt = MotionStream.num # MotionStream.num += 1 # Ensures all are unique # return [ManipMotion(mq1, mq2, bq, mt)] def sample_motion_plan(self, mq1, mq2, bq): set_manipulator_values(manipulator, mq1.value) set_base_values(robot, bq.value) return motion_plan(env, cspace, mq2.value, self_collisions=True) def get_values(self, universe, dependent_atoms=set(), **kwargs): mq1, mq2, bq = map(get_value, self.inputs) collision_atoms = filter(lambda atom: atom.predicate in [MTrajGraspCFree, MTrajPoseCFree], dependent_atoms) collision_params = {atom: atom.args[0] for atom in collision_atoms} grasp = None for atom in collision_atoms: if atom.predicate is MTrajGraspCFree: assert grasp is None # Can't have two grasps _, grasp = map(get_value, atom.args) placed = [] for atom in collision_atoms: if atom.predicate is MTrajPoseCFree: _, pose = map(get_value, atom.args) placed.append(pose) #placed, grasp = [], None print grasp, placed if placed or grasp: assert len(placed) <= len(all_bodies) # How would I handle many constraints on the same traj? enable_all(False) for b, p in zip(all_bodies, placed): b.Enable(True) set_pose(b, p.value) if grasp: assert grasp is None or len(placed) <= len(all_bodies)-1 set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans)) robot.Grab(body1) mt = self.sample_motion_plan(mq1, mq2, bq) if grasp: robot.Release(body1) if mt: self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps... # TODO - could always hash this trajectory for the current set of constraints bound_collision_atoms = [atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms] #bound_collision_atoms = [] return [ManipMotion(mq1, mq2, bq, mt)] + bound_collision_atoms raise ValueError() enable_all(False) mt = self.sample_motion_plan(mq1, mq2, bq) if mt: return [ManipMotion(mq1, mq2, bq, mt)] return [] ################################################## cond_streams = [ #MultiEasyGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses), #EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[], # effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=sample_arm_traj), ClassStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[], effects=[ManipMotion(MQ, MQ2, BQ, MT)], StreamClass=MotionStream, order=1, max_level=0), #MultiEasyGenStream(inputs=[P, G, BQ], outputs=[MQ], conditions=[], # effects=[Kin(P, G, BQ, MQ)], generator=sample_ik), EasyListGenStream(inputs=[P, G, BQ], outputs=[MQ, GT], conditions=[], effects=[GraspTraj(P, G, BQ, MQ, GT)], generator=sample_grasp_traj), #EasyTestStream(inputs=[O, P, T], conditions=[], effects=[CFree(O, P, T)], # test=collision_free, eager=True), EasyTestStream(inputs=[P, P2], conditions=[], effects=[PoseCFree(P, P2)], test=collision_free, eager=True), #ClassStream(inputs=[P, P2], conditions=[], outputs=[], # effects=[PoseCFree(P, P2)], StreamClass=CollisionStream, eager=True), EasyTestStream(inputs=[MT, P], conditions=[], effects=[MTrajPoseCFree(MT, P)], test=lambda mt, p: True, eager=True), EasyTestStream(inputs=[MT, G], conditions=[], effects=[MTrajGraspCFree(MT, G)], test=lambda mt, g: True, eager=True), EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[MTrajGraspPoseCFree(MT, G, P)], test=lambda mt, g, p: True, eager=True), #ClassStream(inputs=[P], conditions=[], outputs=[], # effects=[CheckedInitial(P)], StreamClass=EnvCollisionStream), ] ################################################## constants = map(GRASP, grasps) + map(POSE, poses) initial_full = Config(get_full_config(robot)) initial_base = get_base_conf(initial_full) initial_manip = get_arm_conf(manipulator, initial_full) initial_atoms = [ BaseEq(initial_base), ManipEq(initial_manip), HandEmpty(), ] + [ PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems() ] goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, operators, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager') #plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF #plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF from misc.profiling import run_profile, str_profile #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=True) #with env: env.Lock() (plan, universe), prof = run_profile(solve) env.Unlock() print SEPARATOR universe.print_domain_statistics() universe.print_statistics() print SEPARATOR print str_profile(prof) print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i+1, action, args else: print 'Failure' ################################################## def step_path(traj): #for j, conf in enumerate(traj.path()): for j, conf in enumerate([traj.end()]): set_manipulator_values(manipulator, conf) raw_input('%s/%s) Step?'%(j, len(traj.path()))) if viewer and plan is not None: print SEPARATOR # Resets the initial state open_gripper2(manipulator) set_base_values(robot, initial_base.value) set_manipulator_values(manipulator, initial_manip.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_arm': mq1, mq2, bq, mt = args #set_manipulator_values(manipulator, mq2.value) step_path(mt) elif action.name == 'pick': #o, p, q, mq, bq = args o, p, g, mq, bq, gt = args step_path(gt.reverse()) #grasp_gripper2(manipulator, g) # NOTE - g currently isn't a real grasp robot.Grab(bodies[o]) step_path(gt) elif action.name == 'place': #o, p, q, mq, bq = args o, p, g, mq, bq, gt = args step_path(gt.reverse()) robot.Release(bodies[o]) #open_gripper2(manipulator) step_path(gt) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] set_base_values(robot, (-.75, .2, -math.pi/2)) initialize_openrave(env, ARM) manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()} assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry all_bodies = bodies.values() body1 = all_bodies[-1] body2 = all_bodies[-2] if len(bodies) >= 2 else body1 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1] poses = problem.known_poses if problem.known_poses else [] open_gripper2(manipulator) initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot))) def enable_all(enable): for body in all_bodies: body.Enable(enable) #################### def cfree_pose_pose(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def cfree_gtraj_pose(gt, p): return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(gt, gt.grasp, p) #################### def cfree_mtraj_grasp(mt, g): enable_all(False) body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): print 'cfree_mtraj_grasp' return False return True def cfree_mtraj_pose(mt, p): enable_all(False) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) if env.CheckCollision(robot, body2): print 'cfree_mtraj_pose' return False return True def cfree_mtraj_grasp_pose(mt, g, p): enable_all(False) body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): print 'cfree_mtraj_grasp_pose' return False return True #################### def sample_grasp_traj(pose, grasp): enable_all(False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_trans) if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) robot.Grab(body1) grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp yield [(Config(grasp_traj.end()), grasp_traj)] def sample_manip_motion(mq1, mq2): enable_all(False) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if not mt: return yield [(mt,)] #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[], effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj), # Move trajectory EasyListGenStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[], effects=[ManipMotion(MQ, MQ2, MT)], generator=sample_manip_motion, order=1, max_level=0), # Pick/place collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)], test=cfree_pose_pose, eager=True), EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)], test=cfree_gtraj_pose), # Move collisions EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)], test=cfree_mtraj_pose), EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)], test=cfree_mtraj_grasp), EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)], test=cfree_mtraj_grasp_pose), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ManipEq(initial_manip), HandEmpty(), ] + [ PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems() ] goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager', max_time=10) #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False) env.Lock() plan, universe = solve() env.Unlock() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i+1, action, args else: print 'Failure' #################### def step_path(traj): #for j, conf in enumerate(traj.path()): for j, conf in enumerate([traj.end()]): set_manipulator_values(manipulator, conf) raw_input('%s/%s) Step?'%(j, len(traj.path()))) if viewer and plan is not None: print SEPARATOR # Resets the initial state set_manipulator_values(manipulator, initial_manip.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': mq1, mq2, mt = args step_path(mt) elif action.name == 'pick': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Grab(bodies[o]) step_path(gt) elif action.name == 'place': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Release(bodies[o]) step_path(gt) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] set_base_values(robot, (-.75, .2, -math.pi / 2)) initialize_openrave(env, ARM) manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} geom_hashes = { body.GetKinematicsGeometryHash() for body in bodies.values() } assert len( geom_hashes) == 1 # NOTE - assuming all objects has the same geometry all_bodies = bodies.values() body1 = all_bodies[-1] body2 = all_bodies[-2] if len(bodies) >= 2 else body1 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1] poses = problem.known_poses if problem.known_poses else [] open_gripper2(manipulator) initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot))) def enable_all(enable): for body in all_bodies: body.Enable(enable) #################### def cfree_pose_pose(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def cfree_gtraj_pose(gt, p): return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose( gt, gt.grasp, p) #################### def cfree_mtraj_grasp(mt, g): enable_all(False) body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): return False return True def cfree_mtraj_pose(mt, p): enable_all(False) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) if env.CheckCollision(body2): return False return True def cfree_mtraj_grasp_pose(mt, g, p): enable_all(False) body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): return False return True #################### def sample_grasp_traj(pose, grasp): enable_all(False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_trans) if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) robot.Grab(body1) grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp yield [(Config(grasp_traj.end()), grasp_traj)] class MotionStream(Stream): def get_values(self, universe, dependent_atoms=set(), **kwargs): mq1, mq2 = map(get_value, self.inputs) collision_atoms = filter( lambda atom: atom.predicate in [CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms) collision_params = {atom: atom.args[0] for atom in collision_atoms} grasp = None for atom in collision_atoms: if atom.predicate is CFreeMTrajGrasp: assert grasp is None # Can't have two grasps _, grasp = map(get_value, atom.args) placed = [] for atom in collision_atoms: if atom.predicate is CFreeMTrajPose: _, pose = map(get_value, atom.args) placed.append(pose) #placed, grasp = [], None #print grasp, placed if placed or grasp: assert len(placed) <= len(all_bodies) enable_all(False) for b, p in zip(all_bodies, placed): b.Enable(True) set_pose(b, p.value) if grasp: assert grasp is None or len(placed) <= len(all_bodies) - 1 set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans)) robot.Grab(body1) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if grasp: robot.Release(body1) if mt: self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps... # TODO - could always hash this trajectory for the current set of constraints bound_collision_atoms = [ atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms ] #bound_collision_atoms = [] return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms raise ValueError() enable_all(False) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if mt: return [ManipMotion(mq1, mq2, mt)] return [] #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[], effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj), # Move trajectory ClassStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[], effects=[ManipMotion(MQ, MQ2, MT)], StreamClass=MotionStream, order=1, max_level=0), # Pick/place collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)], test=cfree_pose_pose, eager=True), EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)], test=cfree_gtraj_pose), # Move collisions EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)], test=cfree_mtraj_pose), EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)], test=cfree_mtraj_grasp), EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)], test=cfree_mtraj_grasp_pose), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ManipEq(initial_manip), HandEmpty(), ] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()] goal_formula = And( ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager') #plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF #plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF from misc.profiling import run_profile, str_profile #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False) #with env: env.Lock() (plan, universe), prof = run_profile(solve) env.Unlock() print SEPARATOR universe.print_domain_statistics() universe.print_statistics() print SEPARATOR print str_profile(prof) print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### def step_path(traj): #for j, conf in enumerate(traj.path()): for j, conf in enumerate([traj.end()]): set_manipulator_values(manipulator, conf) raw_input('%s/%s) Step?' % (j, len(traj.path()))) if viewer and plan is not None: print SEPARATOR # Resets the initial state set_manipulator_values(manipulator, initial_manip.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': mq1, mq2, mt = args step_path(mt) elif action.name == 'pick': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Grab(bodies[o]) step_path(gt) elif action.name == 'place': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Release(bodies[o]) step_path(gt) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')