def constraint_to_stripstream(fts_problem, do_axioms=True, test_streams=True):
expanded_state_vars = expand_variables(fts_problem.state_vars)
#expanded_control_vars = expand_variables(fts_problem.control_vars) # TODO - use this to check variables
var_map = fts_problem.get_variable_map()
axiom_map = {}
initial_atoms = convert_initial_state(fts_problem.initial_state, var_map,
expanded_state_vars)
goal_literals = convert_goal_constraints(fts_problem.goal_constraints,
var_map, axiom_map)
actions, axioms = convert_transition(fts_problem.transition, var_map,
axiom_map)
cond_streams = map(
convert_sampler,
fts_problem.samplers) # TODO - prune not useful samplers
convert_constraint_forms(fts_problem.get_constraint_forms(),
initial_atoms,
cond_streams,
test_streams=test_streams)
objects = convert_domains(fts_problem.state_vars +
fts_problem.control_vars)
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, objects)
def compile_problem(oracle):
problem = oracle.problem
CSpace.robot_base(oracle.robot).set_active()
initial_config = Config(
base_values_from_full_config(oracle.initial_config.value))
initial_atoms = [
AtConfig(Conf(initial_config)),
]
print 'Initial', initial_config.value
goal_literals = [AtConfig(Conf(problem.goal_config))
] if problem.goal_config is not None else []
print 'Goal', problem.goal_config.value
actions = [
Move(oracle),
]
axioms = [
#InRegionAxiom(),
]
cond_streams = [
CollisionFreeTest(oracle),
ConfStream(oracle),
]
objects = []
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, objects)
def strips_planner(universe, search, max_time=INF, max_cost=INF, verbose=False):
initial_atoms = filter(lambda atom: not isinstance(atom, Initialize), universe.get_initial_atoms())
objects = universe.type_to_objects
problem = STRIPStreamProblem(initial_atoms, universe.goal_formula, universe.actions + universe.axioms, []) # TODO - constants?
static_map = {pr: [] for pr in problem.get_static_predicates()}
for atom in initial_atoms:
if atom.predicate in static_map:
static_map[atom.predicate].append(atom)
initial_state = State(PyLiteral(atom) for atom in initial_atoms if atom.predicate not in static_map)
dnf_goal = list(universe.goal_formula.propositional(objects).get_literals())
if not dnf_goal:
return []
if len(dnf_goal) != 1:
raise NotImplementedError('Currently only support a conjunctive goal: %s'%universe.goal_formula)
goal_state = PartialState(map(PyLiteral, dnf_goal[0]))
strips_actions = list(get_actions(universe.actions, static_map, objects))
strips_axioms = list(get_axioms(universe.axioms, static_map, objects))
if verbose:
print 'Actions:', len(strips_actions)
print 'Axioms:', len(strips_axioms)
plan, state_space = default_derived_plan(initial_state, goal_state, strips_actions, strips_axioms)
if verbose:
print plan
print state_space
if plan is None:
return None
return [(universe.name_to_action[action.lifted.name], action.args) for action in plan.operators]
def observable_problem(belief, goal, costs=True): # NOTE - costs is really important here
#objects = belief.objLoc.keys()
locations = belief.occupancies.keys()
states = [dirty, clean, dry, wet]
initial_atoms = []
occupied = set()
for obj in belief.objLoc.keys():
dist = belief.objLocDist(obj)
loc, p = dist.computeMPE() # TODO - concentration inequalities for Gaussian
if p >= LOC_CONFIDENCE:
initial_atoms.append(At(obj, loc))
occupied.add(loc)
else:
initial_atoms.append(UnsureLoc(obj))
for loc in locations:
p = dist.prob(loc)
cost = int(COST_SCALE*1./max(p, MIN_P)) if costs else COST_SCALE*1 # TODO - set to infinity if too large (equivalent to blocking)
initial_atoms.append(Initialize(FindCost(OBJ(obj), LOC(loc)), cost))
#if p < MIN_CONFIDENCE:
# initial_atoms.append(NotAtLoc(obj, loc))
for obj in belief.objState.keys():
dist = belief.objState[obj]
state, p = dist.computeMPE()
if p >= STATE_CONFIDENCE:
initial_atoms.append(HasState(obj, state))
else:
initial_atoms.append(UnsureState(obj))
for state in states:
p = dist.prob(state)
cost = int(COST_SCALE*1./max(p, MIN_P)) if costs else COST_SCALE*1
initial_atoms.append(Initialize(InspectStateCost(OBJ(obj), STATE(state)), cost))
#if p < MIN_CONFIDENCE:
# initial_atoms.append(NotHasState(obj, loc))
for loc in belief.occupancies.keys():
if loc == 'washer':
initial_atoms.append(IsWasher(loc))
elif loc == 'painter':
initial_atoms.append(IsPainter(loc))
elif loc == 'dryer':
initial_atoms.append(IsDryer(loc))
goal_literals = []
for fluent in goal.fluents:
if isinstance(fluent, Bd):
literal, arg, prob = fluent.args
if isinstance(literal, ObjLoc):
goal_literals.append(At(literal.args[0], literal.value))
elif isinstance(literal, ObjState):
goal_literals.append(HasState(literal.args[0], arg))
elif isinstance(literal, toyTest.Clear):
goal_literals.append(Clear(literal.args[0]))
else:
raise NotImplementedError(literal)
else:
raise NotImplementedError(fluent)
return STRIPStreamProblem(initial_atoms, goal_literals, actions, [], [])
def get_problem():
region = [1, 2, 3]
#mergeable_types = {BLOCK}
block = C('block%s' % 1, BLOCK)
obstacle = C('block%s' % 2, BLOCK)
blocks = [block]
#blocks = [block, obstacle]
pose = Pose(10)
config = Config(0)
region = C('region%s' % 1, REGION)
initial_atoms = [
AtConfig(config),
HandEmpty(),
AtPose(block, pose),
#AtPose(obstacle, Pose(20)),
#Unsafe(pose),
] + [
#AtPose(obj, pose) for obj, pose in object_poses.iteritems()
]
goal_literals = [
Holding(block),
#Not(HandEmpty(robot)), # NOTE - negative goal
#Not(AtPose(obj, Pose(0))),
#AtPose(block, Pose(20)),
#AtPose(obj, Pose(4)),
#AtConfig(Config(5)),
#AtConfig(robot, Config(4)),
#InRegion(block, region),
]
operators = [
Move(),
Pick(),
Place(blocks),
#InRegionAxiom(),
]
if COLLISIONS:
operators.append(SafeAxiom())
cond_streams = [
PoseStream(),
#IKStream(),
CollisionFreeTest(),
ConfigStream(),
#IKStreamBad(),
IKTest(),
]
objects = [ # Assorted objects
]
return STRIPStreamProblem(initial_atoms, goal_literals, operators,
cond_streams, objects)
def create_problem():
"""
Creates a blocksworld STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
BLOCK = Type()
# Fluent predicates
Clear = Pred(BLOCK)
OnTable = Pred(BLOCK)
ArmEmpty = Pred()
Holding = Pred(BLOCK)
On = Pred(BLOCK, BLOCK)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
rename_easy(locals())
actions = [
Action(name='pickup', parameters=[B1],
condition=And(Clear(B1), OnTable(B1), ArmEmpty()),
effect=And(Holding(B1), Not(Clear(B1)), Not(OnTable(B1)), Not(ArmEmpty()))),
Action(name='putdown', parameters=[B1],
condition=Holding(B1),
effect=And(Clear(B1), OnTable(B1), ArmEmpty(), Not(Holding(B1)))),
Action(name='stack', parameters=[B1, B2],
condition=And(Clear(B2), Holding(B1)),
effect=And(Clear(B1), On(B1, B2), ArmEmpty(), Not(Clear(B2)), Not(Holding(B1)))),
Action(name='unstack', parameters=[B1, B2],
condition=And(Clear(B1), On(B1, B2), ArmEmpty()),
effect=And(Clear(B2), Holding(B1), Not(Clear(B1)), Not(On(B1, B2)), Not(ArmEmpty()))),
]
axioms = []
cond_streams = []
constants = []
initial_atoms = [
OnTable('A'),
OnTable('B'),
OnTable('C'),
Clear('A'),
Clear('B'),
Clear('C'),
ArmEmpty(),
]
goal_literals = [On('A', 'B'), On('B', 'C')]
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
def compile_problem(tamp_problem):
blocks = [BlockO(block) for block in tamp_problem.get_blocks()]
initial_atoms = [
AtConfig(ConfigO(tamp_problem.initial_config)),
] + [
AtPose(BlockO(block), PoseO(pose))
for block, pose in tamp_problem.initial_poses
] + [
IsPose(BlockO(block), PoseO(pose))
for block, pose in tamp_problem.initial_poses
]
if tamp_problem.initial_holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(BlockO(tamp_problem.initial_holding)))
goal_literals = []
if tamp_problem.goal_config is not None:
goal_literals.append(AtConfig(ConfigO(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(BlockO(tamp_problem.goal_holding)))
for block, goal in tamp_problem.goal_poses:
goal_literals.append(AtPose(BlockO(block), PoseO(goal)))
for block, goal in tamp_problem.goal_regions:
goal_literals.append(InRegion(BlockO(block), RegionO(goal)))
operators = [
Pick(),
Place(),
InRegionAxiom(),
]
cond_streams = [
GraspStream(tamp_problem.robot),
RegionStream(),
RegionTest(),
IKStream(tamp_problem),
]
if COLLISIONS:
operators.append(SafeAxiom())
cond_streams.append(CollisionFreeTest())
if USE_BASE:
operators.append(Move(blocks))
operators.append(MoveHolding(blocks))
cond_streams.append(MotionStream(tamp_problem))
objects = [ # Assorted objects
RegionO(tamp_problem.env_region),
]
return STRIPStreamProblem(initial_atoms, goal_literals, operators,
cond_streams, objects)
def observable_problem(env, start, goal):
locations = start.details.occupancies.keys()
initial_atoms = []
constants = []
occupied = set()
for obj in env.objects:
for attr in env.objects[obj]:
if attr == 'clean':
initial_atoms.append(Clean(obj))
elif attr == 'dirty':
pass
elif attr in locations:
initial_atoms.append(At(obj, attr))
occupied.add(attr)
else:
raise NotImplementedError(attr)
for loc in locations:
if loc not in occupied:
initial_atoms.append(Clear(loc))
constants.append(LOC(loc))
if loc == 'washer':
initial_atoms.append(IsWasher(loc))
if loc == 'painter':
initial_atoms.append(IsPainter(loc))
if loc == 'dryer':
initial_atoms.append(IsDryer(loc))
goal_literals = []
for fluent in goal.fluents:
if isinstance(fluent, Bd):
literal, arg, prob = fluent.args
if isinstance(literal, ObjLoc):
goal_literals.append(At(literal.args[0], literal.value))
elif isinstance(literal, ObjState) and arg == 'clean':
goal_literals.append(Clean(literal.args[0]))
elif isinstance(literal, ObjState) and arg == 'wetPaint':
goal_literals.append(WetPaint(literal.args[0]))
elif isinstance(literal, ObjState) and arg == 'dryPaint':
goal_literals.append(DryPaint(literal.args[0]))
else:
raise NotImplementedError(literal)
else:
raise NotImplementedError(fluent)
return STRIPStreamProblem(initial_atoms, goal_literals, actions, [],
constants)
def compile_belief(belief, goal):
constants = map(OBJ, belief.objLoc.keys()) + map(LOC,
belief.occupancies.keys())
#constants = []
initial_atoms = [UnknownAt(obj) for obj in belief.objLoc]
initial_atoms += [
BAt(obj, belief.objLocDist(obj)) for obj in belief.objLoc
] # NOTE - objLocDist != objLoc
goal_literals = []
for fluent in goal.fluents:
if isinstance(fluent, Bd):
literal, arg, prob = fluent.args
if isinstance(literal, ObjLoc):
goal_literals.append(
BAtAbove(literal.args[0], literal.value, prob))
#elif isinstance(literal, ObjState) and arg == 'clean':
# goal_literals.append(BClean(literal.args[0], prob))
else:
raise NotImplementedError(literal)
else:
raise NotImplementedError(fluent)
cond_streams = [
# Forward streams
#LookUpdate(),
#PerfectLookUpdate(),
# Oriented streams
#LookPlan(),
#LookJump(),
PerfectLookJump(),
#PerfectLookJump2(),
# Independent costs
LookCostFn(),
MoveCostFn(),
MoveUpdate(),
#GeneratorStream(inputs=[L, B, L2], outputs=[B2], conditions=[], effects=[IsMoveUpdate(L, B, L2, B2)],
# generator=lambda l, b, l2: [move_update(l, b, l2)[0]]),
#TestStream(inputs=[B, L, P], conditions=[], effects=[BSatisfies(B, L, P)],
# test=is_above, eager=True),
]
return STRIPStreamProblem(initial_atoms, goal_literals, actions,
cond_streams, constants)
def get_problem():
satellite = C('satellite1', SATELLITE)
satellite_pos = 50
initial_atoms = [
#AtState(Position(0), Velocity(0)),
AtState(State((0, 0))),
Carrying(satellite),
ArePair(State((satellite_pos, 0)), Position(satellite_pos)),
Landed(),
]
goal_literals = [
#Above(Position(20)),
#AtState(State((50, 0))),
AtOrbit(satellite, Position(satellite_pos)),
Landed(),
]
operators = [
Burst(),
Glide(),
Deploy(),
TakeOff(),
Land(),
AboveAxiom(),
]
cond_streams = [
ForwardBurstStream(
), # TODO - BackwardBurstStream in order to move back from the goal
#FixedBurstStream(),
TargetBurstStream(),
ForwardGlideStream(),
AboveTest(),
]
objects = [ # Assorted objects
#Acceleration(ACCELERATION_RANGE[1]),
#Acceleration(ACCELERATION_RANGE[0]),
#Acceleration(0),
]
return STRIPStreamProblem(initial_atoms, goal_literals, operators,
cond_streams, objects)
def create_abstract_problem(state, goal):
# Goal serialization is basically like this
# Necessary and sufficient?
tables, initial_poses, initial_config, holding = state
# TODO: connect this to goals on the actual states
# The quantification of poses and grasps is independent
# Conditions are derived or atoms
actions = [
Action(
name='pick',
parameters=[B, S],
condition=And(On(B, S), HandEmpty()), #, AtConf(Q)),
#condition=And(On(B, S), HandEmpty(), Not(Holding(B))), # , AtConf(Q)),
effect=And(Holding(B), Not(On(B, S)), Not(HandEmpty()))),
Action(
name='place',
parameters=[B, S],
condition=And(Holding(B)), #, AtConf(Q)),
effect=And(On(B, S), HandEmpty(), Not(Holding(B)))),
#Action(name='move', parameters=[S1, Q2],
# condition=AtConf(Q),
# effect=And(AtConf(Q2), Not(AtConf(Q)))),
]
axioms = []
cond_streams = []
initial_atoms = [
#AtConf(initial_config),
HandEmpty()
] + [On(block, pose.sur) for block, pose in initial_poses.items()]
goal_literals = [On(b, s) for b, s in goal.on.items()]
if goal.holding is not None:
goal_literals.append(Holding(goal.holding))
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams)
return problem
def compile_problem(tamp_problem, stream_fn=generative_streams):
blocks = [C(block, BLOCK) for block in tamp_problem.get_blocks()]
initial_atoms = [
AtConfig(Config(tamp_problem.initial_config)),
] + [
AtPose(C(block, BLOCK), Pose(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(C(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(C(tamp_problem.goal_holding, BLOCK)))
for block, goal in tamp_problem.goal_poses.iteritems():
goal_literals.append(AtPose(C(block, BLOCK), Pose(goal)))
operators = [
Move(),
#H1Pick(),
Pick(),
Place(blocks),
#InRegionAxiom(),
]
if COLLISIONS:
operators.append(SafeAxiom())
objects = [ # Assorted objects
]
return STRIPStreamProblem(initial_atoms, goal_literals, operators,
stream_fn(), objects)
def create_problem(goal, obstacles=(), distance=.25, digits=3):
"""
Creates a Probabilistic Roadmap (PRM) motion planning problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POINT = Type()
REGION = Type()
# Fluent predicates
AtPoint = Pred(POINT)
# Derived predicates
InRegion = Pred(REGION)
#IsReachable = Pred(POINT, POINT)
IsReachable = Pred(POINT)
# Stream predicates
IsEdge = Pred(POINT, POINT)
Contained = Pred(POINT, REGION)
# Free parameters
P1, P2 = Param(POINT), Param(POINT)
R = Param(REGION)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='move',
parameters=[P1, P2],
condition=And(AtPoint(P1), IsReachable(P2)),
effect=And(AtPoint(P2), Not(AtPoint(P1))))
]
axioms = [
Axiom(effect=InRegion(R),
condition=Exists([P1], And(AtPoint(P1), Contained(P1, R)))),
Axiom(effect=IsReachable(P2),
condition=Or(AtPoint(P2),
Exists([P1], And(IsReachable(P1), IsEdge(P1,
P2))))),
]
####################
def sampler():
for _ in inf_sequence():
yield [(sample(digits), ) for _ in range(10)]
roadmap = set()
def test(p1, p2):
if not (get_distance(p1, p2) <= distance and is_collision_free(
(p1, p2), obstacles)):
return False
roadmap.add((p1, p2))
return True
####################
# Conditional stream declarations
cond_streams = [
EasyListGenStream(
inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=sampler
), # NOTE - version that only generators collision-free points
GeneratorStream(inputs=[R],
outputs=[P1],
conditions=[],
effects=[Contained(P1, R)],
generator=lambda r:
(sample_box(r) for _ in inf_sequence())),
TestStream(inputs=[P1, P2],
conditions=[],
effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
test=test,
eager=True),
]
####################
constants = []
initial_atoms = [
AtPoint((0, 0)),
]
goal_literals = []
if is_region(goal):
goal_literals.append(InRegion(goal))
else:
goal_literals.append(AtPoint(goal))
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem, roadmap
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]]
# oracle.initial_config = ??? # TODO - set the initial configuration
transit_conf = Config(oracle.default_left_arm_config)
####################
O, P, G, T = Param(OBJ), Param(POSE), Param(GRASP), Param(TRAJ)
OB, R = Param(OBJ), Param(REG)
BQ, MQ = Param(BASE_CONF), Param(MANIP_CONF)
BQ2, MQ2 = Param(BASE_CONF), Param(MANIP_CONF)
BT, MT = Param(BASE_TRAJ), Param(MANIP_TRAJ)
rename_easy(locals())
# Separate arm and base configs and trajectories?
# Make a fixed configuration for arm stuff
# If I separate arm and base poses, I can avoid worrying about collision when the base and things
# Would I ever want to actually separate these and use the same grasp at a different config?
# - Maybe for two arms?
# - I don't want to reuse trajectories at different base configs. That wouldn't make sense
# - Although if I manipulate two configs at once, maybe it would!
# - Make constant arm transit configs?
# TODO - maybe make an axiom to handle BT and MT when doing these things?
# TODO - I could equip a manip conf with a base conf at all times if I really don't want them separate
# NOTE - put then would need to update them whenever moving the base
#actions = [
# Action(name='pick', parameters=[O, P, G, BQ, MQ],
# condition=And(PoseEq(O, P), HandEmpty(), Kin(O, P, G, BQ, MQ), BaseEq(BQ), ManipEq(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), Kin(O, P, G, BQ, MQ), BaseEq(BQ), ManipEq(MQ)),
# #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
# effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
#
# Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT],
# condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT)),
# effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
#
# Action(name='move_base', parameters=[BQ, BQ2, MQ, BT], # TODO - put the arm motion stuff in an axiom
# condition=And(BaseEq(BQ), TransitManip(MQ), BaseMotion(BQ, BQ2, BT)),
# effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))),
# ]
# TODO - should I include the grasp in move backwards trajectory?
actions = [
abs_action(
name='pick',
parameters=[O, P, G, BQ, MQ],
conditions=[
And(PoseEq(O, P), HandEmpty(),
Kin(O, P, G, BQ,
MQ)), #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
And(BaseEq(BQ), ManipEq(MQ))
],
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
abs_action(
name='place',
parameters=[O, P, G, BQ, MQ],
conditions=[
And(GraspEq(O, G),
Kin(O, P, G, BQ,
MQ)), #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
And(BaseEq(BQ), ManipEq(MQ))
],
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
# NOTE - the hierarchical versions of this
abs_action(
name='move_arm',
parameters=[MQ, MQ2, BQ,
MT], # TODO - Or(TransitManip(MQ), TransitManip(MQ2))
conditions=[
And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ)),
#And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), Or(TransitManip(MQ), TransitManip(MQ2))) # This does something odd
#And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ),
# Or(TransitManip(MQ), LegalConf(BQ, MQ)),
# Or(TransitManip(MQ2), LegalConf(BQ, MQ2))) # This does something odd
], # TODO - put an Or(Pair(MQ, BQ), Pair(MQ2, BQ)) or something
effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
abs_action(name='move_base',
parameters=[BQ, BQ2, BT],
conditions=[
And(BaseEq(BQ), SafeManip(BT), BaseMotion(BQ, BQ2, BT))
],
effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))),
#abs_action(name='move_base', parameters=[BQ, BQ2, MQ, BT],
# conditions=[
# And(BaseEq(BQ), TransitManip(MQ), BaseMotion(BQ, BQ2, BT))],
# effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))),
]
# NOTE - the parameters really aren't necessary
# TODO - could remove any dependence on tests because there are so cheap with the evaluation (already can't use them with axioms)
# TODO - could also just make the cost only depend on the introduced objects within the effects
axioms = [
Axiom(effect=Holding(O),
condition=Exists([G], And(GraspEq(O, G), LegalGrasp(O, G)))),
Axiom(effect=InRegion(O, R),
condition=Exists([P], And(PoseEq(O, P), Contained(R, O, P)))),
#Axiom(effect=Safe(O, T), condition=Exists([P], And(PoseEq(O, P), CFree(O, P, T)))),
#Axiom(effect=SafeArm(O, MQ, T), condition=Exists([P, MQ], And(PoseEq(O, P), ManipEq(MQ), CFree(O, P, T)))),
Axiom(effect=SafeManip(BT),
condition=Exists(
[MQ], And(ManipEq(MQ),
TransitManip(MQ)))), # TODO - add condition here
]
# TODO - include parameters in STRIPS axiom?
####################
def get_base_conf(conf):
return Config(base_values_from_full_config(conf.value))
def get_arm_conf(conf):
return Config(
arm_from_full_config(oracle.robot.GetActiveManipulator(),
conf.value))
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=False,
sample_arm=False,
check_base=False):
break
#pap.obj = o
yield [(get_base_conf(pap.grasp_config),
get_arm_conf(pap.grasp_config))]
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),
#MultiEasyGenStream(inputs=[O, P, G], outputs=[BQ, MQ], conditions=[LegalPose(O, P), LegalGrasp(O, G)],
# effects=[Kin(O, P, G, BQ, MQ)], generator=sample_motion),
EasyListGenStream(inputs=[O, P, G],
outputs=[BQ, MQ],
conditions=[LegalPose(O, P),
LegalGrasp(O, G)],
effects=[Kin(O, P, G, BQ, MQ),
LegalConf(BQ, MQ)],
generator=sample_motion),
# TODO - this doesn't work for constants
# TODO - make a condition that MQ, MQ2 both work for BQ. Otherwise, it will still create a ton of streams
#EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
# effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1),
EasyGenStream(inputs=[MQ, MQ2, BQ],
outputs=[MT],
conditions=[TransitManip(MQ),
LegalConf(BQ, MQ2)],
effects=[ManipMotion(MQ, MQ2, BQ, MT)],
generator=lambda *args: [None],
order=1),
EasyGenStream(inputs=[MQ, MQ2, BQ],
outputs=[MT],
conditions=[LegalConf(BQ, MQ),
TransitManip(MQ2)],
effects=[ManipMotion(MQ, MQ2, BQ, MT)],
generator=lambda *args: [None],
order=1),
#EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[LegalConf(BQ, MQ), LegalConf(BQ, MQ2)],
# effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1),
EasyGenStream(inputs=[BQ, BQ2],
outputs=[BT],
conditions=[],
effects=[BaseMotion(BQ, BQ2, BT)],
generator=lambda *args: [None],
order=1),
#effects=[BaseMotion(BQ, BQ2, BT)], generator=lambda *args: [None], order=2), # NOTE - causes a bug!
#EasyTestStream(inputs=[O, P, T], conditions=[LegalPose(O, P)], effects=[CFree(O, P, T)],
# test=collision_free, eager=EAGER_TESTS),
]
####################
#print transit_conf.value
#print oracle.initial_config.value
initial_base = get_base_conf(oracle.initial_config)
initial_manip = get_arm_conf(oracle.initial_config)
print initial_base.value
print initial_manip.value
constants = []
initial_atoms = [
BaseEq(initial_base),
ManipEq(initial_manip),
HandEmpty(),
TransitManip(transit_conf),
LegalConf(initial_base, initial_manip),
]
for obj, pose in oracle.initial_poses.iteritems():
initial_atoms += [PoseEq(obj, pose), LegalPose(obj, pose)]
# TODO - serialize goals by object name
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))
#goal_formula = And(*goal_literals)
#goal_formula = AbsCondition(map(And, goal_literals)) # TODO - bug where goals must have And
goal_formula = AbsCondition(
goal_literals) # TODO - bug where goals must have And
return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
cond_streams, constants)
def amber_stripstream(use_window=True, step=False):
problem = dantam2()
window_3d = Window3D(title='World',
windowWidth=500,
noWindow=(not use_window),
viewport=problem.workspace.flatten('F'))
if is_window_active(window_3d):
draw_window_3d(
window_3d, problem.initial_conf,
[body.applyTrans(pose) for body, pose in problem.initial_poses])
raw_input('Start?')
robot = problem.initial_conf.robot
name_to_body = {get_name(body): body for body, _ in problem.initial_poses}
static_bodies = [
body.applyTrans(pose) for body, pose in problem.initial_poses
if get_name(body) not in problem.movable_names
]
block = name_to_body[problem.movable_names[-1]] # Generic object
####################
# NOTE - can either modify the world state or create new bodies
# Collision free test between an object at pose1 and an object at pose2
def cfree_pose(pose1, pose2):
return not ((pose1 == pose2) or block.applyTrans(pose1).collides(
block.applyTrans(pose2)))
def cfree_traj(
traj, pose
): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose
if DISABLE_TRAJ_COLLISIONS:
return True
placed = [body.applyTrans(pose)]
attached = None
if traj.grasp is not None:
attached = {
ARM:
body.applyTrans(
get_wrist_frame(traj.grasp, robot, ARM).inverse())
}
# TODO - cache bodies for these configurations
collision_fn = get_collision_fn(placed, attached)
return not any(collision_fn(q) for q in traj.path)
####################
# Sample pregrasp config and motion plan that performs a grasp
def sample_grasp_traj(pose, grasp):
attached = {
ARM: body.applyTrans(get_wrist_frame(grasp, robot, ARM).inverse())
}
collision_fn = get_collision_fn(static_bodies, attached)
manip_frame = manip_from_pose_grasp(pose, grasp, robot, ARM)
grasp_conf = next(
inverse_kinematics(problem.initial_conf, manip_frame, ARM),
None) # Grasp configuration
if grasp_conf is None or collision_fn(grasp_conf):
return
# NOTE - I could also do this in the opposite order
pregrasp_frame = approach_from_manip(manip_frame)
pregrasp_conf = next(
inverse_kinematics(grasp_conf, pregrasp_frame,
ARM)) # Pregrasp configuration
if pregrasp_conf is None or collision_fn(pregrasp_conf):
return
if DISABLE_GRASP_TRAJECTORIES:
yield (pregrasp_conf, Traj([grasp_conf], grasp))
return
grasp_path = direct_path(
grasp_conf,
pregrasp_conf, # Trajectory from grasp configuration to pregrasp
get_extend_fn([robot.armChainNames[ARM]], STEP_SIZE),
collision_fn)
if grasp_path is None:
return
yield (pregrasp_conf, Traj(grasp_path, grasp))
def sample_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_MOTION_TRAJECTORIES:
yield (Traj([conf2]), )
return
active_chains = [robot.armChainNames[ARM]]
path = birrt(conf1,
conf2,
get_distance_fn(active_chains),
get_sample_fn(conf1, active_chains),
get_extend_fn(active_chains, STEP_SIZE),
get_collision_fn(static_bodies),
restarts=0,
iterations=50,
smooth=None)
if path is None:
return
yield (Traj(path), )
def sample_holding_motion(conf1, conf2,
grasp): # Sample motion while holding
if DISABLE_MOTION_TRAJECTORIES:
yield (Traj([conf2], grasp), )
return
active_chains = [robot.armChainNames[ARM]]
attached = {
ARM: body.applyTrans(get_wrist_frame(grasp, robot, ARM).inverse())
}
path = birrt(conf1,
conf2,
get_distance_fn(active_chains),
get_sample_fn(conf1, active_chains),
get_extend_fn(active_chains, STEP_SIZE),
get_collision_fn(static_bodies, attached),
restarts=0,
iterations=50,
smooth=None)
if path is None:
return
yield (Traj(path, grasp), )
####################
cond_streams = [
# Pick/place trajectory
EasyGenStream(inputs=[P, G],
outputs=[Q, T],
conditions=[],
effects=[GraspMotion(P, G, Q, T)],
generator=sample_grasp_traj),
# Move trajectory
EasyGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_motion,
order=1,
max_level=0),
EasyGenStream(inputs=[Q, Q2, G],
outputs=[T],
conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)],
generator=sample_holding_motion,
order=1,
max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePose(P, P2)],
test=cfree_pose,
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=cfree_traj),
]
####################
constants = map(GRASP, TOP_GRASPS[:MAX_GRASPS]) + map(
POSE, problem.known_poses)
initial_atoms = [
ConfEq(problem.initial_conf),
HandEmpty(),
] + [
PoseEq(get_name(body), pose) for body, pose in problem.initial_poses
if get_name(body) in problem.movable_names
]
goal_literals = [
PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()
]
if problem.goal_conf is not None:
goal_literals.append(ConfEq(problem.goal_conf))
stream_problem = STRIPStreamProblem(initial_atoms, And(*goal_literals),
actions + axioms, cond_streams,
constants)
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
plan, _ = incremental_planner(stream_problem,
search=search_fn,
frequency=1,
waves=True,
debug=False)
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action
else:
print 'Failure'
if is_window_active(window_3d) and plan is not None:
robot_conf = problem.initial_conf
placements = {
get_name(body): body.applyTrans(pose)
for body, pose in problem.initial_poses
}
attached = {'left': None, 'right': None}
def _execute_traj(path):
for j, conf in enumerate(path):
draw_window_3d(window_3d, conf, placements.values(), attached)
if step:
raw_input('%s/%s) Step?' % (j, len(path)))
else:
window_3d.update()
sleep(0.01)
draw_window_3d(window_3d, robot_conf, placements.values())
raw_input('Start?')
for i, (action, args) in enumerate(plan):
if action.name == 'move':
_, _, traj = args
_execute_traj(traj.path)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
_execute_traj(traj.path)
elif action.name == 'pick':
obj, _, grasp, _, traj = args
_execute_traj(traj.path[::-1])
attached[ARM] = name_to_body[obj].applyTrans(
get_wrist_frame(grasp, robot, ARM).inverse())
del placements[obj]
_execute_traj(traj.path)
elif action.name == 'place':
obj, pose, _, _, traj = args
_execute_traj(traj.path[::-1])
placements[obj] = name_to_body[obj].applyTrans(pose)
attached[ARM] = None
_execute_traj(traj.path)
else:
raise ValueError(action.name)
raw_input('Finish?')
def create_problem(initRobotPos = (0.5, 0.5),
initRobotVar = 0.01,
maxMoveDist = 5.0,
beaconPos = (1, 1),
homePos = (0, 0),
goalPosEps = 0.1,
goalVar = 0.1,
odoErrorRate = 0.1,
obsVarPerDistFromSensor = 10.0,
minObsVar = 0.001,
domainSize = 20,
verboseFns = True):
"""
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POS = Type() # 2D position
VAR = Type() # 2D variance
DIST = Type() # positive scalar
# Fluent predicates
RobotPos = Pred(POS)
RobotVar = Pred(VAR)
# Derived predicates
KnowYouAreHome = Pred()
# Static predicates
DistBetween = Pred(POS, POS, DIST) # Distance between two positions (function)
LessThanV = Pred(VAR, VAR) # Less than, on distances
LessThanD = Pred(DIST, DIST) # Less than, on distances
OdometryVar = Pred(VAR, VAR, DIST) # How does odometry variance increase?
SensorVar = Pred(VAR, VAR, DIST) # How does an observation decrease variance?
LegalPos = Pred(POS) # Any legal robot pos
# Free parameters
RPOS1, RPOS2, RVAR1, RVAR2 = Param(POS), Param(POS), Param(VAR), Param(VAR)
DIST1, DIST2 = Param(DIST), Param(DIST)
def odoVarFun(rv, d):
odoVar = (d * odoErrorRate)**2
result = rv + odoVar
if verboseFns: print 'ovf:', rv, d, result
return [result]
def sensorVarFun(rv, d):
obsVar = max(d / obsVarPerDistFromSensor, minObsVar)
result = 1.0 / ((1.0 / rv) + (1.0 / obsVar))
if verboseFns: print 'svf:', rv, d, result
return [result]
def randPos():
while True:
result = (random.random() * domainSize, random.random() * domainSize)
print 'rp:', result
yield [result]
def legalTest(rp):
(x, y) = rp
result = (0 <= x <= domainSize) and (0 <= y <= domainSize)
if not result: print 'not legal:', rp
return result
actions = [
Action(name='Move',
parameters=[RPOS1, RPOS2, RVAR1, RVAR2, DIST1],
condition = And(RobotPos(RPOS1),
RobotVar(RVAR1),
LegalPos(RPOS2), # Generate an intermediate pos
DistBetween(RPOS1, RPOS2, DIST1),
LessThanD(DIST1, maxMoveDist),
OdometryVar(RVAR1, RVAR2, DIST1)),
effect = And(RobotPos(RPOS2),
RobotVar(RVAR2),
Not(RobotPos(RPOS1)),
Not(RobotVar(RVAR1)))),
# Action(name='Look',
# parameters=[RPOS1, RVAR1, RVAR2, DIST1],
# condition = And(RobotPos(RPOS1),
# RobotVar(RVAR1),
# DistBetween(RPOS1, beaconPos, DIST1),
# SensorVar(RVAR1, RVAR2, DIST1)),
# effect = And(RobotVar(RVAR2),
# Not(RobotVar(RVAR1))))
]
axioms = [
Axiom(effect = KnowYouAreHome(),
condition = Exists([RPOS1, RVAR1, DIST1],
And(RobotPos(RPOS1),
RobotVar(RVAR1),
DistBetween(RPOS1, homePos, DIST1),
LessThanD(DIST1, goalPosEps),
LessThanV(RVAR1, goalVar))))
]
# Conditional stream declarations
cond_streams = [
TestStream(inputs = [RPOS1],
conditions = [],
effects = [LegalPos(RPOS1)],
test = legalTest,
eager = True),
GeneratorStream(inputs = [],
outputs = [RPOS1],
conditions = [],
effects = [LegalPos(RPOS1)],
generator = randPos),
GeneratorStream(inputs = [RPOS1, RPOS2],
outputs = [DIST1],
conditions = [],
effects = [DistBetween(RPOS1, RPOS2, DIST1)],
generator = lambda rp1, rp2: [distance(rp1, rp2)]),
GeneratorStream(inputs = [RVAR1, DIST1],
outputs = [RVAR2],
conditions = [],
effects = [OdometryVar(RVAR1, RVAR2, DIST1)],
generator = odoVarFun),
# GeneratorStream(inputs = [RVAR1, DIST1],
# outputs = [RVAR2],
# conditions = [],
# effects = [SensorVar(RVAR1, RVAR2, DIST1)],
# generator = sensorVarFun),
TestStream(inputs = [DIST1, DIST2],
conditions = [],
effects = [LessThanD(DIST1, DIST2)],
test = lt,
eager = True),
TestStream(inputs = [RVAR1, RVAR2],
conditions = [],
effects = [LessThanV(RVAR1, RVAR2)],
test = lt, eager = True)
]
####################
constants = [
]
initial_atoms = [
RobotPos(initRobotPos),
RobotVar(initRobotVar),
LegalPos(homePos),
LegalPos((.1, .1)),
LegalPos((3.0, .1)),
LegalPos((6.0, .1))
]
goal_literals = [
KnowYouAreHome()
]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
return problem
def solve_tamp(env, use_focused):
use_viewer = env.GetViewer() is not None
problem = PROBLEM(env)
# TODO: most of my examples have literally had straight-line motions plans
robot, manipulator, base_manip, ir_model = initialize_openrave(
env, ARM, min_delta=MIN_DELTA)
set_manipulator_conf(ir_model.manip, CARRY_CONFIG)
bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names}
surfaces = {surface.name: surface for surface in problem.surfaces}
open_gripper(manipulator)
initial_q = Conf(robot.GetConfigurationValues())
initial_poses = {
name: Pose(name, get_pose(body))
for name, body in bodies.iteritems()
}
# TODO: just keep track of the movable degrees of freedom
# GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues
saver = EnvironmentStateSaver(env)
#cfree_pose = cfree_pose_fn(env, bodies)
#cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies)
#base_generator = base_generator_fn(ir_model)
#base_values = base_values_from_full_config(initial_q.value)
#goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value)
#goal_conf = Conf(goal_values)
#return
####################
# TODO - should objects contain their geometry
cond_streams = [
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[IsPose(O, P),
IsGrasp(O, G)],
effects=[GraspMotion(O, P, G, Q, T)],
generator=sample_grasp_traj_fn(
base_manip, ir_model, bodies, CARRY_CONFIG)),
EasyGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_base_motion_fn(base_manip, bodies),
order=1,
max_level=0),
EasyTestStream(
inputs=[O, P, O2, P2],
conditions=[IsPose(O, P), IsPose(O2, P2)],
effects=[CFreePose(P, P2)],
test=lambda *args: True, #cfree_pose,
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=lambda *args: True),
#test=cfree_traj),
EasyListGenStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[IsGrasp(O, G)],
generator=grasp_generator_fn(bodies, TOP_GRASPS,
SIDE_GRASPS,
MAX_GRASPS)),
EasyListGenStream(inputs=[O, S],
outputs=[P],
conditions=[],
effects=[IsPose(O, P), Stable(P, S)],
generator=pose_generator_fn(bodies, surfaces)),
#EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)],
# effects=[], generator=base_generator),
]
####################
constants = []
initial_atoms = [ConfEq(initial_q), HandEmpty()]
for name in initial_poses:
initial_atoms += body_initial_atoms(name, initial_poses, bodies,
surfaces)
goal_formula = And(
ConfEq(initial_q), *[
OnSurface(obj, surface)
for obj, surface in problem.goal_surfaces.iteritems()
])
#goal_formula = ConfEq(goal_conf)
#obj, _ = problem.goal_surfaces.items()[0]
#goal_formula = And(Holding(obj))
#goal_formula = Holding(obj) # TODO: this cause a bug
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
print stream_problem
handles = draw_affine_limits(robot)
if use_viewer:
for surface in problem.surfaces:
surface.draw(env)
raw_input('Start?')
max_time = INF
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
if use_focused:
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False,
max_time=max_time)
else:
solve = lambda: incremental_planner(stream_problem,
search=search_fn,
frequency=10,
waves=False,
debug=False,
max_time=max_time)
with env:
plan, universe = solve()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if plan is not None:
commands = process_plan(robot, bodies, plan)
if use_viewer:
print SEPARATOR
saver.Restore()
visualize_solution(commands, step=False)
raw_input('Finish?')
def compile_problem(oracle):
problem = oracle.problem
block_poses = [(C(obj, BLOCK), Pose(obj, oracle.initial_poses[obj]))
for obj in oracle.get_objects()]
#region = C('region%s'%1, REGION)
initial_atoms = [
HandEmpty(), # TODO - toggle
] + [AtPose(block, pose) for block, pose in block_poses
] + [IsPose(block, pose) for block, pose in block_poses]
goal_literals = []
if problem.goal_holding is not None:
if problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif isinstance(problem.goal_holding, Holding):
goal_literals.append(
HasGrasp(C(problem.goal_holding.object_name, BLOCK),
C(problem.goal_holding.grasp, GRASP)))
elif problem.goal_holding in oracle.get_objects():
goal_literals.append(Holding(C(problem.goal_holding, BLOCK)))
else:
raise Exception()
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:
problem.goal_poses[obj] = oracle.initial_poses[
problem.goal_poses[obj]]
goal_literals.append(
AtPose(C(obj, BLOCK), C(obj, problem.goal_poses[obj])))
for obj, region in problem.goal_regions.iteritems():
#for obj, region in problem.goal_regions.items()[:1]:
goal_literals.append(InRegion(C(obj, BLOCK), C(region, REGION)))
# NOTE - need to add the IsPose to everything a priori
"""
block = C(oracle.get_objects()[0], BLOCK)
goal_literals = [
Holding(block),
#Not(HandEmpty(robot)), # NOTE - negative goal
#Not(AtPose(obj, Pose(0))),
#AtPose(block, Pose(20)),
#AtPose(obj, Pose(4)),
#AtConfig(Config(5)),
#AtConfig(robot, Config(4)),
#InRegion(block, region),
]
"""
actions = [
Pick(oracle),
Place(oracle),
]
axioms = [
#HoldingAxiom(),
InRegionAxiom(),
SafeAxiom(),
]
cond_streams = [
PoseStream(oracle),
GraspStream(oracle),
ContainedStream(oracle),
#ContainedTest(oracle),
IKStream(oracle),
CollisionFreeTest(oracle),
]
objects = []
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, objects)
def create_problem(goal, obstacles=(), distance=.25, digits=3):
"""
Creates a Probabilistic Roadmap (PRM) motion planning problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POINT = Type()
REGION = Type()
# Fluent predicates
AtPoint = Pred(POINT)
# Derived predicates
InRegion = Pred(REGION)
# Stream predicates
AreNearby = Pred(POINT, POINT)
IsEdge = Pred(POINT, POINT)
Contained = Pred(POINT, REGION)
# Functions
Distance = Func(POINT, POINT)
# Free parameters
P1, P2 = Param(POINT), Param(POINT)
R = Param(REGION)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
#STRIPSAction(name='move', parameters=[P1, P2],
# conditions=[AtPoint(P1), IsEdge(P1, P2)],
# effects=[AtPoint(P2), Not(AtPoint(P1))], cost=1), # Fixed cost
#STRIPSAction(name='move', parameters=[P1, P2],
# conditions=[AtPoint(P1), IsEdge(P1, P2)],
# effects=[AtPoint(P2), Not(AtPoint(P1)), Cost(Distance(P1, P2))]), # Cost depends on parameters
Action(name='move',
parameters=[P1, P2],
condition=And(AtPoint(P1), IsEdge(P1, P2)),
effect=And(AtPoint(P2), Not(AtPoint(P1))),
costs=[1, Distance(P1, P2)]),
]
axioms = [
#Axiom(effect=GoalReached(), condition=Exists([P1], And(AtPos(P1), Contained(P1)))),
STRIPSAxiom(conditions=[AtPoint(P1), Contained(P1, R)],
effects=[InRegion(R)])
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(
inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: ((sample(digits), ) for _ in inf_sequence())
), # NOTE - version that only generators collision-free points
GeneratorStream(inputs=[R],
outputs=[P1],
conditions=[],
effects=[Contained(P1, R)],
generator=lambda r:
((sample_box(r), ) for _ in inf_sequence())),
#TestStream(inputs=[P1, P2], conditions=[], effects=[AreNearby(P1, P2), AreNearby(P2, P1)],
# test=lambda p1, p2: get_distance(p1, p2) <= distance, eager=True),
#TestStream(inputs=[P1, P2], conditions=[AreNearby(P1, P2)], effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
# test=lambda p1, p2: is_collision_free((p1, p2), obstacles), eager=True),
TestStream(
inputs=[P1, P2],
conditions=[],
effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
#test=lambda p1, p2: is_collision_free((p1, p2), obstacles), eager=True),
test=lambda p1, p2:
(get_distance(p1, p2) <= distance) and is_collision_free(
(p1, p2), obstacles),
eager=True),
#TestStream(inputs=[P1, P2], conditions=[], effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
# test=lambda p1, p2: get_distance(p1, p2) <= distance and is_collision_free((p1, p2), obstacles), eager=True),
#TestStream(inputs=[P1, R], conditions=[], effects=[Contained(P1, R)],
# test=lambda p, r: contains(p, r), eager=True),
CostStream(inputs=[P1, P2],
conditions=[],
effects=[Distance(P1, P2),
Distance(P2, P1)],
function=get_distance,
scale=100,
eager=True),
]
####################
constants = []
initial_atoms = [
AtPoint((0, 0)),
]
goal_literals = []
if is_region(goal):
goal_literals.append(InRegion(goal))
else:
goal_literals.append(AtPoint(goal))
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem
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
goal_poses = {block: i+1 for i, block in enumerate(blocks)} # the goal pose for block i is i+1
####################
VALUE = Type()
# Fluent predicates
AtConf = Pred(VALUE)
AtPose = Pred(VALUE, VALUE)
HandEmpty = Pred()
Holding = Pred(VALUE)
# Derived predicates
Safe = Pred(VALUE, VALUE, VALUE)
# Static predicates
IsBlock = Pred(VALUE)
IsPose = Pred(VALUE)
IsConf = Pred(VALUE)
LegalKin = Pred(VALUE, VALUE)
CollisionFree = Pred(VALUE, VALUE, VALUE, VALUE)
# Free parameters
B1, B2 = Param(VALUE), Param(VALUE)
P1, P2 = Param(VALUE), Param(VALUE)
Q1, Q2 = Param(VALUE), Param(VALUE)
rename_easy(locals()) # Trick to make debugging easier
####################
# NOTE - it would be easier to just update an in hand pose
actions = [
STRIPSAction(name='pick', parameters=[B1, P1, Q1],
conditions=[IsBlock(B1), IsPose(P1), IsConf(Q1), LegalKin(P1, Q1),
AtPose(B1, P1), HandEmpty(), AtConf(Q1)],
effects=[Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty())]),
STRIPSAction(name='place', parameters=[B1, P1, Q1],
conditions=[IsBlock(B1), IsPose(P1), IsConf(Q1), LegalKin(P1, Q1),
Holding(B1), AtConf(Q1)] + [Safe(b2, B1, P1) for b2 in blocks],
effects=[AtPose(B1, P1), HandEmpty(), Not(Holding(B1))]),
STRIPSAction(name='move', parameters=[Q1, Q2],
conditions=[IsConf(Q1), IsConf(Q2), AtConf(Q1)],
effects=[AtConf(Q2), Not(AtConf(Q1))]),
]
axioms = [ # TODO - need to combine 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=[IsPose(P1)],
generator=lambda: xrange(num_poses)), # Enumerating all the poses
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[IsPose(P1)], effects=[IsConf(Q1), LegalKin(P1, Q1)],
generator=lambda p: [p]), # Inverse kinematics
TestStream(inputs=[B1, P1, B2, P2], conditions=[IsBlock(B1), IsPose(P1), IsBlock(B2), IsPose(P2)],
effects=[CollisionFree(B1, P1, B2, P2)], test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
]
####################
constants = []
initial_atoms = [
IsConf(initial_config),
AtConf(initial_config),
HandEmpty()
]
for block, pose in initial_poses.iteritems():
initial_atoms += [
IsBlock(block),
IsPose(pose),
AtPose(block, pose),
]
goal_literals = []
for block, pose in goal_poses.iteritems():
initial_atoms += [
IsBlock(block),
IsPose(pose),
]
goal_literals.append(AtPose(block, pose))
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
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 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(estimator, task):
# Data types
CONF = Type()
SURFACE = Type() # Difference between fixed and movable objects
ITEM = Type()
POSE = Type()
CLASS = Type()
# Fluent predicates
AtConf = Pred(CONF)
HandEmpty = Pred()
Holding = Pred(ITEM)
AtPose = Pred(ITEM, POSE)
Supported = Pred(POSE, SURFACE) # Fluent
Localized = Pred(OBJECT)
Measured = Pred(OBJECT)
# Static predicates
IsKin = Pred(POSE, CONF)
IsClass = Pred(OBJECT, CLASS)
IsVisible = Pred(SURFACE, CONF)
IsSupported = Pred(POSE, SURFACE) # Static
# Functions
ScanRoom = Func(SURFACE)
#ScanTable = Func(SURFACE, TYPE)
ScanTable = Func(
SURFACE, ITEM) # TODO: could include more specific vantage point costs
Distance = Func(CONF, CONF)
# Derived
On = Pred(ITEM, SURFACE)
ComputableP = Pred(POSE)
ComputableQ = Pred(CONF)
# Free parameters
Q1, Q2 = Param(CONF), Param(CONF)
S1 = Param(SURFACE)
B1, B2 = Param(ITEM), Param(ITEM)
P1, P2 = Param(POSE), Param(POSE)
rename_easy(locals()) # Trick to make debugging easier
# TODO: could just do easier version of this that doesn't require localized to start
actions = [
Action(
name='pick',
parameters=[B1, P1, Q1], # TODO: Visibility constraint
condition=And(Localized(B1), AtPose(B1, P1), HandEmpty(),
AtConf(Q1), IsKin(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), IsKin(P1, Q1)),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=And(AtConf(Q1), ComputableQ(Q2)),
effect=And(AtConf(Q2), Not(AtConf(Q1)), Cost(Distance(Q1,
Q2)))),
Action(name='scan_room',
parameters=[S1],
condition=Not(Localized(S1)),
effect=And(Localized(S1), Cost(ScanRoom(S1)))),
# TODO: need to set later poses to be usable or not to constrain order
Action(name='scan_table',
parameters=[S1, B1, P1, Q1],
condition=And(Localized(S1), AtConf(Q1), IsVisible(S1, Q1),
Not(Localized(B1))),
effect=And(Localized(B1), Measured(P1), Supported(P1, S1),
Cost(ScanTable(S1, B1)))),
]
axioms = [
# TODO: axiom for on? Might need a stream that generates initial fluents for On
# TODO: axiom that says that all fake values depending on a certain one now are usable
# TODO: could use stream predicates as fluents (as long as it doesn't break anything...)
Axiom(effect=On(B1, S1),
condition=Exists([P1],
And(AtPose(B1, P1),
Or(IsSupported(P1, S1), Supported(P1,
S1))))),
# TODO: compile automatically
Axiom(effect=ComputableQ(Q1),
condition=Or(Measured(Q1),
Exists([P1], And(IsKin(P1, Q1), ComputableP(P1))),
Exists([S1], And(IsVisible(S1, Q1),
Localized(S1))))),
Axiom(effect=ComputableP(P1),
condition=Or(
Measured(P1),
Exists([S1], And(IsSupported(P1, S1), Localized(S1))))),
]
#####
surface_types = estimator.surface_octomaps.keys()
item_types = estimator.object_octomaps.keys()
names_from_type = defaultdict(list)
known_poses = {}
holding = None
def add_type(cl):
name = '{}{}'.format(cl, len(names_from_type[cl]))
names_from_type[cl].append(name)
return name
# TODO: this is all very similar to the generic open world stuff
if estimator.holding is not None:
holding = add_type(estimator.holding)
for cl, octomap in estimator.surface_octomaps.items(
): # TODO: generic surface object
for pose in octomap.get_occupied():
known_poses[add_type(cl)] = pose
for cl, octomap in estimator.object_octomaps.items():
for pose in octomap.get_occupied():
known_poses[add_type(cl)] = pose
print dict(names_from_type), known_poses
# Human tells you to move block -> at least one block
# At least one block -> at least one surface
# TODO: generate fake properties about these fake values?
goal_objects, goal_surfaces = entities_from_task(task)
for cl in surface_types:
add_type(cl)
#for cl in goal_surfaces:
# for i in xrange(len(names_from_type[cl]), goal_surfaces[cl]):
# add_type(cl)
#for cl in item_types:
for cl in goal_objects:
for i in xrange(len(names_from_type[cl]), goal_objects[cl]):
add_type(cl)
#####
initial_atoms = [
AtConf(estimator.robot_conf),
Measured(CONF(estimator.robot_conf))
]
if holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(holding))
class_from_name = {
name: ty
for ty in names_from_type for name in names_from_type[ty]
}
for name, ty in class_from_name.iteritems():
ENTITY = SURFACE if ty in surface_types else ITEM
initial_atoms.append(IsClass(ENTITY(name), ty))
if name in known_poses:
initial_atoms.append(Localized(ENTITY(name)))
if ENTITY == ITEM:
pose = known_poses[name]
initial_atoms += [AtPose(name, pose), Measured(POSE(pose))]
else:
if ENTITY == ITEM:
pose = 'p_init_{}'.format(
name
) # The object should always be at this pose (we just can't do anything about it yet)
initial_atoms += [AtPose(name, pose)]
goal_literals = []
if task.holding is None:
goal_literals.append(HandEmpty())
elif task.holding is not False:
goal_literals.append(
Exists([B1], And(Holding(B1), IsClass(B1, task.holding))))
for obj, surface in task.object_surfaces:
goal_literals.append(
Exists([B1, S1],
And(On(B1, S1), IsClass(B1, obj), IsClass(S1, surface))))
goal_formula = And(*goal_literals)
####################
TOLERANCE = 0.1
def is_visible(table, conf):
x, y = conf
pose = known_poses[table]
#return (pose == x) and (y == 2)
#return (pose == x) and (y == 2)
return (pose == x) and (abs(y - 2) < TOLERANCE)
def is_kinematic(pose, conf):
x, y = conf
#return (pose == x) and (y == 1)
return (pose == x) and (abs(y - 1) < TOLERANCE)
####################
def sample_visible(table): # TODO: could generically do this with poses
if table in known_poses:
y = 2
#y += round(uniform(-TOLERANCE, TOLERANCE), 3)
conf = (known_poses[table], y)
assert is_visible(table, conf)
else:
conf = 'q_vis_{}'.format(table)
yield (conf, )
def inverse_kinematics(pose): # TODO: list stream that uses ending info
# TODO: only do if localized as well?
# TODO: is it helpful to have this even if the raw value is kind of wrong (to steer the search)
if type(pose) != str:
y = 1
#y += round(uniform(-TOLERANCE, TOLERANCE), 3)
conf = (pose, y)
assert is_kinematic(pose, conf)
else:
conf = 'q_ik_{}'.format(pose)
yield (conf, )
def sample_table(table):
if table in known_poses:
pose = known_poses[table]
else:
pose = 'p_{}'.format(table)
yield (pose, )
####################
MAX_DISTANCE = 10
# TODO: maybe I don't need to worry about normalizing. I can just pretend non-parametric again for planning
def scan_surface_cost(
surface, obj): # TODO: what about multiple scans of the belief?
fail_cost = 100
surface_cl = class_from_name[surface]
obj_cl = class_from_name[obj]
prob = 1.0
if obj_cl in estimator.object_prior:
prob *= estimator.object_prior[obj_cl].get(surface_cl, 0)
if surface in known_poses:
prob *= estimator.object_octomaps[obj_cl].get_prob(
known_poses[surface])
else:
prob *= 0.1 # Low chance if you don't even know the table exists
# TODO: could even include the probability the table exists
#return expected_cost(1, fail_cost, prob)
return mdp_cost(1, fail_cost, prob)
def scan_room_cost(surface):
# TODO: try to prove some sort of bound on the cost to recover will suffice?
fail_cost = 100
cl = class_from_name[surface]
occupied_poses = {
known_poses[n]
for n in names_from_type[cl] if n in known_poses
}
p_failure = 1.0
for pose in estimator.poses:
if pose not in occupied_poses:
p_failure *= (1 -
estimator.surface_octomaps[cl].get_prob(pose))
return 1 * (1 - p_failure) + fail_cost * p_failure
def distance_cost(q1, q2):
if str in (type(q1), type(q2)):
return MAX_DISTANCE # TODO: take the max possible pose distance
# TODO: can use the info encoded within these to obtain better bounds
return np.linalg.norm(np.array(q2) - np.array(q1))
####################
# TODO: could add measured as the output to these
streams = [
GeneratorStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[IsKin(P1, Q1)],
generator=inverse_kinematics),
GeneratorStream(inputs=[S1],
outputs=[Q1],
conditions=[],
effects=[IsVisible(S1, Q1)],
generator=sample_visible),
GeneratorStream(inputs=[S1],
outputs=[P1],
conditions=[],
effects=[IsSupported(P1, S1)],
generator=sample_table),
CostStream(inputs=[S1, B1],
conditions=[],
effects=[ScanTable(S1, B1)],
function=scan_surface_cost),
CostStream(inputs=[Q1, Q2],
conditions=[],
effects=[Distance(Q1, Q2),
Distance(Q2, Q1)],
function=distance_cost),
CostStream(inputs=[S1],
conditions=[],
effects=[ScanRoom(S1)],
function=scan_room_cost),
# TODO: make an is original precondition and only apply these to original values?
# I suppose I could apply to all concrete things but that's likely not useful
#TestStream(inputs=[S1, Q1], conditions=[IsOriginal(Q1)], effects=[IsVisible(S1, Q1)],
# test=is_visible, eager=True),
#TestStream(inputs=[P1, Q1], conditions=[IsOriginal(Q1), IsOriginal(Q1)], effects=[IsKin(P1, Q1)],
# test=is_kinematic, eager=True),
#GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[IsVisible(P1, Q1)],
# generator=sample_visible),
]
problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
streams, [])
def command_from_action((action, args)):
if action.name == 'scan_room':
return simulate_scan, []
if action.name in ('scan_table', 'look_block'):
return simulate_look, []
if action.name == 'move':
q1, q2 = map(get_value, args)
return simulate_move, [q2]
if action.name == 'pick':
o, p, q = map(get_value, args)
return simulate_pick, [class_from_name[o]]
if action.name == 'place':
return simulate_place, []
raise ValueError(action.name)
return problem, command_from_action
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(dt, GoalTest, ClearTest, CalcDiffSystem, InitialState):
"""
Creates a generic non-holonomic motion planning problem
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
X, DX, U = Type(), Type(), Type()
# Fluent predicates
AtX = Pred(X)
# Fluent predicates
GoalReached = Pred()
# Static predicates
ComputeDX = Pred(X, U, DX)
Dynamics = Pred(X, DX, X)
AtGoal = Pred(X)
Clear = Pred(X, X)
# Free parameters
X1, X2 = Param(X), Param(X)
DX1 = Param(DX)
U1 = Param(U)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='simulate',
parameters=[X1, U1, X2, DX1],
condition=And(AtX(X1), ComputeDX(X1, U1, DX1),
Dynamics(X1, DX1, X2), Clear(X1, X2)),
effect=And(Not(AtX(X1)), AtX(X2))),
]
axioms = [
Axiom(effect=GoalReached(), condition=Exists([X1], AtGoal(X1))),
]
####################
# Conditional stream declarations
def ComputeDXCalculator(X1F, DX1F):
X2F = X1F[:]
for i in range(len(X2F)):
X2F[i] = X1F[i] + dt * DX1F[i]
return X2F
cond_streams = [
FunctionStream(inputs=[X1, DX1],
outputs=[X2],
conditions=[],
effects=[Dynamics(X1, DX1, X2)],
function=ComputeDXCalculator),
FunctionStream(inputs=[X1, U1],
outputs=[DX1],
conditions=[],
effects=[ComputeDX(X1, U1, DX1)],
function=CalcDiffSystem),
TestStream(inputs=[X1, X2],
conditions=[],
effects=[Clear(X1, X2)],
test=ClearTest,
eager=True),
TestStream(inputs=[X1],
conditions=[],
effects=[AtGoal(X1)],
test=GoalTest,
eager=True),
]
####################
constants = [
# TODO - need to declare control inputs (or make a stream for them)
]
initial_atoms = [
AtX(InitialState),
]
goal_literals = [GoalReached()]
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(oracle):
problem = oracle.problem
oracle.surface_poses = {
surface: oracle.get_pose(surface)
for surface in oracle.get_surfaces()
}
manips = []
if ACTIVE_LEFT:
manips.append(C('left', MANIP))
if ACTIVE_RIGHT:
manips.append(C('right', MANIP))
initial_atoms = [
AtConfig(Config(oracle.initial_config)),
]
if problem.start_holding is False: # TODO - handle if a grasp happens
initial_atoms += [HandEmpty(manip) for manip in manips]
#for surface in oracle.get_surfaces():
for surface in oracle.get_counters():
body, pose = C(surface, BODY), Pose(surface,
oracle.surface_poses[surface])
initial_atoms += [
IsFixed(body),
AtPose(body, pose),
IsPose(body, pose),
]
goal_literals = []
if problem.goal_holding is not None:
if problem.goal_holding is False:
goal_literals += [HandEmpty(manip) for manip in manips]
elif isinstance(problem.goal_holding, ObjGrasp):
# TODO - add the manipulator here
goal_literals.append(
HasGrasp(C(problem.goal_holding.object_name, BODY),
C(problem.goal_holding.grasp, GRASP)))
elif problem.goal_holding in oracle.get_objects():
goal_literals.append(Holding(C(problem.goal_holding, BODY)))
else:
raise Exception()
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]]
goal_literals.append(
AtPose(C(obj, BODY), Pose(obj, problem.goal_poses[obj])))
initial_atoms += [
IsPose(C(obj, BODY), Pose(obj, problem.goal_poses[obj])
) # NOTE - need to add the IsPose to everything a priori
# TODO - stuff here
]
for obj, region in problem.goal_regions.iteritems():
goal_literals.append(InRegion(C(obj, BODY), C(region, REGION)))
for obj, base in problem.goal_stackings.items():
goal_literals.append(On(C(obj, BODY), C(base, BODY)))
initial_atoms.append(AreStackable(C(obj, BODY), C(
base, BODY))) # NOTE - do I want to have this?
# TODO - I guess I can handle this by making something only stackable when it needs to be
# TODO - similarly, I could also make a condition that we only want to place in a region if it is a goal
# TODO - make regions a type of stackable body
# TODO - dynamically add/remove stacking
#goal_literals = [Holding(C('blue_box', BODY))]
goal_literals = [On(C('blue_box', BODY), C('table2', BODY))]
goal_blocks = set()
for atom in And(*goal_literals).get_atoms():
for body in atom.args:
if body.type == BODY and body.name in oracle.initial_poses:
goal_blocks.add(body.name)
#goal_blocks = oracle.get_objects()
print 'Initial objects:', goal_blocks
oracle.introduced_objects = set()
for obj in goal_blocks:
#for base in goal_blocks:
# if obj != base:
# initial_atoms.append(AreStackable(C(obj, BODY), C(base, BODY)))
initial_atoms += get_object_initial_atoms(obj, oracle)
oracle.region_supporters = {}
for region in oracle.goal_regions:
for surface in oracle.get_surfaces():
if oracle.get_region(surface).region_on(oracle.get_region(region)):
assert region not in oracle.region_supporters
print 'Region supporter:', region, surface
oracle.region_supporters[region] = surface
initial_atoms.append(
IsRegionOn(C(region, REGION), C(surface, BODY)))
assert region in oracle.region_supporters
actions = [
Move(oracle),
Pick(oracle),
Place(oracle),
]
axioms = [
HoldingAxiom(),
InRegionAxiom(),
SafeAxiom(),
OnAxiom(),
]
cond_streams = [
PoseStream(oracle),
GraspStream(oracle),
ContainedStream(oracle),
#ContainedTest(oracle),
IKStream(oracle),
#NearbyTest(oracle),
]
if COLLISIONS:
cond_streams.append(CollisionFreeTest(oracle))
objects = []
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, objects)
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 solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
initialize_openrave(env, ARM, min_delta=.01)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash() for body in all_bodies}) == 1 # NOTE - assuming all objects has the same geometry
body1 = all_bodies[-1] # Generic object 1
body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:MAX_GRASPS]
poses = problem.known_poses if problem.known_poses else []
open_gripper(manipulator)
initial_conf = Conf(robot.GetConfigurationValues()[manipulator.GetArmIndices()])
def _enable_all(enable): # Enables or disables all bodies for collision checking
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at pose2
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
_enable_all(False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
_enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
def cfree_traj(traj, pose): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose
if DISABLE_TRAJ_COLLISIONS:
return True
return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))
####################
def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
_enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration
if grasp_conf is None: return
if DISABLE_TRAJECTORIES:
yield [(Conf(grasp_conf), object())]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp
#grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration
yield [(pregrasp_conf, grasp_traj)]
def sample_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
_enable_all(False)
set_manipulator_conf(manipulator, conf1.value)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
if not traj: return
traj.grasp = None
yield [(traj,)]
def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
_enable_all(False)
body1.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
robot.Grab(body1)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
robot.Release(body1)
if not traj: return
traj.grasp = grasp
yield [(traj,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[Q, T], conditions=[],
effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2], outputs=[T], conditions=[],
effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0),
EasyListGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)],
test=cfree_pose, eager=True),
EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)],
test=cfree_traj),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ConfEq(initial_conf), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
if viewer:
env.UpdatePublishedBodies()
raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10)
solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
#solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
#universe.print_statistics()
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
####################
def _execute_traj(traj):
#for j, conf in enumerate(traj.path()):
#for j, conf in enumerate([traj.end()]):
path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
for j, conf in enumerate(path):
set_manipulator_conf(manipulator, conf)
env.UpdatePublishedBodies()
raw_input('%s/%s) Step?'%(j, len(path)))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
#set_manipulator_conf(manipulator, initial_conf.value)
set_manipulator_conf(manipulator, initial_conf)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
env.UpdatePublishedBodies()
if not DISABLE_TRAJECTORIES:
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, _, traj = args
_execute_traj(traj)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
_execute_traj(traj)
elif action.name == 'pick':
obj, _, _, _, traj = args
_execute_traj(traj.reverse())
robot.Grab(bodies[obj])
_execute_traj(traj)
elif action.name == 'place':
obj, _, _, _, traj = args
_execute_traj(traj.reverse())
robot.Release(bodies[obj])
_execute_traj(traj)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
else:
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, q2, _ = args
set_manipulator_conf(manipulator, q2)
elif action.name == 'move_holding':
_, q2, _, _, _ = args
set_manipulator_conf(manipulator, q2)
elif action.name == 'pick':
obj, _, _, _, traj = args
robot.Grab(bodies[obj])
elif action.name == 'place':
obj, _, _, _, traj = args
robot.Release(bodies[obj])
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def create_problem(initRobotPos = (0.5, 0.5),
initRobotVar = 0.01,
maxMoveDist = 5.0,
beaconPos = (1, 1),
homePos = (0, 0),
goalPosEps = 0.1,
goalVar = 0.1,
odoErrorRate = 0.1,
obsVarPerDistFromSensor = 10.0,
minObsVar = 0.001,
domainSize = 20,
verboseFns = False):
"""
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POS = Type() # 2D position
VAR = Type() # 2D variance
BEACON = Type() # 2D position
# Fluent predicates
RobotPos = Pred(POS)
RobotVar = Pred(VAR)
# Derived predicates
KnowYouAreHome = Pred()
# Static predicates
Odometry = Pred(POS, VAR, POS, VAR)
Sensor = Pred(POS, VAR, BEACON, VAR)
LegalPosVar = Pred(POS, VAR)
NearbyPos = Pred(POS, POS)
NearGoalPos = Pred(POS)
NearGoalVar = Pred(VAR)
# Free parameters
RPOS1, RPOS2 = Param(POS), Param(POS)
RVAR1, RVAR2 = Param(VAR), Param(VAR)
BPOS = Param(BEACON)
rename_easy(locals())
# Helper functions
def distance(p1, p2):
return math.sqrt(sum([(a - b)**2 for (a,b) in zip(p1, p2)]))
def odoVarFun(rp1, rv, rp2):
d = distance(rp1, rp2)
odoVar = (d * odoErrorRate)**2
result = rv + odoVar
if verboseFns: print 'ovf:', rv, d, result
return [result]
def sensorVarFun(rp, rv, bp):
d = distance(rp, bp)
obsVar = max(d / obsVarPerDistFromSensor, minObsVar)
#result = round(1.0 / ((1.0 / rv) + (1.0 / obsVar)), 5) # Causes zero variance which is bad
result = 1.0 / ((1.0 / rv) + (1.0 / obsVar))
if verboseFns: print 'svf:', rv, d, result
return [result]
def randPos():
while True:
result = (random.random() * domainSize, random.random() * domainSize)
print 'rp:', result
yield [result]
def legalTest(rp):
(x, y) = rp
result = (0 <= x <= domainSize) and (0 <= y <= domainSize)
if not result: print 'not legal:', rp
return result
# TODO - combine variance and positions
actions = [
Action(name='Move',
parameters=[RPOS1, RVAR1, RPOS2, RVAR2],
condition = And(RobotPos(RPOS1),
RobotVar(RVAR1),
Odometry(RPOS1, RVAR1, RPOS2, RVAR2)),
effect = And(RobotPos(RPOS2),
RobotVar(RVAR2),
Not(RobotPos(RPOS1)),
Not(RobotVar(RVAR1)))),
Action(name='Look',
parameters=[RPOS1, RVAR1, BPOS, RVAR2],
condition = And(RobotPos(RPOS1),
RobotVar(RVAR1),
Sensor(RPOS1, RVAR1, BPOS, RVAR2)),
effect = And(RobotVar(RVAR2),
Not(RobotVar(RVAR1))))
]
axioms = [
]
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs = [],
outputs = [RPOS1],
conditions = [],
effects = [],
generator = randPos),
# TODO - the number of variances grows incredibly but we just want to consider variances possible with the move
# Each var only has one pos because moving changes
GeneratorStream(inputs = [RPOS1, RVAR1, RPOS2],
outputs = [RVAR2],
conditions = [LegalPosVar(RPOS1, RVAR1), NearbyPos(RPOS1, RPOS2)],
effects = [Odometry(RPOS1, RVAR1, RPOS2, RVAR2), LegalPosVar(RPOS2, RVAR2)],
generator = odoVarFun),
GeneratorStream(inputs = [RPOS1, RVAR1, BPOS],
outputs = [RVAR2],
conditions = [LegalPosVar(RPOS1, RVAR1)],
effects = [Sensor(RPOS1, RVAR1, BPOS, RVAR2), LegalPosVar(RPOS1, RVAR2)],
generator = sensorVarFun),
TestStream(inputs = [RPOS1, RPOS2],
conditions = [],
effects = [NearbyPos(RPOS1, RPOS2)],
test = lambda rp1, rp2: distance(rp1, rp2) < maxMoveDist,
eager = True),
TestStream(inputs = [RPOS1],
conditions = [],
effects = [NearGoalPos(RPOS1)],
test = lambda rp1: distance(rp1, homePos) < goalPosEps,
eager = True),
TestStream(inputs = [RVAR1],
conditions = [],
effects = [NearGoalVar(RVAR1)],
test = lambda a: a < goalVar,
eager = True)
# NOTE - the problem seems to be the fast that this is eager
]
####################
constants = [
BEACON(beaconPos),
#POS((.1, .1)),
#POS((3., .1)),
#POS((6., .1)),
POS(homePos),
]
initial_atoms = [
RobotPos(initRobotPos),
RobotVar(initRobotVar),
LegalPosVar(initRobotPos, initRobotVar), # TODO - I might as well keep track of pairs. Make this a legal on both
]
goal_formula = Exists([RPOS1, RVAR1],
And(RobotPos(RPOS1),
RobotVar(RVAR1),
LegalPosVar(RPOS1, RVAR1),
NearGoalPos(RPOS1),
NearGoalVar(RVAR1)))
problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
cond_streams, constants)
return problem