if Grid: model = GRIDModel(size=(4, 4), constraint_states=[(1, 2), (1, 3)], rewrd_states={ (0, 3): 0, (3, 3): 0 }, prob_right_transition=.8, prob_right_observation=.85, goal=(3, 2), step_cost=1, walls=[(1, 1), (2, 2)]) algo = RAOStar(model, cc=0.1, debugging=False, cc_type='o', fixed_horizon=6, random_node_selection=False) policy_current_belief = { (1, 0): 1.0 } # initial state is: current vehicle s_time = time.time() P, G = algo.search(policy_current_belief) print('expanded nodes', len(G.nodes)) print('objective:', G.root.value) print('Time:', time.time() - s_time) IPython.embed() elif Network: model = NetworkModel()
obstacles = np.array([[3, 3], [4, 3], [4, 4], [3, 4]]) # obstacles = np.array([[4,4],[3,5],[1,3],[2,3]]) goal = Polygon([[8, 8], [10, 8], [10, 10], [8, 10]]) # goal = Polygon([[8,2],[10,2],[10,4],[8,4]]) model = HybridRoverModel(goal_region=goal, obstacle=obstacles, goal_num_sample=3, prob_sample_success=0.95, DetermObs=True) algo = RAOStar(model, cc=cc, debugging=False, cc_type='o', fixed_horizon=13, random_node_selection=False) # algo = RAOStar(model, cc=cc, debugging=False, cc_type='o', fixed_horizon = 3, random_node_selection=False, time_limit=60*45) sigma_b0 = 0.01 n = 4 initial_mu = np.zeros((2 * n, 1)) initial_sigma = np.zeros((2 * n, 2 * n)) initial_sigma[0:4, 0:4] = sigma_b0 * np.eye(n) initial_gaussian_state = GaussianState(mu=initial_mu, sigma=initial_sigma) b_init = {(initial_gaussian_state, 0): 1.0} P, G = algo.search(b_init)
import graph_to_json from iterative_raostar import * #### Run RAO star on Scenario #### # Simulation conditions world_size = (7, 7) # note the boundaries are walls goal_state = (5, 5, 90) quad_init = (1, 1, 90, 0) # (x,y,theta,t) guest_init = (3, 1, 90, 0) # note the boundary of the world (ex anything with row column 0 and the upper bound) # is the wall model = QuadModel(world_size, goal_state) algo = RAOStar(model, cc=0.5, debugging=False) b_init = {(quad_init, guest_init): 1.0} # initialize belief state P, G = algo.search(b_init) # # get the policies that does not give none # P_notNone = {} # for i in P: # if P[i] != 'None': # P_notNone[i] = P[i] # print(P_notNone) # # print out the policy for each state of guest # most_likely_policy(G, model)
import sys import graph_to_json if __name__ == '__main__': # min_distance = 1 # print('risk', risk_for_two_gaussians(1, 0.5, 2.5, 0.5, min_distance)) # default cc value cc = 0.4 if len(sys.argv) > 1: cc = float(sys.argv[1]) model = Ashkan_ICAPS_Model(str(cc * 100) + "% risk") algo = RAOStar(model, cc=cc, debugging=False, cc_type='e', ashkan_continuous=True) agent_coords = [6, 9] agent_coords = [7, 3] b0 = ContinuousBeliefState(9, 1) b0.set_agent_coords(agent_coords[0], agent_coords[1]) P, G = algo.search(b0) most_likely_policy(G, model) Sim = Simulator(10, 10, G, P, model, grid_size=50) # Convert all matrices to strings for json # print(G)
# geordi_model = GeordiModel() geordi_model = GeordiModel( [ego_vehicle, agent1_vehicle], road_model, goal_state={'x':100,'y':200}) print(geordi_model.road_model) actions = geordi_model.get_available_actions(geordi_model.current_state) print(actions) print(agent1_vehicle.action_list[0].precondition_check(agent1_vehicle.name, geordi_model.current_state, geordi_model)) new_states = geordi_model.state_transitions( geordi_model.current_state, actions[0]) print('new_states', new_states) algo = RAOStar(geordi_model, cc=0.001, debugging=False, cc_type='o', fixed_horizon = 2) b_init = {geordi_model.current_state: 1.0} P, G = algo.search(b_init) # print(P) prettyprint(P) # permutations = calculate_permutations( # geordi_model, geordi_model.current_state, agent2_vehicle.action_list, 5) # print(len(permutations)) # print(permutations) # for p in permutations: # print(p['actions'])
if storing_quad_state == 0: storing_quad_state = 1 elif storing_quad_state == 2: storing_guest_state = 1 quad_state = quad_state[1:] qs = quad_state.split(',') q = tuple([int(i) for i in qs]) gs = guest_state.split(',') g = tuple([int(i) for i in gs]) return (q, g) # note the boundary of the world (ex anything with row column 0 and the upper bound) # is the wall model = QuadModel(world_size, goal_state) algo = RAOStar(model, cc=0.0001) b_init = {(quad_init, guest_init): 1.0} # initialize belief state P, G = algo.search(b_init) # print(P) sorted_policy = {} # sort policy by quad state for statestring in P.keys(): state = get_state_from_string(statestring) if state[0] not in sorted_policy: sorted_policy[state[0]] = {state[1]: P[statestring]} else: sorted_policy[state[0]][state[1]] = P[statestring] print(sorted_policy) S = quad_sim.Simulator(world_size[0], world_size[1], sorted_policy, model,
# author: Yun Chang # email: [email protected] # rao* on fire-ice-sand model from utils import import_models import_models() from fismodel import fire_ice_sand_Model from raostar import RAOStar from iterative_raostar import * chance_constraint = 0.15 start = [3, 0] goal = [3, 6] model = fire_ice_sand_Model(7, [], [], [], start, goal) algo = RAOStar(model, cc=chance_constraint, debugging=False) b_init = {(start[0], start[1], 0): 1.0} P, G = algo.search(b_init) P = clean_policy(P) model.print_model() model.print_policy(P) most_likely_policy(G, model) # g = graph_to_json.policy_to_json(G, chance_constraint, "results/r2d2_raos.json") # print(g)
import_models() from r2d2model import R2D2Model from raostar import RAOStar import graph_to_json from iterative_raostar import * # Now you can give command line cc argument after filename if __name__ == '__main__': # default chance constraint value cc = 0.08 if len(sys.argv) > 1: cc = float(sys.argv[1]) length = 3 model = R2D2Model(length) algo = RAOStar(model, cc=cc, debugging=True, cc_type='o') b_init = {(1, 0, 0): 1.0} P, G = algo.search(b_init) print(P) P = clean_policy(P) model.print_model() model.print_policy(P) print(P) most_likely_policy(G, model) print(len(G.nodes), G.nodes) print(G.root) next = next_child(G, G.root)
# geordi_model = GeordiModel() geordi_model = GeordiModel([ego_vehicle, agent1_vehicle, agent2_vehicle], road_model) print(geordi_model.road_model) actions = geordi_model.get_available_actions(geordi_model.current_state) print(actions) print(agent1_vehicle.action_list[0].precondition_check( agent1_vehicle.name, geordi_model.current_state, geordi_model)) new_states = geordi_model.state_transitions(geordi_model.current_state, actions[0]) print('new_states', new_states) algo = RAOStar(geordi_model, cc=0.1, debugging=True, cc_type='o') b_init = {geordi_model.current_state: 1.0} P, G = algo.search(b_init) print(P) # permutations = calculate_permutations( # geordi_model, geordi_model.current_state, agent2_vehicle.action_list, 5) # print(len(permutations)) # print(permutations) # for p in permutations: # print(p['actions']) # geordi_model.add_vehicle_model(ego_vehicle)
road_model, goal_state={'x': 200}) print(geordi_model.road_model) actions = geordi_model.get_available_actions(geordi_model.current_state) print(actions) print(agent1_vehicle.action_list[0].precondition_check( agent1_vehicle.name, geordi_model.current_state, geordi_model)) new_states = geordi_model.state_transitions(geordi_model.current_state, actions[0]) print('new_states', new_states) algo = RAOStar(geordi_model, cc=0.01, debugging=False, cc_type='o', fixed_horizon=1) b_init = {geordi_model.current_state: 1.0} P, G = algo.search(b_init, iter_limit=5) for keys, values in P.items(): state, probability, depth = keys best_action = values node_info = {} node_info['state'] = state node_info['probability'] = probability node_info['depth'] = depth node_info['the_best_action'] = best_action