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