def build_environ(complexity, start):
"""
Generate the right number of randomized obstacles, habitats, and shark trajectories
corresponding to the complexity level
Parameter:
complexity: an integer; represents the number of obstacles and habitats
"""
habitats = []
obstacles = []
shark_dict = {}
test_a = []
test_b = []
GRID_RANGE = 10
DELTA_T = 2 # dt = 2s
for shark_num in range(complexity):
shark_dict = main_shark_traj_function(GRID_RANGE, shark_num+1, DELTA_T)
i = 0
while i != complexity:
obs_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000)
obs_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000)
pos = catalina.create_cartesian((obs_x, obs_y), catalina.ORIGIN_BOUND)
obs_size = int(decimal.Decimal(random.randrange(5, 50))/1)
obs = Motion_plan_state(pos[0], pos[1], size=obs_size)
# print ("obs generated: ", (obs.x, obs.y, obs.size))
if usable_obs(obs, start, obstacles):
# print("usable")
obstacles.append(obs)
i += 1
for i in range(complexity):
habitat_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000)
habitat_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000)
pos = catalina.create_cartesian((habitat_x, habitat_y), catalina.ORIGIN_BOUND)
habitat_size = int(decimal.Decimal(random.randrange(50, 120))/1)
habi = Motion_plan_state(pos[0], pos[1], size=habitat_size)
habitats.append(habi)
for obs in obstacles:
test_a.append((obs.x, obs.y, obs.size))
for habi in habitats:
test_b.append((habi.x, habi.y, habi.size))
print("\n", "obstacles = ", test_a, "habitats = ", test_b)
print ("Number of obstacles = ", len(obstacles), "Number of habitats = ", len(habitats))
print ("Shark trajectories = ", shark_dict)
return {"obstacles" : obstacles, "habitats" : habitats, "sharks" : shark_dict}
def build_obstacles(complexity, start):
"""
Generate the right number of randomized obstacles corresponding to the complexity level
Parameter:
complexity: an integer represents the number of obstacles generated
"""
obstacles = []
test_b = []
i = 0
while i != complexity:
obs_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000)
obs_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000)
pos = catalina.create_cartesian((obs_x, obs_y), catalina.ORIGIN_BOUND)
obs_size = int(decimal.Decimal(random.randrange(5, 50))/1)
obs = Motion_plan_state(pos[0], pos[1], size=obs_size)
# print ("obs generated: ", (obs.x, obs.y, obs.size))
if usable_obs(obs, start, obstacles):
# print("usable")
obstacles.append(obs)
i += 1
for obs in obstacles:
test_b.append((obs.x, obs.y, obs.size))
print("\n", "obstacles = ", test_b)
print("obstacle account = ", len(obstacles))
return obstacles
def main():
weights = [0, 10, 10]
start_cartesian = create_cartesian((33.446056, -118.489111),
catalina.ORIGIN_BOUND)
print(start_cartesian)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print("start: ", start)
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES,
catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]
boat_list = environ[2]
habitat_list = environ[3]
astar_solver = astar(start, obstacle_list + boat_list, boundary_list)
final_path_mps = astar_solver.astar(habitat_list,
obstacle_list + boat_list,
boundary_list, start, 400, weights)
print("\n", "final trajectory: ", final_path_mps[0])
print("\n", "cost of each node on the final trajectory: ",
final_path_mps[1])
def create_grid_map(self, current_node, neighbor):
"""
Find the grid value of the current node's neighbor;
the magnitude of the grid value is influenced by the angle of the current node to the nearest habitat
and whether the current_node covers a habitat
Parameter:
current_node: a Node object
neighbor: a position tuple of two elements (x_in_meters, y_in_meters)
"""
habitat_list = []
constant = 1
for habi in catalina.HABITATS:
pos = catalina.create_cartesian((habi.x, habi.y),
catalina.ORIGIN_BOUND)
habitat_list.append(
Motion_plan_state(pos[0], pos[1], size=habi.size))
target_habitat = habitat_list[0]
min_distance = euclidean_dist((target_habitat.x, target_habitat.y),
current_node.position)
for habi in habitat_list:
dist = euclidean_dist((habi.x, habi.y), current_node.position)
if dist < min_distance:
target_habitat = habi
# compute Nj
vector_1 = [
neighbor[0] - current_node.position[0],
neighbor[1] - current_node.position[1]
]
vector_2 = [
target_habitat.x - current_node.position[0],
target_habitat.y - current_node.position[1]
]
unit_vector_1 = vector_1 / np.linalg.norm(vector_1)
unit_vector_2 = vector_2 / np.linalg.norm(vector_2)
dot_product = np.dot(unit_vector_1, unit_vector_2)
angle = np.arccos(dot_product)
Nj = math.cos(angle)
# compute Gj
for item in habitat_list:
dist = math.sqrt((current_node.position[0] - item.x)**2 +
(current_node.position[1] - item.y)**2)
if dist <= item.size: # current_node covers a habitat
Gj = 1
else:
Gj = 0
# compute grid value bj
bj = constant * Nj + Gj
return (bj)
def sharks_vs_costTime(times, start, XLIST, weights, pathLenLimit, total_traj_time):
"""
Keep the information of shark trajectories and habitats constant to see the association between the number of obstacles and cost
Parameter:
times: an integer represents the number of iterations
start: a position tuple of two elements in the system of longtitudes and latitudes
XLIST: a list of integers each of which signifies the complexity level(i.e. item account)
shark_dict: a list of Motion_plan_state objects; represents multiple shark trajectories
weights: a list of four integers; we use [0, 10, 10, 100]
total_traj_time: in seconds; set it to 200s
"""
COST = []
TIME = []
start_cartesian = catalina.create_cartesian(start, catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
habitat_list = environ[3]
boundary_list = environ[1]
obstacle_list = environ[0]+environ[2]
for complexity in XLIST: # for each kind of environment
cost_list = []
time_list = []
for sim in range(times): # run a number of simulations
shark_dict = build_sharkDict(complexity)
astar_solver = astar(start, obstacle_list, boundary_list, habitat_list, {}, shark_dict, AUV_velocity=1)
start_time = timeit.timeit()
final_path_mps = astar_solver.astar(pathLenLimit, weights, shark_dict)
end_time = timeit.timeit()
peri_boundary = cal_boundary_peri(astar_solver)
traj_node = final_path_mps["node"]
traj_mps = final_path_mps["path"]
print ("\n", "Final trajectory : ", traj_mps)
cost = get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, habitat_list, astar_solver.sharkGrid, weights)
cost_list.append(cost)
print ("\n", "cost list: ", cost_list)
time_list.append(abs(end_time-start_time))
print ("\n", "time list: ", time_list)
if len(cost_list) >= 1:
COST.append(mean(cost_list))
print ("\n","cost values: ", COST)
if len(time_list) >= 1:
TIME.append(mean(time_list))
print("\n", "time values: ", TIME)
return {"cost" : COST, "time" : TIME}
def build_environ(complexity):
"""
Generate the right number of randomized obstacles and habitats corresponding to the complexity level
Parameter:
complexity: an integer; represents the number of obstacles and habitats
"""
habitats = []
obstacles = []
test_a = []
test_b = []
for i in range(complexity):
obs_x = float(
decimal.Decimal(random.randrange(33443758, 33445914)) / 1000000)
obs_y = float(
decimal.Decimal(random.randrange(-118488471, -118485219)) /
1000000)
pos = catalina.create_cartesian((obs_x, obs_y), catalina.ORIGIN_BOUND)
obs_size = int(decimal.Decimal(random.randrange(10, 30)) / 1)
obs = Motion_plan_state(pos[0], pos[1], size=obs_size)
obstacles.append(obs)
habitat_x = float(
decimal.Decimal(random.randrange(33443758, 33445914)) / 1000000)
habitat_y = float(
decimal.Decimal(random.randrange(-118488471, -118485219)) /
1000000)
pos = catalina.create_cartesian((habitat_x, habitat_y),
catalina.ORIGIN_BOUND)
habitat_size = int(decimal.Decimal(random.randrange(50, 120)) / 1)
habi = Motion_plan_state(pos[0], pos[1], size=habitat_size)
habitats.append(habi)
for obs in obstacles:
test_a.append((obs.x, obs.y, obs.size))
for habi in habitats:
test_b.append((habi.x, habi.y, habi.size))
print("\n", "obstacles = ", test_a, "habitats = ", test_b)
return {"obstacles": obstacles, "habitats": habitats}
def create_grid_map(self, current_node, neighbor):
habitat_list = []
constant = 1
for habi in catalina.HABITATS:
pos = catalina.create_cartesian((habi.x, habi.y),
catalina.ORIGIN_BOUND)
habitat_list.append(
Motion_plan_state(pos[0], pos[1], size=habi.size))
print('habitats: ', habitat_list)
target_habitat = habitat_list[0]
min_distance = euclidean_dist((target_habitat.x, target_habitat.y),
current_node.position)
for habi in habitat_list:
dist = euclidean_dist((habi.x, habi.y), current_node.position)
if dist < min_distance:
target_habitat = habi
print('target: ', target_habitat)
# compute Nj
vector_1 = [
neighbor[0] - current_node.position[0],
neighbor[1] - current_node.position[1]
]
print('vector_1: ', vector_1)
vector_2 = [
target_habitat.x - current_node.position[0],
target_habitat.y - current_node.position[1]
]
print('vector_2: ', vector_2)
unit_vector_1 = vector_1 / np.linalg.norm(vector_1)
unit_vector_2 = vector_2 / np.linalg.norm(vector_2)
dot_product = np.dot(unit_vector_1, unit_vector_2)
angle = np.arccos(dot_product)
print('angle: ', angle)
Nj = math.cos(angle)
# compute Gj
for item in habitat_list:
dist = math.sqrt((current_node.position[0] - item.x)**2 +
(current_node.position[1] - item.y)**2)
if dist <= item.size: # current_node covers a habitat
Gj = 1
else:
Gj = 0
# compute grid value bj
bj = constant * Nj + Gj
return (bj)
def main():
start = create_cartesian((33.444686, -118.484716), catalina.ORIGIN_BOUND)
print("start: ", start)
# print ('start: ', start)
# print ('goal: ', goal)
obstacle_list = []
boundary_list = []
habitat_list = []
goal_list = []
weights = [0, 10, 10]
final_cost = []
for obs in catalina.OBSTACLES:
pos = create_cartesian((obs.x, obs.y), catalina.ORIGIN_BOUND)
obstacle_list.append(Motion_plan_state(pos[0], pos[1], size=obs.size))
for b in catalina.BOUNDARIES:
pos = create_cartesian((b.x, b.y), catalina.ORIGIN_BOUND)
boundary_list.append(Motion_plan_state(pos[0], pos[1]))
for habi in catalina.HABITATS:
pos = catalina.create_cartesian((habi.x, habi.y),
catalina.ORIGIN_BOUND)
habitat_list.append(Motion_plan_state(pos[0], pos[1], size=habi.size))
goal1 = create_cartesian((33.445779, -118.486976), catalina.ORIGIN_BOUND)
print("goal: ", goal1)
astar_solver = astar(start, goal1, obstacle_list, boundary_list)
final_path_mps = astar_solver.astar(habitat_list, obstacle_list,
boundary_list, start, goal1, weights)
print("\n", "final cost: ", final_path_mps[1][0])
print("\n", "final path: ", final_path_mps[0])
def __init__(self):
#initialize start, goal, obstacle, boundary, habitats for path planning
start = catalina.create_cartesian(catalina.START, catalina.ORIGIN_BOUND)
self.start = Motion_plan_state(start[0], start[1])
goal = catalina.create_cartesian(catalina.GOAL, catalina.ORIGIN_BOUND)
self.goal = Motion_plan_state(goal[0], goal[1])
self.obstacles = []
for ob in catalina.OBSTACLES:
pos = catalina.create_cartesian((ob.x, ob.y), catalina.ORIGIN_BOUND)
self.obstacles.append(Motion_plan_state(pos[0], pos[1], size=ob.size))
self.boundary = []
for b in catalina.BOUNDARIES:
pos = catalina.create_cartesian((b.x, b.y), catalina.ORIGIN_BOUND)
self.boundary.append(Motion_plan_state(pos[0], pos[1]))
self.boat_list = []
for boat in catalina.BOATS:
pos = catalina.create_cartesian((boat.x, boat.y), catalina.ORIGIN_BOUND)
self.boat_list.append(Motion_plan_state(pos[0], pos[1], size=boat.size))
#testing data for habitats
self.habitats = []
for habitat in catalina.HABITATS:
pos = catalina.create_cartesian((habitat.x, habitat.y), catalina.ORIGIN_BOUND)
self.habitats.append(Motion_plan_state(pos[0], pos[1], size=habitat.size))
#testing data for shark trajectories
self.shark_dict = {1: [Motion_plan_state(-102 + (0.1 * i), -91 + (0.1 * i), traj_time_stamp=i) for i in range(1,501)],
2: [Motion_plan_state(-150 - (0.1 * i), 0 + (0.1 * i), traj_time_stamp=i) for i in range(1,501)]}
#initialize path planning algorithm
#A* algorithm
self.astar = astar(start, goal, self.obstacles+self.boat_list, self.boundary)
self.A_star_traj = []
#RRT algorithm
self.rrt = RRT(self.goal, self.goal, self.boundary, self.obstacles+self.boat_list, self.habitats, freq=10)
self.rrt_traj = []
#Reinforcement Learning algorithm
#initialize live3Dgraph
self.live3DGraph = Live3DGraph()
#set up for sharks
#self.load_real_shark(2)
self.curr_time = 0
self.sonar_range = 50
def main():
weights = [0, 10, 10, 100]
start_cartesian = create_cartesian((33.446198, -118.486652), catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]
boat_list = environ[2]
habitat_list = environ[3]
# testing data for shark trajectories
shark_dict = {1: [Motion_plan_state(-120 + (0.3 * i), -60 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
2: [Motion_plan_state(-65 - (0.3 * i), -50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
3: [Motion_plan_state(-110 + (0.3 * i), -40 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
4: [Motion_plan_state(-105 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
5: [Motion_plan_state(-120 + (0.3 * i), -50 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
6: [Motion_plan_state(-85 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
7: [Motion_plan_state(-270 + (0.3 * i), 50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
8: [Motion_plan_state(-250 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
9: [Motion_plan_state(-260 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
10: [Motion_plan_state(-275 + (0.3 * i), 80 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)]}
astar_solver = astar(start, obstacle_list+boat_list, boundary_list, habitat_list, {}, shark_dict, AUV_velocity=1)
final_path_mps = astar_solver.astar(300, weights, shark_dict)
print ("\n", "Final Trajectory: ", final_path_mps["path"])
print ("\n", "Trajectory Length: ", final_path_mps["path length"])
print ("\n", "Trajectory Cost: ", final_path_mps["cost"])
print ("\n", "Trajectory Cost List: ", final_path_mps["cost list"])
boundary_poly = []
for pos in boundary_list:
boundary_poly.append((pos.x, pos.y))
boundary = Polygon(boundary_poly) # a Polygon object that represents the boundary of our workspace
sharkOccupancyGrid = SharkOccupancyGrid(shark_dict, 10, boundary, 50, 50)
grid_dict = sharkOccupancyGrid.convert()
plot(sharkOccupancyGrid, grid_dict[0], final_path_mps["path"])
def habitats_time_spent(self, current_node):
"""
Find the approximate time spent in habitats if the current node is within the habitat(s)
Parameter:
current_node: a position tuple of two elements (x,y)
habitats: a list of motion_plan_state
"""
dist_in_habitats = 0
velocity = 1 # velocity of exploration in m/s
for habi in catalina.HABITATS:
pos_habi = create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND)
dist, _ = self.get_distance_angle(Motion_plan_state(pos_habi[0], pos_habi[1], size=habi.size), Motion_plan_state(current_node.position[0], current_node.position[1]))
if dist <= habi.size:
dist_in_habitats += 10
return dist_in_habitats/velocity
def main():
weights = [0, 10, 50]
start_cartesian = create_cartesian((33.445089, -118.486933), catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]+environ[2]
habitat_list = environ[3]
single_AUV = singleAUV(start, obstacle_list, boundary_list, habitat_list, [])
final_traj = single_AUV.astar(habitat_list, obstacle_list, boundary_list, start, 200, weights)
print ("\n", "final trajectory: ", final_traj["path"])
print ("\n", "Trajectory length: ", len(final_traj["path"]))
print ("\n", "cost of each node on the final trajectory: ", final_traj["cost"])
def build_habitats(complexity):
"""
Generate the right number of randomized habitats corresponding to the complexity level
Parameter:
complexity: an integer represents the number of habitats generated
"""
habitats = []
test_a = []
for i in range(complexity):
habitat_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000)
habitat_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000)
pos = catalina.create_cartesian((habitat_x, habitat_y), catalina.ORIGIN_BOUND)
habitat_size = int(decimal.Decimal(random.randrange(50, 120))/1)
habi = Motion_plan_state(pos[0], pos[1], size=habitat_size)
habitats.append(habi)
for habi in habitats:
test_a.append((habi.x, habi.y, habi.size))
print("\n", "habitats = ", test_a)
print("habitat account = ", len(habitats))
return habitats
def plot_2d_astar_traj(self, astar_x_array, astar_y_array, A_star_traj_cost):
"""
Plot a trajectory with defined boundaries and obstacles
"""
plt.close()
fig, ax = plt.subplots()
# plot the boundaries as polygon lines
Path = mpath.Path
path_data = []
for i in range(len(catalina.BOUNDARIES)):
pos = create_cartesian((catalina.BOUNDARIES[i].x, catalina.BOUNDARIES[i].y), catalina.ORIGIN_BOUND)
if i == 0:
path_data.append((Path.MOVETO, pos))
else:
path_data.append((Path.LINETO, pos))
last = create_cartesian((catalina.BOUNDARIES[0].x, catalina.BOUNDARIES[0].y), catalina.ORIGIN_BOUND)
path_data.append((Path.CLOSEPOLY, last))
codes, verts = zip(*path_data)
path = mpath.Path(verts, codes)
patch = mpatches.PathPatch(path, facecolor=None, alpha=0)
ax.add_patch(patch)
# plot obstacels as circles
for obs in catalina.OBSTACLES:
pos_circle = create_cartesian((obs.x, obs.y), catalina.ORIGIN_BOUND)
ax.add_patch(plt.Circle(pos_circle, obs.size, color = '#000000', fill = False))
# plot boats as circles
for boat in catalina.BOATS:
pos_boat = create_cartesian((boat.x, boat.y), catalina.ORIGIN_BOUND)
ax.add_patch(plt.Circle(pos_boat, boat.size, color = 'y', fill = False))
# plot habitats as circles:
for habi in catalina.HABITATS:
pos_habi = create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND)
ax.add_patch(plt.Circle(pos_habi, habi.size, color = 'b', fill = False))
for habitat in catalina.HABITATS:
pos_habitat = create_cartesian((habitat.x, habitat.y), catalina.ORIGIN_BOUND)
ax.add_patch(plt.Circle(pos_habitat, habitat.size, color = 'b', fill = False))
x, y = zip(*path.vertices)
line, = ax.plot(x, y, 'go-')
ax.grid()
ax.axis('equal')
# plot A* trajectory
plt.plot(astar_x_array, astar_y_array, marker = ',', color = 'r', label=A_star_traj_cost)
legend = plt.legend()
plt.setp(legend.get_texts(), color='#000000')
# plt.title('number of habitats covered vs. cost visualization')
plt.xlabel('meters')
plt.ylabel('meters')
plt.show()
def plot_multiple_2d_astar_traj(self, x_list, y_list, traj_cost_list):
<<<<<<< HEAD
=======
>>>>>>> 96f4bdb0ee8c8c20b7f0c4becf194855f19cc5a6
plt.close()
fig, ax = plt.subplots()
# plot the boundaries as polygon lines
Path = mpath.Path
path_data = []
for i in range(len(catalina.BOUNDARIES)):
pos = create_cartesian((catalina.BOUNDARIES[i].x, catalina.BOUNDARIES[i].y), catalina.ORIGIN_BOUND)
if i == 0:
path_data.append((Path.MOVETO, pos))
else:
path_data.append((Path.LINETO, pos))
last = create_cartesian((catalina.BOUNDARIES[0].x, catalina.BOUNDARIES[0].y), catalina.ORIGIN_BOUND)
path_data.append((Path.CLOSEPOLY, last))
codes, verts = zip(*path_data)
path = mpath.Path(verts, codes)
patch = mpatches.PathPatch(path, facecolor=None, alpha=0)
ax.add_patch(patch)
# plot obstacels as circles
def iterate(times):
weights = [0, 10, 10, 100]
pathLenLimit = 200 # in meters
total_traj_time = 200 # in seconds
start_cartesian = catalina.create_cartesian((33.446198, -118.486652), catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]
boat_list = environ[2]
x_list = [1, 3, 5, 7, 9]
y_list = [] # holds averaged costs
z_list = [] # holds averaged planning time
'''shark_dict = {1: [Motion_plan_state(-120 + (0.3 * i), -60 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
2: [Motion_plan_state(-65 - (0.3 * i), -50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
3: [Motion_plan_state(-110 + (0.3 * i), -40 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
4: [Motion_plan_state(-105 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
5: [Motion_plan_state(-120 + (0.3 * i), -50 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
6: [Motion_plan_state(-85 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
7: [Motion_plan_state(-270 + (0.3 * i), 50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
8: [Motion_plan_state(-250 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
9: [Motion_plan_state(-260 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
10: [Motion_plan_state(-275 + (0.3 * i), 80 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)]} '''
for complexity in x_list: # for each kind of environment
cost_list = []
time_list = []
for sim in range(times): # run a number of simulations
environ_dict = build_environ(complexity, start) # returns {"obstacles" : obstacles, "habitats" : habitats}
final_obstacle_list = obstacle_list+boat_list+environ_dict["obstacles"]
final_habitat_list = environ_dict["habitats"]
shark_dict = environ_dict["sharks"]
astar_solver = astar(start, final_obstacle_list, boundary_list, final_habitat_list, {}, shark_dict, AUV_velocity=1)
start_time = timeit.timeit()
final_path_mps = astar_solver.astar(pathLenLimit, weights, shark_dict)
end_time = timeit.timeit()
peri_boundary = cal_boundary_peri(astar_solver)
traj_node = final_path_mps["node"]
traj_mps = final_path_mps["path"]
print ("\n", "Final trajectory : ", traj_mps)
cost = get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, final_habitat_list, astar_solver.sharkGrid, weights)
cost_list.append(cost)
print ("\n", "cost list: ", cost_list)
time_list.append(abs(end_time-start_time))
print ("\n", "time list: ", time_list)
if len(cost_list) >= 1:
y_list.append(mean(cost_list))
print ("\n","y values: ", y_list)
if len(time_list) >= 1:
z_list.append(mean(time_list))
print("\n", "z values: ", z_list)
return {"Y" : y_list, "Z" : z_list}
currTrial += 1
print(end='')
aveCost = [item/numTrials for item in sumCost]
numAUV_list = list(range(1, maxNumAUV+1))
print("numAUV_list: ", numAUV_list)
print("aveCost: ", aveCost)
fig, ax = plt.subplots()
ax.plot(numAUV_list, aveCost)
ax.set(xlabel='AUV Number', ylabel='Trajectory Cost', title='Number of AUVs vs. Average Trajectory Cost')
ax.grid()
plt.show()
def main():
# Visualize trajectories
<<<<<<< HEAD
numAUV = 3
pos = create_cartesian((33.445089, -118.486933), catalina.ORIGIN_BOUND)
test_robot = astarSim(round(pos[0], 2), round(pos[1], 2), 0, pathLenLimit=200, weights=[0, 10, 1000, 100])
# test_robot.display_single_astar_trajectory()
=======
numAUV = 4
pos = create_cartesian((33.446019, -118.489441), catalina.ORIGIN_BOUND)
test_robot = astarSim(round(pos[0], 2), round(pos[1], 2), 0, pathLenLimit=250, weights=[10, 1000, 100])
>>>>>>> 96f4bdb0ee8c8c20b7f0c4becf194855f19cc5a6
test_robot.display_multi_astar_trajectory(numAUV)
if __name__ == "__main__":
main()
def plot_2d_traj(self, traj_dict, shark_dict):
"""
Plot a trajectory with defined boundaries and obstacles
"""
plt.close()
fig, ax = plt.subplots()
# plot the boundaries as polygon lines
Path = mpath.Path
path_data = []
for i in range(len(catalina.BOUNDARIES)):
pos = create_cartesian(
(catalina.BOUNDARIES[i].x, catalina.BOUNDARIES[i].y),
catalina.ORIGIN_BOUND)
if i == 0:
path_data.append((Path.MOVETO, pos))
else:
path_data.append((Path.LINETO, pos))
last = create_cartesian(
(catalina.BOUNDARIES[0].x, catalina.BOUNDARIES[0].y),
catalina.ORIGIN_BOUND)
path_data.append((Path.CLOSEPOLY, last))
codes, verts = zip(*path_data)
path = mpath.Path(verts, codes)
patch = mpatches.PathPatch(path, facecolor=None, alpha=0)
ax.add_patch(patch)
# plot obstacels as circles
for obs in catalina.OBSTACLES:
pos_circle = create_cartesian((obs.x, obs.y),
catalina.ORIGIN_BOUND)
ax.add_patch(
plt.Circle(pos_circle, obs.size, color='#000000', fill=False))
# plot boats as circles
for boat in catalina.BOATS:
pos_boat = create_cartesian((boat.x, boat.y),
catalina.ORIGIN_BOUND)
ax.add_patch(
plt.Circle(pos_boat, boat.size, color='#000000', fill=False))
for habitat in catalina.HABITATS:
pos_habitat = create_cartesian((habitat.x, habitat.y),
catalina.ORIGIN_BOUND)
ax.add_patch(
plt.Circle(pos_habitat, habitat.size, color='b', fill=False))
x, y = zip(*path.vertices)
line, = ax.plot(x, y, 'go-')
ax.grid()
ax.axis('equal')
# plot trajectory
for planner, traj in traj_dict.items():
if traj != []:
color = self.traj_checkbox_dict[planner][2]
ax.plot(traj[0],
traj[1],
marker=',',
color=color,
label=planner)
# plot sharks
for shark_id, shark_pos in shark_dict.items():
ax.plot([mps.x for mps in shark_pos], [mps.y for mps in shark_pos],
label=shark_id)
ax.legend()
plt.show()
bin_list = time_stamp_list.keys()
num_time_list = []
for time_bin in bin_list:
num_time_list.append(len(time_stamp_list[time_bin]))
plt.title("time stamp distribution")
plt.xlabel("time stamp bin")
plt.ylabel("number of motion_plan_states")
#plt.xticks(self.bin_list)
plt.bar(bin_list, num_time_list, color="g")
plt.show()
#initialize start, goal, obstacle, boundary, habitats for path planning
start = catalina.create_cartesian(catalina.START, catalina.ORIGIN_BOUND)
start = Motion_plan_state(start[0], start[1])
goal = catalina.create_cartesian(catalina.GOAL, catalina.ORIGIN_BOUND)
goal = Motion_plan_state(goal[0], goal[1])
obstacles = []
for ob in catalina.OBSTACLES:
pos = catalina.create_cartesian((ob.x, ob.y), catalina.ORIGIN_BOUND)
obstacles.append(Motion_plan_state(pos[0], pos[1], size=ob.size))
for boat in catalina.BOATS:
pos = catalina.create_cartesian((boat.x, boat.y), catalina.ORIGIN_BOUND)
obstacles.append(Motion_plan_state(pos[0], pos[1], size=boat.size))
boundary = []
boundary_poly = []
# update overall abitat coverage
self.habitat_open_list = single_AUV.habitat_open_list[:]
self.habitat_closed_list = single_AUV.habitat_closed_list[:]
print("\n", "Open Habitats ", i + 1, ": ", self.habitat_open_list)
print("Closed Habitats ", i + 1, ": ", self.habitat_closed_list)
print("path ", i + 1, ": ", single_planner["path"])
print("path length ", i + 1, ": ", len(single_planner["path"]))
print("path cost ", i + 1, ": ", single_planner["cost"])
return {"trajs": self.trajectories, "costs": self.costs}
if __name__ == "__main__":
numAUV = 2
pathLenLimit = 150
weights = [0, 10, 10]
start_cartesian = catalina.create_cartesian((33.446019, -118.489441),
catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES,
catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0] + environ[2]
boundary_list = environ[1]
habitat_list = environ[3]
AUVs = multiAUV(start, numAUV, habitat_list, boundary_list, obstacle_list)
AUVs.multi_AUV_planner(pathLenLimit, weights)
