def generate_obstacle(rng,
path,
vessel,
displacement_dist_std=150,
obst_radius_distr=np.random.poisson,
obst_radius_mean=30):
min_distance = 0
while min_distance <= 0:
obst_displacement_dist = np.random.normal(0, displacement_dist_std)
obst_arclength = (0.1 + 0.8 * rng.rand()) * path.length
obst_position = path(obst_arclength)
obst_displacement_angle = geom.princip(
path.get_direction(obst_arclength) - np.pi / 2)
obst_position += obst_displacement_dist * np.array(
[np.cos(obst_displacement_angle),
np.sin(obst_displacement_angle)])
obst_radius = max(1, obst_radius_distr(obst_radius_mean))
vessel_distance_vec = geom.Rzyx(0, 0, -vessel.heading).dot(
np.hstack([obst_position - vessel.position, 0]))
vessel_distance = np.linalg.norm(
vessel_distance_vec) - vessel.width - obst_radius
goal_distance = np.linalg.norm(obst_position -
path(path.length)) - obst_radius
min_distance = min(vessel_distance, goal_distance)
return (obst_position, obst_radius)
def update_control_errors(self):
# Update cruise speed error
self.u_error = np.clip(
(self.cruise_speed - self.vessel.relative_velocity[0]) / 2, -1, 1)
self.chi_error = 0.0
self.e = 0.0
self.upsilon_error = 0.0
self.h = 0.0
# Get path course and elevation
s = self.prog
chi_p, upsilon_p = self.path.get_direction_angles(s)
# Calculate tracking errors
SF_rotation = geom.Rzyx(0, upsilon_p, chi_p)
epsilon = np.transpose(SF_rotation).dot(self.vessel.position -
self.path(self.prog))
e = epsilon[1]
h = epsilon[2]
# Calculate course and elevation errors from tracking errors
chi_r = np.arctan2(-e, self.la_dist)
upsilon_r = np.arctan2(h, np.sqrt(e**2 + self.la_dist**2))
chi_d = chi_p + chi_r
upsilon_d = upsilon_p + upsilon_r
self.chi_error = np.clip(
geom.ssa(self.vessel.chi - chi_d) / np.pi, -1, 1)
#self.e = np.clip(e/12, -1, 1)
self.e = e
self.upsilon_error = np.clip(
geom.ssa(self.vessel.upsilon - upsilon_d) / np.pi, -1, 1)
#self.h = np.clip(h/12, -1, 1)
self.h = h
def _sim(self):
psi = self._state[2]
nu = self._state[3:]
eta_dot = geom.Rzyx(0, 0, geom.princip(psi)).dot(nu)
nu_dot = const.M_inv.dot(
const.B(nu).dot(self.input) - const.D(nu).dot(nu) -
const.C(nu).dot(nu) - const.L(nu).dot(nu))
state_dot = np.concatenate([eta_dot, nu_dot])
self._state += state_dot * self.t_step
self._state[2] = geom.princip(self._state[2])
def _state_dot(self, state):
psi = state[2]
nu = state[3:]
tau = np.array([self.input[0], 0, self.input[1]])
eta_dot = geom.Rzyx(0, 0, geom.princip(psi)).dot(nu)
nu_dot = const.M_inv.dot(tau
#- const.D.dot(nu)
- const.N(nu).dot(nu))
state_dot = np.concatenate([eta_dot, nu_dot])
return state_dot
def observe(self):
"""
Generates the observation of the environment.
Parameters
----------
action : np.array
[propeller_input, rudder_position].
Returns
-------
obs : np.array
[
surge velocity,
sway velocity,
heading error,
distance to goal,
propeller_input,
rudder_positione,
self.nsectors*[closeness to closest obstacle in sector]
]
All observations are between -1 and 1.
"""
obst_range = self.config["obst_detection_range"]
goal_vector = self.goal - self.vessel.position
goal_direction = np.arctan2(goal_vector[1], goal_vector[0])
heading_error = float(
geom.princip(goal_direction - self.vessel.heading))
obs = np.zeros((self.nstates + self.nsectors, ))
obs[0] = np.clip(self.vessel.velocity[0] / self.vessel.max_speed, -1,
1)
obs[1] = np.clip(self.vessel.velocity[1] / 0.26, -1, 1)
obs[2] = np.clip(self.vessel.yawrate / 0.55, -1, 1)
obs[3] = np.clip(heading_error / np.pi, -1, 1)
for obst in self.obstacles:
distance_vec = geom.Rzyx(0, 0, -self.vessel.heading).dot(
np.hstack([obst.position - self.vessel.position, 0]))
dist = linalg.norm(distance_vec)
if dist < obst_range + obst.radius + self.vessel.radius:
ang = ((float(np.arctan2(distance_vec[1], distance_vec[0])) +
np.pi) / (2 * np.pi))
closeness = 1 - np.clip(
(dist - self.vessel.radius - obst.radius) / obst_range, 0,
1)
isector = (self.nstates + int(np.floor(ang * self.nsectors)))
if isector == self.nstates + self.nsectors:
isector = self.nstates
if obs[isector] < closeness:
obs[isector] = closeness
return obs
def observe(self):
"""
Generates the observation of the environment.
Parameters
----------
action : np.array
[propeller_input, rudder_position].
Returns
-------
obs : np.array
[
surge velocity,
sway velocity,
yawrate,
heading error,
cross track error,
propeller_input,
rudder_position,
]
All observations are between -1 and 1.
"""
la_heading = self.path.get_direction(
self.path_prog[-1] + self.config["la_dist"])
heading_error_la = float(geom.princip(la_heading
- self.vessel.heading))
path_position = (self.path(self.path_prog[-1]
+ self.config["la_dist"])
- self.vessel.position)
target_heading = np.arctan2(path_position[1], path_position[0])
heading_error = float(geom.princip(target_heading
- self.vessel.heading))
path_direction = self.path.get_direction(self.path_prog[-1])
cross_track_error = geom.Rzyx(0, 0, -path_direction).dot(
np.hstack([self.path(self.path_prog[-1])
- self.vessel.position, 0]))[1]
obs = np.zeros((self.nstates,))
obs[0] = np.clip(self.vessel.velocity[0]
/self.vessel.max_speed, -1, 1)
obs[1] = np.clip(self.vessel.velocity[1]
/0.26, -1, 1)
obs[2] = np.clip(self.vessel.yawrate/0.55, -1, 1)
obs[3] = np.clip(heading_error_la/np.pi, -1, 1)
obs[4] = np.clip(heading_error/np.pi, -1, 1)
obs[5] = np.clip(cross_track_error
/25, -1, 1)
return obs
def __call__(self, state):
"""Returns the current velcotiy in {b} to use in in AUV kinematics"""
phi = state[3]
theta = state[4]
psi = state[5]
omega_body = state[9:12]
vel_current_NED = np.array([self.Vc*np.cos(self.alpha)*np.cos(self.beta), self.Vc*np.sin(self.beta), self.Vc*np.sin(self.alpha)*np.cos(self.beta)])
vel_current_BODY = np.transpose(geom.Rzyx(phi, theta, psi)).dot(vel_current_NED)
vel_current_BODY_dot = -geom.S_skew(omega_body).dot(vel_current_BODY)
nu_c = np.array([*vel_current_BODY, 0, 0, 0])
nu_c_dot = np.array([*vel_current_BODY_dot, 0, 0, 0])
return nu_c
def update_nearby_obstacles(self):
"""
Updates the nearby_obstacles array.
"""
self.nearby_obstacles = []
for obstacle in self.obstacles:
distance_vec_NED = obstacle.position - self.vessel.position
distance = np.linalg.norm(distance_vec_NED)
distance_vec_BODY = np.transpose(
geom.Rzyx(*self.vessel.attitude)).dot(distance_vec_NED)
heading_angle_BODY = np.arctan2(distance_vec_BODY[1],
distance_vec_BODY[0])
pitch_angle_BODY = np.arctan2(
distance_vec_BODY[2],
np.sqrt(distance_vec_BODY[0]**2 + distance_vec_BODY[1]**2))
# check if the obstacle is inside the sonar window
if distance - self.vessel.safety_radius - obstacle.radius <= self.sonar_range and abs(heading_angle_BODY) <= self.sensor_span[0]*np.pi/180 \
and abs(pitch_angle_BODY) <= self.sensor_span[1]*np.pi/180:
self.nearby_obstacles.append(obstacle)
elif distance <= obstacle.radius + self.vessel.safety_radius:
self.nearby_obstacles.append(obstacle)
def navigate(self, path: Path) -> np.ndarray:
"""
Calculates and returns navigation states representing the vessel's attitude
with respect to the desired path.
Returns
-------
navigation_states : np.ndarray
"""
# Calculating path arclength at reference point, i.e. the point closest to the vessel
vessel_arclength = path.get_closest_arclength(self.position)
# Calculating tangential path direction at reference point
path_direction = path.get_direction(vessel_arclength)
cross_track_error = geom.Rzyx(0, 0, -path_direction).dot(
np.hstack([path(vessel_arclength) - self.position, 0]))[1]
# Calculating tangential path direction at look-ahead point
target_arclength = min(
path.length, vessel_arclength + self.config["look_ahead_distance"])
look_ahead_path_direction = path.get_direction(target_arclength)
look_ahead_heading_error = float(
geom.princip(look_ahead_path_direction - self.heading))
# Calculating vector difference between look-ahead point and vessel position
target_vector = path(target_arclength) - self.position
# Calculating heading error
target_heading = np.arctan2(target_vector[1], target_vector[0])
heading_error = float(geom.princip(target_heading - self.heading))
# Calculating path progress
progress = vessel_arclength / path.length
self._progress = progress
# Concatenating states
self._last_navi_state_dict = {
'surge_velocity': self.velocity[0],
'sway_velocity': self.velocity[1],
'yaw_rate': self.yaw_rate,
'look_ahead_heading_error': look_ahead_heading_error,
'heading_error': heading_error,
'cross_track_error': cross_track_error / 100,
'target_heading': target_heading,
'look_ahead_path_direction': look_ahead_path_direction,
'path_direction': path_direction,
'vessel_arclength': vessel_arclength,
'target_arclength': target_arclength
}
navigation_states = np.array([
self._last_navi_state_dict[state]
for state in Vessel.NAVIGATION_FEATURES
])
# Deciding if vessel has reached the goal
goal_distance = linalg.norm(path.end - self.position)
reached_goal = goal_distance <= self.config[
"min_goal_distance"] or progress >= self.config["min_path_progress"]
self._reached_goal = reached_goal
return navigation_states
def observe(self):
"""
Generates the observation of the environment.
Parameters
----------
action : np.array
[propeller_input, rudder_position].
Returns
-------
obs : np.array
[
surge velocity,
sway velocity,
yawrate,
heading error,
cross track error,
propeller_input,
rudder_position,
]
All observations are between -1 and 1.
"""
la_heading = self.path.get_direction(self.path_prog[-1] +
self.config["la_dist"])
heading_error_la = float(geom.princip(la_heading -
self.vessel.heading))
path_position = (
self.path(self.path_prog[-1] + self.config["la_dist"]) -
self.vessel.position)
target_heading = np.arctan2(path_position[1], path_position[0])
heading_error = float(
geom.princip(target_heading - self.vessel.heading))
path_direction = self.path.get_direction(self.path_prog[-1])
cross_track_error = geom.Rzyx(0, 0, -path_direction).dot(
np.hstack(
[self.path(self.path_prog[-1]) - self.vessel.position, 0]))[1]
obs = np.zeros((self.nstates + self.nsectors, ))
obs[0] = np.clip(self.vessel.velocity[0] / self.vessel.max_speed, -1,
1)
obs[1] = np.clip(self.vessel.velocity[1] / 0.26, -1, 1)
obs[2] = np.clip(self.vessel.yawrate / 0.55, -1, 1)
obs[3] = np.clip(heading_error_la / np.pi, -1, 1)
obs[4] = np.clip(heading_error / np.pi, -1, 1)
obs[5] = np.clip(cross_track_error / 50, -10000, 10000)
obst_range = self.config["obst_detection_range"]
for obst in self.obstacles:
distance_vec = geom.Rzyx(0, 0, -self.vessel.heading).dot(
np.hstack([obst.position - self.vessel.position, 0]))
dist = linalg.norm(distance_vec)
if dist < obst_range + obst.radius + self.vessel.radius:
ang = ((float(np.arctan2(distance_vec[1], distance_vec[0])) +
np.pi) / (2 * np.pi))
closeness = 1 - np.clip(
(dist - self.vessel.radius - obst.radius) / obst_range, 0,
1)
isector = (self.nstates + int(np.floor(ang * self.nsectors)))
if isector == self.nstates + self.nsectors:
isector = self.nstates
if obs[isector] < closeness:
obs[isector] = closeness
return obs