def create_problem(goal, obstacles=(), distance=.25, digits=3):
"""
Creates a Probabilistic Roadmap (PRM) motion planning problem.
:return: a :class:`.FTSProblem`
"""
R_Q = 'R_Q'
CONF, REGION = VarType(), VarType(domain=[goal] if is_region(goal) else [])
Q, R = Par(CONF), Par(REGION)
IsEdge = ConType([CONF, CONF],
test=lambda q1, q2: is_collision_free(
(q1, q2), obstacles)) # CFree
Contained = ConType([CONF, REGION])
rename_variables(locals()) # Trick to make debugging easier
state_vars = [Var(R_Q, CONF)]
control_vars = []
##########
clauses = [
Clause([IsEdge(X[R_Q], nX[R_Q])], name='move'),
]
##########
samplers = [
Sampler([Q],
gen=lambda: ([(sample(), )] for _ in inf_sequence()),
inputs=[]),
Sampler([Contained(Q, R)],
gen=lambda r: ([(sample_box(r), )] for _ in inf_sequence()),
inputs=[R]),
]
##########
initial_state = [Eq(X[R_Q], (0, 0))]
goal_constraints = []
if is_region(goal):
goal_constraints.append(Contained(X[R_Q], goal))
else:
goal_constraints.append(Eq(X[R_Q], goal))
return FTSProblem(state_vars, control_vars, clauses, samplers,
initial_state, goal_constraints)
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 sampler():
for _ in inf_sequence():
yield [(sample(digits), ) for _ in range(10)]
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