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