def create_environs(obstacles, boundaries, boats, habitats):
"""
Given the obstacles, boundaries, boats, and habitats in a system of longtitudes and latitudes
this function converts them into positions of Cartesian coordinates
Parameter:
obstacles: a list of Motion_plan_state objects
boundaries: a list of Motion_plan_state objects
boats: a list of Motion_plan_state objects
habitats: a list of Motion_plan_state objects
"""
obstacle_list = []
boundary_list = []
boat_list = []
habitat_list = []
for obs in obstacles:
pos = create_cartesian((obs.x, obs.y), ORIGIN_BOUND)
obstacle_list.append(Motion_plan_state(pos[0], pos[1], size=obs.size))
for b in boundaries:
pos = create_cartesian((b.x, b.y), ORIGIN_BOUND)
boundary_list.append(Motion_plan_state(pos[0], pos[1]))
for boat in boats:
pos = create_cartesian((boat.x, boat.y), ORIGIN_BOUND)
boat_list.append(Motion_plan_state(pos[0], pos[1], size=boat.size))
for habi in habitats:
pos = create_cartesian((habi.x, habi.y), ORIGIN_BOUND)
habitat_list.append(Motion_plan_state(pos[0], pos[1], size=habi.size))
return ([obstacle_list, boundary_list, boat_list, habitat_list])
def generate_rand_init_position(distance, quadrant):
if quadrant == "1":
min_auv_x = 0.0
max_auv_x = distance
min_auv_y = 0.0
max_auv_y = distance
min_goal_x = 0.0
max_goal_x = distance * 2.0
min_goal_y = 0.0
max_goal_y = distance * 2.0
elif quadrant == "2":
min_auv_x = -distance
max_auv_x = 0.0
min_auv_y = 0.0
max_auv_y = distance
min_goal_x = -distance * 2.0
max_goal_x = 0.0
min_goal_y = 0.0
max_goal_y = distance * 2.0
elif quadrant == "3":
min_auv_x = -distance
max_auv_x = 0.0
min_auv_y = -distance
max_auv_y = 0.0
min_goal_x = -distance * 2.0
max_goal_x = 0.0
min_goal_y = -distance * 2.0
max_goal_y = 0.0
else:
min_auv_x = 0.0
max_auv_x = distance
min_auv_y = -distance
max_auv_y = 0.0
min_goal_x = 0.0
max_goal_x = distance * 2.0
min_goal_y = -distance * 2.0
max_goal_y = 0.0
auv_init_pos = Motion_plan_state(x=np.random.uniform(min_auv_x, max_auv_x),
y=np.random.uniform(min_auv_y, max_auv_y),
z=-5.0,
theta=0)
shark_init_pos = Motion_plan_state(
x=np.random.uniform(min_goal_x, max_goal_x),
y=np.random.uniform(min_goal_y, max_goal_y),
z=-5.0,
theta=0)
return auv_init_pos, shark_init_pos
def main():
start = (0,0)
goal = (100,100)
boundary = [Motion_plan_state(0,0), Motion_plan_state(100,100)]
obstacle_list = []
astar_solver = astar(start, goal, obstacle_list, boundary)
astar_solver.perform_analysis(start, goal, boundary)
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 get_auv_trajectory(self, v, delta_t):
"""
Create an array of trajectory points representing a square path
Parameters:
v - linear velocity of the robot (m/s)
delta_t - the time interval between each time stamp (sec)
"""
traj_list = []
t = 0
x = 760
y = 300
z = -10
for i in range(20):
x = x + v * delta_t
y = y
theta = 0
t = t + delta_t
traj_list.append(
Motion_plan_state(x, y, z, theta, traj_time_stamp=t))
for i in range(20):
x = x
y = y + v * delta_t
theta = math.pi / 2
t = t + delta_t
traj_list.append(
Motion_plan_state(x, y, z, theta, traj_time_stamp=t))
for i in range(20):
x = x - v * delta_t
y = y
theta = math.pi
t = t + delta_t
traj_list.append(
Motion_plan_state(x, y, z, theta, traj_time_stamp=t))
for i in range(20):
x = x
y = y - v * delta_t
theta = -(math.pi) / 2
t = t + delta_t
traj_list.append(
Motion_plan_state(x, y, z, theta, traj_time_stamp=t))
return traj_list
def main():
obstacle_list = [Motion_plan_state(5,5,size=1),Motion_plan_state(3,6,size=2),Motion_plan_state(3,8,size=2),\
Motion_plan_state(3,10,size=2),Motion_plan_state(7,5,size=2),Motion_plan_state(9,5,size=2),Motion_plan_state(8,10,size=1)]
boundary = [Motion_plan_state(0,0), Motion_plan_state(15,15)]
testing1 = Performance(obstacle_list, boundary)
length = testing1.performing()
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 __init__(self,
shark_id,
x_pos_array,
y_pos_array,
x_vel_array=[],
y_vel_array=[]):
"""
Note:
x_pos_array and y_pos_array are passed in as array of strings when they get read from
the csv file
"""
self.id = shark_id
self.traj_pts_array = []
# keeps track of the index into shark's trajectory array
self.index = 0
# decided to update the position arrays as we draw the shark's position
# use these arrays to draw the shark's trajectory & for summary plots at the
# end of the simulation
# initialize the array with the first point, so we can do the orientation calculation
# for the direction vector
self.x_pos_array = [float(x_pos_array[0])]
self.y_pos_array = [float(y_pos_array[0])]
self.z_pos_array = [0]
# use map to convert the array of strings to array of float
self.x_vel_array = list(map(lambda x: float(x), x_vel_array))
self.y_vel_array = list(map(lambda y: float(y), y_vel_array))
for i in range(len(x_pos_array)):
# the shark tracking video has frame rate 30 fps
# which means that the time interval will be 1/30, or about 0.03
self.traj_pts_array.append(\
Motion_plan_state(x = float(x_pos_array[i]), y = float(y_pos_array[i]), traj_time_stamp = i * 0.03))
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 get_auv_state(self):
"""
Return a Motion_plan_state representing the orientation and the time stamp
of the robot
"""
return Motion_plan_state(self.x,
self.y,
theta=self.theta,
traj_time_stamp=self.curr_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 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 connect_to_goal_curve(self, mps1):
a = (self.goal.y - mps1.y) / (np.cosh(self.goal.x) - np.cosh(mps1.x))
b = mps1.y - a * np.cosh(mps1.x)
x = np.linspace(mps1.x, self.goal.x, 15)
x = x.tolist()
y = a * np.cosh(x) + b
y = y.tolist()
new_mps = Motion_plan_state(x[-2], y[-2])
new_mps.path.append(mps1)
for i in range(1, len(x)):
new_mps.path.append(Motion_plan_state(x[i], y[i]))
new_mps.length += math.sqrt(
(new_mps.path[i].x - new_mps.path[i - 1].x)**2 +
(new_mps.path[i].y - new_mps.path[i - 1].y)**2)
'''plt.plot(mps1.x, mps1.y, color)
plt.plot(self.goal.x, self.goal.y, color)
plt.plot([mps.x for mps in new_mps.path], [mps.y for mps in new_mps.path], color)
plt.show()'''
return new_mps
def get_random_mps(self, size_max=15):
x_min, y_min, x_max, y_max = self.boundary_poly.bounds
ran_x = random.uniform(x_min, x_max)
ran_y = random.uniform(y_min, y_max)
ran_theta = random.uniform(-math.pi, math.pi)
ran_size = random.uniform(0, size_max)
mps = Motion_plan_state(ran_x, ran_y, theta=ran_theta, size=ran_size)
#ran_z = random.uniform(self.min_area.z, self.max_area.z)
return mps
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 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 get_habitats(self):
'''
get the location of all habitats within the boundary, represented as a list of motion_plan_states
'''
habitats = []
#testing habitat lists
habitats = [Motion_plan_state(750, 300, size=5), Motion_plan_state(750, 320, size=2), Motion_plan_state(780, 240, size=10),\
Motion_plan_state(775, 330, size=3), Motion_plan_state(760, 320, size=5), Motion_plan_state(770, 250, size=4),\
Motion_plan_state(800, 295, size=4), Motion_plan_state(810, 320, size=5), Motion_plan_state(815, 300, size=5),\
Motion_plan_state(825, 330, size=6), Motion_plan_state(830, 335, size=5)]
return habitats
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 connect_to_goal(self, mps, exp_rate, dist_to_end=float("inf")):
new_mps = Motion_plan_state(mps.x, mps.y)
d, theta = self.get_distance_angle(new_mps, self.goal)
new_mps.path = [new_mps]
if dist_to_end > d:
dist_to_end = d
n_expand = math.floor(dist_to_end / exp_rate)
for _ in range(n_expand):
new_mps.x += exp_rate * math.cos(theta)
new_mps.y += exp_rate * math.sin(theta)
new_mps.path.append(Motion_plan_state(new_mps.x, new_mps.y))
d, _ = self.get_distance_angle(new_mps, self.goal)
if d <= dist_to_end:
new_mps.path.append(self.goal)
new_mps.path[0] = mps
return new_mps
def get_random_mps(self, size_max=15):
x_max = max([mps.x for mps in self.boundary])
x_min = min([mps.x for mps in self.boundary])
y_max = max([mps.y for mps in self.boundary])
y_min = min([mps.y for mps in self.boundary])
ran_x = random.uniform(x_min, x_max)
ran_y = random.uniform(y_min, y_max)
ran_theta = random.uniform(-math.pi, math.pi)
ran_size = random.uniform(0, size_max)
mps = Motion_plan_state(ran_x, ran_y, theta=ran_theta, size=ran_size)
#ran_z = random.uniform(self.min_area.z, self.max_area.z)
return mps
def __init__(self, x, y, z, theta, velocity_1, w_1, curr_traj_pt_index, i):
self.state = Motion_plan_state(x, y, z=0, theta=0)
self.velocity_1 = velocity_1
self.w_1 = w_1
self.curr_traj_pt_index = 0
self.i = i
self.x_list = []
self.y_list = []
self.z_list = []
self.x_list += [self.state.x]
self.y_list += [self.state.y]
self.z_list += [self.state.z]
self.sensor_time = const.NUM_ITER_FOR_NEW_SENSOR_DATA
self.shark_sensor_data_list = []
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 create_trajectory_list(self, traj_list):
"""
Run this function to update the trajectory list by including intermediate positions
Parameters:
traj_list - a list of Motion_plan_state, represent positions of each node
"""
time_stamp = 0.1
#constant_gain = 1
trajectory_list = []
step = 0
for i in range(len(traj_list) - 1):
trajectory_list.append(traj_list[i])
x1 = traj_list[i].x
y1 = traj_list[i].y
x2 = traj_list[i + 1].x
y2 = traj_list[i + 1].y
dx = abs(x1 - x2)
dy = abs(y1 - y2)
dist = math.sqrt(dx**2 + dy**2)
velocity = 1
time = dist / velocity
counter = 0
while counter < time:
if x1 < x2:
x1 += time_stamp * velocity
if y1 < y2:
y1 += time_stamp * velocity
step += time_stamp
counter += time_stamp
trajectory_list.append(
Motion_plan_state(x1, y1, traj_time_stamp=step))
trajectory_list.append(traj_list[i + 1])
return trajectory_list
def generate_rand_obstacles(auv_init_pos, shark_init_pos, num_of_obstacles):
"""
"""
obstacle_array = []
for _ in range(num_of_obstacles):
obs_x = np.random.uniform(MIN_X, MAX_X)
obs_y = np.random.uniform(MIN_Y, MAX_Y)
obs_size = np.random.randint(1, 5)
while validate_new_obstacle([obs_x, obs_y], obs_size, auv_init_pos,
shark_init_pos, obstacle_array):
obs_x = np.random.uniform(MIN_X, MAX_X)
obs_y = np.random.uniform(MIN_Y, MAX_Y)
obstacle_array.append(
Motion_plan_state(x=obs_x, y=obs_y, z=-5, size=obs_size))
return obstacle_array
def collision_free(self, position, obstacleList): # obstacleList lists of motion plan state
"""
Check if the current position is collision-free
Parameter:
position - a tuple with two elements, x and y coordinates
obstacleList - a list of Motion_plan_state objects
"""
position_mps = Motion_plan_state(position[0], position[1])
for obstacle in obstacleList:
d, _ = self.get_distance_angle(obstacle, position_mps)
if d <= obstacle.size:
return False # collision
return True
def collision_free(self, position, obstacleList): # obstacleList lists of motion plan state
"""
Check if the input position collide with any of the obstacles
Parameters:
start_pos -- a position tuple (x, y)
obstacle_list -- a list of Motion_plan_state objects, obstacles
Returns:
True if start_pos does not collide with obstacle_list
False if start_pos collides with obstacle_list
"""
position_mps = Motion_plan_state(position[0], position[1])
for obstacle in obstacleList:
d, _ = self.get_distance_angle(obstacle, position_mps)
if d <= obstacle.size:
return False
return True
def summary_4(rrt, habitats, shark_dict, weight):
# res = rrt.exploring(Motion_plan_state(-200, 0), habitats, 0.5, 5, 2, 50, traj_time_stamp=True, max_plan_time=10, max_traj_time=500, plan_time=True, weights=weight)
# traj = res["path"][0]
# print(traj, res['cost'])
res = rrt.replanning(Motion_plan_state(-200, 0), habitats, 10.0, 500.0,
0.1)
traj = res[0]
# print(traj, res[2])
#initialize
total_cost = 0
# num_steps = 0
t = 1
visited = [False for _ in range(len(habitats))]
#helper
curr = 0
time_diff = float("inf")
bin_list = list(shark_dict.keys())
curr_time = 0
while t <= (traj[-1].traj_time_stamp // 1):
diff = abs(traj[curr].traj_time_stamp - t)
while diff <= time_diff:
time_diff = diff
curr += 1
if curr == len(traj) - 1:
break
diff = abs(traj[curr].traj_time_stamp - t)
curr = curr - 1
time_diff = float("inf")
if traj[curr].traj_time_stamp > bin_list[curr_time][1]:
curr_time += 1
AUVGrid = shark_dict[bin_list[curr_time]]
cost, visited = habitat_shark_cost_point(traj[curr], habitats, visited,
AUVGrid, weight)
total_cost += cost
t += 1
# num_steps += 1
return total_cost / traj[-1].traj_time_stamp
def get_auv_sensor_measurements(self, curr_time):
"""
Return an Motion_plan_state object that represents the measurements
of the auv's x,y,z,theta position with a time stamp
The measurement has random gaussian noise
# 0 is the mean of the normal distribution you are choosing from
# 1 is the standard deviation of the normal distribution
# np.random.normal returns a single sample drawn from the parameterized normal distribution
# we actually omitted the third parameter which determines the number of samples that we would like to draw
"""
x = self.state.x + np.random.normal(0, 1)
y = self.state.y + np.random.normal(0, 1)
z = self.state.z + np.random.normal(0, 1)
#theta = angle_wrap(self.state.theta + np.random.normal(0,0.05))
theta = 0
return Motion_plan_state(x, y, z, theta, curr_time)
def habitats_time_spent(self, current_node):
"""
Get the approximate time spent in habitats if the current node is within the habitat(s)
Parameters:
current_node -- a position tuple (x,y)
Returns:
Time spent in habitats
"""
dist_in_habitats = 0
velocity = 1
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 connect_to_goal_curve_alt(self, mps, exp_rate):
new_mps = Motion_plan_state(mps.x,
mps.y,
theta=mps.theta,
traj_time_stamp=mps.traj_time_stamp)
theta_0 = new_mps.theta
_, theta = self.get_distance_angle(mps, self.goal)
diff = theta - theta_0
diff = self.angle_wrap(diff)
if abs(diff) > math.pi / 2:
return
#polar coordinate
r_G = math.hypot(self.goal.x - new_mps.x, self.goal.y - new_mps.y)
phi_G = math.atan2(self.goal.y - new_mps.y, self.goal.x - new_mps.x)
#arc
phi = 2 * self.angle_wrap(phi_G - new_mps.theta)
radius = r_G / (2 * math.sin(phi_G - new_mps.theta))
length = radius * phi
if phi > math.pi:
phi -= 2 * math.pi
length = -radius * phi
elif phi < -math.pi:
phi += 2 * math.pi
length = -radius * phi
new_mps.length += length
ang_vel = phi / (length / exp_rate)
#center of rotation
x_C = new_mps.x - radius * math.sin(new_mps.theta)
y_C = new_mps.y + radius * math.cos(new_mps.theta)
n_expand = math.floor(length / exp_rate)
for i in range(n_expand + 1):
new_mps.x = x_C + radius * math.sin(ang_vel * i + theta_0)
new_mps.y = y_C - radius * math.cos(ang_vel * i + theta_0)
new_mps.theta = ang_vel * i + theta_0
new_mps.path.append(
Motion_plan_state(new_mps.x,
new_mps.y,
theta=new_mps.theta,
plan_time_stamp=time.time() - self.t_start))
return new_mps