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)