def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
initialize_openrave(env, ARM, min_delta=.01)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash() for body in all_bodies}) == 1 # NOTE - assuming all objects has the same geometry
body1 = all_bodies[-1] # Generic object 1
body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:MAX_GRASPS]
poses = problem.known_poses if problem.known_poses else []
open_gripper(manipulator)
initial_conf = Conf(robot.GetConfigurationValues()[manipulator.GetArmIndices()])
def _enable_all(enable): # Enables or disables all bodies for collision checking
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at pose2
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
_enable_all(False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
_enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
def cfree_traj(traj, pose): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose
if DISABLE_TRAJ_COLLISIONS:
return True
return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))
####################
def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
_enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration
if grasp_conf is None: return
if DISABLE_TRAJECTORIES:
yield [(Conf(grasp_conf), object())]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp
#grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration
yield [(pregrasp_conf, grasp_traj)]
def sample_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
_enable_all(False)
set_manipulator_conf(manipulator, conf1.value)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
if not traj: return
traj.grasp = None
yield [(traj,)]
def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
_enable_all(False)
body1.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
robot.Grab(body1)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
robot.Release(body1)
if not traj: return
traj.grasp = grasp
yield [(traj,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[Q, T], conditions=[],
effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2], outputs=[T], conditions=[],
effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0),
EasyListGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)],
test=cfree_pose, eager=True),
EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)],
test=cfree_traj),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ConfEq(initial_conf), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
if viewer:
env.UpdatePublishedBodies()
raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10)
solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
#solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
#universe.print_statistics()
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
####################
def _execute_traj(traj):
#for j, conf in enumerate(traj.path()):
#for j, conf in enumerate([traj.end()]):
path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
for j, conf in enumerate(path):
set_manipulator_conf(manipulator, conf)
env.UpdatePublishedBodies()
raw_input('%s/%s) Step?'%(j, len(path)))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
#set_manipulator_conf(manipulator, initial_conf.value)
set_manipulator_conf(manipulator, initial_conf)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
env.UpdatePublishedBodies()
if not DISABLE_TRAJECTORIES:
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, _, traj = args
_execute_traj(traj)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
_execute_traj(traj)
elif action.name == 'pick':
obj, _, _, _, traj = args
_execute_traj(traj.reverse())
robot.Grab(bodies[obj])
_execute_traj(traj)
elif action.name == 'place':
obj, _, _, _, traj = args
_execute_traj(traj.reverse())
robot.Release(bodies[obj])
_execute_traj(traj)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
else:
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, q2, _ = args
set_manipulator_conf(manipulator, q2)
elif action.name == 'move_holding':
_, q2, _, _, _ = args
set_manipulator_conf(manipulator, q2)
elif action.name == 'pick':
obj, _, _, _, traj = args
robot.Grab(bodies[obj])
elif action.name == 'place':
obj, _, _, _, traj = args
robot.Release(bodies[obj])
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def 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(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 solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot, manipulator, base_manip, _ = initialize_openrave(env,
ARM,
min_delta=.01)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash()
for body in all_bodies
}) == 1 # 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 []
open_gripper(manipulator)
initial_conf = Conf(
robot.GetConfigurationValues()[manipulator.GetArmIndices()])
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[Q, T],
conditions=[],
effects=[GraspMotion(P, G, Q, T)],
generator=sample_grasp_traj_fn(
env, manipulator, body1, all_bodies)),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_motion_fn(
manipulator, base_manip, all_bodies),
order=1,
max_level=0),
EasyListGenStream(inputs=[Q, Q2, G],
outputs=[T],
conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)],
generator=sample_holding_motion_fn(
manipulator, base_manip, body1, all_bodies),
order=1,
max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePose(P, P2)],
test=cfree_pose_fn(env, body1, body2),
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=cfree_traj_fn(env, manipulator, body1, body2,
all_bodies)),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ConfEq(initial_conf),
*(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if viewer and plan is not None:
print SEPARATOR
visualize_solution(env, problem, initial_conf, robot, manipulator,
bodies, plan)
raw_input('Finish?')
def solve_tamp(env, use_focused):
use_viewer = env.GetViewer() is not None
problem = PROBLEM(env)
# TODO: most of my examples have literally had straight-line motions plans
robot, manipulator, base_manip, ir_model = initialize_openrave(
env, ARM, min_delta=MIN_DELTA)
set_manipulator_conf(ir_model.manip, CARRY_CONFIG)
bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names}
surfaces = {surface.name: surface for surface in problem.surfaces}
open_gripper(manipulator)
initial_q = Conf(robot.GetConfigurationValues())
initial_poses = {
name: Pose(name, get_pose(body))
for name, body in bodies.iteritems()
}
# TODO: just keep track of the movable degrees of freedom
# GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues
saver = EnvironmentStateSaver(env)
#cfree_pose = cfree_pose_fn(env, bodies)
#cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies)
#base_generator = base_generator_fn(ir_model)
#base_values = base_values_from_full_config(initial_q.value)
#goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value)
#goal_conf = Conf(goal_values)
#return
####################
# TODO - should objects contain their geometry
cond_streams = [
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[IsPose(O, P),
IsGrasp(O, G)],
effects=[GraspMotion(O, P, G, Q, T)],
generator=sample_grasp_traj_fn(
base_manip, ir_model, bodies, CARRY_CONFIG)),
EasyGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_base_motion_fn(base_manip, bodies),
order=1,
max_level=0),
EasyTestStream(
inputs=[O, P, O2, P2],
conditions=[IsPose(O, P), IsPose(O2, P2)],
effects=[CFreePose(P, P2)],
test=lambda *args: True, #cfree_pose,
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=lambda *args: True),
#test=cfree_traj),
EasyListGenStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[IsGrasp(O, G)],
generator=grasp_generator_fn(bodies, TOP_GRASPS,
SIDE_GRASPS,
MAX_GRASPS)),
EasyListGenStream(inputs=[O, S],
outputs=[P],
conditions=[],
effects=[IsPose(O, P), Stable(P, S)],
generator=pose_generator_fn(bodies, surfaces)),
#EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)],
# effects=[], generator=base_generator),
]
####################
constants = []
initial_atoms = [ConfEq(initial_q), HandEmpty()]
for name in initial_poses:
initial_atoms += body_initial_atoms(name, initial_poses, bodies,
surfaces)
goal_formula = And(
ConfEq(initial_q), *[
OnSurface(obj, surface)
for obj, surface in problem.goal_surfaces.iteritems()
])
#goal_formula = ConfEq(goal_conf)
#obj, _ = problem.goal_surfaces.items()[0]
#goal_formula = And(Holding(obj))
#goal_formula = Holding(obj) # TODO: this cause a bug
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
print stream_problem
handles = draw_affine_limits(robot)
if use_viewer:
for surface in problem.surfaces:
surface.draw(env)
raw_input('Start?')
max_time = INF
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
if use_focused:
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False,
max_time=max_time)
else:
solve = lambda: incremental_planner(stream_problem,
search=search_fn,
frequency=10,
waves=False,
debug=False,
max_time=max_time)
with env:
plan, universe = solve()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if plan is not None:
commands = process_plan(robot, bodies, plan)
if use_viewer:
print SEPARATOR
saver.Restore()
visualize_solution(commands, step=False)
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
#problem = dantam(env)
problem = dantam2(env)
#problem = move_several_4(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, 'leftarm')
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#USE_GRASP_APPROACH = GRASP_APPROACHES.SIDE
USE_GRASP_APPROACH = GRASP_APPROACHES.TOP
#USE_GRASP_TYPE = GRASP_TYPES.TOUCH
USE_GRASP_TYPE = GRASP_TYPES.GRASP
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
##################################################
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
def collision_free(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def grasp_env_cfree(mt, g):
enable_all(False) # TODO - base config?
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def grasp_pose_cfree(mt, g, p):
enable_all(False) # TODO - base config?
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
##################################################
"""
class CollisionStream(Stream): # TODO - could make an initial state version of this that doesn't need pose2
def get_values(self, **kwargs):
self.enumerated = True
pose1, pose2 = map(get_value, self.inputs)
if collision_free(pose1, pose2):
return [PoseCFree(pose1, pose2)]
return []
#return [Movable()] # NOTE - only make movable when it fails a collision check
CheckedInitial = Pred(POSE) # How should this be handled? The planner will need to revisit on the next state anyways
class EnvCollisionStream(Stream): # NOTE - I could also make an environment OBJECT which I mutate. I'm kind of doing that now
movable = []
def get_values(self, **kwargs):
self.enumerated = True
pose, = map(get_value, self.inputs)
results = [CheckedInitial(pose)]
for obj, pose2 in problem.initial_poses.iteritems():
if obj not in self.movable and collision_free(pose, pose2):
pass
#results.append(PoseCFree(pose, pose2))
else:
self.movable.append(obj)
results.append(PoseEq(obj, pose2))
#results.append(Movable(obj))
if results:
pass # NOTE - I could make this fail if there is a collision
# I could prevent the binding by directly adding CheckedInitial to the universe
# In general, I probably can just mutate the problem however I see fit here
return results
"""
##################################################
# NOTE - can do pose, approach manip, true approach traj, motion plan
# NOTE - can make something that produces approach trajectories
def get_manip_vector(pose, grasp):
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
#enable_all(False)
#if manipulator.CheckEndEffectorCollision(manip_trans):
# return None
return ManipVector(manip_trans, approach_vector)
def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
#print manipulator.CheckEndEffectorCollision(manip_trans)
if grasp_config is not None:
yield [Config(grasp_config)]
#traj = workspace_traj_helper(base_manip, approach_vector)
def sample_grasp_traj(pose, grasp, base_conf):
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector)
if grasp_config is None: return
yield [(Config(grasp_traj.end()), grasp_traj)]
##################################################
# NOTE - can either include the held object in the traj or have a special condition that not colliding
def sample_arm_traj(mq1, mq2, bq): # TODO - need to add holding back in
yield None
#enable_all(False)
#with robot:
# set_base_values(robot, bq.value)
# pass
# TODO - does it make sense to make a new stream for the biasing or to continuously just pass things
# I suppose I could cache the state or full plan as context
class MotionStream(Stream): # TODO - maybe make this produce the correct values
num = 0
#def get_values(self, **kwargs):
# self.enumerated = True
# mq1, mq2, bq = map(get_value, self.inputs)
# #mt = None
# mt = MotionStream.num
# MotionStream.num += 1 # Ensures all are unique
# return [ManipMotion(mq1, mq2, bq, mt)]
def sample_motion_plan(self, mq1, mq2, bq):
set_manipulator_values(manipulator, mq1.value)
set_base_values(robot, bq.value)
return motion_plan(env, cspace, mq2.value, self_collisions=True)
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2, bq = map(get_value, self.inputs)
collision_atoms = filter(lambda atom: atom.predicate in [MTrajGraspCFree, MTrajPoseCFree], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is MTrajGraspCFree:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is MTrajPoseCFree:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies) # How would I handle many constraints on the same traj?
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies)-1
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans))
robot.Grab(body1)
mt = self.sample_motion_plan(mq1, mq2, bq)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, bq, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
mt = self.sample_motion_plan(mq1, mq2, bq)
if mt:
return [ManipMotion(mq1, mq2, bq, mt)]
return []
##################################################
cond_streams = [
#MultiEasyGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses),
#EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
# effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=sample_arm_traj),
ClassStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, BQ, MT)], StreamClass=MotionStream, order=1, max_level=0),
#MultiEasyGenStream(inputs=[P, G, BQ], outputs=[MQ], conditions=[],
# effects=[Kin(P, G, BQ, MQ)], generator=sample_ik),
EasyListGenStream(inputs=[P, G, BQ], outputs=[MQ, GT], conditions=[],
effects=[GraspTraj(P, G, BQ, MQ, GT)], generator=sample_grasp_traj),
#EasyTestStream(inputs=[O, P, T], conditions=[], effects=[CFree(O, P, T)],
# test=collision_free, eager=True),
EasyTestStream(inputs=[P, P2], conditions=[], effects=[PoseCFree(P, P2)],
test=collision_free, eager=True),
#ClassStream(inputs=[P, P2], conditions=[], outputs=[],
# effects=[PoseCFree(P, P2)], StreamClass=CollisionStream, eager=True),
EasyTestStream(inputs=[MT, P], conditions=[], effects=[MTrajPoseCFree(MT, P)],
test=lambda mt, p: True, eager=True),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[MTrajGraspCFree(MT, G)],
test=lambda mt, g: True, eager=True),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[MTrajGraspPoseCFree(MT, G, P)],
test=lambda mt, g, p: True, eager=True),
#ClassStream(inputs=[P], conditions=[], outputs=[],
# effects=[CheckedInitial(P)], StreamClass=EnvCollisionStream),
]
##################################################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_full = Config(get_full_config(robot))
initial_base = get_base_conf(initial_full)
initial_manip = get_arm_conf(manipulator, initial_full)
initial_atoms = [
BaseEq(initial_base),
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, operators, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=True)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
##################################################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
open_gripper2(manipulator)
set_base_values(robot, initial_base.value)
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move_arm':
mq1, mq2, bq, mt = args
#set_manipulator_values(manipulator, mq2.value)
step_path(mt)
elif action.name == 'pick':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
#grasp_gripper2(manipulator, g) # NOTE - g currently isn't a real grasp
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
#open_gripper2(manipulator)
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
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 solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
print 'cfree_mtraj_grasp'
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(robot, body2):
print 'cfree_mtraj_pose'
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
print 'cfree_mtraj_grasp_pose'
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_manip_motion(mq1, mq2):
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if not mt: return
yield [(mt,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[],
effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)], generator=sample_manip_motion, order=1, max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose, eager=True),
EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def solve_tamp(env):
use_viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot, manipulator, base_manip, ir_model = initialize_openrave(
env, ARM, min_delta=MIN_DELTA)
set_manipulator_conf(ir_model.manip, CARRY_CONFIG)
bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names}
surfaces = {surface.name: surface for surface in problem.surfaces}
open_gripper(manipulator)
initial_conf = Conf(robot.GetConfigurationValues())
initial_poses = {
name: Pose(get_pose(body))
for name, body in bodies.iteritems()
}
saver = EnvironmentStateSaver(env)
stable_test = get_stable_test(bodies, surfaces)
####################
cond_streams = [
# # Pick/place trajectory
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[IsPose(O, P),
IsGrasp(O, G)],
effects=[GraspMotion(O, P, G, Q, T)],
generator=sample_grasp_traj_fn(base_manip,
ir_model,
bodies,
CARRY_CONFIG,
max_failures=200)),
#
# # Move trajectory
#MultiEasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[],
# effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0),
#MultiEasyGenStream(inputs=[Q, Q2, O, G], outputs=[T], conditions=[IsGrasp(O, G)],
# effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0),
#
# # Collisions
EasyTestStream(inputs=[O, P, O2, P2],
conditions=[IsPose(O, P), IsPose(O2, P2)],
effects=[CFreePose(P, P2)],
test=lambda *args: True,
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=lambda *args: True),
EasyListFnStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[IsGrasp(O, G)],
function=grasp_generator_fn(bodies)),
#EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[],
# effects=[IsPose(O, P), Stable(P, S)],
# generator=pose_generator_fn(bodies, surfaces)),
#EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[],
# effects=[IsPose(O, P), Stable(P, S), IsEdge(P)],
# generator=edge_generator_fn(bodies, surfaces)),
# TODO: remove O from here
RawListGenStream(
inputs=[O, P, P2],
outputs=[Q, T],
#conditions=[IsPushable(O), IsPose(O, P), IsPose(O, P2), Stable(P, S), Stable(P2, S)],
conditions=[IsPose(O, P), IsPose(O, P2)],
effects=[PushMotion(O, P, P2, Q, T)],
generator=sample_push_traj_fn(ir_model, bodies, surfaces,
CARRY_CONFIG)),
# TODO: could have fixed obstacle blocking a push
#MultiEasyGenStream(inputs=[O, S, P], outputs=[P2],
# conditions=[IsPose(O, P), Stable(P, S)],
# effects=[PushMotion(O, P, P2, Q, T)],
# generator=edge_generator_fn(bodies, surfaces)),
#EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)],
# effects=[], generator=base_generator),
]
####################
constants = []
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [PointEq(name, pose) for name, pose in initial_poses.iteritems()
] + [IsPose(name, pose)
for name, pose in initial_poses.iteritems()] + [
Stable(pose, surface)
for obj, pose in initial_poses.iteritems()
for surface in surfaces if stable_test(obj, pose, surface)
]
#goal_formula = And(ConfEq(initial_conf), *[
# OnSurface(obj, surface) for obj, surface in problem.goal_surfaces.iteritems()
#])
#goal_formula = ConfEq(goal_conf)
obj, _ = problem.goal_surfaces.items()[0]
goal_formula = And(Holding(obj))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
print stream_problem
handles = draw_affine_limits(robot)
if use_viewer:
for surface in problem.surfaces:
surface.draw(env)
raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
solve = lambda: incremental_planner(
stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
#solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
with env:
plan, universe = solve()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if plan is not None:
commands = process_plan(robot, bodies, plan)
if use_viewer:
print SEPARATOR
saver.Restore()
visualize_solution(commands)
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi / 2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {
body.GetKinematicsGeometryHash()
for body in bodies.values()
}
assert len(
geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH,
USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(
gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(body2):
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
class MotionStream(Stream):
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2 = map(get_value, self.inputs)
collision_atoms = filter(
lambda atom: atom.predicate in
[CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is CFreeMTrajGrasp:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is CFreeMTrajPose:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
#print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies)
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies) - 1
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
grasp.grasp_trans))
robot.Grab(body1)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [
atom.instantiate({collision_params[atom]: MTRAJ(mt)})
for atom in collision_atoms
]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if mt:
return [ManipMotion(mq1, mq2, mt)]
return []
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[MQ, GT],
conditions=[],
effects=[GraspMotion(P, G, MQ, GT)],
generator=sample_grasp_traj),
# Move trajectory
ClassStream(inputs=[MQ, MQ2],
outputs=[MT],
conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)],
StreamClass=MotionStream,
order=1,
max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose,
eager=True),
EasyTestStream(inputs=[GT, P],
conditions=[],
effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P],
conditions=[],
effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G],
conditions=[],
effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P],
conditions=[],
effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ManipEq(initial_manip),
*(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?' % (j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?' % (i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')