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 create_problem(time_step=.5, fuel_rate=5, start_fuel=10, goal_p=10):
"""
Creates the Tsiolkovsky rocket problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
STATE, RATE, MASS, TIME = Type(), Type(), Type(), Type()
HEIGHT = Type()
# Fluent predicates
AtState = Pred(STATE)
# Derived predicates
Above = Pred(HEIGHT)
# Static predicates
IsBurst = Pred(STATE, RATE, TIME, STATE)
IsAbove = Pred(STATE, HEIGHT)
# Free parameters
X1, X2 = Param(STATE), Param(STATE)
Q, T = Param(RATE), Param(TIME)
H = Param(HEIGHT)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='burst',
parameters=[X1, Q, T, X2],
condition=And(AtState(X1), IsBurst(X1, Q, T, X2)),
effect=And(AtState(X2), Not(AtState(X1)))),
]
axioms = [
Axiom(effect=Above(H),
condition=Exists([X1], And(AtState(X1), IsAbove(X1, H)))),
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs=[X1, Q, T],
outputs=[X2],
conditions=[],
effects=[IsBurst(X1, Q, T, X2)],
generator=forward_burst),
TestStream(inputs=[X1, H],
conditions=[],
effects=[IsAbove(X1, H)],
test=lambda (p, v, m), h: p >= h,
eager=True),
]
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 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
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 create_problem():
STATE = Type()
POSITION = Type()
ACCELERATION = Type()
TIME = Type()
SATELLITE = Type()
#FORCE = Type()
#MASS = Type('mass')
#ROCKET = Type('rocket')
######
#AtState = Predicate(POSITION, VELOCITY)
AtState = Pred(STATE)
Above = Pred(POSITION)
AtOrbit = Pred(SATELLITE, POSITION)
Carrying = Pred(SATELLITE)
Flying = Pred()
Landed = Pred()
######
IsBurst = Pred(STATE, ACCELERATION, TIME, STATE)
IsGlide = Pred(STATE, TIME, STATE)
#IsCrashed = Pred(POSITION)
IsAbove = Pred(STATE, POSITION)
ArePair = Pred(STATE, POSITION)
######
X1, X2 = Param(STATE), Param(STATE)
A, T = Param(ACCELERATION), Param(TIME)
S = Param(SATELLITE)
H = Param(POSITION)
rename_easy(locals()) # Trick to make debugging easier
######
actions = [
Action(name='burst',
parameters=[X1, A, T, X2],
condition=And(AtState(X1), IsBurst(X1, A, T, X2), Flying()),
effect=And(AtState(X2), Not(AtState(X1)))),
Action(name='glide',
parameters=[X1, A, T, X2],
condition=And(AtState(X1), IsGlide(X1, T, X2), Flying()),
effect=And(AtState(X2), Not(AtState(X1)))),
Action(name='deploy',
parameters=[S, X1, H],
condition=And(Carrying(S), AtState(X1), ArePair(X1, H)),
effect=And(AtOrbit(S, H), Not(Carrying(S)))),
Action(name='take_off',
parameters=[],
condition=And(Landed(), AtState((0, 0))),
effect=And(Flying(), Not(Landed()))),
Action(name='land',
parameters=[],
condition=And(Flying(), AtState((0, 0))),
effect=And(Landed(), Not(Flying()))),
]
axioms = [
Axiom(effect=Above(H),
condition=Exists([X1], And(AtState(X1), IsAbove(X1, H)))),
]
cond_streams = [
GeneratorStream(inputs=[X1],
outputs=[A, T, X2],
conditions=[],
effects=[IsBurst(X1, A, T, X2)],
generator=forward_burst),
#GeneratorStream(inputs=[X1, A], outputs=[T, X2], conditions=[], effects=[IsBurst(X1, A, T, X2)],
# generator=fixed_burst),
GeneratorStream(inputs=[X1, X2],
outputs=[A, T],
conditions=[],
effects=[IsBurst(X1, A, T, X2)],
generator=target_burst),
GeneratorStream(inputs=[X1],
outputs=[T, X2],
conditions=[],
effects=[IsGlide(X1, T, X2)],
generator=forward_glide),
TestStream(inputs=[X1, H],
conditions=[],
effects=[IsAbove(X1, H)],
test=lambda (p, v), h: p >= h,
eager=True),
]
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]]
# 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 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
def create_problem(n=50):
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i' % i for i in xrange(n)]
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
goal_poses = {blocks[0]: 1} # the goal pose for block i is i+1
#goal_poses = {blocks[0]: 100} # 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)
IsPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
Moved = Pred() # Prevents double movements
# Derived predicates
Safe = Pred(BLOCK, POSE)
#Unsafe = Pred(BLOCK, BLOCK, POSE)
Unsafe = Pred(BLOCK, POSE)
#Unsafe = Pred(POSE)
# Static predicates
Kin = Pred(POSE, CONF)
CFree = Pred(POSE, POSE)
Collision = Pred(POSE, 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
####################
# TODO: drp_pddl_adl/domains/tmp.py has conditional effects When(Colliding(pose, trajectory), Not(Safe(obj, trajectory))))
# TODO: maybe this would be okay if the effects really are sparse (i.e. not many collide)
# http://www.fast-downward.org/TranslatorOutputFormat
# FastDownward will always make an axiom for the quantified expressions
# I don't really understand why FastDownward does this... It doesn't seem to help
# It creates n "large" axioms that have n-1 conditions (removing the Equal)
# universal conditions: Universal conditions in preconditions, effect conditions and the goal are internally compiled into axioms by the planner.
# Therefore, heuristics that do not support axioms (see previous point) do not support universal conditions either.
# http://www.fast-downward.org/PddlSupport
# TODO: the compilation process actually seems to still make positive axioms for things.
# The default value is unsafe and it creates positive axioms...
# A heuristic cost of 4 is because it does actually move something out the way
# drp_pddl/domains/tmp_separate.py:class CollisionAxiom(Operator, Refinable, Axiom):
# TODO: maybe I didn't actually try negative axioms like I thought?
# See also 8/24/16 and 8/26/16 notes
# Maybe the translator changed sometime making it actually invert these kinds of axioms
# TODO: maybe this would be better if I did a non-boolean version that declared success if at any pose other than this one
# It looks like temporal fast downward inverts axioms as well
actions = [
Action(
name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), IsPose(B1, P1),
Kin(P1, Q1)), # AtConf(Q1),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()),
Not(Moved()))),
Action(
name='place',
parameters=[B1, P1, Q1],
condition=And(
Holding(B1),
IsPose(B1, P1),
Kin(P1, Q1), # AtConf(Q1),
#*[Safe(b, P1) for b in blocks]),
*[Not(Unsafe(b, P1)) for b in blocks]),
#*[Not(Unsafe(b, B1, P1)) for b in blocks]),
#*[Or(Equal(b, B1), Not(Unsafe(b, B1, P1))) for b in blocks]),
#ForAll([B2], Or(Equal(B1, B2), Not(Unsafe(B2, P1))))),
#ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
#ForAll([B2], Not(Unsafe(B2, B1, P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)),
Not(Moved()))),
# Action(name='place', parameters=[B1, P1, Q1],
# condition=And(Holding(B1), AtConf(Q1), IsPose(B1, P1), Kin(P1, Q1),
# #ForAll([B2], Or(Equal(B1, B2),
# # Exists([P2], And(AtPose(B2, P2), CFree(P1, P2)))))),
# ForAll([B2], Or(Equal(B1, B2), # I think this compiles to the forward axioms that achieve things...
# Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(Collision(P1, P2))))))),
# #ForAll([B2], Or(Equal(B1, B2),
# # Not(Exists([P2], And(AtPose(B2, P2), Not(CFree(P1, P2)))))))),
# #ForAll([B2], Or(Equal(B1, B2), # Generates a ton of axioms...
# # Not(Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Collision(P1, P2))))))),
# effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)), Not(Moved()))),
#Action(name='place', parameters=[B1, P1, Q1],
# condition=And(Holding(B1), AtConf(Q1), IsPose(B1, P1), Kin(P1, Q1), Not(Unsafe(P1))),
# effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)), Not(Moved()))),
#Action(name='move', parameters=[Q1, Q2],
# condition=And(AtConf(Q1), Not(Moved())),
# effect=And(AtConf(Q2), Moved(), Not(AtConf(Q1)))),
# TODO: a lot of the slowdown is because of the large number of move axioms
# Inferred Safe
#Translator operators: 1843
#Translator axioms: 3281
#Search Time: 10.98
# Explicit Safe
#Translator operators: 1843
#Translator axioms: 3281
#Search Time: 9.926
]
# TODO: translate_strips_axiom in translate.py
# TODO: maybe this is bad because of shared poses...
# 15*15*15*15 = 50625
# Takeaways: using the implicit collision is good because it results in fewer facts
# The negated axiom does better than the normal axiom by a little bit for some reason...
axioms = [
# For some reason, the unsafe version of this is much better than the safe version in terms of making axioms?
# Even with one collision recorded, it makes a ton of axioms
#Axiom(effect=Safe(B2, P1),
# condition=Or(Holding(B2), Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(Collision(P1, P2)))))),
#Axiom(effect=Unsafe(B2, B1, P1),
# condition=And(Not(Equal(B1, B2)),
# # Exists([P2], And(AtPose(B2, P2), Not(CFree(P1, P2)))))),
# Exists([P2], And(AtPose(B2, P2), Collision(P1, P2))))),
#Axiom(effect=Unsafe(B2, B1, P1),
# condition=Exists([P2], And(AtPose(B2, P2), Not(CFree(P1, P2))))),
# TODO: I think the inverting is implicitly doing the same thing I do where I don't bother making an axiom if always true
Axiom(effect=Unsafe(B2, P1),
condition=Exists([P2],
And(AtPose(B2, P2), IsPose(B2, P2),
Collision(P1,
P2)))), # Don't even need IsPose?
# This is the best config. I think it is able to work well because it can prune the number of instances when inverting
# It starts to take up a little time when there are many possible placements for things though
# TODO: the difference is that it first instantiates axioms and then inverts!
#Axiom(effect=Unsafe(B2, P1),
# condition=Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(CFree(P1, P2)))))
# This doesn't result in too many axioms but takes a while to instantiate...
#Axiom(effect=Unsafe(P1), # Need to include the not equal thing
# condition=Exists([B2, P2], And(AtPose(B2, P2), IsPose(B2, P2), Collision(P1, P2)))),
# TODO: Can turn off options.filter_unreachable_facts
]
####################
# Conditional stream declarations
cond_streams = [
#GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[],
# generator=lambda: ((p,) for p in xrange(n, num_poses))),
GeneratorStream(
inputs=[B1],
outputs=[P1],
conditions=[],
effects=[IsPose(B1, P1)],
#generator=lambda b: ((p,) for p in xrange(n, num_poses))),
generator=lambda b: iter([(n + blocks.index(b), )])
), # Unique placements
GeneratorStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[Kin(P1, Q1)],
generator=lambda p: [(p, )]), # Inverse kinematics
#TestStream(inputs=[P1, P2], conditions=[], effects=[CFree(P1, P2)],
# test=lambda p1, p2: p1 != p2, eager=True),
# #test = lambda p1, p2: True, eager = True),
TestStream(inputs=[P1, P2],
conditions=[],
effects=[Collision(P1, P2)],
test=lambda p1, p2: p1 == p2,
eager=False,
sign=False),
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty(),
] + [AtPose(block, pose) for block, pose in initial_poses.items()] + [
IsPose(block, pose)
for block, pose in (initial_poses.items() + goal_poses.items())
]
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
CONF, BLOCK, POSE = Type(), Type(), Type()
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
#Safe = Pred(BLOCK, BLOCK, POSE)
Safe = Pred(BLOCK, POSE)
LegalKin = Pred(POSE, CONF)
#CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
CollisionFree = Pred(POSE, POSE)
rename_easy(locals()) # Trick to make debugging easier
##################################################
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)
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)
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 compile_observable_problem(world, task):
# Data types
CONF = Type()
TABLE = Type() # Difference between fixed and movable objects
BLOCK = Type()
POSE = Type()
# Fluent predicates
AtConf = Pred(CONF)
HandEmpty = Pred()
Holding = Pred(BLOCK)
AtPose = Pred(BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
# Free parameters
Q1, Q2 = Param(CONF), Param(CONF)
T = Param(TABLE)
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
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], # Localize table?
# condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1)),
# effect=And(AtPoseB(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=InRoom(R),
# condition=Exists([Q1], And(AtConf(Q1), ConfIn(Q1, R)))), # Infers B2 is at a safe pose wrt B1 at P1
]
def inverse_kinematics(pose): # TODO: list stream that uses ending info
yield (pose,)
#def sample_table(table):
# yield (pose,)
streams = [
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=inverse_kinematics),
]
initial_atoms = [AtConf(world.robot_conf)]
if world.holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(world.holding))
for obj, pose in world.object_poses:
initial_atoms.append(AtPose(obj, pose))
goal_literals = []
if task.robot_conf is not False:
goal_literals.append(AtConf(task.robot_conf))
if task.holding is None:
goal_literals.append(HandEmpty())
elif task.holding:
goal_literals.append(Holding(task.holding))
#for obj, pose in task.object_poses.iteritems():
# goal_literals.append(AtPoseB(obj, pose))
goal_formula = And(*goal_literals)
problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, streams, [])
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
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 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(p_init=0,
v_init=0,
a_init=0,
dt=.5,
max_a=10,
min_a=-10,
p_goal=10):
"""
Creates a 1D car STRIPStream problem.
https://github.com/KCL-Planning/SMTPlan/blob/master/benchmarks/car_nodrag/car_domain_nodrag.pddl
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
STATE, ACCEL, TIME = Type(), Type(), Type()
# Fluent predicates
AtState = Pred(STATE)
AtAccel = Pred(ACCEL)
NewTime = Pred()
# Fluent predicates
Running = Pred()
Stopped = Pred()
EngineBlown = Pred()
TransmissionFine = Pred()
GoalReached = Pred()
# Static predicates
Delta1 = Pred(ACCEL, ACCEL) # A2 - A1 = 1
Dynamics = Pred(STATE, ACCEL, STATE)
Contained = Pred(STATE)
# Free parameters
A1, A2 = Param(ACCEL), Param(ACCEL)
S1 = Param(STATE)
S2 = Param(STATE)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='accelerate',
parameters=[A1, A2],
condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A1,
A2)),
effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1)))),
Action(name='decelerate',
parameters=[A1, A2],
condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A2,
A1)),
effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1)))),
Action(name='simulate',
parameters=[S1, A1, S2],
condition=And(AtState(S1), AtAccel(A1), Dynamics(S1, A1, S2)),
effect=And(NewTime(), AtState(S2), Not(AtState(S1)))),
]
axioms = [
Axiom(effect=GoalReached(),
condition=Exists([S1], And(AtState(S1), Contained(S1)))),
]
####################
# Conditional stream declarations
cond_streams = [
FunctionStream(inputs=[S1, A1],
outputs=[S2],
conditions=[],
effects=[Dynamics(S1, A1, S2)],
function=lambda (p1, v1), a1:
(p1 + v1 * dt + .5 * a1 * dt**2, v1 + a1 * dt)),
GeneratorStream(inputs=[A1],
outputs=[A2],
conditions=[],
effects=[Delta1(A1, A2)],
generator=lambda a1: [a1 + 1]
if a1 + 1 <= max_a else []),
GeneratorStream(inputs=[A2],
outputs=[A1],
conditions=[],
effects=[Delta1(A2, A1)],
generator=lambda a2: [a2 + 1]
if a2 - 1 > -min_a else []),
TestStream(inputs=[S1],
conditions=[],
effects=[Contained(S1)],
test=lambda (p1, v1): p1 > p_goal,
eager=True),
]
####################
constants = []
initial_atoms = [
AtState((p_init, v_init)),
AtAccel(a_init),
NewTime(),
Running(),
]
goal_literals = [
GoalReached()
#AtAccel(0),
]
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem
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 create_problem(p_init=0, v_init=0, a_init=0,
dt=.5, max_a=10, min_a=-10, p_goal=10):
"""
Creates a 1D car STRIPStream problem.
https://github.com/KCL-Planning/SMTPlan/blob/master/benchmarks/car_nodrag/car_domain_nodrag.pddl
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POS, VEL, ACCEL, TIME = Type(), Type(), Type(), Type()
# Fluent predicates
AtPos = Pred(POS)
AtVel = Pred(VEL)
AtAccel = Pred(ACCEL)
NewTime = Pred()
# Fluent predicates
Running = Pred()
Stopped = Pred()
EngineBlown = Pred()
TransmissionFine = Pred()
GoalReached = Pred()
# Static predicates
Delta1 = Pred(ACCEL, ACCEL) # A2 - A1 = 1
Dynamics = Pred(POS, VEL, ACCEL, POS, VEL)
Contained = Pred(POS)
# Free parameters
A1, A2 = Param(ACCEL), Param(ACCEL)
P1, V1 = Param(POS), Param(VEL)
P2, V2 = Param(POS), Param(VEL)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
#Action(name='accelerate', parameters=[A1, A2],
# #condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A1, A2)),
# condition=And(NewTime(), AtAccel(A1), Delta1(A1, A2)),
# effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1))), cost=5),
STRIPSAction(name='accelerate', parameters=[A1, A2],
conditions=[NewTime(), AtAccel(A1), Delta1(A1, A2)],
effects=[AtAccel(A2), Not(NewTime()), Not(AtAccel(A1))], cost=5),
#Action(name='decelerate', parameters=[A1, A2],
# condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A2, A1)),
# condition=And(NewTime(), AtAccel(A1), Delta1(A2, A1)),
# effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1)))),
STRIPSAction(name='decelerate', parameters=[A1, A2],
conditions=[NewTime(), AtAccel(A1), Delta1(A2, A1)],
effects=[AtAccel(A2), Not(NewTime()), Not(AtAccel(A1))]),
#Action(name='simulate', parameters=[P1, V1, A1, P2, V2],
# condition=And(AtPos(P1), AtVel(V1), AtAccel(A1), Dynamics(P1, V1, A1, P2, V2)),
# effect=And(NewTime(), AtPos(P2), AtVel(V2), Not(AtPos(P1)), Not(AtVel(V1))), cost=1),
STRIPSAction(name='simulate', parameters=[P1, V1, A1, P2, V2],
conditions=[AtPos(P1), AtVel(V1), AtAccel(A1), Dynamics(P1, V1, A1, P2, V2)],
effects=[NewTime(), AtPos(P2), AtVel(V2), Not(AtPos(P1)), Not(AtVel(V1))], cost=1),
#STRIPSAction(name='goal', parameters=[P1],
# conditions=[AtPos(P1), Contained(P1)],
# effects=[GoalReached()], cost=None),
]
axioms = [
#Axiom(effect=GoalReached(), condition=Exists([P1], And(AtPos(P1), Contained(P1)))),
STRIPSAxiom(conditions=[AtPos(P1), Contained(P1)], effects=[GoalReached()])
]
####################
# Conditional stream declarations
cond_streams = [
FunctionStream(inputs=[P1, V1, A1], outputs=[P2, V2], conditions=[], effects=[Dynamics(P1, V1, A1, P2, V2)],
function=lambda p1, v1, a1: (p1 + v1*dt + .5*a1*dt**2, v1 + a1*dt)),
FunctionStream(inputs=[A1], outputs=[A2], conditions=[], effects=[Delta1(A1, A2)],
function=lambda a1: (a1+1,) if a1+1 <= max_a else None),
#GeneratorStream(inputs=[A1], outputs=[A2], conditions=[], effects=[Delta1(A1, A2)],
# generator=lambda a1: [a1+1] if a1+1 <= max_a else []),
FunctionStream(inputs=[A2], outputs=[A1], conditions=[], effects=[Delta1(A2, A1)],
function=lambda a2: (a2+1,) if a2-1 >= min_a else None),
#GeneratorStream(inputs=[A2], outputs=[A1], conditions=[], effects=[Delta1(A2, A1)],
# generator=lambda a2: [a2+1] if a2-1 >- min_a else []),
TestStream(inputs=[P1], conditions=[], effects=[Contained(P1)],
test=lambda p1: p1 >= p_goal, eager=True),
]
####################
constants = []
initial_atoms = [
AtPos(p_init),
AtVel(v_init),
AtAccel(a_init),
NewTime(),
#Running(),
]
goal_literals = [
GoalReached(),
AtAccel(0),
]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
FreeMotion = Pred(CONF, CONF, TRAJ)
HoldingMotion = Pred(CONF, CONF, GRASP, TRAJ)
GraspMotion = Pred(POSE, GRASP, CONF, TRAJ)
# Static collision
CFreePose = Pred(POSE, POSE)
CFreeTraj = Pred(TRAJ, POSE)
####################
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)))),
def create_problem(n=40):
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i'%i for i in xrange(n)]
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 = {blocks[0]: 1} # 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)
IsPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, POSE)
Unsafe = Pred(BLOCK, POSE)
# Static predicates
Kin = Pred(POSE, CONF)
CFree = Pred(POSE, POSE)
Collision = Pred(POSE, 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='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
#Axiom(effect=Safe(B2, P1),
# condition=Or(Holding(B2), Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(Collision(P1, P2)))))),
Axiom(effect=Unsafe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Collision(P1, P2)))),
]
####################
# Conditional stream declarations
cond_streams = [
#GeneratorStream(inputs=[B1], outputs=[P1], conditions=[], effects=[IsPose(B1, P1)],
# generator=lambda b: ((p,) for p in xrange(n, num_poses))),
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[Kin(P1, Q1)],
generator=lambda p: [(p,)]), # Inverse kinematics
TestStream(inputs=[P1, P2], conditions=[], effects=[Collision(P1, P2)],
test=lambda p1, p2: p1 == p2, eager=True),
]
# TODO: this actually isn't slow at all. What was the problem then?
# Maybe it was that I was creating many predicates?
# It could have also been the lack of IsPose predicates
for b in blocks:
axioms += [
# TODO: to do this, need to make b a parameter and set it using inequality
#Axiom(effect=Unsafe(b, P1),
# condition=Exists([P2], And(AtPose(b, P2), IsPose(b, P2), Collision(P1, P2)))),
]
actions += [
Action(name='pick-{}'.format(b), parameters=[P1, Q1],
condition=And(AtPose(b, P1), HandEmpty(), AtConf(Q1), IsPose(b, P1), Kin(P1, Q1)),
effect=And(Holding(b), Not(AtPose(b, P1)), Not(HandEmpty()))),
Action(name='place-{}'.format(b), parameters=[P1, Q1],
condition=And(Holding(b), AtConf(Q1), IsPose(b, P1), Kin(P1, Q1),
#*[Safe(b2, P1) for b2 in blocks if b2 != b]),
*[Not(Unsafe(b2, P1)) for b2 in blocks if b2 != b]),
effect=And(AtPose(b, P1), HandEmpty(), Not(Holding(b)))),
]
cond_streams += [
GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[IsPose(b, P1)],
generator=lambda: ((p,) for p in xrange(n, num_poses))),
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty(),
] + [
AtPose(block, pose) for block, pose in initial_poses.items()
] + [
IsPose(block, pose) for block, pose in (initial_poses.items() + goal_poses.items())
]
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
