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 _get_parametric_params(self): diff = np.diff(self.waypoints, axis=0) for i in range(self.nwaypoints-1): derivative = diff[i] / self.segment_lengths[i] alpha = np.arctan2(derivative[1], derivative[0]) beta = np.arctan2(-derivative[2], np.sqrt(derivative[0]**2 + derivative[1]**2)) self.azimuth_angles = np.append(self.azimuth_angles, geom.ssa(alpha)) self.elevation_angles = np.append(self.elevation_angles, geom.ssa(beta))
def _sim(self, nu_c): #self.state += self.state_dot(nu_c)*self.step_size w, q = odesolver45(self.state_dot, self.state, self.step_size, nu_c) #self.state = q self.state = w self.state[3] = geom.ssa(self.state[3]) self.state[4] = geom.ssa(self.state[4]) self.state[5] = geom.ssa(self.state[5]) self.position_dot = self.state_dot(self.state, nu_c)[0:3]
def roll(self): """ Returns the roll of the AUV wrt NED. """ return geom.ssa(self.state[3])
def pitch(self): """ Returns the pitch of the AUV wrt NED. """ return geom.ssa(self.state[4])
def heading(self): """ Returns the heading of the AUV wrt true north. """ return geom.ssa(self.state[5])