def run_observable_2():
problem_fn = test8 # test0 | test1 | test8
env, start, goal = problem_fn()
print
print convert_start(env, start)
print convert_goal(goal)
#operators = makeOperators(glob.failProbs)
#for name, operator in operators.iteritems():
# print 'Name:', name
# print 'Args:', [arg for i, arg in enumerate(operator.args) if i not in operator.ignorableArgs]
# print 'Pre:', operator.preconditions
# print 'Eff:', operator.results
# print 'Side Eff:', operator.sideEffects
# print
#problem = make_problem(convert_start(env, start), goal)
problem = make_problem(maximum_likelihood_obs(start.details), goal)
#problem = make_problem(sample_obs(start.details), goal)
#problem = make_problem(sample_consistent_obs(start.details), goal)
print problem
search = get_fast_downward(
'dijkstra'
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, _ = incremental_planner(problem, search=search, frequency=1)
print convert_plan(plan)
def fn(belief):
problem = compile_belief(belief, goal)
search = get_fast_downward(
'eager'
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, _ = incremental_planner(problem, search=search, frequency=1)
print convert_plan(plan)
# TODO - need to apply axioms to do this
# TODO - need to also be careful that constants contains all the necessary constants if using quantifiers
"""
if goal_formula.holds(initial_atoms, constants):
return []
if bias_plan is not None:
# NOTE - testing if any subplan works is NP-Complete
#candidate_plans = [bias_plan[1:], bias_plan]
candidate_plans = [bias_plan[1:]]
for candidate_plan in candidate_plans:
instantiated_plan = [action.instantiate(args) for action, args in candidate_plan]
if len(instantiated_plan) >= 1:
if not isinstance(instantiated_plan[0], RefinableOperator) and \
is_valid_plan(initial_atoms, goal_formula, constants, instantiated_plan):
return candidate_plan
"""
plan = convert_plan(plan)
if plan is None or not plan:
return None
online_policy.last_plan = plan # TODO - use the next action on the last plan if it still is valid
action, params = plan[0]
return action.executable(operators, params)
def run_observable():
problem_fn = test8 # test0 | test1 | test8
env, start, goal = problem_fn()
global operatorDict
operatorDict = makeOperators(glob.failProbs)
print
for name, operator in operatorDict.iteritems():
print 'Name:', name
print 'Args:', [
arg for i, arg in enumerate(operator.args)
if i not in operator.ignorableArgs
]
print 'Pre:', operator.preconditions
print 'Eff:', operator.results
print 'Side Eff:', operator.sideEffects
print
problem = observable_problem(env, start, goal)
search = get_fast_downward(
'eager'
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, _ = incremental_planner(problem, search=search, frequency=1)
print convert_plan(plan)
def solve_single(env):
use_viewer = (env.GetViewer() is not None)
robot, occupancy = load_env(env)
arm = robot.GetActiveManipulator()
#get_problem = get_problem_1_0
#get_problem = get_problem_1_1
#get_problem = get_problem_1_2
get_problem = get_problem_1_3
#get_problem = get_problem_2_1
#get_problem = get_problem_2_2
#get_problem = get_problem_3_2
#get_problem = get_problem_4
object_meshes = get_object_meshes(MESHES_DIR)
belief, task = get_problem(object_meshes)
print SEPARATOR
print 'Belief:', belief
print 'Task:', task
avaliable_actions = None
#avaliable_actions = ['pick']
stream_problem, belief_from_name = get_ground_problem(
arm,
object_meshes,
belief,
task,
occupancy,
available_actions=avaliable_actions)
print stream_problem
env.Lock()
plan, universe = incremental_planner(stream_problem,
search=get_fast_downward(
'astar', verbose=False),
optimal=False,
frequency=1,
waves=True,
debug=False,
max_time=5.0)
env.Unlock()
print SEPARATOR
length = plan_length(universe, plan)
cost = plan_cost(universe, plan)
plan = convert_plan(plan)
print 'Plan: {}\nLength: {} | Cost: {}'.format(plan, length, cost)
if use_viewer and (plan is not None):
trajectories = process_plan(arm, belief_from_name, plan)
print trajectories
raw_input('Start?')
while True:
with EnvironmentStateSaver(env):
simulate_trajectories(env, trajectories)
response = get_input('Replay?', ('y', 'n'))
if response != 'y':
break
# simulate_single(arm, plan)
if use_viewer:
raw_input('Finish?')
def flat_plan(initial, goal):
#problem = create_abstract_problem(initial, goal)
problem = create_real_problem(initial, goal)
print problem
search = get_fast_downward(config='ff-astar', remove=False, verbose=False)
plan, _ = incremental_planner(problem, search=search)
return convert_plan(plan)
def main():
"""
Creates and solves the 1D car STRIPStream problem.
"""
problem = create_problem(p_goal=5) # 0 | 1 | 2 | 5 | 10
#print problem, '\n'
#problem.replace_axioms()
#print problem
#plan, _ = progression(problem, frequency=10, useful_actions=True, verbose=False)
#plan, _ = simultaneous(problem, frequency=10, verbose=False, debug=False)
#plan, _ = simultaneous_strips(problem, frequency=10, verbose=False, debug=False)
from misc.profiling import run_profile, str_profile
#solve = lambda: progression(problem, frequency=INF, useful_actions=True, verbose=False) # frequency = 10 | INF
#solve = lambda: simultaneous(problem, frequency=10, verbose=False, debug=False)
#solve = lambda: simultaneous_strips(problem, frequency=10, verbose=False, debug=False)
#(plan, _), prof = run_profile(solve)
#print str_profile(prof)
search = get_fast_downward('eager') # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, _ = incremental_planner(problem, search=search, frequency=1, waves=True)
#plan, _ = incremental_planner(problem, search=search, frequency=100, waves=False)
#plan, _ = focused_planner(problem, search=search, greedy=False)
print
print 'Plan:', convert_plan(plan)
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 solve_online(env):
# TODO: simulate the work for real
robot, occupancy = load_env(env)
arm = robot.GetActiveManipulator()
available_actions = ['pick', 'place', 'move_head', 'move_base', 'scan_room', 'scan_table', 'register']
get_world = get_lis_world # get_world_1 | get_world_2 | get_world_3 | get_world_4 | get_world_5 | get_world_6
object_meshes = get_object_meshes(MESHES_DIR)
world, task = get_world(object_meshes)
#add_world(robot, object_meshes, world)
initialize_world(robot, object_meshes, world)
initial_tform = robot.GetTransform()
print world
print task
#prior = unknown(world) # TODO: start off not facing the object
prior = initial_surface_belief(world)
belief = belief_from_task(prior, task)
#belief = update_belief(robot, world, belief, []) # Updates with initial observations
#belief = observable_belief(robot, world)
history = []
while True:
#belief = belief_from_task(belief, task)
print SEPARATOR
print belief
saver = EnvironmentStateSaver(env)
handles = [
draw_circle_2d(env, get_point(robot), MAX_LOOK_DISTANCE, color=np.array([1, 0, 0, 1])),
draw_circle_2d(env, get_point(robot), MAX_REG_DISTANCE, color=np.array([0, 1, 0, 1])),
draw_affine_limits(robot)] + draw_viewcone(robot, z=2.5)
# TODO: draw view cone
with env:
#plan = solve_tamp(arm, object_meshes, belief, task, occupancy,
# available_actions=available_actions)
plan = solve_hierarchical_tamp(arm, object_meshes, belief, task, occupancy,
initial_tform, available_actions=available_actions)
# TODO: some sort of shift when this happens
if plan is None:
raise RuntimeError('Unable to find a plan')
if not plan:
break
plan_prefix = get_plan_prefix(convert_plan(plan))
print plan_prefix
trajectories = [process_plan(arm, [action]) for action in plan_prefix]
print trajectories
saver.Restore()
raw_input('Execute?')
for action, trajs in zip(plan_prefix, trajectories):
simulate_trajectories(env, trajs, time_step=0.01, realtime=False)
belief = update_belief(robot, world, belief, [action])
#belief = observable_belief(robot, world)
history += plan_prefix
raw_input('Finish?')
def main():
"""
Creates and solves a blocksworld STRIPStream problem.
"""
problem = create_problem()
print problem
plan, _ = incremental_planner(problem)
print
print 'Plan:', convert_plan(plan)
def main(): """ Creates and solves the 1D task and motion planning STRIPStream problem. """ problem = create_problem() print problem plan, _ = incremental_planner(problem) print print 'Plan:', convert_plan(plan)
def replan(initial, goal):
state = initial
history = []
while True:
problem = create_real_problem(state, goal)
search = get_fast_downward(config='ff-astar',
remove=False,
verbose=False)
plan, _ = incremental_planner(problem, search=search)
plan = convert_plan(plan)
print_plan(plan)
raw_input('awefwaef')
if plan is None:
return None
if not plan:
return convert_plan(history)
plan_prefix = plan[:1]
state = execute(state, plan_prefix)
history += plan_prefix
print SEPARATOR
def main():
#planning_problem = get_problem()
planning_problem = create_problem()
print planning_problem
t0 = time()
plan, _ = incremental_planner(planning_problem,
frequency=1) # 20 | 100 | INF
print
print 'Plan:', convert_plan(plan)
print 'Time:', time() - t0
def main():
"""
Creates and solves the 1D task and motion planning STRIPStream problem.
"""
problem = create_problem()
print problem
search = get_fast_downward('eager', max_time=10, verbose=True) # dijkstra | astar
plan, _ = incremental_planner(problem, search=search, max_time=30, optimal=False)
print
print 'Plan:', convert_plan(plan)
def update_best_plan(universe, search, best_plan, best_cost, max_time):
if max_time is None:
return best_plan, best_cost
t0 = time()
plan = search(universe, max_time, best_cost)
universe.search_time += time() - t0
if plan is not None:
cost = plan_cost(universe, plan) # plan_cost | plan_length
if cost < best_cost:
best_plan, best_cost = plan, cost
if universe.verbose:
print best_cost, convert_plan(best_plan)
return best_plan, best_cost
def hierarchy(initial, goal):
abstract_problem = create_abstract_problem(initial, goal)
abstract_plan, abstract_universe = incremental_planner(
abstract_problem,
search=get_fast_downward(config='ff-astar',
remove=False,
verbose=False))
print_plan(convert_plan(abstract_plan))
goal_sequence = preimage_sequence(abstract_universe, abstract_plan)
#goal_sequence = [action.instantiate(args).condition for action, args in abstract_plan[1:]] + \
# [abstract_problem.goal_literals]
# TODO: this doesn't really work because it doesn't accomplish the necessary subgoals
#goal_sequence = list(And(*atoms) for atoms in get_states(abstract_universe, abstract_plan))[1:]
# TODO: note that the subgoal actually has quite a few literals but most of them are satisfied
# Each abstract operator reflects out confidence that each abstract operator represents the ability to go between conditions
state = initial
history = []
for goal_formula in goal_sequence:
print SEPARATOR
problem = create_real_problem(state, goal)
problem.goal_literals = goal_formula
print state
print problem.goal_literals
plan, universe = incremental_planner(problem,
search=get_fast_downward(
config='ff-astar',
remove=False,
verbose=False))
print_plan(convert_plan(plan))
if plan is None:
return None
state = execute(state, convert_plan(
plan)) # TODO: would be better if directly applied the actions
history += plan
return convert_plan(history)
def main():
"""
Creates and solves the Tsiolkovsky rocket problem.
"""
problem = create_problem(goal_p=100)
print problem
#plan, _ = simultaneous(problem, frequency=10, verbose=False, debug=False)
search = get_fast_downward(
'eager'
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, _ = incremental_planner(problem, search=search, frequency=1)
print
print 'Plan:', convert_plan(plan)
def main(argv):
testNum = int(argv[0]) if argv else 0
print 'Test number', testNum
print 'Args = ', testArgs[testNum]
problem = create_problem(**testArgs[testNum])
# dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
search = get_fast_downward('eager')
#plan, _ = incremental_planner(problem, search=search,
# frequency=10, waves=False)
plan, _ = focused_planner(problem, search=search, greedy=False)
print '\nPlan:', convert_plan(plan)
def main():
"""
Creates and solves the 1D task and motion planning STRIPStream problem.
"""
pr = cProfile.Profile()
pr.enable()
problem = create_problem()
print problem
search = get_fast_downward('eager', max_time=10,
verbose=True) # dijkstra | astar
#plan, _ = incremental_planner(problem, search=search, max_time=30, optimal=False)
#plan, _ = simple_focused(problem, search=search)
plan, _ = signed_focused(problem, search=search, verbose=True)
print
print 'Plan:', convert_plan(plan)
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10) # tottime
def main():
"""
Creates and solves the 1D car STRIPStream problem.
"""
problem = create_problem(p_goal=10)
#plan, _ = simultaneous(problem, frequency=10)
plan, _ = progression(problem,
frequency=10,
useful_actions=True,
verbose=False)
search = get_fast_downward(
'eager'
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
#plan, _ = incremental_planner(problem, search=search, frequency=1, waves=True)
#plan, _ = focused_planner(problem, search=search, greedy=False)
print
print 'Plan:', convert_plan(plan)
def fn(belief):
problem = observable_problem(belief, goal)
#print problem
# NOTE - the cost sensitivity starts to factor into this...
# NOTE - max cost prevents extremely large cost actions
search = get_fast_downward('astar', verbose=False, max_cost=max_cost) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, universe = incremental_planner(problem, search=search, frequency=1)
cost = plan_cost(universe, plan)
plan = convert_plan(plan)
print 'Plan:', plan
print 'Cost:', cost
print 'Length:', len(plan)
if plan is None or not plan:
return None
action, params = plan[0]
print 'Action:', action, params
print
return OPERATOR_MAP[action.name](operators, *params)
def policy(estimator):
problem, command_from_action = compile_problem(estimator, task)
print problem
plan, universe = incremental_planner(
problem,
search=get_fast_downward('astar'),
frequency=100,
waves=False,
optimal=True,
debug=False
) # TODO: need to make sure I do all costs (they are always eager)
print 'Plan: {}\nLength: {} | Cost: {}'.format(
convert_plan(plan), plan_length(universe, plan),
plan_cost(universe, plan))
if plan is None:
raise RuntimeError('Unable to find a plan')
if not plan:
return None
return command_from_action(plan[0])
def main():
"""
Creates and solves a generic non-holonomic motion planning problem
"""
init_state = (0, 0)
problem = create_problem(.5, DGoalTest, DClearTest, DCalcDiffSystem,
init_state)
search = get_fast_downward(
'eager'
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, _ = incremental_planner(problem,
search=search,
frequency=1,
waves=True,
debug=True)
#plan, _ = focused_planner(problem, search=search, greedy=False)
print
print 'Plan:', convert_plan(plan)
def fn(belief):
problem = make_problem(maximum_likelihood_obs(belief), goal)
#problem = make_problem(sample_obs(belief), goal)
#problem = make_problem(sample_consistent_obs(belief), goal)
#print problem
search = get_fast_downward(
'astar', verbose=False
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, universe = incremental_planner(problem,
search=search,
frequency=1)
plan = convert_plan(plan)
print 'Plan:', plan
print 'Length:', len(plan)
if plan is None or not plan:
return None
action, params = plan[0]
print 'Action:', action, params
print
return OPERATOR_MAP[action.name](operators, *params)
def main():
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), .2, .2)]
#goal = (1, 1)
goal = create_box((.8, .8), .4, .4)
problem, roadmap = create_problem(goal, obstacles)
print problem
search = get_fast_downward('astar') # dijkstra | astar
plan, _ = incremental_planner(problem,
search=search,
frequency=1,
waves=False)
plan = convert_plan(plan)
print 'Plan:', plan
print 'Roadmap', len(roadmap)
draw_roadmap(roadmap, goal, obstacles)
raw_input('Continue?')
draw_solution(plan, goal, obstacles)
raw_input('Finish?')
def main(optimal=True, max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), .2, .2)]
#goal = (1, 1)
goal = create_box((.8, .8), .4, .4)
problem = create_problem(goal, obstacles)
print problem
search = get_fast_downward('ff-astar', max_time=10,
verbose=True) # dijkstra | astar
plan, _ = incremental_planner(problem,
search=search,
frequency=10,
waves=False,
optimal=optimal,
max_time=max_time)
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 = env.GetRobots()[0]
initialize_openrave(env, ARM, min_delta=.01)
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}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash() for body in all_bodies}) == 1 # NOTE - 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 = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:MAX_GRASPS]
poses = problem.known_poses if problem.known_poses else []
open_gripper(manipulator)
initial_conf = Conf(robot.GetConfigurationValues()[manipulator.GetArmIndices()])
def _enable_all(enable): # Enables or disables all bodies for collision checking
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at 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_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
_enable_all(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_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
_enable_all(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 cfree_traj(traj, pose): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose
if DISABLE_TRAJ_COLLISIONS:
return True
return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))
####################
def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
_enable_all(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_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
_enable_all(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 sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
_enable_all(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,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[Q, T], conditions=[],
effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2], outputs=[T], conditions=[],
effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0),
EasyListGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)],
test=cfree_pose, eager=True),
EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)],
test=cfree_traj),
]
####################
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:
env.UpdatePublishedBodies()
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, max_time=15)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
#universe.print_statistics()
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 _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)
env.UpdatePublishedBodies()
raw_input('%s/%s) Step?'%(j, len(path)))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
#set_manipulator_conf(manipulator, initial_conf.value)
set_manipulator_conf(manipulator, initial_conf)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
env.UpdatePublishedBodies()
if not DISABLE_TRAJECTORIES:
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()
else:
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, q2, _ = args
set_manipulator_conf(manipulator, q2)
elif action.name == 'move_holding':
_, q2, _, _, _ = args
set_manipulator_conf(manipulator, q2)
elif action.name == 'pick':
obj, _, _, _, traj = args
robot.Grab(bodies[obj])
elif action.name == 'place':
obj, _, _, _, traj = args
robot.Release(bodies[obj])
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def simple_focused(problem,
search=DEFAULT_SEARCH,
max_time=INF,
max_iterations=INF,
optimal=False,
stream_cost=10,
check_feasible=False,
max_level=0,
greedy=True,
shared=True,
dfs=True,
verbose=False,
debug=False):
universe = Universe(
problem,
use_ground=False,
make_stream_instances=True,
make_action_instances=True) # Need to assign a cost to all
#universe = Universe(problem, use_ground=False, make_stream_instances=True, make_action_instances=not optimal)
initialize_universe(universe)
universe.action_to_function = get_stream_functions(
universe) if stream_cost is not None else {}
####################
for _ in irange(0, max_iterations):
if verbose: print
print_status(universe)
if (time() - universe.start_time) >= max_time:
break
universe.iterations += 1
if debug:
focused_debug(universe)
if check_feasible: # Checks if the problem is feasible with no parameters
plan = solve_real(universe, search, max_time)
if plan is not None:
assert is_real_solution(universe, plan)
return plan, universe
make_streams(universe,
max_level) # TODO - can also do these in iterations/waves
if debug:
abstract_focused_debug(universe)
####################
atom_nodes = determine_reachable(universe)
for instance in universe.action_instances: # TODO - do I need to reset this ever?
if isinstance(instance, ActionInstance):
add_stream_cost(
universe, atom_nodes, instance, stream_cost
) # NOTE - need to add these before the state resets
plan, goals = solve_abstract(universe, atom_nodes, search, max_time)
if verbose:
print 'Cost:', plan_cost(universe, plan), 'Plan:', convert_plan(
plan) # TODO - use an upper bound and continue?
raw_input('Continue?')
if plan is None:
if not universe.temp_blocked:
break
universe.temp_blocked = set()
universe.resets += 1
continue
####################
constants = set_union(set(args) for _, args in plan)
abstract_constants = filter(lambda c: isinstance(c, AbstractConstant),
constants)
new_goals = goals.copy()
new_goals.update(
map(Concrete, abstract_constants)
) # NOTE - could do this for all supporting goals without distinction
order = extract_streams(atom_nodes, new_goals)
#visualize_streams(goals, abstract_constants)
if verbose:
print 'Goals:', len(goals)
print 'Abstract constants:', len(abstract_constants)
print 'Order:', order
####################
bound_plan = produce_bindings(universe, order, plan, greedy, shared,
dfs)
if verbose: print 'Bound plan:', bound_plan
if bound_plan is not None:
assert is_real_solution(universe, bound_plan)
return bound_plan, universe
return None, universe
def solve_tamp(env, mproblem, display, incremental, focused, movie):
RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug)
mproblem.set_viewer(env)
mproblem.load_env(env)
oracle = ManipulationOracle(mproblem, env)
stream_problem = compile_problem(oracle)
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_fn = get_fast_downward('eager', verbose=False) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
verbose = False
debug = False
profile = False
assert incremental != focused
planner = INCREMENTAL if incremental else FOCUSED
#planner = 'hierarchy' # 'hierarchy' | 'serial'
t0 = time()
def solve():
if planner == INCREMENTAL:
return incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, verbose=verbose, debug=debug) # 1 | 20 | 100 | INF
elif planner == FOCUSED:
return focused_planner(stream_problem, search=search_fn, greedy=False, stream_cost=10, verbose=verbose, debug=debug)
elif planner == 'serial':
return goal_serialization(stream_problem, verbose=verbose, debug=debug)
elif planner == 'hierarchy':
selector = first_selector # first_selector | all_selector
return replan_hierarchy(stream_problem, selector=selector, first_only=False, execute=True, verbose=verbose)
else:
raise ValueError(planner)
if profile:
from misc.profiling import run_profile
(plan, universe), prof = run_profile(solve)
else:
(plan, universe), prof = solve(), None
if prof is not None:
from misc.profiling import str_profile
print SEPARATOR
print str_profile(prof)
universe.print_statistics()
#print_plan_stats(plan, universe)
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 not None:
print 'Plan:' #, convert_plan(plan)
for i, (action, args) in enumerate(convert_plan(plan)):
print '%s) %s%s'%(i, action, tuple(args))
if plan is None:
return
#if is_viewer_active(oracle.env):
# raw_input('Done!')
#return
#trace_plan(stream_problem, plan)
#print get_raw_states(stream_problem, plan)
images_directory = os.path.join(get_directory_path(mproblem.name, os.path.basename(__file__)), 'images') if movie else None
if display:
#display_solution(oracle, plan, directory_path, 'plan', movie)
#states = map(lambda s: convert_state(oracle, s), get_states(pddl_problem, plan))
#visualize_states(states, oracle, display=True, save=images_directory)
start = convert_state(oracle, stream_problem.initial_atoms)
executable = executable_plan(oracle, plan)
visualize_plan(Plan(start, executable), oracle, display=True, save=images_directory)
def simple_focused(problem,
search=DEFAULT_SEARCH,
max_time=INF,
max_iterations=INF,
optimal=False,
stream_cost=10,
check_feasible=False,
max_level=0,
greedy=True,
shared=True,
dfs=True,
verbose=False,
debug=False):
universe = Universe(problem,
use_ground=False,
make_stream_instances=True,
make_action_instances=True)
initialize_universe(universe)
universe.action_to_function = get_stream_functions(
universe) if stream_cost is not None else {}
for _ in irange(0, max_iterations):
if verbose:
print
print_status(universe)
if time() - universe.start_time >= max_time:
break
universe.iterations += 1
if debug:
focused_debug(universe)
if check_feasible:
plan = solve_real(universe, search, max_time)
if plan is not None:
assert is_real_solution(universe, plan)
return plan, universe
make_streams(universe, max_level)
if debug:
abstract_focused_debug(universe)
atom_nodes = determine_reachable(universe)
for instance in universe.action_instances:
if isinstance(instance, ActionInstance):
add_stream_cost(universe, atom_nodes, instance, stream_cost)
plan, goals = solve_abstract(universe, atom_nodes, search, max_time,
stream_cost)
if verbose:
print 'Cost:', plan_cost(universe,
plan), 'Plan:', convert_plan(plan)
raw_input('Continue?')
if plan is None:
if not universe.temp_blocked:
break
universe.temp_blocked = set()
universe.resets += 1
continue
constants = set_union(set(args) for _, args in plan)
abstract_constants = filter(lambda c: isinstance(c, AbstractConstant),
constants)
new_goals = goals.copy()
new_goals.update(map(Concrete, abstract_constants))
order = extract_streams(atom_nodes, new_goals)
if verbose:
print 'Goals:', len(goals)
print 'Abstract constants:', len(abstract_constants)
print 'Order:', order
bound_plan = produce_bindings(universe, order, plan, greedy, shared,
dfs)
if verbose:
print 'Bound plan:', bound_plan
if bound_plan is not None:
assert is_real_solution(universe, bound_plan)
return bound_plan, universe
return None, universe
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?')
