def main():
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), .2, .2)]
goal = create_box((.9, .9), .2, .2)
constraint_problem = create_problem(goal, obstacles)
print
print constraint_problem
stream_problem = constraint_to_stripstream(constraint_problem)
print
print stream_problem
search = get_fast_downward('astar') # dijkstra | astar
plan, _ = incremental_planner(stream_problem,
search=search,
frequency=25,
waves=False)
plan = convert_plan(plan)
print 'Plan:', plan
draw_solution(plan, goal, obstacles)
raw_input('Finish?')
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 main():
"""
Creates and solves the 1D task and motion planning FTSProblem problem.
"""
constraint_problem = create_problem()
print
print constraint_problem
stream_problem = constraint_to_stripstream(constraint_problem)
print
print stream_problem
# 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
search_fn = get_fast_downward('eager')
plan, _ = incremental_planner(stream_problem, search=search_fn)
print
print 'Plan:', convert_plan(plan)
def solve_constraint_tamp(env, manip_problem, display, focused, movie, execute = True, verbose=False):
#RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug)
manip_problem.set_viewer(env)
manip_problem.load_env(env)
oracle = ManipulationOracle(manip_problem, env)
fts_problem = tamp_problem(oracle)
print
print fts_problem
stream_problem = constraint_to_stripstream(fts_problem)
print
print stream_problem
oracle.draw_goals() # NOTE - this must be after compile_problem to ensure the goal_poses convert
if is_viewer_active(oracle.env):
raw_input('Start?')
search = get_fast_downward('eager') # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
planner = FOCUSED if focused else INCREMENTAL
print SEPARATOR
t0 = time()
if planner == FOCUSED:
# TODO - why do I need make_stream_instances=True here?
plan, _ = focused_planner(stream_problem, search=search, check_feasible=False, greedy=False, dfs=True, verbose=verbose)
elif planner == INCREMENTAL:
frequency = 1 # 1 | 20 | 100 | INF
# NOTE - the versions I submitted to RSS had implicitly waves=False
plan, _ = incremental_planner(stream_problem, search=search, frequency=frequency, waves=True, verbose=verbose)
else:
raise ValueError('Must specify a planner')
print SEPARATOR
print 'Planner:', planner
print 'Search:', FAST_DOWNWARD
print 'Solved:', plan is not None
print 'Length:', len(plan) if plan is not None else None
print 'Time:', time() - t0
if plan is None:
return
# NOTE - need to set constants
#PRE_SMOOTH_TRAJECTORIES = True
#REPLAN_TRAJECTORIES = True
#SMOOTH_TRAJECTORIES = identity_trajectories # identity_trajectories | smooth_trajectories
#print oracle.robot.GetAffineTranslationMaxVels(), oracle.robot.GetAffineRotationAxisMaxVels()
#oracle.robot.SetAffineTranslationMaxVels(5*oracle.robot.GetAffineTranslationMaxVels())
#SetAffineRotationAxisMaxVels(oracle.robot) # SetAffineRotationAxisMaxVels | SetAffineRotation3DMaxVels
#SetAffineTranslationMaxVels(oracle.robot)
# SetActiveDOFVelocities, SetDOFVelocities
# SetDOFAccelerationLimits
# GetActiveDOFMaxAccel but not SetActiveDOFMaxAccel
#oracle.robot.SetActiveDOFVelocities(5*oracle.robot.GetActiveDOFVelocities())
#action.base_trajs[0].cspace.set_active()
#indices = oracle.robot.GetActiveDOFIndices()
#print indices
#print oracle.robot.GetActiveDOFMaxAccel(indices)
#oracle.robot.SetActiveDOFMaxAccel(5*oracle.robot.GetActiveDOFMaxAccel(indices), indices)
# NOTE - not sure why the focused algorithm isn't working that well here anymore
images_directory = None
if movie:
directory_path = get_directory_path(manip_problem.name, 'fts')
images_directory = os.path.join(directory_path, 'images/')
print 'Save:', images_directory
if display or movie:
Plan = namedtuple('Plan', ['start', 'operators'])
start = convert_state(oracle, get_single_state(fts_problem.initial_state))
executables = convert_plan(oracle, plan)
if execute:
if movie:
from misc.utils import launch_quicktime
launch_quicktime() # TODO - produces missing value if quicktime is already enabled
execute_plan(Plan(start, executables), oracle, pause=False)
else:
visualize_plan(Plan(start, executables), oracle, display=display, save=images_directory)