def add_effort_evaluations(evaluations,
universe,
bound_streams,
operation=sum):
node_from_atom = determine_atom_effort(evaluations,
bound_streams,
op=operation)
for action in universe.problem.actions:
values = [
universe.atoms_from_predicate.get(a.head.function, {})
for a in action._stream_atoms
]
for combo in product(*values):
mapping = get_mapping(action._stream_atoms, combo)
if mapping is None:
continue
ground_atoms = [
a.substitute(mapping) for a in action._stream_atoms
]
cost = 1
cost += operation(node_from_atom[a].effort for a in ground_atoms)
stream_head = action._stream_head.substitute(mapping)
universe.add_eval(initialize(stream_head, cost))
return node_from_atom
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 state_fluents(self, state):
return frozenset(
filter(lambda e: not self.is_static(e),
(initialize(h, v) for h, v in state.iteritems())))
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 ss_from_problem(robot,
movable=[],
bound='shared',
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
print('Robot:', robot)
print('Movable:', movable)
print('Fixed:', fixed)
conf = BodyConf(robot, get_configuration(robot))
initial_atoms = [
HandEmpty(),
CanMove(),
IsConf(conf),
AtConf(conf),
initialize(TotalCost(), 0),
]
for body in movable:
pose = BodyPose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose)
]
for surface in fixed:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
initial_atoms.append(Sink(body))
if 'stove' in name:
initial_atoms.append(Stove(body))
body = movable[0]
goal_literals = [
AtConf(conf),
#Holding(body),
#On(body, fixed[0]),
#On(body, fixed[2]),
#Cleaned(body),
Cooked(body),
]
PlacedCollision = Predicate([T, O, P],
domain=[IsTraj(T), IsPose(O, P)],
fn=get_movable_collision_test(),
bound=False)
actions = [
Action(name='pick',
param=[O, P, G, Q, T],
pre=[
IsKin(O, P, G, Q, T),
HandEmpty(),
AtPose(O, P),
AtConf(Q), ~Unsafe(T)
],
eff=[HasGrasp(O, G),
CanMove(), ~HandEmpty(), ~AtPose(O, P)]),
Action(
name='place',
param=[O, P, G, Q, T],
pre=[IsKin(O, P, G, Q, T),
HasGrasp(O, G),
AtConf(Q), ~Unsafe(T)],
eff=[HandEmpty(),
CanMove(),
AtPose(O, P), ~HasGrasp(O, G)]),
# Can just do move if we check holding collisions
Action(name='move_free',
param=[Q, Q2, T],
pre=[
IsFreeMotion(Q, Q2, T),
HandEmpty(),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(name='move_holding',
param=[Q, Q2, O, G, T],
pre=[
IsHoldingMotion(Q, Q2, O, G, T),
HasGrasp(O, G),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Sink(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2), Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
axioms = [
Axiom(param=[O, G],
pre=[IsGrasp(O, G), HasGrasp(O, G)],
eff=Holding(O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[T, O, P],
pre=[IsPose(O, P),
PlacedCollision(T, O, P),
AtPose(O, P)],
eff=Unsafe(T)),
]
streams = [
GenStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(robot, grasp_name),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
# TODO: test_support
GenStream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(fixed=fixed),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2)],
bound=bound),
FnStream(name='inverse_kin',
inp=[O, P, G],
domain=[IsPose(O, P), IsGrasp(O, G)],
fn=get_ik_fn(robot, fixed=fixed, teleport=teleport),
out=[Q, T],
graph=[IsKin(O, P, G, Q, T),
IsConf(Q), IsTraj(T)],
bound=bound),
FnStream(name='free_motion',
inp=[Q, Q2],
domain=[IsConf(Q), IsConf(Q2)],
fn=get_free_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsFreeMotion(Q, Q2, T),
IsTraj(T)],
bound=bound),
FnStream(name='holding_motion',
inp=[Q, Q2, O, G],
domain=[IsConf(Q), IsConf(Q2),
IsGrasp(O, G)],
fn=get_holding_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsHoldingMotion(Q, Q2, O, G, T),
IsTraj(T)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
def ss_from_problem(problem,
bound='shared',
remote=False,
teleport=False,
movable_collisions=False):
robot = problem.robot
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
initial_atoms = [
CanMove(),
IsBConf(initial_bq),
AtBConf(initial_bq),
initialize(TotalCost(), 0),
]
for name in problem.arms:
joints = get_arm_joints(robot, name)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
initial_atoms += [
IsArm(name),
HandEmpty(name),
IsAConf(name, conf),
AtAConf(name, conf)
]
for body in problem.movable:
pose = Pose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose),
POSE(pose)
]
for surface in problem.surfaces:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
initial_atoms += map(Washer, problem.sinks)
initial_atoms += map(Stove, problem.stoves)
initial_atoms += [IsConnected(*pair) for pair in problem.buttons]
initial_atoms += [IsButton(body) for body, _ in problem.buttons]
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
initial_atoms += [IsBConf(goal_conf)]
goal_literals += [AtBConf(goal_conf)]
goal_literals += [Holding(*pair) for pair in problem.goal_holding]
goal_literals += [On(*pair) for pair in problem.goal_on]
goal_literals += map(Cleaned, problem.goal_cleaned)
goal_literals += map(Cooked, problem.goal_cooked)
#GraspedCollision = Predicate([A, G, AT], domain=[IsArm(A), POSE(G), IsArmTraj(AT)],
# fn=lambda a, p, at: False, bound=False)
PlacedCollision = Predicate([AT, P],
domain=[IsArmTraj(AT), POSE(P)],
fn=lambda at, p: False,
bound=False)
actions = [
Action(name='pick',
param=[A, O, P, G, BQ, AT],
pre=[
IsKin(A, O, P, G, BQ, AT),
HandEmpty(A),
AtPose(O, P),
AtBConf(BQ), ~Unsafe(AT)
],
eff=[
HasGrasp(A, O, G),
CanMove(), ~HandEmpty(A), ~AtPose(O, P),
Increase(TotalCost(), 1)
]),
Action(name='place',
param=[A, O, P, G, BQ, AT],
pre=[
IsKin(A, O, P, G, BQ, AT),
HasGrasp(A, O, G),
AtBConf(BQ), ~Unsafe(AT)
],
eff=[
HandEmpty(A),
CanMove(),
AtPose(O, P), ~HasGrasp(A, O, G),
Increase(TotalCost(), 1)
]),
Action(
name='move',
param=[BQ, BQ2, BT],
pre=[IsMotion(BQ, BQ2, BT),
CanMove(),
AtBConf(BQ), ~Unsafe(BT)],
eff=[
AtBConf(BQ2), ~CanMove(), ~AtBConf(BQ),
Increase(TotalCost(), 1)
]),
# TODO: should the robot be allowed to have different carry configs or a defined one?
#Action(name='move_arm', param=[A, BQ, BQ2],
# pre=[IsAConf(A, BQ), IsAConf(A, BQ),
# CanMove(), AtBConf(BQ)],
# eff=[AtAConf(BQ2), ~AtAConf(BQ),
# Increase(TotalCost(), 1)]),
# Why would one want to move to the pick configuration for something anywhere but the goal?
# Stream that generates arm motions as long as one is a shared configuration
# Maybe I should make a base
]
if remote:
actions += [
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Washer(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2),
Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
else:
actions += [
Action(name='press_clean',
param=[O, O2, A, B, BQ, AT],
pre=[
Stackable(O, O2),
Washer(O2),
IsConnected(B, O2),
IsPress(A, B, BQ, AT), ~Cooked(O),
On(O, O2),
HandEmpty(A),
AtBConf(BQ)
],
eff=[Cleaned(O), CanMove()]),
Action(name='press_cook',
param=[O, O2, A, B, BQ, AT],
pre=[
Stackable(O, O2),
Stove(O2),
IsConnected(B, O2),
IsPress(A, B, BQ, AT),
Cleaned(O),
On(O, O2),
HandEmpty(A),
AtBConf(BQ)
],
eff=[Cooked(O), ~Cleaned(O),
CanMove()]),
]
axioms = [
Axiom(param=[A, O, G],
pre=[IsArm(A), IsGrasp(O, G),
HasGrasp(A, O, G)],
eff=Holding(A, O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[O, P, AT],
pre=[IsPose(O, P),
PlacedCollision(AT, P),
AtPose(O, P)],
eff=Unsafe(AT)),
]
streams = [
FnStream(name='motion',
inp=[BQ, BQ2],
domain=[IsBConf(BQ), IsBConf(BQ2)],
fn=get_motion_gen(problem, teleport=teleport),
out=[BT],
graph=[IsMotion(BQ, BQ2, BT),
IsBaseTraj(BT)],
bound=bound),
ListStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(problem),
out=[G],
graph=[IsGrasp(O, G), GRASP(G)],
bound=bound),
# TODO: test_support
Stream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(problem),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2),
POSE(P)],
bound=bound),
GenStream(
name='ik_ir',
inp=[A, O, P, G],
domain=[IsArm(A), IsPose(O, P),
IsGrasp(O, G)],
fn=get_ik_ir_gen(problem, teleport=teleport),
out=[BQ, AT],
graph=[IsKin(A, O, P, G, BQ, AT),
IsBConf(BQ),
IsArmTraj(AT)],
bound=bound),
GenStream(name='press',
inp=[A, B],
domain=[IsArm(A), IsButton(B)],
fn=get_press_gen(problem, teleport=teleport),
out=[BQ, AT],
graph=[IsPress(A, B, BQ, AT),
IsBConf(BQ),
IsArmTraj(AT)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
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
def create_problem(initial_poses, initial_atoms, goal_literals,
make_kitchen, make_body,
expid_pour, expid_scoop,
do_motion=True, do_collisions=True,
do_grasp=True, buffer_distance=0.):
'''
Create a STRIPStream problem.
Args:
initial_poses: a dictionary mapping from the object names to their poses.
initial_atoms: initial state of the system, expressed by a list of literals.
goal_literals: list of goal conjunctive literals.
make_kitchen: function to create kitchen.
make_body: function to create Box2D bodies.
expid_pour: experiment ID for learning to pour.
expid_scoop: experiment ID for learning to scoop.
do_motion: whether to check feasibility of motion planning when planning.
do_collisions: whether to check collisions when planning.
do_grasp: whether to check grasp feasibility when planning.
buffer_distance: the buffer on obstacle objects when doing motion planning.
'''
ph.initial_poses = initial_poses
ph.make_kitchen = make_kitchen
ph.make_body = make_body
ph.gripperInitPos = initial_poses['gripper']
ph.cupInitPos = initial_poses['cup']
ph.expid_pour = expid_pour
ph.expid_scoop = expid_scoop
ph.do_motion = do_motion
ph.do_collisions = do_collisions
ph.do_grasp = do_grasp
ph.buffer_distance = buffer_distance
if not do_motion:
print 'Warning! Disabled motion planning'
if not do_collisions:
print 'Warning! Movable object collisions are disabled'
##################################################
# Objective is the numeric function to minimize
objective = TotalCost()
initial_atoms += [
# Fluent function
initialize(TotalCost(), 0) # Maintains the total plan cost
]
# Ensure the list of initial atoms is complete
for name, pose in initial_poses.items():
if 'gripper' in name:
initial_atoms += [IsGripper(name)]
if 'cup' in name:
initial_atoms += [IsCup(name)]
if 'spoon' in name:
initial_atoms += [IsSpoon(name)]
initial_atoms += [IsStirrer(name)]
if 'stirrer' in name:
initial_atoms += [IsStirrer(name)]
if 'block' in name:
initial_atoms += [IsBlock(name)]
initial_atoms += [IsPose(name, pose), AtPose(name, pose), TableSupport(pose)]
actions = get_actions()
axioms = get_axioms()
streams = get_streams()
return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=objective)