def convert_axioms_to_effects( problem ): # NOTE - can always use the previous compilation process for some problematic axioms derived_predicates = problem.get_derived_predicates() fluent_predicates = problem.get_fluent_predicates() constants = defaultdict(set) for const in problem.get_constants(): constants[const.type].add(const) mapping = [] for op in problem.operators: # NOTE - can always just convert one if isinstance(op, Axiom): [conditions] = op.condition.dequantify(constants).get_literals( ) # TODO - more complicated if multiple ways to achieve assert not filter( lambda a: a.predicate in derived_predicates, conditions) # TODO - difficulty when levels of axioms print conditions [fluent] = filter( lambda a: a.predicate in fluent_predicates, conditions) # NOTE - could easily expand to conjunctive case others = filter(lambda a: a != fluent, conditions) mapping.append((fluent, others, op.effect)) new_operators = [] for op in problem.operators: if isinstance(op, Action): new_op = op.clone() new_operators.append(new_op) [literals] = new_op.effect.get_literals() # TODO for literal in literals: effect = literal.formula if isinstance(literal, Not) else literal for fluent, conditions, derived in mapping: if effect.predicate == fluent.predicate: free_params = set( flatten( map(lambda a: a.args, [derived] + conditions))) - set(fluent.args) param_map = dict(zip(fluent.args, effect.args)) for i, param in enumerate( free_params): # Prevents conflicting names #param_map[param] = Parameter('_%s'%i, param.type) # NOTE - FF can't parse this param_map[param] = Parameter('l%s' % i, param.type) new_params = list( set(param_map.values()) - set(effect.args)) new_derived = derived.instantiate(param_map) new_conditions = [ cond.instantiate(param_map) for cond in conditions ] # TODO - could put in the negated version of new_derived if isinstance(literal, Not): new_op.add_effects( ForAll(new_params, When(And(*new_conditions), new_derived))) else: new_op.add_effects( ForAll( new_params, When(And(*new_conditions), Not(new_derived))) ) # Not necessary, but it could reduce the number of instances #op.add_effects(ForAll(new_params, Not(new_derived))) # One failure can break return new_operators
def compile_problem(tamp_problem): """ Constructs a STRIPStream problem for the continuous TMP problem. :param tamp_problem: a :class:`.TMPProblem` :return: a :class:`.STRIPStreamProblem` """ B1, B2 = Param(BLOCK), Param(BLOCK) P1, P2 = Param(POSE), Param(POSE) Q1, Q2 = Param(CONF), Param(CONF) R = Param(REGION) 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)))), 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='clean', parameters=[B1, R], condition=And(InRegion(B1, R), IsSink(R)), effect=And(Cleaned(B1))), Action(name='cook', parameters=[B1, R], condition=And(InRegion(B1, R), IsStove(R)), effect=And(Cooked(B1))), ] axioms = [ Axiom(effect=InRegion(B1, R), condition=Exists([P1], And(AtPose(B1, P1), Contained(B1, P1, R)))), Axiom(effect=Safe(B2, B1, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), ] cond_streams = [ EasyGenStream(inputs=[R, B1], outputs=[P1], conditions=[CanPlace(B1, R)], effects=[Contained(B1, P1, R)], generator=lambda r, b: (sample_region_pose(r, b) for _ in irange(0, INF))), EasyGenStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)], generator=lambda p: iter([inverse_kinematics(p)])), EasyTestStream(inputs=[R, B1, P1], conditions=[], effects=[Contained(B1, P1, R)], test=in_region, eager=EAGER_TESTS, plannable=False), EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)], test=lambda *args: not are_colliding(*args), eager=EAGER_TESTS), ] constants = [ REGION(tamp_problem.env_region), ] initial_atoms = [ AtConf(tamp_problem.initial_config), ] + [ AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems() ] + [ CanPlace(block, tamp_problem.env_region) for block in tamp_problem.initial_poses ] if tamp_problem.initial_holding is None: initial_atoms.append(HandEmpty()) else: initial_atoms.append(Holding(tamp_problem.initial_holding)) goal_literals = [] if tamp_problem.goal_config is not None: goal_literals.append(AtConf(tamp_problem.goal_config)) if tamp_problem.goal_holding is False: goal_literals.append(HandEmpty()) elif tamp_problem.goal_holding is not None: goal_literals.append(Holding(tamp_problem.goal_holding)) for block, goal in tamp_problem.goal_poses: goal_literals.append(AtPose(block, goal)) for block, goal in tamp_problem.goal_regions: initial_atoms.append(CanPlace(block, goal)) goal_literals.append(InRegion(block, goal)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
OBJ, LOC, STATE = Type(), Type(), Type() At = Predicate(OBJ, LOC) HasState = Predicate(OBJ, STATE) Clear = Predicate(LOC) IsDryer = Predicate(LOC) IsPainter = Predicate(LOC) IsWasher = Predicate(LOC) O, L1, L2, S = Param(OBJ), Param(LOC), Param(LOC), Param(STATE) actions = [ Action(name='transport', parameters=[O, L1, L2], condition=And(At(O, L1), Clear(L2)), effect=And(At(O, L2), Clear(L1), Not(At(O, L1)), Not( Clear(L2)))), # NOTE - Leslie and Tomas call this Move Action(name='wash', parameters=[O, L1, S], condition=And(At(O, L1), HasState(O, S), IsWasher(L1)), effect=And(HasState(O, clean), Not(HasState(O, S)))), Action(name='paint', parameters=[O, L1], condition=And(At(O, L1), HasState(O, clean), IsPainter(L1)), effect=And(HasState(O, wet), Not(HasState(O, clean)))), Action(name='dry', parameters=[O, L1], condition=And(At(O, L1), HasState(O, wet), IsDryer(L1)), effect=And(HasState(O, dry), Not(HasState(O, wet)))), ]
# NOTE - can add the movable objects here operators = [ # Without grasp trajectory #Action(name='pick', parameters=[O, P, G, BQ, MQ], # condition=And(PoseEq(O, P), HandEmpty(), BaseEq(BQ), ManipEq(MQ), Kin(P, G, BQ, 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), BaseEq(BQ), ManipEq(MQ), Kin(P, G, BQ, MQ), # ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))), # effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), # With grasp trajectory Action(name='pick', parameters=[O, P, G, BQ, MQ, GT], condition=And(PoseEq(O, P), HandEmpty(), BaseEq(BQ), ManipEq(MQ), GraspTraj(P, G, BQ, MQ, GT)), effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))), Action(name='place', parameters=[O, P, G, BQ, MQ, GT], condition=And(GraspEq(O, G), BaseEq(BQ), ManipEq(MQ), GraspTraj(P, G, BQ, MQ, GT), ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))), effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), # NOTE - I could make actions that backup or move forward instead Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT], #condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([O2], SafePoseMove(O2, MT))), condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([O2], SafeMove(O2, MT))), #condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), # ForAll([O2], Or(SafeGraspMove(O2, MT), SafePoseMove(O2, MT)))), # NOTE - bad idea, creates one per each combo effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
def compile_belief(belief, goal): locations, _, _ = maximum_likelihood_obs(belief) constants = map(OBJ, belief.objLoc.keys()) + map(POSE, belief.occupancies.keys()) initial_atoms = [] #initial_atoms += [At(obj, p) for obj, p in locations.iteritems()] initial_atoms += [ BAt(obj, p, round(belief.objLocDist(obj).prob(p), 3)) for obj, p in locations.iteritems() ] # NOTE - using the maximum belief here goal_literals = [] for fluent in goal.fluents: if isinstance(fluent, Bd): literal, arg, prob = fluent.args if isinstance(literal, ObjLoc): goal_literals.append( BAtAbove(literal.args[0], literal.value, prob)) elif isinstance(literal, ObjState) and arg == 'clean': goal_literals.append(BClean(literal.args[0], prob)) else: raise NotImplementedError(literal) else: raise NotImplementedError(fluent) O, P, B = Param(OBJ), Param(POSE), Param(BELIEF) B1, B2 = Param(BELIEF), Param(BELIEF) p_obs_t = 1 - glob.failProbs['Look'] p_obs_f = 1 - p_obs_t #cost = prob_cost(p_obs_t) # This isn't informative #cost = prob_cost(float(prior.name)*p_obs_t) # TODO - need to automatically derive the costs actions = [ #PerfectLook(), Look(p_obs_t), #BackwardLook(p_obs_t), Transport(), ] axioms = [ Axiom(BAtAbove(O, P, B2), Exists([B1], And(BAt(O, P, B1), Above(B1, B2)))), #Axiom(IsPossible(B1, B2), Exists([B], And(IsUpdate(B, B2), Above(B1, B)))), # NOTE - this only uses static facts ] cond_streams = [ GeneratorStream(inputs=[B1], outputs=[B2], conditions=[], effects=[IsUpdate(B1, B2)], generator=lambda b1: [round(forward_belief(b1, p_obs_t, p_obs_f), 3)]), #GeneratorStream(inputs=[B2], outputs=[B1], conditions=[], effects=[IsUpdate(B1, B2)], # generator=lambda b2: [round(inverse_belief(b2, p_obs_t, p_obs_f), 3)]), # NOTE - I previously used a Deferred Literal to produce the initial belief #TestStream(inputs=[O, P, B1], conditions=[], effects=[BAt(O, P, B1)], # test=lambda o, p, b: belief.objLocDist(o).prob(p) >= b, eager=True), TestStream(inputs=[B1, B2], conditions=[], effects=[Above(B1, B2)], test=lambda b1, b2: b1 >= b2, eager=True), #TestStream(inputs=[B1, B2, B], conditions=[IsUpdate(B, B2), Above(B1, B)], effects=[IsPossible(B1, B2)], # test=lambda *args: True, eager=True), ] return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
def get_axioms(operators, static_map, objects): axioms = [] for _, conditions, effects in get_dnf_operators(operators, objects): [effect] = effects axioms.append(Axiom(effect, And(*conditions))) return get_operators(axioms, static_map, objects, PyAxiom)
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] set_base_conf(robot, (-.75, .2, -math.pi / 2)) 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()]) #################### 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, robot, 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, cspace, 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( robot, manipulator, base_manip, cspace, 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, robot, 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, max_time=15) 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?')
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1 return [ IsMoveUpdate(loc1, prior, loc2, post), Initialize(MoveCost(*self.inputs[:2]), cost) ] # TODO - make this a test stream ################################################## # NOTE - the location here is like a control parameter actions = [ Action( name='transport', parameters=[O, L, L2, B, B2], # NOTE - Leslie and Tomas call this Move condition=And(BAt(O, B), IsMoveUpdate(L, B, L2, B2)), effect=And( BAt(O, B2), Not(BAt(O, B)), #)), Cost(MoveCost(L, B)))), #Action(name='transport', parameters=[O, B, L, B2], # NOTE - I could include belief preconditions for safety or effectiveness # condition=And(BAt(O, B), BAtAbove(L, B2, ), IsMoveUpdate(B, L, B2)), # effect=And(BAt(O, B2), Not(BAt(O, B)))), # NOTE - Leslie and Tomas call this Move Action( name='find', parameters=[O, L, B, B2], condition=And(BAt(O, B), IsLookUpdate(B, L, B2)), effect=And( BAt(O, B2), Not(BAt(O, B)), #)),
def add_conditions(self, *new_conditions): self.condition = And(self.condition, *new_conditions)
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
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(): """ 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 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?')
O, P, G = Param(OBJ), Param(POSE), Param(GRASP) O2, O3 = Param(OBJ), Param(OBJ) P2 = Param(POSE) MQ, MQ2 = Param(MCONF), Param(MCONF) MT = Param(MTRAJ) GT = Param(GTRAJ) rename_easy(locals()) #################### actions = [ Action(name='pick', parameters=[O, P, G, MQ, GT], condition=And(PoseEq(O, P), HandEmpty(), ManipEq(MQ), GraspMotion(P, G, MQ, GT), ForAll([O2], Or(Equal(O, O2), SafeGTraj(O2, GT)))), #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, MQ, GT], condition=And(GraspEq(O, G), ManipEq(MQ), GraspMotion(P, G, MQ, GT), ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))), effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), Action(name='move', parameters=[MQ, MQ2, MT], condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, MT), ForAll([O2], SafeMTraj(O2, MT))), effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))), ] axioms = [ # Pick/Place collisions
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 add_effects(self, *new_effects): self.effect = And(self.effect, *new_effects)
def get_actions(operators, static_map, objects): actions = [] for action, conditions, effects in get_dnf_operators(operators, objects): actions.append(Action(action.name, action.parameters, And(*conditions), And(*effects))) return get_operators(actions, static_map, objects, PyAction)
def create_real_problem(state, goal): tables, initial_poses, initial_config, holding = state # TODO: surface parameter here? # TODO: what happens if I drop preconditions here and things # TODO: add on here as well? Drop all preconditions mentioning continuous params # TODO: can then drop all preconditions and effects associated # Well I won't have these in the effects actions = [ #Action(name='pick', parameters=[B, P, G, Q], # condition=And(AtPose(B, P), HandEmpty(), AtConf(Q), # IsPose(B, P), IsKin(P, G, Q)), # effect=And(HasGrasp(B, G), Not(AtPose(B, P)), Not(HandEmpty()))), #Action(name='place', parameters=[B, P, G, Q], # condition=And(HasGrasp(B, G), AtConf(Q), # IsPose(B, P), IsKin(P, G, Q)), # #ForAll([B2], Or(Equal(B, B2), Safe(B2, B, P)))), # effect=And(AtPose(B, P), HandEmpty(), Not(HasGrasp(B, G)))), Action(name='pick', parameters=[B, S, P, G, Q], condition=And(AtPose(B, P), HandEmpty(), AtConf(Q), IsPose(B, P), IsKin(P, G, Q), IsSupported(P, S)), effect=And(HasGrasp(B, G), Holding(B), Not(On(B, S)), Not(AtPose(B, P)), Not(HandEmpty()))), Action( name='place', parameters=[B, S, P, G, Q], condition=And(HasGrasp(B, G), AtConf(Q), IsPose(B, P), IsKin(P, G, Q), IsSupported(P, S)), # ForAll([B2], Or(Equal(B, B2), Safe(B2, B, P)))), effect=And(AtPose(B, P), Not(Holding(B)), HandEmpty(), On(B, S), Not(HasGrasp(B, G)))), Action(name='move', parameters=[Q, Q2], condition=AtConf(Q), effect=And(AtConf(Q2), Not(AtConf(Q)))), # TODO: can do a similar thing with is near here # TODO: should also probably have two costs so one can be dropped ] # TODO: can relax preconditions as normal to make a strictly easier problem # If you remove all preconditions, can also remove effects that aren't needed and then parameters axioms = [ #Axiom(effect=Safe(B2, B, P), # condition=Exists([P2], And(AtPose(B2, P2), CFree(B, P, B2, P2)))), #Axiom(effect=On(B, S), # condition=Exists([P], And(IsPose(B, P), AtPose(B, P), IsSupported(P, S)))), #Axiom(effect=Holding(B), # condition=Exists([G], And(IsGrasp(B, G), HasGrasp(B, G)))), # TODO: one for the robot near a surface as well ] # TODO: axioms make it less clear what is going on for hierarchy #################### # Conditional stream declarations cond_streams = [ GeneratorStream( inputs=[B, S], outputs=[P], conditions=[], effects=[IsPose(B, P), IsSupported(P, S)], #generator=lambda b, s: ((Pose(b, randint(*tables[s])),) for _ in xrange(10))), generator=lambda b, s: ((Pose(b, s, x), ) for x in xrange(*tables[s]))), GeneratorStream(inputs=[B, P, G], outputs=[Q], conditions=[IsPose(B, P), IsGrasp(B, G)], effects=[IsKin(P, G, Q)], generator=lambda _, p, g: [(Conf(p.x, g.y), )]), ListStream(inputs=[B], outputs=[G], conditions=[], effects=[IsGrasp(B, G)], function=lambda b: [(Grasp(b, v), ) for v in [-1, +1]]), #TestStream(inputs=[B, P, B2, P2], conditions=[], effects=[CFree(B, P, B2, P2)], # test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking ] #################### # TODO: need to add surfaces as constants initial_atoms = [ AtConf(initial_config), ] + [AtPose(block, pose) for block, pose in initial_poses.items()] + [ IsPose(block, pose) for block, pose in initial_poses.items() ] + [IsSupported(pose, pose.sur) for pose in initial_poses.values() ] + [On(block, pose.sur) for block, pose in initial_poses.items()] if state.holding is None: initial_atoms.append(HandEmpty()) else: block, grasp = state.holding initial_atoms += [HasGrasp(block, grasp), Holding(block)] goal_literals = [On(b, s) for b, s in goal.on.items()] if goal.holding is not None: goal_literals.append(Holding(goal.holding)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams)
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)))), Action(name='move', parameters=[Q, Q2, T], condition=And(ConfEq(Q), HandEmpty(), FreeMotion(Q, Q2, T), ForAll([O2], SafeTraj(O2, T))),
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(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 = {blocks[1]: 2} # the goal pose for block i is i+1 #################### # TODO: the last version of this would be to make a separate pose type per object (I think I did do this) CONF = Type() HandEmpty = Pred() AtConf = Pred(CONF) Q1, Q2 = Param(CONF), Param(CONF) #POSE = Type() #Kin = Pred(POSE, CONF) #Collision = Pred(POSE, POSE) #P1, P2 = Param(POSE), Param(POSE) #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 = [] cond_streams = [ #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), ] initial_atoms = [ AtConf(initial_config), HandEmpty(), ] goal_literals = [] #################### # TODO: I think thinking invariants gets harder with many predicates. Can cap this time I believe though #153 initial candidates #Finding invariants: [2.250s CPU, 2.263s wall - clock] for b in blocks: # TODO: can toggle using individual pose types POSE = Type() Kin = Pred(POSE, CONF) P1 = Param(POSE) AtPose = Pred(POSE) IsPose = Pred(POSE) Holding = Pred() #Unsafe = Pred(BLOCK, POSE) initial_atoms += [ AtPose(initial_poses[b]), IsPose(initial_poses[b]), ] if b in goal_poses: goal_literals += [AtPose(goal_poses[b])] initial_atoms += [IsPose(goal_poses[b])] 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(P1), HandEmpty(), AtConf(Q1), IsPose(P1), Kin(P1, Q1)), effect=And(Holding(), Not(AtPose(P1)), Not(HandEmpty()))), Action( name='place-{}'.format(b), parameters=[P1, Q1], condition=And(Holding(), AtConf(Q1), IsPose(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(P1), HandEmpty(), Not(Holding()))), ] cond_streams += [ GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[IsPose(P1)], generator=lambda: ((p, ) for p in xrange(n, num_poses))), GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[Kin(P1, Q1)], generator=lambda p: [(p, )]), # Inverse kinematics ] problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, []) 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 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 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_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) Q1, Q2 = Param(CONF), Param(CONF) actions = [ Action(name='pick', parameters=[B1, P1, Q1], condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)), effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))), Action( name='place', parameters=[B1, P1, Q1], condition=And( Holding(B1), AtConf(Q1), LegalKin(P1, Q1), #ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))), #*[Or(Equal(B1, BLOCK(b2)), Safe(b2, P1)) for b2 in tamp_problem.initial_poses]), 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)))), Axiom(effect=Safe(B2, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1, P2)))), ] # NOTE - this needs to be inside the method so you make new streams each time cond_streams = [ #EasyGenStream(inputs=[P2], outputs=[P1], conditions=[], effects=[], # generator=lambda a: irange(0, NUM_POSES)), EasyGenStream(inputs=[], outputs=[P1], conditions=[], effects=[], generator=lambda: irange(0, NUM_POSES)), EasyGenStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)], generator=lambda p: iter([p])), #EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)], # test=lambda b1, p1, b2, p2: p1 != p2, eager=EAGER_TESTS), EasyTestStream(inputs=[P1, P2], conditions=[], effects=[CollisionFree(P1, P2)], test=lambda p1, p2: p1 != p2, eager=EAGER_TESTS), ] constants = [] initial_atoms = [ AtConf(tamp_problem.initial_config), ] + [ AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems() ] if tamp_problem.initial_holding is False: initial_atoms.append(HandEmpty()) else: initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK)) goal_literals = [] if tamp_problem.goal_holding is False: goal_literals.append(HandEmpty()) elif tamp_problem.goal_holding is not None: goal_literals.append(Holding(tamp_problem.goal_holding)) for block, goal in tamp_problem.goal_poses.iteritems(): goal_literals.append(AtPose(block, goal)) return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
def create_problem(): """ 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 #################### # 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), Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))))), 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)))), ] #################### # 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, cond_streams, constants) return problem
WetPaint = Predicate(OBJ) DryPaint = Predicate(OBJ) Clear = Predicate(LOC) Safe = Predicate(OBJ, LOC) IsDryer = Predicate(LOC) IsPainter = Predicate(LOC) IsWasher = Predicate(LOC) O, O2, L1, L2 = Param(OBJ), Param(OBJ), Param(LOC), Param(LOC) actions = [ Action(name='transport', parameters=[O, L1, L2], condition=And(At(O, L1), Clear(L2)), effect=And(At(O, L2), Not(At(O, L1)))), # NOTE - Leslie and Tomas call this Move Action( name='wash', parameters=[O, L1], condition=And(At(O, L1), IsWasher(L1)), #effect=And(Clean(O))) effect=And(Clean(O), Not(WetPaint(O)), Not(DryPaint(O)))), Action( name='paint', parameters=[O, L1], condition=And(At(O, L1), Clean(O), IsPainter(L1)), #effect=And(WetPaint(O))), effect=And(WetPaint(O), Not(Clean(O)))),
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 # 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 ] #################### 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(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
class STRIPStreamProblem(object): def __init__(self, initial_atoms, goal_literals, operators, cond_streams, objects=[]): # TODO - warnings for improperly passed types here self.initial_atoms = list(initial_atoms) if isinstance(goal_literals, Atom): self.goal_literals = And(goal_literals) elif isinstance(goal_literals, Iterable): self.goal_literals = And(*goal_literals) elif isinstance( goal_literals, Formula): # or isinstance(goal_literals, AbsCondition): self.goal_literals = goal_literals else: raise ValueError(goal_literals) # TODO - assert goals aren't empty self.operators = operators self.cond_streams = cond_streams self.objects = objects def get_constants(self): objects = set(self.objects) for atom in self.initial_atoms: objects.update(atom.args) if isinstance(self.goal_literals, Formula): for atom in self.goal_literals.get_atoms(): objects.update(atom.args) #elif isinstance(self.goal_literals, AbsCondition): # for formula in self.goal_literals.conditions: # for atom in formula.get_atoms(): # objects.update(atom.args) for operator in self.operators: for atom in operator.condition.get_atoms( ) | operator.effect.get_atoms(): objects.update(atom.args) for cs in self.cond_streams: for atom in cs.conditions + cs.effects: objects.update(atom.args) return filter(lambda o: isinstance(o, Constant), objects) def get_functions(self): from stripstream.pddl.operators import get_function_atoms functions = set() for operator in self.operators: if isinstance(operator, Action): functions.update( {atom.predicate for atom in get_function_atoms(operator)}) return functions def get_fluent_predicates( self ): # TODO - enforce consistency for these and make them a property from stripstream.pddl.operators import Action fluents = set() for operator in self.operators: if isinstance(operator, Action): fluents.update( {atom.predicate for atom in operator.effect.get_atoms()}) return fluents def get_derived_predicates(self): from stripstream.pddl.operators import Axiom return set(operator.effect.predicate for operator in self.operators if isinstance(operator, Axiom)) def get_stream_predicates(self): return { atom.predicate for cs in self.cond_streams for atom in cs.conditions + cs.effects } def get_predicates(self): return {atom.predicate for op in self.operators for atom in op.condition.get_atoms() | op.effect.get_atoms()} | \ {atom.predicate for cs in self.cond_streams for atom in cs.conditions+cs.effects} | \ {atom.predicate for atom in self.initial_atoms} | \ {atom.predicate for atom in self.goal_literals.get_atoms()} def get_static_predicates(self): return self.get_predicates() - self.get_fluent_predicates() - \ self.get_derived_predicates() - self.get_stream_predicates() def __repr__(self): return 'STRIPStream Problem\n' \ 'Initial: %s\n' \ 'Goal: %s\n' \ 'Operators: %s\n' \ 'Streams: %s\n' \ 'Constants: %s'%(self.initial_atoms, self.goal_literals, self.operators, self.cond_streams, self.objects)