def create_problem():
"""
Creates the 1D task and motion planning STRIPStream problem.
This models the same problem as :module:`.run_tutorial` but does so without using any streams.
:return: a :class:`.STRIPStreamProblem`
"""
num_blocks = 3
blocks = ['block%i'%i for i in range(num_blocks)]
num_poses = num_blocks+1
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i for i, block in enumerate(blocks)} # the initial pose for block i is i
goal_poses = {block: i+1 for i, block in enumerate(blocks)} # the goal pose for block i is i+1
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place', parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), # TODO - convert to finite blocks case?
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), # Infers B2 is at a safe pose wrt B1 at P1
]
####################
cond_streams = []
constants = []
initial_atoms = [
AtConf(initial_config),
HandEmpty()
] + [
AtPose(block, pose) for block, pose in initial_poses.iteritems()
] + [
LegalKin(i, i) for i in range(num_poses)
] + [
CollisionFree(b1, p1, b2, p2) for b1, p1, b2, p2 in product(blocks, range(num_poses), blocks, range(num_poses)) if p1 != p2 and b1 != b2
]
goal_literals = [AtPose(block, pose) for block, pose in goal_poses.iteritems()]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
def compile_problem(oracle):
problem = oracle.problem
for obj in problem.goal_poses:
if problem.goal_poses[obj] == 'initial':
problem.goal_poses[obj] = oracle.initial_poses[obj]
elif problem.goal_poses[obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial)
problem.goal_poses[obj] = oracle.initial_poses[problem.goal_poses[obj]]
####################
def sample_poses(o, num_samples=1):
while True:
if AVOID_INITIAL:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
yield list(islice(random_region_placements(oracle, o, oracle.get_counters(), region_weights=True), num_samples))
def sample_grasps(o):
yield get_grasps(oracle, o)
def sample_region(o, r, num_samples=1):
while True:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
yield list(islice(random_region_placements(oracle, o, [r], region_weights=True), num_samples))
def sample_motion(o, p, g, max_calls=1, max_failures=50):
oracle.set_all_object_poses({o: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(o, p, g):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
if not pap.sample(oracle, o, max_failures=max_failures,
sample_vector=DO_ARM_MOTION, sample_arm=DO_ARM_MOTION, check_base=CHECK_BASE):
break
pap.obj = o
yield [(pap.approach_config, pap)]
def collision_free(o, p, t):
if p is None or o == t.obj:
return True
holding = ObjGrasp(t.obj, t.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
if not DO_ARM_MOTION:
return not oracle.holding_collision(t.grasp_config, o, p, holding)
return not oracle.traj_holding_collision(t.approach_config, t.trajs, o, p, holding)
####################
# Types
CONF, TRAJ, REG = Type(), Type(), Type()
BLOCK, POSE, GRASP = Type(), Type(), Type()
# Fluent predicates
AtConfig = Pred(CONF)
HandEmpty = Pred()
AtPose = Pred(BLOCK, POSE)
Holding = Pred(BLOCK, GRASP)
# Static predicates
IsPose = Pred(BLOCK, POSE)
IsGrasp = Pred(BLOCK, GRASP)
IsKin = Pred(BLOCK, POSE, GRASP, CONF, TRAJ)
IsCollisionFree = Pred(BLOCK, POSE, TRAJ)
IsContained = Pred(REG, BLOCK, POSE)
# Derived predicates
Safe = Pred(BLOCK, TRAJ)
InRegion = Pred(BLOCK, REG)
# Parameters
O, P, G = Param(BLOCK), Param(POSE), Param(GRASP)
Q, Q2, T = Param(CONF), Param(CONF), Param(TRAJ)
OB, R = Param(BLOCK), Param(REG)
actions = [
Action(name='pick', parameters=[O, P, G, Q, T],
condition=And(AtPose(O, P), HandEmpty(),
IsKin(O, P, G, Q, T), AtConfig(Q),
ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
effect=And(Holding(O, G),
Not(HandEmpty()), Not(AtPose(O, P)))),
Action(name='place', parameters=[O, P, G, Q, T],
condition=And(Holding(O, G),
IsKin(O, P, G, Q, T), AtConfig(Q),
ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
effect=And(AtPose(O, P), HandEmpty(),
Not(Holding(O, G)))),
Action(name='move', parameters=[Q, Q2],
condition=AtConfig(Q),
effect=And(AtConfig(Q2),
Not(AtConfig(Q))))]
axioms = [
Axiom(effect=InRegion(O, R), condition=Exists([P],
And(AtPose(O, P), IsContained(R, O, P)))),
Axiom(effect=Safe(O, T), condition=Exists([P],
And(AtPose(O, P), IsCollisionFree(O, P, T))))]
cond_streams = [
GenStream(inputs=[O], outputs=[P],
conditions=[],
effects=[IsPose(O, P)],
generator=sample_poses),
GenStream(inputs=[O], outputs=[G],
conditions=[],
effects=[IsGrasp(O, G)],
generator=sample_grasps),
GenStream(inputs=[O, R], outputs=[P],
conditions=[],
effects=[IsPose(O, P), IsContained(R, O, P)],
generator=sample_region),
GenStream(inputs=[O, P, G], outputs=[Q, T],
conditions=[IsPose(O, P), IsGrasp(O, G)],
effects=[IsKin(O, P, G, Q, T)],
generator=sample_motion),
TestStream(inputs=[O, P, T],
conditions=[IsPose(O, P)],
effects=[IsCollisionFree(O, P, T)],
test=collision_free)]
####################
constants = [POSE(None)]
initial_atoms = [AtConfig(oracle.initial_config)] # TODO - toggle
holding = set()
if problem.start_holding is not False:
obj, grasp = problem.start_holding
initial_atoms += [Holding(obj, grasp), AtPose(obj, None), IsGrasp(obj, grasp)]
holding.add(obj)
if not holding:
initial_atoms.append(HandEmpty())
for obj, pose in oracle.initial_poses.iteritems():
if obj not in holding:
initial_atoms += [AtPose(obj, pose), IsPose(obj, pose)]
goal_literals = []
if problem.goal_holding is not None:
if problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif isinstance(problem.goal_holding, ObjGrasp):
goal_literals.append(Holding(problem.goal_holding.object_name, problem.goal_holding.grasp))
elif problem.goal_holding in oracle.get_objects():
goal_literals.append(Holding(problem.goal_holding))
else:
raise Exception()
for obj, pose in problem.goal_poses.iteritems():
goal_literals.append(AtPose(obj, pose))
initial_atoms.append(IsPose(obj, pose))
for obj, region in problem.goal_regions.iteritems():
goal_literals.append(InRegion(obj, region))
goal_formula = goal_literals
return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
def compile_problem(tamp_problem):
O = Param(OBJECT)
O1, O2 = Param(OBJECT), Param(OBJECT)
L = Param(LOCATION)
L_s, L_g = Param(LOCATION), Param(LOCATION) # generic location
Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param(
STOVE_L_G) # locations for stove and sink
Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G)
actions = [
Action(name='wash',
parameters=[O],
condition=And(InSink(O)),
effect=And(Clean(O))),
Action(name='cook',
parameters=[O],
condition=And(InStove(O), Clean(O)),
effect=And(Cooked(O))),
Action(name='pickplace',
parameters=[O, L_s, L_g],
condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)),
effect=And(AtPose(O, L_g),
Not(AtPose(O, L_s)))) # You should delete!
]
axioms = [
# For all objects in the world, either object is O1 or if not, then it is not in the region
Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\
Or(Equal(O,O2),\
Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))),
Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))),
Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))),
]
cond_streams = [
EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\
generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))),
EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS),
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS),
# OutsideRegion tests if the block at L is outside of the region (Ls,Lg)
]
####################
# instantiate the environment region?
constants = [
STOVE_L_S(tamp_problem.stove_region_s),
STOVE_L_G(tamp_problem.stove_region_g),
SINK_L_S(tamp_problem.sink_region_s),
SINK_L_G(tamp_problem.sink_region_g),
]
# define initial state using initial poses of objects
initial_atoms = [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [
IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
]
# static predicate but on steroid
# define goal state as target object to be cooked - can you infer that target object is on Stove
goal_literals = []
# goal_literals.append( AtPose(tamp_problem.blue_obj,1) ) #NOTE: This works; so planner knows how to clear the area
# goal_literals.append( AtPose(tamp_problem.target_obj,3.0) ) #NOTE: But doing this does not work
goal_literals.append(Cooked(tamp_problem.target_obj))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants), static_pred_names
def convert_axioms_to_effects(self):
derived_predicates = self.get_derived_predicates()
fluent_predicates = self.get_fluent_predicates()
constants = defaultdict(set)
for const in self.get_constants():
constants[const.type].add(const)
mapping = []
for op in self.operators:
if isinstance(op, Axiom):
[conditions
] = op.condition.dequantify(constants).get_literals()
assert not filter(lambda a: a.predicate in derived_predicates,
conditions)
print conditions
[fluent] = filter(lambda a: a.predicate in fluent_predicates,
conditions)
others = filter(lambda a: a != fluent, conditions)
mapping.append((fluent, others, op.effect))
new_operators = []
for op in self.operators:
if isinstance(op, Action):
new_op = op.clone()
new_operators.append(new_op)
[literals] = new_op.effect.get_literals()
for literal in literals:
effect = literal.formula if isinstance(literal,
Not) else literal
for fluent, conditions, derived in mapping:
if effect.predicate == fluent.predicate:
free_params = set(
flatten(
map(lambda a: a.args, [derived] +
conditions))) - set(fluent.args)
param_map = dict(zip(fluent.args, effect.args))
for i, param in enumerate(free_params):
param_map[param] = Parameter(
'l%s' % i, param.type)
new_params = list(
set(param_map.values()) - set(effect.args))
new_derived = derived.instantiate(param_map)
new_conditions = [
cond.instantiate(param_map)
for cond in conditions
]
if isinstance(literal, Not):
new_op.add_effects(
ForAll(
new_params,
When(And(*new_conditions),
new_derived)))
else:
new_op.add_effects(
ForAll(
new_params,
When(And(*new_conditions),
Not(new_derived))))
return new_operators
def solve_incrementally():
O = Param(OBJECT)
O1, O2 = Param(OBJECT), Param(OBJECT)
L = Param(LOCATION)
L_s, L_g = Param(LOCATION), Param(LOCATION) # generic location
Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param(
STOVE_L_G) # locations for stove and sink
Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G)
actions = [
Action(name='wash',
parameters=[O],
condition=And(InSink(O)),
effect=And(Clean(O))),
Action(name='cook',
parameters=[O],
condition=And(InStove(O), Clean(O)),
effect=And(Cooked(O))),
Action(name='pickplace',
parameters=[O, L_s, L_g],
condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)),
effect=And(AtPose(O, L_g),
Not(AtPose(O, L_s)))) # You should delete!
]
axioms = [
# For all objects in the world, either object is O1 or if not, then it is not in the region
Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\
Or(Equal(O,O2),\
Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))),
# Object is in the stove if it is at pose L for which Ls and Lg define stove
Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))),
Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))),
]
cond_streams = [
EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\
generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))),
EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS),
# Generate static predicates that object is contained in sink for which Ls and Lg define the sink. If L was not continuous value,
# then we would define this in the intial condition and would not be changed by any of the actions (hence static predicate)
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
# OutsideRegion tests if O2 is is outside of the region (Ls,Lg)
EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS),
]
####################
tamp_problem = sample_one_d_kitchen_problem()
# instantiate the environment region?
constants = [
STOVE_L_S(tamp_problem.stove_region_s),
STOVE_L_G(tamp_problem.stove_region_g),
SINK_L_S(tamp_problem.sink_region_s),
SINK_L_G(tamp_problem.sink_region_g),
]
# define initial state using initial poses of objects
initial_atoms = [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [
IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
] # initial_atoms = static predicates, but on steroid
goal_literals = []
subgoal_list = [ InSink(tamp_problem.target_obj), \
InStove(tamp_problem.target_obj), Cooked(tamp_problem.target_obj) ]
for i in range(len(subgoal_list)):
goal_literals = [subgoal_list[0]]
stream_problem = STRIPStreamProblem(initial_atoms, goal_literals, \
actions+axioms, cond_streams, constants)
search = get_fast_downward('eager')
plan, universe = incremental_planner(stream_problem, search=search,\
frequency=1, verbose=True, max_time=200)
plan = convert_plan(plan)
# move the object to the new locations; todo: add new predicates
if len(plan) > 0: # if no action needed, keep last initial atoms
initial_atoms = []
for action in plan:
action_name = action[0].name
action_args = action[1]
if action_name == 'pickplace':
O = action_args[0].name
pick_l = action_args[1]
place_l = action_args[2]
block = [b for b in tamp_problem.blocks if b.name == O][0]
initial_atoms += [AtPose(block, place_l)]
if len(initial_atoms) == 1:
block = [b for b in tamp_problem.blocks if b.name != O][0]
initial_atoms += [AtPose(block, tamp_problem.initial_poses[block])]
initial_atoms += [
IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)
]
initial_atoms += [
IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
]
def convert_axioms_to_effects(
problem
): # NOTE - can always use the previous compilation process for some problematic axioms
derived_predicates = problem.get_derived_predicates()
fluent_predicates = problem.get_fluent_predicates()
constants = defaultdict(set)
for const in problem.get_constants():
constants[const.type].add(const)
mapping = []
for op in problem.operators: # NOTE - can always just convert one
if isinstance(op, Axiom):
[conditions] = op.condition.dequantify(constants).get_literals(
) # TODO - more complicated if multiple ways to achieve
assert not filter(
lambda a: a.predicate in derived_predicates,
conditions) # TODO - difficulty when levels of axioms
print conditions
[fluent] = filter(
lambda a: a.predicate in fluent_predicates,
conditions) # NOTE - could easily expand to conjunctive case
others = filter(lambda a: a != fluent, conditions)
mapping.append((fluent, others, op.effect))
new_operators = []
for op in problem.operators:
if isinstance(op, Action):
new_op = op.clone()
new_operators.append(new_op)
[literals] = new_op.effect.get_literals() # TODO
for literal in literals:
effect = literal.formula if isinstance(literal,
Not) else literal
for fluent, conditions, derived in mapping:
if effect.predicate == fluent.predicate:
free_params = set(
flatten(
map(lambda a: a.args, [derived] +
conditions))) - set(fluent.args)
param_map = dict(zip(fluent.args, effect.args))
for i, param in enumerate(
free_params): # Prevents conflicting names
#param_map[param] = Parameter('_%s'%i, param.type) # NOTE - FF can't parse this
param_map[param] = Parameter('l%s' % i, param.type)
new_params = list(
set(param_map.values()) - set(effect.args))
new_derived = derived.instantiate(param_map)
new_conditions = [
cond.instantiate(param_map) for cond in conditions
] # TODO - could put in the negated version of new_derived
if isinstance(literal, Not):
new_op.add_effects(
ForAll(new_params,
When(And(*new_conditions),
new_derived)))
else:
new_op.add_effects(
ForAll(
new_params,
When(And(*new_conditions),
Not(new_derived)))
) # Not necessary, but it could reduce the number of instances
#op.add_effects(ForAll(new_params, Not(new_derived))) # One failure can break
return new_operators
Action(name='wash', parameters=[O, L1, S],
condition=And(At(O, L1), HasState(O, S), IsWasher(L1)),
effect=And(HasState(O, clean), Not(HasState(O, S))),
cost=COST_SCALE*1),
Action(name='paint', parameters=[O, L1],
condition=And(At(O, L1), HasState(O, clean), IsPainter(L1)),
effect=And(HasState(O, wet), Not(HasState(O, clean))),
cost=COST_SCALE*1),
Action(name='dry', parameters=[O, L1],
condition=And(At(O, L1), HasState(O, wet), IsDryer(L1)),
effect=And(HasState(O, dry), Not(HasState(O, wet))),
cost=COST_SCALE*1),
Axiom(effect=Clear(L2), condition=ForAll([O2], Safe(O2, L2))),
Axiom(effect=Safe(O, L1), condition=Exists([L2], And(At(O, L2), Not(Equal(L1, L2))))), # NOTE - automatically includes UnsureLoc
]
# TODO - do I need lower confidence bound that the object isn't there to prevent it from doing nothing or impossible things?
LOC_CONFIDENCE = .95
STATE_CONFIDENCE = LOC_CONFIDENCE
CLEAR_CONFIDENCE = STATE_CONFIDENCE
MIN_CONFIDENCE = .001 # TODO - how does this work in continuous domains?
MIN_P = 1e-6
def observable_problem(belief, goal, costs=True): # NOTE - costs is really important here
#objects = belief.objLoc.keys()
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the countable TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
# NOTE - the simple focused algorithm gives "Could not find instantiation for PNE!" when this is moved outside
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(
name='place',
parameters=[B1, P1, Q1],
condition=And(
Holding(B1),
AtConf(Q1),
LegalKin(P1, Q1),
#ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
#*[Or(Equal(B1, BLOCK(b2)), Safe(b2, P1)) for b2 in tamp_problem.initial_poses]),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
#Axiom(effect=Safe(B2, B1, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))),
Axiom(effect=Safe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1,
P2)))),
]
# NOTE - this needs to be inside the method so you make new streams each time
cond_streams = [
#EasyGenStream(inputs=[P2], outputs=[P1], conditions=[], effects=[],
# generator=lambda a: irange(0, NUM_POSES)),
EasyGenStream(inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: irange(0, NUM_POSES)),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([p])),
#EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)],
# test=lambda b1, p1, b2, p2: p1 != p2, eager=EAGER_TESTS),
EasyTestStream(inputs=[P1, P2],
conditions=[],
effects=[CollisionFree(P1, P2)],
test=lambda p1, p2: p1 != p2,
eager=EAGER_TESTS),
]
constants = []
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
]
if tamp_problem.initial_holding is False:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))
goal_literals = []
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses.iteritems():
goal_literals.append(AtPose(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the continuous TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
R = Param(REGION)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place',
parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1,
P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
Action(name='clean',
parameters=[B1, R],
condition=And(InRegion(B1, R), IsSink(R)),
effect=And(Cleaned(B1))),
Action(name='cook',
parameters=[B1, R],
condition=And(InRegion(B1, R), IsStove(R)),
effect=And(Cooked(B1))),
]
axioms = [
Axiom(effect=InRegion(B1, R),
condition=Exists([P1], And(AtPose(B1, P1), Contained(B1, P1,
R)))),
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2],
And(AtPose(B2, P2),
CollisionFree(B1, P1, B2, P2)))),
]
cond_streams = [
EasyGenStream(inputs=[R, B1],
outputs=[P1],
conditions=[CanPlace(B1, R)],
effects=[Contained(B1, P1, R)],
generator=lambda r, b:
(sample_region_pose(r, b) for _ in irange(0, INF))),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([inverse_kinematics(p)])),
EasyTestStream(inputs=[R, B1, P1],
conditions=[],
effects=[Contained(B1, P1, R)],
test=in_region,
eager=EAGER_TESTS,
plannable=False),
EasyTestStream(inputs=[B1, P1, B2, P2],
conditions=[],
effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda *args: not are_colliding(*args),
eager=EAGER_TESTS),
]
constants = [
REGION(tamp_problem.env_region),
]
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [
CanPlace(block, tamp_problem.env_region)
for block in tamp_problem.initial_poses
]
if tamp_problem.initial_holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding))
goal_literals = []
if tamp_problem.goal_config is not None:
goal_literals.append(AtConf(tamp_problem.goal_config))
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses:
goal_literals.append(AtPose(block, goal))
for block, goal in tamp_problem.goal_regions:
initial_atoms.append(CanPlace(block, goal))
goal_literals.append(InRegion(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def compile_problem(oracle):
problem = oracle.problem
for obj in problem.goal_poses:
if problem.goal_poses[obj] == 'initial':
problem.goal_poses[obj] = oracle.initial_poses[obj]
elif problem.goal_poses[
obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial)
problem.goal_poses[obj] = oracle.initial_poses[
problem.goal_poses[obj]]
####################
O, P, G, Q, T = Param(OBJ), Param(POSE), Param(GRASP), Param(CONF), Param(
TRAJ)
Q1, Q2, OB, R = Param(CONF), Param(CONF), Param(OBJ), Param(REG)
#BT = Param(BASE_TRAJ)
rename_easy(locals())
actions = [
Action(
name='pick',
parameters=[O, P, G, Q, T],
condition=And(
PoseEq(O, P),
HandEmpty(),
Manip(O, P, G, Q, T),
ConfEq(Q), # NOTE - can remove ConfEq(Q)
ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
effect=And(PoseEq(O, None), GraspEq(O, G), Not(HandEmpty()),
Not(PoseEq(O, P)))),
Action(
name='place',
parameters=[O, P, G, Q, T],
condition=And(
PoseEq(O, None),
GraspEq(O, G),
Manip(O, P, G, Q, T),
ConfEq(Q), # NOTE - can remove ConfEq(Q)
ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
effect=And(PoseEq(O, P), HandEmpty(), Not(PoseEq(O, None)),
Not(GraspEq(O, G)))),
Action('clean',
parameters=[O, R],
condition=And(InRegion(O, R), IsSink(R)),
effect=Cleaned(O)),
Action('cook',
parameters=[O, R],
condition=And(Cleaned(O), InRegion(O, R), IsStove(R)),
effect=And(Cooked(O), Not(Cleaned(O)))),
Action(name='move',
parameters=[Q1, Q2],
condition=ConfEq(Q1),
effect=And(ConfEq(Q2), Not(ConfEq(Q1)))),
]
axioms = [
#Axiom(effect=Holding(O), condition=Exists([G], And(GraspEq(O, G), LegalGrasp(O, G)))),
STRIPSAxiom(conditions=[GraspEq(O, G), LegalGrasp(O, G)],
effects=[Holding(O)]),
Axiom(effect=InRegion(O, R),
condition=Exists([P], And(PoseEq(O, P), Contained(R, O, P)))),
#STRIPSAxiom(conditions=[PoseEq(O, P), ContainedCon(R, O, P)], effects=[InRegion(O, R)]),
Axiom(effect=Safe(O, T),
condition=Exists([P], And(PoseEq(O, P), CFree(O, P, T)))),
#STRIPSAxiom(conditions=[PoseEq(O, P), CFreeCon(O, P, T)], effects=[Safe(O, T)]),
]
# TODO - include parameters in STRIPS axiom?
####################
def sample_poses(o, num_samples=1):
while True:
if AVOID_INITIAL:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
yield list(
islice(
random_region_placements(oracle,
o,
oracle.get_counters(),
region_weights=True),
num_samples))
def sample_grasps(o):
yield get_grasps(oracle, o)
def sample_region(o, r, num_samples=1):
while True:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
yield list(
islice(
random_region_placements(oracle,
o, [r],
region_weights=True),
num_samples))
def sample_motion(o, p, g, max_calls=1, max_failures=50):
oracle.set_all_object_poses(
{o: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(o, p, g):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
if not pap.sample(oracle,
o,
max_failures=max_failures,
sample_vector=DO_ARM_MOTION,
sample_arm=DO_ARM_MOTION,
check_base=CHECK_BASE):
break
pap.obj = o
yield [(pap.approach_config, pap)]
def collision_free(o, p, t):
if p is None or o == t.obj:
return True
holding = ObjGrasp(t.obj, t.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
if not DO_ARM_MOTION:
return not oracle.holding_collision(t.grasp_config, o, p, holding)
return not oracle.traj_holding_collision(t.approach_config, t.trajs, o,
p, holding)
cond_streams = [
EasyListGenStream(inputs=[O],
outputs=[P],
conditions=[],
effects=[LegalPose(O, P)],
generator=sample_poses),
EasyListGenStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[LegalGrasp(O, G)],
generator=sample_grasps),
EasyListGenStream(inputs=[O, R],
outputs=[P],
conditions=[],
effects=[LegalPose(O, P),
Contained(R, O, P)],
generator=sample_region),
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[LegalPose(O, P),
LegalGrasp(O, G)],
effects=[Manip(O, P, G, Q, T)],
generator=sample_motion),
#MultiEasyGenStream(inputs=[Q1, Q2], outputs=[BT], conditions=[],
# effects=[Motion(Q1, BT, Q2)], generator=lambda q1, q2: [[None]]),
EasyTestStream(inputs=[O, P, T],
conditions=[LegalPose(O, P)],
effects=[CFree(O, P, T)],
test=collision_free,
eager=EAGER_TESTS),
]
####################
constants = [POSE(None)]
initial_atoms = [ConfEq(oracle.initial_config)] # TODO - toggle
holding = set()
if problem.start_holding is not False:
obj, grasp = problem.start_holding
initial_atoms += [
GraspEq(obj, grasp),
PoseEq(obj, None),
LegalGrasp(obj, grasp)
]
holding.add(obj)
if not holding:
initial_atoms.append(HandEmpty())
for obj, pose in oracle.initial_poses.iteritems():
if obj not in holding:
initial_atoms += [PoseEq(obj, pose), LegalPose(obj, pose)]
initial_atoms += [IsSink(region) for region in oracle.sinks]
initial_atoms += [IsStove(region) for region in oracle.stoves]
goal_literals = []
if problem.goal_holding is not None:
if problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif isinstance(problem.goal_holding, ObjGrasp):
goal_literals.append(
GraspEq(problem.goal_holding.object_name,
problem.goal_holding.grasp))
elif problem.goal_holding in oracle.get_objects():
goal_literals.append(Holding(problem.goal_holding))
else:
raise Exception()
for obj, pose in problem.goal_poses.iteritems():
goal_literals.append(PoseEq(obj, pose))
initial_atoms.append(LegalPose(obj, pose))
for obj, region in problem.goal_regions.iteritems():
goal_literals.append(InRegion(obj, region))
for obj in problem.goal_cleaned:
goal_literals.append(Cleaned(obj))
for obj in problem.goal_cooked:
goal_literals.append(Cooked(obj))
goal_formula = goal_literals
#goal_formula = And(*goal_literals)
#goal_formula = AbsCondition(goal_literals) # TODO - bug where goals must have And
return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
cond_streams, constants)
# condition=And(PoseEq(O, P), HandEmpty(), BaseEq(BQ), ManipEq(MQ), Kin(P, G, BQ, MQ)), #ForAll([OB], Or(Equal(O, OB), Safe(OB, T))))),
# effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
#
#Action(name='place', parameters=[O, P, G, BQ, MQ],
# condition=And(GraspEq(O, G), BaseEq(BQ), ManipEq(MQ), Kin(P, G, BQ, MQ),
# ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))),
# effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
# With grasp trajectory
Action(name='pick', parameters=[O, P, G, BQ, MQ, GT],
condition=And(PoseEq(O, P), HandEmpty(), BaseEq(BQ), ManipEq(MQ), GraspTraj(P, G, BQ, MQ, GT)),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place', parameters=[O, P, G, BQ, MQ, GT],
condition=And(GraspEq(O, G), BaseEq(BQ), ManipEq(MQ), GraspTraj(P, G, BQ, MQ, GT),
ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
# NOTE - I could make actions that backup or move forward instead
Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT],
#condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([O2], SafePoseMove(O2, MT))),
condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([O2], SafeMove(O2, MT))),
#condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ),
# ForAll([O2], Or(SafeGraspMove(O2, MT), SafePoseMove(O2, MT)))), # NOTE - bad idea, creates one per each combo
effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
#Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT],
# condition=And(HandEmpty(), ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([OB], SafePoseMove(OB, MT))),
# effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
#
#Action(name='move_arm_holding', parameters=[MQ, MQ2, BQ, O, G, MT],
def create_problem():
table = 'table'
room = 'room'
block = 'block'
table_pose = None
block_pose = None
if table_pose is None:
table_belief = frozenset({(room, 0.5), (None, 0.5)}) # Discrete distribution over poses
else:
table_belief = frozenset({(table_pose, 1.0)}) # Gaussian
if block_pose is None:
block_belief = frozenset({(table, 0.5), (None, 0.5)}) # Particle filter
else:
block_belief = frozenset({(table_pose, 1.0)}) # Gaussian
# I definitely am implicitly using belief conditions by asserting we will know the resultant pose
# Tables and Objects have three beliefs
# 1) Unknown
# 2) Coarse
# 3) Fine (with respect to base pose). Or could just add LowVariance condition when true
# Data types
CONF = Type()
ROOM = Type()
TABLE = Type() # Difference between fixed and movable objects
BLOCK = Type()
POSE = Type()
# Fluent predicates
AtConf = Pred(CONF)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Static predicates
LegalKin = Pred(POSE, CONF)
# Know that each block is at one pose at once (but don't know which one). Well
# Tables can be at only one pose. Only need to have argument for whether localized
UncertainT = Pred(TABLE)
UncertainB = Pred(BLOCK) # Has an internal distribution in it
AtPoseT = Pred(TABLE) # Has a fixed pose / convex hull in it
AtPoseB = Pred(BLOCK, POSE)
LocalizedT = Pred(TABLE)
LocalizedB = Pred(BLOCK)
#Scanned = Pred(ROOM)
#IsReal = Pred(POSE) # Could also specify all the fake values upfront
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
R, T = Param(ROOM), Param(TABLE)
rename_easy(locals()) # Trick to make debugging easier
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPoseB(B1, P1), LocalizedB(B1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPoseB(B1, P1)), Not(HandEmpty()), Not(LocalizedB(B1)))),
Action(name='place', parameters=[B1, P1, Q1], # Localize table?
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(AtPoseB(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move_base', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)), ForAll([B1], Not(LocalizedB(B1))))), # Set all known poses to be high uncertainty
#Action(name='scan', parameters=[R, T],
# condition=And(InRoom(R), AtConf(Q1)), # Should have a trajectory really
# condition=And(Believe(T), Not(Scanned(R))), # Scan from anywhere in the room
# effect=And(T)),
Action(name='move_head', parameters=[Q1, Q2], # Head conf, base conf, manip conf?
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))), # Should I undo localized if I move the head at all?
Action(name='scan_room', parameters=[T],
condition=UncertainT(T),
effect=And(AtPoseT(T), Not(UncertainT(T)))),
Action(name='scan_table', parameters=[T, B1, P1, Q1],
condition=And(AtPoseT(T), AtConf(Q1)),
effect=And(AtPoseB(B1, P1), Not(UncertainB(B1)))),
Action(name='look_table', parameters=[T, Q1],
condition=And(AtPoseT(T), AtConf(Q1)),
effect=LocalizedT(T)),
Action(name='look_block', parameters=[B1, P1, Q1],
condition=And(AtPoseB(B1, P1), AtConf(Q1)), # Visibility constraint
effect=LocalizedB(B1)),
#Action(name='stop', parameters=[T, Q1],
# condition=And(AtPoseT(T), AtConf(Q1)),
# effect=LocalizedT(T)),
]
axioms = [
#Axiom(effect=InRoom(R),
# condition=Exists([Q1], And(AtConf(Q1), ConfIn(Q1, R)))), # Infers B2 is at a safe pose wrt B1 at P1
]
# TODO: partially observable version of this
def inverse_kinematics(pose): # TODO: list stream that uses ending info
if type(pose) == str:
yield (pose + '_conf',) # Represents a hypothetical
yield (pose,)
def sample_table(table):
if not localized:
yield # Stuff
yield (pose,)
streams = [
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=inverse_kinematics),
]
constants = [
POSE('pose'), # Strings denote fake values
]
initial_atoms = [
AtConf(1),
HandEmpty(),
UncertainT(table),
UncertainB(block),
]
goal_literals = [Holding(block)]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, streams, constants)
return problem
def create_problem2():
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
# How would I specify table position
# From goal specification can derive prior
# Everything of same object type should be one variable? Otherwise, how would I update?
# I do actually have limits on the number of things
# Doing with would relive the strangeness when you have to update the others
# The strange thing is that we would like to distinguish the clusters in space when we do find them
p_table = 0.9
p_hit_exists = 0.99
p_miss_notexists = p_hit_exists
# Could use count based things or could just indicate confidence in sensor model
# Goal, object in hand
# Object starts out with high probability that its on a surface
surfaces = ['table%i'%i for i in range(3)]
# Different predicates for course belief and fine belief?
# Do I want to expose blocks as objects to belief?
# The probability that another table exists drops immensely once we find 3
# I think I always have to fix this number
# I suppose I could make a stream that generates new objects if desired
# Decrease the likelihood of later numbered objects
# Maybe I just use one table and allow it not to integrate to one?
# Why does the online deferral to use objects in the focused algorithm work?
# We often have streams for continuous values and these are the ones we want to defer
# Could I do this for discrete objects as well?
# Sure, just make a stream to generate them
# This is all about hte optimistic, I think there is a pose but I don't actually know it stuff
# Should the imaginary pose be explicit then?
# Maybe I should find a true plan but allow some objects to be imaginary
# Simultaneous actions to look and observe multiple things
blocks = ['block%i'%i for i in range(3)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i for i, block in enumerate(blocks)} # the initial pose for block i is i
goal_poses = {block: i+1 for i, block in enumerate(blocks)} # the goal pose for block i is i+1
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
ROOM = Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place', parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), # TODO - convert to finite blocks case?
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
Action(name='scan', parameters=[Q1], # Looks at a particular object. Discount costs for subsequent looks from that spot
condition=AtConf(Q1),
effect=And()),
Action(name='look', parameters=[Q1, O], # Look at surface vs object
condition=AtConf(Q1),
effect=And()),
]
axioms = [
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), # Infers B2 is at a safe pose wrt B1 at P1
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[],
generator=lambda: ((p,) for p in xrange(num_poses))), # Enumerating all the poses
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=lambda p: [(p,)]), # Inverse kinematics
TestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty()
] + [
AtPose(block, pose) for block, pose in initial_poses.iteritems()
]
goal_literals = [AtPose(block, pose) for block, pose in goal_poses.iteritems()]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
O, P, G = Param(OBJ), Param(POSE), Param(GRASP)
O2, O3 = Param(OBJ), Param(OBJ)
P2 = Param(POSE)
MQ, MQ2 = Param(MCONF), Param(MCONF)
MT = Param(MTRAJ)
GT = Param(GTRAJ)
rename_easy(locals())
####################
actions = [
Action(name='pick', parameters=[O, P, G, MQ, GT],
condition=And(PoseEq(O, P), HandEmpty(), ManipEq(MQ), GraspMotion(P, G, MQ, GT),
ForAll([O2], Or(Equal(O, O2), SafeGTraj(O2, GT)))),
#ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place', parameters=[O, P, G, MQ, GT],
condition=And(GraspEq(O, G), ManipEq(MQ), GraspMotion(P, G, MQ, GT),
ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
Action(name='move', parameters=[MQ, MQ2, MT],
condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, MT), ForAll([O2], SafeMTraj(O2, MT))),
effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
]
axioms = [
# Pick/Place collisions
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the countable TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place',
parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
Axiom(effect=Safe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1,
P2)))),
]
cond_streams = [
EasyGenStream(inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: irange(0, NUM_POSES)),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([p])),
EasyTestStream(inputs=[P1, P2],
conditions=[],
effects=[CollisionFree(P1, P2)],
test=lambda p1, p2: p1 != p2,
eager=EAGER_TESTS),
]
constants = []
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
]
if tamp_problem.initial_holding is False:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))
goal_literals = []
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses.iteritems():
goal_literals.append(AtPose(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def create_problem():
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i' % i for i in range(3)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i for i, block in enumerate(
blocks)} # the initial pose for block i is i
# the goal pose for block i is i+1
goal_poses = {block: i + 1 for i, block in enumerate(blocks)}
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(),
AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place', parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), # TODO - convert to finite blocks case?
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), # Infers B2 is at a safe pose wrt B1 at P1
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[],
generator=lambda: ((p,) for p in xrange(num_poses))), # Enumerating all the poses
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=lambda p: [(p,)]), # Inverse kinematics
TestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty()
] + [
AtPose(block, pose) for block, pose in initial_poses.iteritems()
]
goal_literals = [AtPose(block, pose)
for block, pose in goal_poses.iteritems()]
problem = STRIPStreamProblem(
initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
####################
O, P, G = Param(OBJ), Param(POSE), Param(GRASP)
O2, P2 = Param(OBJ), Param(POSE)
Q, Q2 = Param(CONF), Param(CONF)
T = Param(TRAJ)
rename_easy(locals())
####################
actions = [
Action(name='pick', parameters=[O, P, G, Q, T],
condition=And(PoseEq(O, P), HandEmpty(), ConfEq(Q), GraspMotion(P, G, Q, T),
ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))),
#ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place', parameters=[O, P, G, Q, T],
condition=And(GraspEq(O, G), ConfEq(Q), GraspMotion(P, G, Q, T),
ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeTraj(O2, T))))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
Action(name='move', parameters=[Q, Q2, T],
condition=And(ConfEq(Q), HandEmpty(), FreeMotion(Q, Q2, T),
ForAll([O2], SafeTraj(O2, T))),
effect=And(ConfEq(Q2), Not(ConfEq(Q)))),
Action(name='move_holding', parameters=[Q, Q2, T, O, G],
condition=And(ConfEq(Q), GraspEq(O, G), HoldingMotion(Q, Q2, G, T),
P2 = Param(POSE)
MQ, MQ2 = Param(MCONF), Param(MCONF)
MT = Param(MTRAJ)
GT = Param(GTRAJ)
rename_easy(locals())
####################
actions = [
Action(name='pick',
parameters=[O, P, G, MQ, GT],
condition=And(PoseEq(O, P), HandEmpty(), ManipEq(MQ),
GraspMotion(P, G, MQ, GT),
ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place',
parameters=[O, P, G, MQ, GT],
condition=And(GraspEq(O, G), ManipEq(MQ), GraspMotion(P, G, MQ, GT),
ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
Action(name='move',
parameters=[MQ, MQ2, MT],
condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, MT),
ForAll([O2], SafeMove(O2, MT))),
effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
]
axioms = [
# Pick/Place collisions
