def cont_dyn(self, X, t, U):
#Fb = np.array([0, 0, 0])
#Mb = np.array([0, 0, 0])
Ueng, Usfc = U[:self.P.eng_nb], U[
self.P.
eng_nb:] # engines and control surfaces parts of input vector
#LOG.info(" engine {} sfc {}".format(Ueng, Usfc))
euler = pal.euler_of_quat(X[self.sv_slice_quat])
omega = X[self.sv_slice_rvel]
R_b2w = pal.rmat_of_quat(X[pat_dyn.SolidFDM.sv_slice_quat])
#pdb.set_trace()
vi_e = X[self.sv_slice_vel]
w_e = [0, 0, 0]
va, alpha, beta = pfw.get_va_alpha_beta(
vi_e, w_e, euler) # fixme: make a fonction that takes vi_b
rho = patm.get_rho(-X[self.sv_z])
Pdyn = 0.5 * rho * va**2
Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, euler, omega,
Pdyn, Ueng, Usfc, self.P)
Mb = get_moments_body(alpha, beta, euler, omega, Pdyn, f_eng_body,
Usfc, self.P)
print(Usfc, Mb)
# try:
# return ps.s1_dyn(X, t, Fb, Mb, P.m, P.I, P.invI, P.inv_mamat)
# except AttributeError:
# return ps.s1_dyn(X, t, Fb, Mb, P.m, P.I, P.invI, None)
return pat_dyn.SolidFDM.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def plot(time, X, U=None, figure=None, window_title="Trajectory"):
figure = ppu.prepare_fig(figure, window_title, (20.48, 10.24))
eulers = np.array([pal.euler_of_quat(_q) for _q in X[:, sv_slice_quat]])
phi, theta, psi = eulers[:, 0], eulers[:, 1], eulers[:, 2]
plots = [
("$x$", "m", 0.5, X[:, sv_x]),
("$y$", "m", 0.5, X[:, sv_y]),
("$z$", "m", 0.5, X[:, sv_z]),
("$\dot{x}$", "m/s", 0.5, X[:, sv_xd]),
("$\dot{y}$", "m/s", 0.5, X[:, sv_yd]),
("$\dot{z}$", "m/s", 0.5, X[:, sv_zd]),
("$\phi$", "deg", 0.5, np.rad2deg(phi)),
("$\\theta$", "deg", 0.5, np.rad2deg(theta)),
("$\psi$", "deg", 0.5, np.rad2deg(psi)),
("$p$", "deg/s", 0.5, np.rad2deg(X[:, sv_p])),
("$q$", "deg/s", 0.5, np.rad2deg(X[:, sv_q])),
("$r$", "deg/s", 0.5, np.rad2deg(X[:, sv_r])),
]
if U is not None:
foo = np.empty((len(time)))
foo.fill(np.nan)
plots += [("$U$", "N", 0.1, foo)]
figure = ppu.plot_in_grid(time, plots, 3, figure, window_title)
if U is not None:
ax = plt.subplot(5, 3, 13)
for i, txt in enumerate(('fr', 'br', 'bl', 'fl')):
plt.plot(time, U[:, i], label=txt)
plt.legend()
return figure
pass
def to_six_dof_body_euler(cls, Xbq):
Xbe = np.zeros(SixDOFBodyEuler.sv_size)
Xbe[SixDOFEuclidianEuler.sv_slice_pos] = Xbq[cls.sv_slice_pos]
Xbe[SixDOFEuclidianEuler.sv_slice_vel] = Xbq[cls.sv_slice_vel]
Xbe[SixDOFEuclidianEuler.sv_slice_eul] = p3_alg.euler_of_quat(
Xbq[cls.sv_slice_quat])
Xbe[SixDOFEuclidianEuler.sv_slice_rvel] = Xbq[cls.sv_slice_rvel]
return Xbe
def to_six_dof_euclidian_euler(cls, Xeq):
Xee = np.zeros(SixDOFEuclidianEuler.sv_size)
Xee[SixDOFEuclidianEuler.sv_slice_pos] = Xeq[cls.sv_slice_pos]
Xee[SixDOFEuclidianEuler.sv_slice_vel] = Xeq[cls.sv_slice_vel]
Xee[SixDOFEuclidianEuler.sv_slice_eul] = p3_alg.euler_of_quat(
Xeq[cls.sv_slice_quat])
Xee[SixDOFEuclidianEuler.sv_slice_rvel] = Xeq[cls.sv_slice_rvel]
return Xee
def to_six_dof_aero_euler(cls, Xeq, wind_ned=[0, 0, 0]):
Xae = np.zeros(SixDOFAeroEuler.sv_size)
Xae[SixDOFAeroEuler.sv_slice_pos] = Xeq[cls.sv_slice_pos]
Xae[SixDOFAeroEuler.sv_slice_vaero] = vel_world_to_aero(
Xeq[cls.sv_slice_vel], Xae[SixDOFAeroEuler.sv_slice_eul], wind_ned)
Xae[SixDOFAeroEuler.sv_slice_eul] = p3_alg.euler_of_quat(
Xeq[cls.sv_slice_quat])
Xae[SixDOFAeroEuler.sv_slice_rvel] = Xeq[cls.sv_slice_rvel]
return Xae
def get(self, t):
Yc = self.traj.get(t)
Xrefq, Uref = self.df.state_and_cmd_of_flat_output(Yc, self.fdm.P)
Xref = np.concatenate(
(Xrefq[fdm.sv_slice_pos], Xrefq[fdm.sv_slice_vel],
pal.euler_of_quat(Xrefq[fdm.sv_slice_quat]),
Xrefq[fdm.sv_slice_rvel]))
#pdb.set_trace()
return Xref, Uref, Xrefq
def plot_sp(time, Yc):
plt.subplot(5, 3, 3)
plt.plot(time, Yc[:,0])
euler_c = np.array([pal.euler_of_quat(_sp[1:]) for _sp in Yc])
plt.subplot(5, 3, 7)
plt.plot(time, np.rad2deg(euler_c[:,0]))
plt.subplot(5, 3, 8)
plt.plot(time, np.rad2deg(euler_c[:,1]))
plt.subplot(5, 3, 9)
plt.plot(time, np.rad2deg(euler_c[:,2]))
def plot(self, time, Yc, U=None, figure=None, window_title="Trajectory"):
if figure is None: figure = plt.gcf()
plt.subplot(5, 3, 3)
plt.plot(time, Yc[:, 0], lw=2., label='setpoint')
euler_c = np.array([pal.euler_of_quat(_sp[1:]) for _sp in Yc])
plt.subplot(5, 3, 7)
plt.plot(time, np.rad2deg(euler_c[:, 0]), label='setpoint')
plt.subplot(5, 3, 8)
plt.plot(time, np.rad2deg(euler_c[:, 1]), label='setpoint')
plt.subplot(5, 3, 9)
plt.plot(time, np.rad2deg(euler_c[:, 2]), label='setpoint')
Uzpqr = np.array([np.dot(self.invH, Uk) for Uk in U])
ax = plt.subplot(5, 3, 14)
plt.plot(time, Uzpqr[:, 0], label='Uz')
ppu.decorate(ax, title='$U_z$', xlab='s', ylab='N', legend=True)
ax = plt.subplot(5, 3, 15)
plt.plot(time, Uzpqr[:, 1], label='Up')
plt.plot(time, Uzpqr[:, 2], label='Uq')
plt.plot(time, Uzpqr[:, 3], label='Ur')
ppu.decorate(ax, title='$U_{pqr}$', xlab='s', ylab='N', legend=True)
return figure
def outerloop(self, X, Xref, Uref):
omega = np.array([np.deg2rad(120), np.deg2rad(120), np.deg2rad(120)])
xi = np.array([0.77, 0.77, 0.77])
euler = pal.euler_of_quat(X[fdm.sv_slice_quat])
cph = np.cos(euler[pal.e_phi])
sph = np.sin(euler[pal.e_phi])
cth = np.cos(euler[pal.e_theta])
sth = np.sin(euler[pal.e_theta])
cps = np.cos(euler[pal.e_psi])
sps = np.sin(euler[pal.e_psi])
ut = Uref[i_ut]
inv_G = np.array(
[[
cph * sth * cps + sph * sps, cph * sth * sps - sph * cps,
cph * cth
],
[
-(sph * sth * cps - cph * sps) / ut,
-(sph * sth * sps + cph * cps) / ut, -sph * cth / ut
], [cth * cps / ut / cph, cth * sps / ut / cph, -sth / ut / cph]])
delta_p = X[fdm.sv_slice_pos] - Xref[
fdm.sv_slice_pos] # Warning Xref wrongly indexed
delta_v = X[fdm.sv_slice_vel] - Xref[fdm.sv_slice_vel]
dpsi = pal.norm_mpi_pi(euler[pal.e_psi] - Xref[r_psi])
P = self.fdm.P
F = np.array([
P.Cd * delta_v[0] + ut * (cph * sth * sps + sph * cps) * dpsi,
P.Cd * delta_v[1] + ut * (cph * sth * cps + sph * sps) * dpsi,
P.Cd * delta_v[2]
])
dyn = -2 * omega * xi * delta_v - omega**2 * delta_p
out = np.dot(inv_G, -dyn * P.m - F)
return out # delta_ut delta_phi delta_theta
def periodic(self):
now = rospy.Time.now()
#print('periodic at {}'.format(now.to_sec()))
t_sim = self.t0 + (now.to_sec() - self.t0)*self.time_factor
Yc = self.traj.get(t_sim)
Xc, Uc, Xdc = self.df.state_and_cmd_of_flat_output(Yc, self.fdm.P)
T_w2b = np.eye(4)
T_w2b[:3,:3] = pal.rmat_of_quat(Xc[fdm.sv_slice_quat]).T # that is freaking weird....
T_w2b[:3,3] = Yc[:3,0]
self.pose_pub.publish([T_w2b])
self.tf_pub.publish(now, T_w2b)
self.traj_pub.publish()
X1 = np.zeros(p1_fw_dyn.sv_size)
X1[p1_fw_dyn.sv_slice_pos] = Xc[fdm.sv_slice_pos]
X1[p1_fw_dyn.sv_slice_eul] = pal.euler_of_quat(Xc[fdm.sv_slice_quat])
U = self.guidance.get(t_sim, X1)
self.carrot_pub.publish(self.guidance.carrot)
#b2c_bfrd = np.dot(T_w2b[:3, :3].T, self.guidance.b2c_ned)
#print self.guidance.carrot
#b2c_bfrd = np.dot(np.linalg.inv(T_w2b), [self.guidance.carrot[0], self.guidance.carrot[1], self.guidance.carrot[2], 1])
b2c_bfrd = self.guidance.b2c_b
self.carrot_pub2.publish(b2c_bfrd)
