def create_problem(time_step=.5, fuel_rate=5, start_fuel=10, goal_p=10): """ Creates the Tsiolkovsky rocket problem. :return: a :class:`.STRIPStreamProblem` """ # Data types STATE, RATE, MASS, TIME = Type(), Type(), Type(), Type() HEIGHT = Type() # Fluent predicates AtState = Pred(STATE) # Derived predicates Above = Pred(HEIGHT) # Static predicates IsBurst = Pred(STATE, RATE, TIME, STATE) IsAbove = Pred(STATE, HEIGHT) # Free parameters X1, X2 = Param(STATE), Param(STATE) Q, T = Param(RATE), Param(TIME) H = Param(HEIGHT) rename_easy(locals()) # Trick to make debugging easier #################### actions = [ Action(name='burst', parameters=[X1, Q, T, X2], condition=And(AtState(X1), IsBurst(X1, Q, T, X2)), effect=And(AtState(X2), Not(AtState(X1)))), ] axioms = [ Axiom(effect=Above(H), condition=Exists([X1], And(AtState(X1), IsAbove(X1, H)))), ] #################### # Conditional stream declarations cond_streams = [ GeneratorStream(inputs=[X1, Q, T], outputs=[X2], conditions=[], effects=[IsBurst(X1, Q, T, X2)], generator=forward_burst), TestStream(inputs=[X1, H], conditions=[], effects=[IsAbove(X1, H)], test=lambda (p, v, m), h: p >= h, eager=True), ]
def __init__(self): obj, p, prior = Param(OBJ), Param(POSE), Param(BELIEF) post = 1 super(PerfectLook, self).__init__( self.__class__.__name__, [obj, p, prior], And( #At(obj, p), BAt(obj, p, prior)), And(BAt(obj, p, 1), Not(BAt(obj, p, prior))))
def __init__(self, p_obs_t): obj, p, prior, post = Param(OBJ), Param(POSE), Param(BELIEF), Param( BELIEF) super(BackwardLook, self).__init__( self.__class__.__name__, [obj, p, prior, post], And( #At(obj, p), BAt(obj, p, prior), #BAtAbove(obj, p, prior), # Problem where we want to delete the old belief, but we need to include the reference belief IsPossible(prior, post)), And(BAt(obj, p, post), Not(BAt(obj, p, prior))))
def __init__(self, min_safe_p=.95): obj, d1, d2 = Param(OBJ), Param(DIST), Param(DIST) Action.__init__( self, self.__class__.__name__, [obj, d], And( BClean(obj, d1), IsClean(d1, d2), #BAtAbove(obj, p1, min_safe_p) ), And( BClean(obj, d2), #Not(At(obj, p1)), Not(BClean(obj, d1))), cost=1)
def __init__(self, min_safe_p=.95): obj, p1, p2, b = Param(OBJ), Param(POSE), Param(POSE), Param(BELIEF) Action.__init__( self, self.__class__.__name__, [obj, p1, p2, b], And( #At(obj, p1), BAt(obj, p1, b), BAtAbove(obj, p1, min_safe_p)), And( #At(obj, p2), BAt(obj, p2, b), #Not(At(obj, p1)), Not(BAt(obj, p1, b))), cost=1)
def pddl(self, costs): s = '(:action ' + self.name + '\n' s += '\t:parameters (' + ' '.join(param.typed_pddl() for param in self.parameters) + ')\n' s += '\t:precondition ' + self.condition.pddl() + '\n' augmented_effect = self.effect if costs and self.cost is not None and not self.cost_included: augmented_effect = And(augmented_effect, Cost(self.cost)) s += '\t:effect ' + augmented_effect.pddl() return s + ')'
def __init__(self, p_obs_t, p_obs_f=None, log_cost=False): #self.p_obs_t = p_obs_t #if p_obs_f is None: self.p_obs_f = 1 - p_obs_t #self.log_cost = log_cost cost = prob_cost( p_obs_t, log_cost=log_cost) # Conditions on successful observation cost = None obj, p, prior, post = Param(OBJ), Param(POSE), Param(BELIEF), Param( BELIEF) super(Look, self).__init__( self.__class__.__name__, [obj, p, prior, post], And( #At(obj, p), BAt(obj, p, prior), IsUpdate(prior, post)), And(BAt(obj, p, post), Not(BAt(obj, p, prior))), cost)
def __init__(self, name, parameters, conditions, effects, cost=None): """ :param name: the string name of the action :param parameters: a list of :class:`.Parameter` :param conditions: a list of :class:`.Atom` or :class:`.Not` :param effects: a list of :class:`.Atom` or :class:`.Not` :param cost: a numeric cost """ if not all(is_literal(c) for c in conditions): raise ValueError('Conditions must all be literals: %s' % conditions) if not all(is_literal(e) for e in effects): raise ValueError('Effects must all be literals: %s' % effects) self.conditions, self.effects = conditions, effects super(STRIPSAction, self).__init__(name, parameters, And(*conditions), And(*effects), cost=cost)
def create_abstract_problem(state, goal): # Goal serialization is basically like this # Necessary and sufficient? tables, initial_poses, initial_config, holding = state # TODO: connect this to goals on the actual states # The quantification of poses and grasps is independent # Conditions are derived or atoms actions = [ Action( name='pick', parameters=[B, S], condition=And(On(B, S), HandEmpty()), #, AtConf(Q)), #condition=And(On(B, S), HandEmpty(), Not(Holding(B))), # , AtConf(Q)), effect=And(Holding(B), Not(On(B, S)), Not(HandEmpty()))), Action( name='place', parameters=[B, S], condition=And(Holding(B)), #, AtConf(Q)), effect=And(On(B, S), HandEmpty(), Not(Holding(B)))), #Action(name='move', parameters=[S1, Q2], # condition=AtConf(Q), # effect=And(AtConf(Q2), Not(AtConf(Q)))), ] axioms = [] cond_streams = [] initial_atoms = [ #AtConf(initial_config), HandEmpty() ] + [On(block, pose.sur) for block, pose in initial_poses.items()] goal_literals = [On(b, s) for b, s in goal.on.items()] if goal.holding is not None: goal_literals.append(Holding(goal.holding)) problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams) return problem
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 abs_action(name, parameters, conditions, effect, cost=None): # NOTE - using And() to indicate no condition assert len(conditions) >= 1 refinement = Action(name, parameters, And(*conditions), effect, cost=cost) for i in reversed(range(1, len(conditions))): abs_name = '_%s_%s' % (name, i) abs_condition = And(*conditions[:1]) parameter_pool = abs_condition.get_parameters( ) | effect.get_parameters() abs_parameters = filter(lambda p: p in parameter_pool, parameters) abs_cost = (cost if cost is not None else 0) + i * ABS_COST refinement = RefinableAction(abs_name, abs_parameters, abs_condition, effect, refinement=[refinement], cost=abs_cost) return refinement
def __init__(self): b, p, t = P('b', BLOCK), P('p', POSE), P('t', TRAJ) Axiom.__init__( self, Safe(b, t), Or( Holding(b), # TODO - could also have equality here Exists( [p], And( AtPose(b, p), #Not(Equal(p1, p2)), # TODO - I could always immediately outlaw them having the same pose... IsCollisionFree(p, t)))))
def pddl(self, costs, durative=False): if durative: return self.durative_pddl() parameters_pddl = ' '.join(param.typed_pddl() for param in self.parameters) augmented_effect = self.effect if costs and ( self.cost is not None ) and not self.cost_included: # Can only have one cost effect (uses last effect parsed) augmented_effect = And(augmented_effect, Cost(self.cost)) return OPERATOR_PDDL.format(self._pddl_name, self.name, parameters_pddl, self.condition.pddl(), augmented_effect.pddl())
def __init__(self): b1, p1, p2 = P('b1', BLOCK), P('p1', POSE), P('p2', POSE) Axiom.__init__( self, Safe(b1, p2), Or( Holding(b1), # TODO - could also have equality here Exists( [p1], And( AtPose(b1, p1), #Not(Equal(p1, p2)), # TODO - I could always immediately outlaw them having the same pose... IsCollisionFree(p1, p2)))))
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_stream_functions(universe): action_to_function = {} for action in universe.name_to_action.values(): if get_cost_atoms(action): continue if any(atom.predicate in universe.stream_predicates for atom in action.condition.get_atoms()): action_to_function[action] = Function( FUNCTION_TEMPLATE % action.name, [param.type for param in action.parameters]) universe.add_function(action_to_function[action]) function = action_to_function[action](*action.parameters) action.effect = And(action.effect, Cost(function)) action.cost_included = True return action_to_function
def __init__(self): b1, p1, b2, p2 = P('b1', BLOCK), P('p1', POSE), P('b2', BLOCK), P('p2', POSE) Axiom.__init__( self, Safe(b1, b2, p2), Or( Holding(b1), # TODO - could also have equality here #Or(Equal(b1, b2), # TODO - could also have equality here Exists( [p1], And( AtPose(b1, p1), #Not(Equal(p1, p2)), # Having the same pose is certainly a collision # NOTE - cannot use equality if I have the same object IsCollisionFree(b1, p1, b2, p2)))))
def preimage_sequence(universe, plan): from stripstream.pddl.logic.connectives import And assert not universe.axioms states = get_states(universe, plan) instances = [action.instantiate(args) for action, args in plan] operators = [(i.condition, i.effect) for i in instances] + [(universe.problem.goal_literals, None)] subgoals = set() goal_sequence = [] for state, (pre, eff) in reversed(zip(states, operators)[1:]): if eff is not None: [effects] = eff.get_literals() for effect in effects: subgoals.discard(effect) subgoals |= pre.positive_supporters(state, universe.type_to_objects) goal_sequence.append(And(*subgoals)) return goal_sequence[::-1]
def get_stream_functions(universe): action_to_function = {} for action in universe.name_to_action.values(): if get_cost_atoms(action): continue if any(atom.predicate in universe.stream_predicates for atom in action.condition.get_atoms()): # TODO - assert that the atom is used positively # TODO - alternatively, could make a stream action that has achieves the cost and plan with it action_to_function[action] = Function( FUNCTION_TEMPLATE % action.name, [param.type for param in action.parameters]) universe.add_function(action_to_function[action]) function = action_to_function[action](*action.parameters) action.effect = And(action.effect, Cost(function)) action.cost_included = True return action_to_function
def __init__(self, initial_atoms, goal_literals, operators, cond_streams, objects=[]): self.initial_atoms = list(initial_atoms) if isinstance(goal_literals, Iterable): self.goal_literals = And(*goal_literals) elif isinstance(goal_literals, Formula): self.goal_literals = goal_literals else: raise ValueError(goal_literals) self.operators = operators self.cond_streams = cond_streams self.objects = objects
def __init__(self, conditions, effects): """ :param conditions: a list of :class:`.Atom` or :class:`.Not` :param effects: a list of :class:`.Atom` or :class:`.Not` """ if len(effects) != 1: raise NotImplementedError( 'Currently only support a single effect: %s' % effects) if not all(is_literal(c) for c in conditions): raise ValueError('Conditions must all be literals: %s' % conditions) if not all(is_literal(e) for e in effects): raise ValueError('Effects must all be literals: %s' % effects) self.conditions, self.effects = conditions, effects self.free_parameters = list( set(flatten(c.get_parameters() for c in conditions)) - set(effects[0].get_parameters())) super(STRIPSAxiom, self).__init__(effects[0], Exists(self.free_parameters, And(*conditions)))
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?')
CFreeTraj = Pred(TRAJ, POSE) #################### O, P, G = Param(OBJ), Param(POSE), Param(GRASP) O2, P2 = Param(OBJ), Param(POSE) Q, Q2 = Param(CONF), Param(CONF) T = Param(TRAJ) rename_easy(locals()) #################### actions = [ Action(name='pick', parameters=[O, P, G, Q, T], condition=And(PoseEq(O, P), HandEmpty(), ConfEq(Q), GraspMotion(P, G, Q, T), ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))), #ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))), effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))), Action(name='place', parameters=[O, P, G, Q, T], condition=And(GraspEq(O, G), ConfEq(Q), GraspMotion(P, G, Q, T), ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeTraj(O2, T))))), effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), Action(name='move', parameters=[Q, Q2, T], condition=And(ConfEq(Q), HandEmpty(), FreeMotion(Q, Q2, T), ForAll([O2], SafeTraj(O2, T))), effect=And(ConfEq(Q2), Not(ConfEq(Q)))), Action(name='move_holding', parameters=[Q, Q2, T, O, G], condition=And(ConfEq(Q), GraspEq(O, G), HoldingMotion(Q, Q2, G, T),
def compile_problem(tamp_problem): """ Constructs a STRIPStream problem for the countable 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) 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, 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)))), ] axioms = [ Axiom(effect=Safe(B2, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1, P2)))), ] cond_streams = [ 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=[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(dt, GoalTest, ClearTest, CalcDiffSystem, InitialState): """ Creates a generic non-holonomic motion planning problem :return: a :class:`.STRIPStreamProblem` """ # Data types X, DX, U = Type(), Type(), Type() # Fluent predicates AtX = Pred(X) # Fluent predicates GoalReached = Pred() # Static predicates ComputeDX = Pred(X, U, DX) Dynamics = Pred(X, DX, X) AtGoal = Pred(X) Clear = Pred(X, X) # Free parameters X1, X2 = Param(X), Param(X) DX1 = Param(DX) U1 = Param(U) rename_easy(locals()) # Trick to make debugging easier #################### actions = [ Action(name='simulate', parameters=[X1, U1, X2, DX1], condition=And(AtX(X1), ComputeDX(X1, U1, DX1), Dynamics(X1, DX1, X2), Clear(X1, X2)), effect=And(Not(AtX(X1)), AtX(X2))), ] axioms = [ Axiom(effect=GoalReached(), condition=Exists([X1], AtGoal(X1))), ] #################### # Conditional stream declarations def ComputeDXCalculator(X1F, DX1F): X2F = X1F[:] for i in range(len(X2F)): X2F[i] = X1F[i] + dt * DX1F[i] return X2F cond_streams = [ FunctionStream(inputs=[X1, DX1], outputs=[X2], conditions=[], effects=[Dynamics(X1, DX1, X2)], function=ComputeDXCalculator), FunctionStream(inputs=[X1, U1], outputs=[DX1], conditions=[], effects=[ComputeDX(X1, U1, DX1)], function=CalcDiffSystem), TestStream(inputs=[X1, X2], conditions=[], effects=[Clear(X1, X2)], test=ClearTest, eager=True), TestStream(inputs=[X1], conditions=[], effects=[AtGoal(X1)], test=GoalTest, eager=True), ] #################### constants = [ # TODO - need to declare control inputs (or make a stream for them) ] initial_atoms = [ AtX(InitialState), ] goal_literals = [GoalReached()] problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants) return problem
def 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 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 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?')
Q, Q2 = Param(CONF), Param(CONF) T = Param(TRAJ) T2 = Param(TRAJ) S = Param(SURFACE) rename_easy(locals()) #################### # NOTE - I suppose I could include O in one trajectory's collision and not the other. However, no point because can't move that object to manipulate actions = [ Action( name='pick', parameters=[O, P, G, Q, T], condition=And(PoseEq(O, P), HandEmpty(), ConfEq(Q), GraspMotion(O, P, G, Q, T)), #ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))), effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))), #condition=And(PoseEq(O, P), HandEmpty(), ConfEq(Q), GraspMotion(O, P, G, Q, T, T2), # ForAll([O2], Or(Equal(O, O2), And(SafeTraj(O2, T), SafeTraj(O2, T2))))), #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(O, P, G, Q, T)), #ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeTraj(O2, T))))), #ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))), effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))), Action(name='move', parameters=[Q, Q2, T], condition=And(ConfEq(Q), FreeMotion(Q, Q2, T)),
def create_problem(initRobotPos = (0.5, 0.5), initRobotVar = 0.01, maxMoveDist = 5.0, beaconPos = (1, 1), homePos = (0, 0), goalPosEps = 0.1, goalVar = 0.1, odoErrorRate = 0.1, obsVarPerDistFromSensor = 10.0, minObsVar = 0.001, domainSize = 20, verboseFns = True): """ :return: a :class:`.STRIPStreamProblem` """ # Data types POS = Type() # 2D position VAR = Type() # 2D variance DIST = Type() # positive scalar # Fluent predicates RobotPos = Pred(POS) RobotVar = Pred(VAR) # Derived predicates KnowYouAreHome = Pred() # Static predicates DistBetween = Pred(POS, POS, DIST) # Distance between two positions (function) LessThanV = Pred(VAR, VAR) # Less than, on distances LessThanD = Pred(DIST, DIST) # Less than, on distances OdometryVar = Pred(VAR, VAR, DIST) # How does odometry variance increase? SensorVar = Pred(VAR, VAR, DIST) # How does an observation decrease variance? LegalPos = Pred(POS) # Any legal robot pos # Free parameters RPOS1, RPOS2, RVAR1, RVAR2 = Param(POS), Param(POS), Param(VAR), Param(VAR) DIST1, DIST2 = Param(DIST), Param(DIST) def odoVarFun(rv, d): odoVar = (d * odoErrorRate)**2 result = rv + odoVar if verboseFns: print 'ovf:', rv, d, result return [result] def sensorVarFun(rv, d): obsVar = max(d / obsVarPerDistFromSensor, minObsVar) result = 1.0 / ((1.0 / rv) + (1.0 / obsVar)) if verboseFns: print 'svf:', rv, d, result return [result] def randPos(): while True: result = (random.random() * domainSize, random.random() * domainSize) print 'rp:', result yield [result] def legalTest(rp): (x, y) = rp result = (0 <= x <= domainSize) and (0 <= y <= domainSize) if not result: print 'not legal:', rp return result actions = [ Action(name='Move', parameters=[RPOS1, RPOS2, RVAR1, RVAR2, DIST1], condition = And(RobotPos(RPOS1), RobotVar(RVAR1), LegalPos(RPOS2), # Generate an intermediate pos DistBetween(RPOS1, RPOS2, DIST1), LessThanD(DIST1, maxMoveDist), OdometryVar(RVAR1, RVAR2, DIST1)), effect = And(RobotPos(RPOS2), RobotVar(RVAR2), Not(RobotPos(RPOS1)), Not(RobotVar(RVAR1)))), # Action(name='Look', # parameters=[RPOS1, RVAR1, RVAR2, DIST1], # condition = And(RobotPos(RPOS1), # RobotVar(RVAR1), # DistBetween(RPOS1, beaconPos, DIST1), # SensorVar(RVAR1, RVAR2, DIST1)), # effect = And(RobotVar(RVAR2), # Not(RobotVar(RVAR1)))) ] axioms = [ Axiom(effect = KnowYouAreHome(), condition = Exists([RPOS1, RVAR1, DIST1], And(RobotPos(RPOS1), RobotVar(RVAR1), DistBetween(RPOS1, homePos, DIST1), LessThanD(DIST1, goalPosEps), LessThanV(RVAR1, goalVar)))) ] # Conditional stream declarations cond_streams = [ TestStream(inputs = [RPOS1], conditions = [], effects = [LegalPos(RPOS1)], test = legalTest, eager = True), GeneratorStream(inputs = [], outputs = [RPOS1], conditions = [], effects = [LegalPos(RPOS1)], generator = randPos), GeneratorStream(inputs = [RPOS1, RPOS2], outputs = [DIST1], conditions = [], effects = [DistBetween(RPOS1, RPOS2, DIST1)], generator = lambda rp1, rp2: [distance(rp1, rp2)]), GeneratorStream(inputs = [RVAR1, DIST1], outputs = [RVAR2], conditions = [], effects = [OdometryVar(RVAR1, RVAR2, DIST1)], generator = odoVarFun), # GeneratorStream(inputs = [RVAR1, DIST1], # outputs = [RVAR2], # conditions = [], # effects = [SensorVar(RVAR1, RVAR2, DIST1)], # generator = sensorVarFun), TestStream(inputs = [DIST1, DIST2], conditions = [], effects = [LessThanD(DIST1, DIST2)], test = lt, eager = True), TestStream(inputs = [RVAR1, RVAR2], conditions = [], effects = [LessThanV(RVAR1, RVAR2)], test = lt, eager = True) ] #################### constants = [ ] initial_atoms = [ RobotPos(initRobotPos), RobotVar(initRobotVar), LegalPos(homePos), LegalPos((.1, .1)), LegalPos((3.0, .1)), LegalPos((6.0, .1)) ] goal_literals = [ KnowYouAreHome() ] problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants) return problem