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 get_problem(env, problem, robot, manipulator, base_manip, bodies,
initial_conf):
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash()
for body in all_bodies
}) == 1 # 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 = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else []
poses = problem.known_poses if problem.known_poses else []
cfree_pose = cfree_pose_fn(env, body1, body2)
cfree_traj = cfree_traj_fn(env, manipulator, body1, body2, all_bodies)
# Variables
R_Q, O_P, O_H, R_T, = 'R_Q', 'O_P', 'O_H', 'R_T'
# Types
OBJ = VarType(domain=list(bodies))
CONF, TRAJ = VarType(), VarType()
BOOL = VarType(domain=[True, False])
POSE = VarType(domain=poses + grasps)
# Trajectory constraints
FreeMotion = ConType([CONF, CONF, TRAJ])
HoldingMotion = ConType([CONF, CONF, POSE, TRAJ])
GraspMotion = ConType([POSE, POSE, CONF, TRAJ])
Stable = ConType([POSE], satisfying=[(p, ) for p in poses])
Grasp = ConType([POSE], satisfying=[(g, ) for g in grasps])
# Static constraints
CFreePose = ConType([POSE, POSE], test=cfree_pose)
CFreeTraj = ConType([TRAJ, POSE], test=cfree_traj)
O = Par(OBJ)
P, G = Par(POSE), Par(POSE)
O2, P2 = Par(OBJ), Par(POSE)
Q, Q2 = Par(CONF), Par(CONF)
T = Par(TRAJ)
rename_variables(locals())
state_vars = [
Var(R_Q, CONF),
Var(O_P, POSE, args=[OBJ]),
Var(O_H, BOOL, args=[OBJ]),
]
control_vars = [Var(R_T, TRAJ)]
####################
transition = [
Clause([
Stable(X[O_P, O]),
Grasp(nX[O_P, O]),
Eq(X[O_H, O], False),
Eq(nX[O_H, O], True),
GraspMotion(X[O_P, O], nX[O_P, O], X[R_Q], U[R_T])
] + [Eq(X[O_H, obj], False) for obj in bodies] +
[CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='pick'),
Clause([
Grasp(X[O_P, O]),
Stable(nX[O_P, O]),
Eq(X[O_H, O], True),
Eq(nX[O_H, O], False),
GraspMotion(nX[O_P, O], X[O_P, O], X[R_Q], U[R_T])
] + [CFreePose(nX[O_P, O], X[O_P, obj]) for obj in bodies] +
[CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='place'),
Clause([FreeMotion(X[R_Q], nX[R_Q], U[R_T])] +
[Eq(X[O_H, obj], False) for obj in bodies] +
[CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='move'),
Clause([
HoldingMotion(X[R_Q], nX[R_Q], X[O_P, O], U[R_T]),
Eq(X[O_H, O], True)
] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='move_holding')
]
####################
sample_grasp_traj = sample_grasp_traj_fn(env, manipulator, body1,
all_bodies)
sample_free_motion = sample_free_motion_fn(manipulator, base_manip,
all_bodies)
sample_holding_motion = sample_holding_motion_fn(manipulator, base_manip,
body1, all_bodies)
samplers = [
# Pick/place trajectory
Sampler([GraspMotion(P, G, Q, T)],
gen=sample_grasp_traj,
inputs=[P, G],
domain=[Stable(P), Grasp(G)]),
# Move trajectory
Sampler([FreeMotion(Q, Q2, T)],
gen=sample_free_motion,
inputs=[Q, Q2],
order=1,
max_level=0),
Sampler([HoldingMotion(Q, Q2, G, T)],
gen=sample_holding_motion,
inputs=[Q, Q2, G],
domain=[Grasp(G)],
order=1,
max_level=0),
]
####################
initial_state = [
Eq(X[R_Q], initial_conf),
] + [
Eq(X[O_P, obj], pose)
for obj, pose in problem.initial_poses.iteritems()
] + [Eq(X[O_H, obj], False) for obj in problem.initial_poses]
goal_constraints = [Eq(X[R_Q], initial_conf)] + [
Eq(X[O_P, obj], pose) for obj, pose in problem.goal_poses.iteritems()
]
return FTSProblem(state_vars, control_vars, transition, samplers,
initial_state, goal_constraints)
def create_problem():
"""
Creates the 1D task and motion planning FTSProblem problem.
:return: a :class:`.FTSProblem`
"""
blocks = ['block%i' % i for i in range(2)]
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)}
####################
R_Q, B_P, B_H = 'R_Q', 'B_P', 'B_H'
R_T = 'R_T'
CONF = VarType()
BOOL = VarType(domain=[True, False])
POSE = VarType()
BLOCK = VarType(domain=blocks)
TRAJ = VarType()
B, Q, P = Par(BLOCK), Par(CONF), Par(POSE)
T, Q2 = Par(TRAJ), Par(CONF)
LegalKin = ConType([POSE, CONF])
CollisionFree = ConType([POSE, POSE], test=lambda p1,
p2: None in (p1, p2) or p1 != p2)
Motion = ConType([CONF, TRAJ, CONF])
rename_variables(locals()) # Trick to make debugging easier
state_vars = [Var(R_Q, CONF), Var(B_P, POSE, args=[BLOCK]),
Var(B_H, BOOL, args=[BLOCK])]
control_vars = [Var(R_T, TRAJ)]
##########
transition = [
Clause([LegalKin(X[B_P, B], X[R_Q]), Eq(nX[B_P, B], None), Eq(X[B_H, B], False), Eq(nX[B_H, B], True)] +
[Eq(X[B_H, block], False) for block in blocks], name='pick'),
Clause([LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_P, B], None), Eq(X[B_H, B], True), Eq(nX[B_H, B], False)] +
[CollisionFree(X[B_P, block], nX[B_P, B]) for block in blocks], name='place'),
Clause([Motion(X[R_Q], U[R_T], nX[R_Q])], name='move'),
]
##########
samplers = [
Sampler([P], gen=lambda: ([(p,)]
for p in xrange(num_poses)), inputs=[]),
Sampler([LegalKin(P, Q)], gen=lambda p: [[(p,)]]
if p is not None else [], inputs=[P]),
Sampler([Motion(Q, T, Q2)], gen=lambda q1,
q2: [[((q1, q2),)]], inputs=[Q, Q2]),
]
##########
initial_state = [Eq(X[R_Q], initial_config)] + \
[Eq(X[B_H, block], False) for block in blocks] + \
[Eq(X[B_P, block], pose)
for block, pose in initial_poses.iteritems()]
goal_constraints = [Eq(X[B_P, block], pose)
for block, pose in goal_poses.iteritems()]
return FTSProblem(state_vars, control_vars, transition, samplers,
initial_state, goal_constraints)
def create_problem():
"""
Creates the 1D task and motion planning FTSProblem problem.
:return: a :class:`.FTSProblem`
"""
blocks = ['block%i' % i for i in range(2)]
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}
####################
# TODO - rethink the naming...
R_Q, B_P, B_H = 'R_Q', 'B_P', 'B_H'
R_T = 'R_T'
# NOTE - these should really just be param type
CONF = VarType()
BOOL = VarType(domain=[True, False])
POSE = VarType()
BLOCK = VarType(domain=blocks) # TODO - VarType vs ParamType?
TRAJ = VarType()
B, Q, P = Par(BLOCK), Par(CONF), Par(POSE)
T, Q2 = Par(TRAJ), Par(CONF)
LegalKin = ConType([POSE, CONF])
CollisionFree = ConType([POSE, POSE],
test=lambda p1, p2: None in (p1, p2) or p1 != p2)
Motion = ConType([CONF, TRAJ, CONF])
rename_variables(locals()) # Trick to make debugging easier
# NOTE - can make a holding variable for each object or just one holding variable
# TODO - maybe declare the variables upfront
state_vars = [
Var(R_Q, CONF),
Var(B_P, POSE, args=[BLOCK]),
Var(B_H, BOOL, args=[BLOCK])
]
control_vars = [Var(R_T, TRAJ)]
##########
# NOTE - didn't I have some kind of bug where things had to be effects for this to work?
# NOTE - this is because I have to write things in the form where we only mention things that change....
# THus, I need the identify constraint or something
transition = [
Clause([
LegalKin(X[B_P, B], X[R_Q]),
Eq(nX[B_P, B], None),
Eq(X[B_H, B], False),
Eq(nX[B_H, B], True)
] + [Eq(X[B_H, block], False) for block in blocks],
name='pick'),
#Clause([LegalKin(X[B_P, B], X[R_Q]), Eq(nX[B_P, B], None), Eq(nX[B_H, B], True)] +
# [Eq(X[B_H, block], False) for block in blocks], name='pick'), # NOTE - this makes bool free params internally
Clause([
LegalKin(nX[B_P, B], X[R_Q]),
Eq(X[B_P, B], None),
Eq(X[B_H, B], True),
Eq(nX[B_H, B], False)
] + [CollisionFree(X[B_P, block], nX[B_P, B]) for block in blocks],
name='place'),
#Clause([LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_P, B], None), Eq(nX[B_H, B], False)], # +
##Clause([LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_H, B], True), Eq(nX[B_H, B], False)], # +
# [CollisionFree(X[B_P, block], nX[B_P, B]) for block in blocks], name='place'),
#Clause([Unconstrained(nX[R_Q])], name='move'), # NOTE - only write what changes
Clause([Motion(X[R_Q], U[R_T], nX[R_Q])], name='move'),
]
##########
# TODO - expand so we don't need to return lists of lists
samplers = [
Sampler([P],
gen=lambda: ([(p, )] for p in xrange(num_poses)),
inputs=[]),
Sampler([LegalKin(P, Q)],
gen=lambda p: [[(p, )]] if p is not None else [],
inputs=[P]),
Sampler([Motion(Q, T, Q2)],
gen=lambda q1, q2: [[((q1, q2), )]],
inputs=[Q, Q2]), # TODO - avoid this much nesting
]
##########
initial_state = [Eq(X[R_Q], initial_config)] + \
[Eq(X[B_H, block], False) for block in blocks] + \
[Eq(X[B_P, block], pose) for block, pose in initial_poses.iteritems()]
goal_constraints = [
Eq(X[B_P, block], pose) for block, pose in goal_poses.iteritems()
]
#goal_constraints = [Eq(X[R_Q], 1)]
return FTSProblem(state_vars, control_vars, transition, samplers,
initial_state, goal_constraints)
def get_problem(env, problem, robot, manipulator, base_manip, bodies,
initial_conf):
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
# TODO - bug that when n=1, it automatically passes holding collisions because only one body used
grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else []
poses = problem.known_poses if problem.known_poses else []
# Limits the number of poses
#needed_poses = list(set(problem.initial_poses.values() + problem.goal_poses.values()))
#poses = needed_poses + list(set(poses) - set(needed_poses))[:9]
cfree_pose = cfree_pose_fn(env, body1, body2)
cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2,
all_bodies)
# Variables
R_Q, O_P, O_H, R_T, = 'R_Q', 'O_P', 'O_H', 'R_T'
#R_H = 'R_H'
# Types
OBJ = VarType(domain=list(bodies))
CONF, TRAJ = VarType(), VarType()
#POSE, GRASP = VarType(domain=poses), VarType(domain=grasps)
BOOL = VarType(domain=[True, False])
#HELD = VarType(domain=[None]+list(bodies))
POSE = VarType(domain=poses + grasps) # TODO - need to combine these
# Trajectory constraints
FreeMotion = ConType([CONF, CONF, TRAJ])
#HoldingMotion = ConType([CONF, CONF, GRASP, TRAJ])
#GraspMotion = ConType([POSE, GRASP, CONF, TRAJ])
HoldingMotion = ConType([CONF, CONF, POSE, TRAJ])
GraspMotion = ConType([POSE, POSE, CONF, TRAJ])
# TODO - can also just make these separate variables
Stable = ConType([POSE], satisfying=[(p, ) for p in poses])
Grasp = ConType([POSE], satisfying=[(g, ) for g in grasps])
# Static constraints
CFreePose = ConType([POSE, POSE], test=cfree_pose)
CFreeTraj = ConType([TRAJ, POSE], test=cfree_traj)
O = Par(OBJ)
#P, G = Par(POSE), Par(GRASP)
P, G = Par(POSE), Par(POSE)
O2, P2 = Par(OBJ), Par(POSE)
Q, Q2 = Par(CONF), Par(CONF)
T = Par(TRAJ)
rename_variables(locals())
state_vars = [
Var(R_Q, CONF),
Var(O_P, POSE, args=[OBJ]),
Var(O_H, BOOL, args=[OBJ]),
#Var(R_H, HELD),
]
control_vars = [Var(R_T, TRAJ)]
####################
# TODO - could use fact that poses and grasp satisfy constraints to replace holding
transition = [
Clause(
[
Stable(X[O_P, O]),
Grasp(
nX[O_P,
O]), # NOTE - not necessary but results in slowdown...
Eq(X[O_H, O], False),
Eq(nX[O_H, O], True),
GraspMotion(X[O_P, O], nX[O_P, O], X[R_Q], U[R_T])
] + [Eq(X[O_H, obj], False) for obj in bodies] +
[CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='pick'), # No need to test placement collisions
Clause([
Grasp(X[O_P, O]),
Stable(nX[O_P, O]),
Eq(X[O_H, O], True),
Eq(nX[O_H, O], False),
GraspMotion(nX[O_P, O], X[O_P, O], X[R_Q], U[R_T])
] + [CFreePose(nX[O_P, O], X[O_P, obj]) for obj in bodies] +
[CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='place'),
# TODO - warning if you mix T and R_T
#Clause([Eq(X[R_H], None), FreeMotion(X[R_Q], nX[R_Q], U[R_T])] +
# [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move'),
Clause(
[FreeMotion(X[R_Q], nX[R_Q], U[R_T])] +
#[Unconstrained(nX[R_Q])] +
[Eq(X[O_H, obj], False) for obj in bodies] +
[CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='move'),
#Clause([Eq(X[R_H], O), HoldingMotion(X[R_Q], nX[R_Q], X[O_P, O], U[R_T])] +
# [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move_holding')
Clause([
HoldingMotion(X[R_Q], nX[R_Q], X[O_P, O], U[R_T]),
Eq(X[O_H, O], True)
] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies],
name='move_holding')
]
####################
sample_grasp_traj = sample_grasp_traj_fn(env, robot, manipulator, body1,
all_bodies)
sample_free_motion = sample_free_motion_fn(manipulator, base_manip,
all_bodies)
sample_holding_motion = sample_holding_motion_fn(robot, manipulator,
base_manip, body1,
all_bodies)
samplers = [
# Pick/place trajectory
Sampler([GraspMotion(P, G, Q, T)],
gen=sample_grasp_traj,
inputs=[P, G],
domain=[Stable(P), Grasp(G)]),
# Move trajectory
Sampler([FreeMotion(Q, Q2, T)],
gen=sample_free_motion,
inputs=[Q, Q2],
order=1,
max_level=0),
Sampler([HoldingMotion(Q, Q2, G, T)],
gen=sample_holding_motion,
inputs=[Q, Q2, G],
domain=[Grasp(G)],
order=1,
max_level=0),
]
####################
initial_state = [
Eq(X[R_Q], initial_conf),
#Eq(X[R_H], None),
] + [
Eq(X[O_P, obj], pose)
for obj, pose in problem.initial_poses.iteritems()
] + [Eq(X[O_H, obj], False) for obj in problem.initial_poses]
goal_constraints = [Eq(X[R_Q], initial_conf)] + [
Eq(X[O_P, obj], pose) for obj, pose in problem.goal_poses.iteritems()
]
#goal_constraints = [Eq(X[O_H, list(problem.goal_poses)[0]], True)]
#goal_constraints = [Eq(X[R_Q], initial_conf), Eq(X[O_H, list(problem.goal_poses)[0]], True)]
return FTSProblem(state_vars, control_vars, transition, samplers,
initial_state, goal_constraints)