def forces_moments(self, state, inputs): # define states phi = state.item(6) theta = state.item(7) psi = state.item(8) R_bi = RotationVehicle2Body(phi, theta, psi).T # control inputs thrust = inputs.item(0) tau_x = inputs.item(1) tau_y = inputs.item(2) tau_z = inputs.item(3) # forces calculations v_thrust = np.array([[0, 0, -thrust]]).T v_gravity = np.array([[0, 0, QUAD.gravity * QUAD.mass]]).T f_total = R_bi @ v_thrust + v_gravity # moments calculations m_body = np.array([[tau_x, tau_y, tau_z]]).T m_total = R_bi @ m_body return np.array([[ f_total.item(0), f_total.item(1), f_total.item(2), m_total.item(0), m_total.item(1), m_total.item(2) ]]).T
def _update_velocity_data(self, wind=np.zeros((6, 1))): e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3])) Rvb = RotationVehicle2Body(phi, theta, psi) # calculate wind components self._wind = wind[0:3] wind_combined = Rvb @ wind[0:3] + wind[3:6] # compute relative airspeed components self._ur = self._state.item(3) - wind_combined.item(0) self._vr = self._state.item(4) - wind_combined.item(1) self._wr = self._state.item(5) - wind_combined.item(2) self.Vg = Rvb.transpose() @ self._state[3:6] # In vehicle frame self._Vg = np.linalg.norm(self.Vg) # compute airspeed self._Va = np.sqrt(self._ur**2 + self._vr**2 + self._wr**2) # compute angle of attack self._alpha = np.arctan2(self._wr, self._ur) # compute sideslip angle self._beta = np.arcsin(self._vr / self._Va)
def get_target_depths(self): R_ib = RotationVehicle2Body(self.orientation.item(0), self.orientation.item(1), self.orientation.item(2)) R_bc = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) R_ic = R_bc @ R_ib p_t = R_ic @ (self.targets.get_targets() - self.position.reshape(-1, 1)) return p_t[2, :]
def get_target_pixels(self): R_ib = RotationVehicle2Body(self.orientation.item(0), self.orientation.item(1), self.orientation.item(2)) R_bc = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) R_ic = R_bc @ R_ib p_t = self.targets.get_targets() - self.position.reshape(-1, 1) e_targets = R_ic @ p_t for i in range(len(p_t[0])): e_targets[:, i] = self.f / e_targets[ 2, i] * e_targets[:, i] * SENSOR.pixels_per_meter return e_targets[0:2, :] + np.array([[SENSOR.pixel_width / 2], [SENSOR.pixel_height / 2]])
def _update_velocity_data(self, wind=np.zeros((6, 1))): # compute airspeed e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) #print("EEEES=",e0,e1,e2,e3) phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3])) #print("angles=",phi,theta,psi) Rv2b = RotationVehicle2Body(phi, theta, psi) wind_result = np.matmul(Rv2b, wind[0:3]) + wind[3:6] self._wind = np.matmul(Rv2b, wind_result) uw = wind_result.item(0) vw = wind_result.item(1) ww = wind_result.item(2) ur = self._state[3] - uw vr = self._state[4] - vw wr = self._state[5] - ww ur = ur.item(0) vr = vr.item(0) wr = wr.item(0) self._Va = sqrt(ur**2 + vr**2 + wr**2) #print("update_velocities =",self._Va) # compute angle of attack self._alpha = atan2(wr, ur) # compute sideslip angle self._beta = np.arcsin(vr / self._Va) # Vg, chi, gamma Rb2v = RotationBody2Vehicle(phi, theta, psi) Vg_result = np.matmul(Rb2v, self._state[3:6]) Vg_n = Vg_result.item(0) Vg_e = Vg_result.item(1) Vg_d = Vg_result.item(2) self._Vg = sqrt(Vg_n**2 + Vg_e**2 + Vg_d**2) self.gamma = atan2(-Vg_d, sqrt(Vg_n**2 + Vg_e**2)) self.chi = atan2(Vg_e, Vg_n) # angle wrapping for commanded phi help if self.chi_prev > 0.75 * np.pi and self.chi < 0.0: self.chi += 2. * np.pi if self.chi_prev < -0.75 * np.pi and self.chi > 0.0: self.chi -= 2. * np.pi self.chi_prev = self.chi
def update(self, cmd, state, pts, pts_depth): f = SENSOR.f u1 = pts[0, 0] / SENSOR.pixels_per_meter v1 = pts[1, 0] / SENSOR.pixels_per_meter u1_e = u1 - CON.desired_targets[0, 0] / SENSOR.pixels_per_meter v1_e = v1 - CON.desired_targets[1, 0] / SENSOR.pixels_per_meter z1 = pts_depth.item(0) Jp1 = self.image_jacobian(u1, v1, z1, f) u2 = pts[0, 1] / SENSOR.pixels_per_meter v2 = pts[1, 1] / SENSOR.pixels_per_meter u2_e = u2 - CON.desired_targets[0, 1] / SENSOR.pixels_per_meter v2_e = v2 - CON.desired_targets[1, 1] / SENSOR.pixels_per_meter z2 = pts_depth.item(1) Jp2 = self.image_jacobian(u2, v2, z2, f) u3 = pts[0, 2] / SENSOR.pixels_per_meter v3 = pts[1, 2] / SENSOR.pixels_per_meter u3_e = (u3 - CON.desired_targets[0, 2] / SENSOR.pixels_per_meter) v3_e = (v3 - CON.desired_targets[1, 2] / SENSOR.pixels_per_meter) z3 = pts_depth.item(2) Jp3 = self.image_jacobian(u3, v3, z3, f) J = np.vstack((Jp1, Jp2, Jp3)) e = np.array([[u1_e, v1_e, u2_e, v2_e, u3_e, v3_e]]).T V_c = -self.Kp * np.linalg.inv(J) @ e R_cb = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]).T R_bi = RotationVehicle2Body(state.phi, state.theta, state.psi).T v_i = R_bi @ R_cb @ V_c[0:3, 0] w_b = R_cb @ V_c[3:6, 0] * 0 cmd.u = v_i.item(0) cmd.v = v_i.item(1) cmd.w = v_i.item(2) cmd.p = w_b.item(0) cmd.q = w_b.item(1) cmd.r = w_b.item(2) x = np.array([[ state.u - v_i.item(0), state.v - v_i.item(1), state.w - v_i.item(2), state.phi, state.theta, state.psi, state.p - w_b.item(0), state.q - w_b.item(1), state.r - w_b.item(2) ]]).T # using beta as an estimate for v u_tilde = self.control.update(x) u = u_tilde + self.trim_input.reshape(-1, 1) u_sat = self._saturate(u) self.inputs = u_sat.reshape(-1, 1) self.commanded_state = cmd return self.inputs, self.commanded_state
def _update_velocity_data(self): Rvb = RotationVehicle2Body(0.0, 0.0, self._state.item(6)) self.Vg = Rvb.transpose() @ self._state[3:6] # In vehicle frame self._Vg = np.linalg.norm(self.Vg)