def plan_commands(problem, search='ff-astar', max_time=60, verbose=True):
ss_problem = ss_from_problem(problem, remote=True, teleport=False)
print(ss_problem)
# ss_problem.dump()
t0 = time.time()
pr = cProfile.Profile()
pr.enable()
# plan, evaluations = incremental(ss_problem, planner=search, max_time=max_time,
# verbose=verbose, terminate_cost=terminate_cost)
plan, evaluations = dual_focused(ss_problem,
planner=search,
max_time=max_time,
effort_weight=None,
verbose=verbose)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10) # tottime | cumtime
print('Time:', time.time() - t0)
# print_plan(plan, evaluations)
if plan is None:
return None
return post_process(problem, plan)
def main(n=2, bound='unique'):
initial_conf = CONF(-1)
blocks = ['b{}'.format(i) for i in xrange(n)]
initial_poses = {b: POSE(b, i) for i, b in enumerate(blocks)}
goal_poses = {'b0': POSE('b0', 1)}
Block = Predicate('?b')
IsPose = Predicate('?b ?p')
Conf = Predicate('?q')
Kin = Predicate('?b ?q ?p')
Collision = Predicate('?b ?p ?b2 ?p2',
domain=[IsPose('?b', '?p'),
IsPose('?b2', '?p2')],
fn=lambda b, p, b2, p2: p.value == p2.value,
eager=True,
bound=False)
AtConf = Predicate('?q')
AtPose = Predicate('?b ?p')
Holding = Predicate('?b')
HandEmpty = Predicate('')
Unsafe = Predicate('?b ?p')
rename_functions(locals())
streams = [
Stream(name='placement',
inp=['?b'],
domain=[Block('?b')],
fn=PoseGen,
out='?p',
graph=[IsPose('?b', '?p')],
bound=bound),
FnStream(name='ik',
inp='?b ?p',
domain=[IsPose('?b', '?p')],
fn=lambda b, p: (CONF(p.value), ),
out='?q',
graph=[Kin('?b', '?q', '?p'),
Conf('?q')],
bound=bound),
]
actions = [
Action(name='Move',
param='?q1 ?q2',
pre=[Conf('?q1'), Conf('?q2'),
AtConf('?q1')],
eff=[AtConf('?q2'), ~AtConf('?q1'),
Increase(TotalCost(), 1)]),
Action(name='Pick',
param='?b ?p ?q',
pre=[
Kin('?b', '?q', '?p'),
AtPose('?b', '?p'),
HandEmpty(),
AtConf('?q')
],
eff=[
Holding('?b'), ~AtPose('?b', '?p'), ~HandEmpty(),
Increase(TotalCost(), 1)
]),
Action(name='Place',
param='?b ?p ?q',
pre=[
Kin('?b', '?q', '?p'),
Holding('?b'),
AtConf('?q'), ~Unsafe('?b', '?p')
],
eff=[
AtPose('?b', '?p'),
HandEmpty(), ~Holding('?b'),
Increase(TotalCost(), 1)
]),
]
axioms = [
Axiom(param='?b ?p ?b2 ?p2',
pre=[Collision('?b', '?p', '?b2', '?p2'),
AtPose('?b2', '?p2')],
eff=Unsafe('?b', '?p')),
]
initial_atoms = [
Conf(initial_conf),
AtConf(initial_conf),
HandEmpty(),
initialize(TotalCost(), 0),
]
for b, p in initial_poses.items():
initial_atoms += [Block(b), IsPose(b, p), AtPose(b, p)]
for b, p in goal_poses.items():
initial_atoms += [IsPose(b, p)]
goal_literals = [AtPose(b, p) for b, p in goal_poses.items()]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
print problem
pr = cProfile.Profile()
pr.enable()
plan, evaluations = dual_focused(problem,
terminate_cost=INF,
verbose=False,
bind=True,
use_context=True,
temp_dir='fish/',
clean=True)
print plan
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
def main(n=5, bound='unique'):
initial_conf = 0
initial_poses = {'b{}'.format(i): i for i in xrange(n)}
goal_poses = {'b1': 2}
Block = Predicate('?b')
Pose = Predicate('?p')
Conf = Predicate('?q')
Kin = Predicate('?q ?p')
CFree = Predicate('?p1 ?p2')
AtConf = Predicate('?q')
AtPose = Predicate('?b ?p')
Holding = Predicate('?b')
HandEmpty = Predicate('')
Safe = Predicate('?b ?p')
rename_functions(locals())
streams = [
Stream(name='placement',
inp=[],
domain=[],
fn=lambda: ([(p, )] for p in xrange(n, 100)),
out='?p',
graph=[Pose('?p')],
bound=bound),
FnStream(name='ik',
inp='?p',
domain=[Pose('?p')],
fn=lambda p: (p, ),
out='?q',
graph=[Kin('?q', '?p'), Conf('?q')],
bound=bound),
TestStream(name='collision',
inp='?p1 ?p2',
domain=[Pose('?p1'), Pose('?p2')],
test=lambda p1, p2: p1 != p2,
graph=[CFree('?p1', '?p2')],
eager=True,
bound=bound),
]
actions = [
Action(name='Move',
param='?q1 ?q2',
pre=[Conf('?q1'), Conf('?q2'),
AtConf('?q1')],
eff=[AtConf('?q2'), ~AtConf('?q1'),
Increase(TotalCost(), 1)]),
Action(name='Pick',
param='?b ?p ?q',
pre=[
Block('?b'),
Kin('?q', '?p'),
AtPose('?b', '?p'),
HandEmpty(),
AtConf('?q')
],
eff=[
Holding('?b'), ~AtPose('?b', '?p'), ~HandEmpty(),
Increase(TotalCost(), 1)
]),
]
for b in initial_poses:
actions += [
Action(name='Place-' + b,
param='?p ?q',
pre=[Block(b),
Kin('?q', '?p'),
Holding(b),
AtConf('?q')] +
[Safe(b2, '?p') for b2 in initial_poses if b2 != b],
eff=[
AtPose(b, '?p'),
HandEmpty(), ~Holding(b),
Increase(TotalCost(), 1)
])
]
axioms = [
Axiom(param='?p1 ?b2 ?p2',
pre=[Block('?b2'),
CFree('?p1', '?p2'),
AtPose('?b2', '?p2')],
eff=Safe('?b2', '?p1')),
]
initial_atoms = [
Conf(initial_conf),
AtConf(initial_conf),
HandEmpty(),
initialize(TotalCost(), 0),
]
for b, p in initial_poses.items():
initial_atoms += [Block(b), Pose(p), AtPose(b, p)]
for b, p in goal_poses.items():
initial_atoms += [Pose(p)]
goal_literals = [AtPose(b, p) for b, p in goal_poses.items()]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
print problem
pr = cProfile.Profile()
pr.enable()
plan, evaluations = dual_focused(problem, terminate_cost=INF, verbose=True)
print plan
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
def main(execute='execute'):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
args = parser.parse_args()
#display = args.display
display = True
connect(use_gui=args.viewer)
robot, block = load_world()
#robot2 = clone_body(robot)
#block2 = clone_body(block)
#dump_world()
saved_world = WorldSaver()
dump_world()
ss_problem = ss_from_problem(robot,
movable=[block],
teleport=False,
movable_collisions=True)
#ss_problem = ss_problem.debug_problem()
#print(ss_problem)
t0 = time.time()
plan, evaluations = dual_focused(ss_problem, verbose=True)
# plan, evaluations = incremental(ss_problem, verbose=True)
print_plan(plan, evaluations)
print(time.time() - t0)
if (not display) or (plan is None):
disconnect()
return
paths = []
for action, params in plan:
if action.name == 'place':
paths += params[-1].reverse().body_paths
elif action.name in ['move_free', 'move_holding', 'pick']:
paths += params[-1].body_paths
print(paths)
command = Command(paths)
if not args.viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
saved_world.restore()
user_input('Execute?')
if execute == 'control':
command.control()
elif execute == 'execute':
command.refine(num_steps=10).execute(time_step=0.001)
elif execute == 'step':
command.step()
else:
raise ValueError(execute)
#dt = 1. / 240 # Bullet default
#p.setTimeStep(dt)
wait_for_interrupt()
disconnect()
def main():
initial_bq = BConf(0, 1)
initial_poses = {
'green': [10, 10, 30],
'table': [10, 20, 30],
}
movable = {'green'}
initial_p_from_name = {}
class_from_name = {}
for cl in initial_poses:
for i, x in enumerate(initial_poses[cl]):
name = cl + str(i)
class_from_name[name] = cl
initial_p_from_name[name] = Pose(name, x)
print initial_p_from_name
print class_from_name
O = '?o'
S = '?s'
R = '?r'
P = '?p'
P2 = '?p2'
G = '?g'
Q = '?q'
Q2 = '?q2'
C = '?c'
base = 'base'
head = 'head'
left = 'left'
right = 'right'
IsPose = Predicate([O, P])
IsGrasp = Predicate([O, G])
IsConf = Predicate([P, Q])
IsMovable = Predicate([O])
IsFixed = Predicate([O])
IsKin = Predicate([R, O, P, G, Q])
IsSupported = Predicate([P, P2])
IsArm = Predicate([R])
IsClass = Predicate([O, C])
AtPose = Predicate([O, P])
AtConf = Predicate([R, Q])
HasGrasp = Predicate([R, O, G])
HandEmpty = Predicate([R])
Holding = Predicate([O])
On = Predicate([O, S])
base_constant_cost = 1
def get_circle(value):
if isinstance(value, BConf):
return value.x, 0
if isinstance(value, Pose):
return value.x, 0
if isinstance(value, InputOutputSet):
if value.stream.name == 'IK':
_, _, p, _ = value.inputs
x, r = get_circle(p)
return x, r + 0
if value.stream.name == 'placement':
_, _, p = value.inputs
x, r = get_circle(p)
return x, r + 0
raise ValueError(value)
def distance_bound_fn(q1, q2):
x1, r1 = get_circle(q1)
x2, r2 = get_circle(q2)
return max(abs(x2 - x1) - (r1 + r2), 0) + base_constant_cost
Distance = Function([Q, Q2],
domain=[IsConf(base, Q),
IsConf(base, Q2)],
fn=lambda q1, q2: abs(q2.x - q1.x) + abs(q2.y - q1.y) +
base_constant_cost,
bound=distance_bound_fn)
rename_functions(locals())
bound = 'shared'
streams = [
FnStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=lambda o: (Grasp(o), ),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
ListStream(name='IK',
inp=[R, O, P, G],
domain=[IsArm(R), IsPose(O, P),
IsGrasp(O, G)],
fn=lambda r, o, p, g: [(BConf(p.x, +1), )],
out=[Q],
graph=[IsKin(R, O, P, G, Q),
IsConf(base, Q)],
bound=bound),
FnStream(name='placement',
inp=[O, S, P],
domain=[IsMovable(O), IsFixed(S),
IsPose(S, P)],
fn=lambda o, s, p: (Pose(o, p.x), ),
out=[P2],
graph=[IsPose(O, P2), IsSupported(P2, P)],
bound=bound),
]
actions = [
Action(name='pick',
param=[R, O, P, G, Q],
pre=[
IsKin(R, O, P, G, Q),
HandEmpty(R),
AtPose(O, P),
AtConf(base, Q)
],
eff=[HasGrasp(R, O, G), ~HandEmpty(R)]),
Action(name='place',
param=[R, O, P, G, Q],
pre=[IsKin(R, O, P, G, Q),
HasGrasp(R, O, G),
AtConf(base, Q)],
eff=[HandEmpty(R),
AtPose(O, P), ~HasGrasp(R, O, G)]),
Action(
name='move_base',
param=[Q, Q2],
pre=[IsConf(base, Q),
IsConf(base, Q2),
AtConf(base, Q)],
eff=[AtConf(base, Q2), ~AtConf(base, Q),
Increase(TotalCost(), 1)]),
Action(name='move_head',
param=[Q, Q2],
pre=[IsConf(head, Q),
IsConf(head, Q2),
AtConf(head, Q)],
eff=[AtConf(head, Q2), ~AtConf(head, Q)]),
]
axioms = [
Axiom(param=[R, O, G],
pre=[IsArm(R), IsGrasp(O, G),
HasGrasp(R, O, G)],
eff=Holding(O)),
Axiom(param=[O, P, S, P2],
pre=[
IsPose(O, P),
IsPose(S, P2),
IsSupported(P, P2),
AtPose(O, P),
AtPose(S, P2)
],
eff=On(O, S)),
]
initial_atoms = [
IsArm(left),
HandEmpty(left),
HandEmpty(right),
IsConf(base, initial_bq),
AtConf(base, initial_bq),
initialize(TotalCost(), 0),
]
for n, p in initial_p_from_name.items():
initial_atoms += [IsPose(n, p), AtPose(n, p)]
if class_from_name[n] not in movable:
continue
for n2, p2 in initial_p_from_name.items():
if class_from_name[n2] in movable:
continue
if p.x == p2.x:
initial_atoms.append(IsSupported(p, p2))
for n, cl in class_from_name.items():
if cl in movable:
initial_atoms.append(IsMovable(n))
else:
initial_atoms.append(IsFixed(n))
goal_literals = [On('green0', 'table1'), On('green1', 'table1')]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
print problem
pr = cProfile.Profile()
pr.enable()
plan, _ = dual_focused(problem)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
print plan
return
print 'Length', len(plan)
for i, act in enumerate(plan):
print i, act