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 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 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 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_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 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 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 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 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 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 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 solve_deterministic(env, hand='left', no_window=True):
test = test0 # test0 | testBusy
exp, goal = test()
print SEPARATOR
test = PlanTest(exp, noWindow=no_window)
test.load() # TODO - do I need to pass regions here?
#test_obj = test.realWorld.objectConfs.keys()[0]
#test.realWorld.getObjectPose()
#print 'Initial collision:', test.realWorld.checkRobotCollision(test.realWorld.robotConf, test.realWorld.objectShapes[test_obj])
print 'Initial collisions:', world_check_collisions(test.realWorld)
manip_problem = real_manipulation_problem(env, test.realWorld, goal, hand)
#manip_problem.set_viewer(env)
oracle = ManipulationOracle(manip_problem,
env,
active_arms=[hand],
reset_robot=False)
stream_problem = compile_problem(oracle)
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
plan, universe = focused_planner(stream_problem,
search=search_fn,
greedy=False,
stream_cost=10,
verbose=False,
debug=False)
#plan, universe = incremental_planner(stream_problem, search=search_fn, verbose=True, debug=False)
if plan is None:
return
print plan
if no_window:
visualize_plan(Plan(
convert_state(oracle, stream_problem.initial_atoms),
executable_plan(oracle, plan)),
oracle,
display=True,
save=None)
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 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 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 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 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 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():
"""
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?')
INCREMENTAL, FOCUSED = 'incremental', 'focused'
DEFAULT_SEARCH_NAME = BFS
if has_fd():
DEFAULT_SEARCH_NAME = FAST_DOWNWARD
elif has_smtplan():
DEFAULT_SEARCH_NAME = SMTPLAN
elif has_ff():
DEFAULT_SEARCH_NAME = FAST_FORWARD
elif has_lapkt():
DEFAULT_SEARCH_NAME = LAPKT
#DEFAULT_SEARCH_NAME = SMTPLAN
#DEFAULT_SEARCH_NAME = TFD
print 'Using', DEFAULT_SEARCH_NAME, 'as the default search implementation'
if DEFAULT_SEARCH_NAME == FAST_DOWNWARD:
DEFAULT_SEARCH = get_fast_downward()
elif DEFAULT_SEARCH_NAME == TFD:
DEFAULT_SEARCH = get_tfd()
elif DEFAULT_SEARCH_NAME == SMTPLAN:
DEFAULT_SEARCH = get_smtplan()
elif DEFAULT_SEARCH_NAME == FAST_FORWARD:
DEFAULT_SEARCH = get_fast_forward()
elif DEFAULT_SEARCH_NAME == LAPKT:
DEFAULT_SEARCH = get_lakpt()
elif DEFAULT_SEARCH_NAME == BFS:
from stripstream.algorithms.search.bfs import get_bfs
DEFAULT_SEARCH = get_bfs()
else:
raise ValueError('Unexpected search type %s' % DEFAULT_SEARCH_NAME)
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 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?')
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)