def create_problem(cupSize=(3, 4),
kettleSize=(5, 6),
cupInitPos=(5, -9, 0),
kettleInitPos=(-5, -9, 0),
faucetPos=(5, 1, 0),
kettleGoalPos=(-5, -9, 0),
goalPosEps=0.5,
maxMoveDist=10.0,
domain=[(-20, 20), (-9, 20), (-np.pi, np.pi)],
verboseFns=True):
CUP = '?cup'
POS1 = '?pos1'
POS2 = '?pos2'
POS3 = '?pos3'
KETTLE = '?kettle'
WATER = '?water'
FAUCET = '?faucet'
IsCup = Predicate([CUP])
IsKettle = Predicate([KETTLE])
IsWater = Predicate([WATER])
IsFaucet = Predicate([FAUCET])
LegalPos = Predicate([POS1])
CanGetWater = Predicate([POS1])
HoldsWater = Predicate([KETTLE])
CanPour = Predicate([POS1, POS2, POS3])
AtPos = Predicate([CUP, POS1])
WaterInKettle = Predicate([WATER, KETTLE])
WaterBoiled = Predicate([WATER, KETTLE])
HasBoiledWater = Predicate([KETTLE])
CanMove = Predicate([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)],
fn=lambda x, y: (distance(x, y) <= maxMoveDist))
CloseTo = Predicate([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)],
fn=lambda x, y: (distance(x, y) <= goalPosEps))
MoveCost = NonNegFunction([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)],
fn=lambda p1, p2: scale_cost(distance(p1, p2)))
def getWaterTest(p):
(x, y, z) = p
result = (faucetPos[0] - 1 <= x <= faucetPos[0] + 1) and (faucetPos[1] -
cupSize[1] - 1 <= y <= faucetPos[1] - cupSize[1]) and (-0.05 <= z <= 0.05)
if verboseFns and not result:
print 'cannot get water from:', p
return result
def fill_cost(pos):
success_cost = 1
fail_cost = 10
p_success = 0.75 if getWaterTest(pos) else 0.25
expected_cost = p_success * success_cost + (1 - p_success) * fail_cost
return scale_cost(expected_cost)
FillCost = NonNegFunction([POS1], domain=[LegalPos(POS1)], fn=fill_cost)
rename_functions(locals())
actions = [
Action(name='Move', param=[CUP, POS1, POS2],
pre=[IsCup(CUP), CanMove(POS1, POS2),
AtPos(CUP, POS1)],
eff=[AtPos(CUP, POS2),
~AtPos(CUP, POS1),
Increase(TotalCost(), MoveCost(POS1, POS2))]),
Action(name='Fill', param=[CUP, FAUCET, POS1, WATER],
pre=[HoldsWater(CUP), IsFaucet(FAUCET), CanGetWater(POS1), IsWater(WATER),
AtPos(CUP, POS1), AtPos(FAUCET, faucetPos)],
eff=[WaterInKettle(WATER, CUP),
Increase(TotalCost(), FillCost(POS1))]),
Action(name='Pour', param=[CUP, KETTLE, POS1, POS3, POS2, WATER],
pre=[HoldsWater(CUP), HoldsWater(KETTLE), CanPour(POS1, POS3, POS2), IsWater(WATER),
AtPos(CUP, POS1), AtPos(KETTLE, POS2), WaterInKettle(WATER, CUP)],
eff=[WaterInKettle(WATER, KETTLE), AtPos(CUP, POS3),
~WaterInKettle(WATER, CUP), ~AtPos(CUP, POS1),
Increase(TotalCost(), scale_cost(1))]),
Action(name='Boil', param=[KETTLE, WATER, POS1],
pre=[IsKettle(KETTLE), IsWater(WATER), CloseTo(kettleGoalPos, POS1),
AtPos(KETTLE, POS1), WaterInKettle(WATER, KETTLE)],
eff=[WaterBoiled(WATER, KETTLE),
Increase(TotalCost(), scale_cost(1))]),
]
axioms = [
Axiom(param=[WATER, KETTLE],
pre=[IsWater(WATER), IsKettle(KETTLE),
WaterInKettle(WATER, KETTLE), WaterBoiled(WATER, KETTLE)],
eff=HasBoiledWater(KETTLE)),
]
def legalTest(rp):
(x, y, z) = rp
result = (domain[0][0] <= x <= domain[0][1]) and (
domain[1][0] <= y <= domain[1][1]) and (domain[2][0] <= z <= domain[2][1])
if verboseFns and not result:
print 'not legal:', rp
return result
def randPos():
while True:
pos = tuple(random.uniform(a[0], a[1]) for a in domain)
if verboseFns:
print 'randPos:', pos
yield (pos,)
def genMove(p):
while True:
pos = tuple(random.uniform(-1, 1) * maxMoveDist + a for a in p)
if distance(pos, p) < maxMoveDist and legalTest(pos):
if verboseFns:
print 'genMove:', pos
yield (pos,)
def genClosePos(p):
while True:
pos = tuple(random.uniform(-1, 1) * goalPosEps + a for a in p)
if (distance(pos, p) < goalPosEps) and legalTest(pos):
if verboseFns:
print 'genClosePos:', pos
yield (pos,)
def genPourPos(kpos):
while True:
x = kpos[0] + random.uniform((cupSize[0] / 2.0 + kettleSize[0] / 2.0),
(cupSize[0] / 2.0 + kettleSize[0] / 2.0 + cupSize[1] / 5.0), )
y = kpos[1] + random.uniform(kettleSize[1] + 2, kettleSize[1] + 5)
initpos = (x, y, 0)
endpos = (x, y, np.pi / 1.4 * np.sign(x - kpos[0]))
if legalTest(initpos) and legalTest(endpos):
if verboseFns:
print 'genPourPos:', initpos, endpos
yield (initpos, endpos)
elif verboseFns:
print 'genPourPos illegal:', initpos, endpos
def genGetWaterPos():
while True:
pos = (random.uniform(-1, 1) * goalPosEps + faucetPos[0],
random.uniform(-cupSize[1] - 1, -cupSize[1]) + faucetPos[1],
0)
if legalTest(pos):
if verboseFns:
print 'genGetWaterPos:', pos
yield (pos,)
streams = [
GenStream(inp=[], domain=[], fn=genGetWaterPos,
out=[POS2], graph=[LegalPos(POS2), CanGetWater(POS2)]),
GenStream(inp=[], domain=[], fn=randPos,
out=[POS1], graph=[LegalPos(POS1)]),
GenStream(inp=[POS1], domain=[LegalPos(POS1)], fn=genMove,
out=[POS2], graph=[LegalPos(POS2), CanMove(POS1, POS2)]),
GenStream(inp=[POS1], domain=[LegalPos(POS1)], fn=genClosePos,
out=[POS2], graph=[LegalPos(POS2), CloseTo(POS1, POS2)]),
GenStream(inp=[POS2], domain=[LegalPos(POS2)], fn=genPourPos,
out=[POS1, POS3], graph=[CanPour(POS1, POS3, POS2), LegalPos(POS1), LegalPos(POS3)]),
TestStream(inp=[POS1], domain=[LegalPos(POS1)], test=getWaterTest,
graph=[CanGetWater(POS1)]),
]
kettle = 'kettle'
cup = 'cup'
faucet = 'faucet'
water = 'water'
initial_atoms = [
IsKettle(kettle),
IsCup(cup),
IsFaucet(faucet),
IsWater(water),
HoldsWater(cup),
HoldsWater(kettle),
LegalPos(cupInitPos),
LegalPos(kettleInitPos),
LegalPos(kettleGoalPos),
LegalPos(faucetPos),
AtPos(kettle, kettleInitPos),
AtPos(faucet, faucetPos),
AtPos(cup, cupInitPos),
initialize(TotalCost(), 0),
]
goal_literals = [
AtPos(cup, cupInitPos),
HasBoiledWater(kettle),
]
objective = TotalCost()
return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=objective)
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)
BlockSupport = Predicate([CUP, POSE, BLOCK, POSE2]) # [POSE, POSE2]
Clear = Predicate([BLOCK])
TableSupport = Predicate([POSE])
# Fluent predicates (predicates that change over time)
AtPose = Predicate([CUP, POSE])
Grasped = Predicate([CUP, GRASP])
Empty = Predicate([GRIPPER])
CanMove = Predicate([GRIPPER])
HasCoffee = Predicate([CUP])
HasSugar = Predicate([CUP])
HasCream = Predicate([CUP])
Mixed = Predicate([CUP])
Scooped = Predicate([CUP])
# Derived predicates (predicates updated by axioms)
Unsafe = Predicate([CONTROL])
Holding = Predicate([CUP])
On = Predicate([CUP, BLOCK])
# External predicates (boolean functions evaluated by fn)
from kitchen_generators import test_collision
Collision = Predicate([CONTROL, GRIPPER, POSE],
domain=[IsControl(CONTROL),
IsPose(GRIPPER, POSE)],
fn=test_collision,
bound=False)
#Collision = Predicate([CONTROL, GRIPPER, POSE])
# This is for showing function names when debugging
rename_functions(locals())
def main(n=2, verbose=False):
Item = Predicate('?b')
Part = Predicate('?r')
Class = Predicate('?c')
IsMovable = Predicate('?i', domain=[Item('?i')])
IsFixed = Predicate('?i', domain=[Item('?i')])
IsClass = Predicate('?i ?c', domain=[Item('?i'), Class('?c')])
IsArm = Predicate('?r', domain=[Part('?r')])
IsStackable = Predicate('?i1 ?i2', domain=[Item('?i1'), Item('?i2')])
HandHolding = Predicate('?r ?b', domain=[IsArm('?r'), Item('?b')])
HandEmpty = Predicate('?r', domain=[IsArm('?r')])
On = Predicate('?i1 ?i2', domain=[Item('?i1'), Item('?i2')])
Nearby = Predicate('?s', domain=[IsFixed('?s')])
Found = Predicate('?b', domain=[Item('?b')])
Localized = Predicate('?b', domain=[Item('?b')])
Holding = Predicate('?c', domain=[Class('?c')])
rename_functions(locals())
actions = [
Action(
name='Pick',
param='?a ?i ?s',
pre=[
IsArm('?a'),
IsStackable('?i', '?s'),
HandEmpty('?a'),
Nearby('?s'),
On('?i', '?s'),
Localized('?i')
],
eff=[HandHolding('?a', '?i'), ~HandEmpty('?a'), ~On('?i', '?s')]),
Action(name='Place',
param='?a ?i ?s',
pre=[
IsArm('?a'),
IsStackable('?i', '?s'),
Nearby('?s'),
HandHolding('?a', '?i'),
Localized('?s')
],
eff=[HandEmpty('?a'),
On('?i', '?s'), ~HandHolding('?a', '?i')]),
Action(name='ScanRoom',
param='?s',
pre=[IsFixed('?s'), ~Found('?s')],
eff=[Found('?s')]),
Action(name='ScanFixed',
param='?s ?i',
pre=[IsStackable('?i', '?s'), ~Found('?i')],
eff=[Found('?i'),
On('?i', '?s'),
Increase(TotalCost(), 1)]),
Action(name='Look',
param='?i',
pre=[Found('?i')],
eff=[Localized('?i')]),
Action(name='Move',
param='?s',
pre=[IsFixed('?s')],
eff=[Nearby('?s')] + [~Nearby(s)
for s in ['table0', 'table1']] +
[~Localized(s) for s in ['soup0']]),
]
initial_atoms = [
initialize(TotalCost(), 0),
HandEmpty('left'),
HandEmpty('right'),
Found('table0'),
Found('table1'),
IsClass('table0', 'table'),
IsClass('table1', 'table'),
IsFixed('table0'),
IsFixed('table1'),
IsStackable('soup0', 'table0'),
IsStackable('soup0', 'table1'),
IsClass('soup0', 'soup'),
Found('soup0'),
On('soup0', 'table0'),
IsStackable('green0', 'table0'),
IsStackable('green0', 'table1'),
IsClass('green0', 'green'),
Found('green0'),
On('green0', 'table0'),
]
axioms = [
Axiom(param='?a ?i ?c',
pre=[IsArm('?a'),
IsClass('?i', '?c'),
HandHolding('?a', '?i')],
eff=Holding('?c')),
]
goal_literals = [Holding('green'), Holding('soup')]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms, [],
objective=TotalCost())
print problem
plan, evaluations = incremental(problem, verbose=verbose)
print plan
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