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 dyn(self, X, t, U, atm): Ueng = U[self.P.u_slice_eng()] # engines part of input vector Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector X_pos = X[self.sv_slice_pos] # X_vel = X[self.sv_slice_vel] # X_quat = X[self.sv_slice_quat] # quaternion X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities earth_to_body_R = p3_alg.rmat_of_quat(X_quat) wind_ned = atm.get_wind_ned(X_pos, t) if atm is not None else [0, 0, 0] va, alpha, beta = p3_fr.vel_world_to_aero_quat(X_vel, X_quat, wind_ned) h = -X[self.sv_z] rho = p3_atm.get_rho(h) Pdyn = 0.5 * rho * va**2 f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P) f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g]) forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P) moments_body = m_aero_body + m_eng_body _U = np.concatenate((forces_body, moments_body)) return p3_fr.SixDOFEuclidianQuat.cont_dyn(X, t, _U, self.P)
def dyn(self, X, t, U, wind_ned=[0, 0, 0]): Ueng = U[self.P.u_slice_eng()] # engines part of input vector Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector X_pos = X[self.sv_slice_pos] # ned pos X_avel = va, alpha, beta = X[ self.sv_slice_vaero] # airvel, alpha, beta X_euler = X[self.sv_slice_eul] # euler angles X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities earth_to_body_R = p3_alg.rmat_of_euler(X_euler) rho = p3_atm.get_rho(-X[self.sv_z]) Pdyn = 0.5 * rho * va**2 # Forces f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) f_eng_body = p3_fw_aero.get_f_eng_body(-X[self.sv_z], va, Ueng, self.P) f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g]) forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body # Moments m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P) m_body = m_aero_body + m_eng_body return p3_fr.SixDOFAeroEuler.cont_dyn( X, t, np.concatenate((forces_body, m_body)), self.P, wind_ned)
def fit_va_and_phi(atm, dm, compute=False): savefile_name = '/home/poine/tmp/pat_glider_trims.npz' h = 0. if compute: phis = np.deg2rad(np.arange(-45, 46, 1.)) vas = np.arange(6, 20, 1.) vzs = np.zeros((len(vas), len(phis))) for iphi, phi in enumerate(phis): for iva, va in enumerate(vas): Xe, Ue = dm.foo_trim_aero_banked_cst_throttle(h=h, va=va, throttle=0., flaps=0., phi=phi, debug=True) vzs[iva, iphi] = Xe[p3_fr.SixDOFEuclidianEuler.sv_zd] np.savez(savefile_name, phis=phis, vas=vas, vzs=vzs) print('saved {}'.format(savefile_name)) else: _data = np.load(savefile_name) phis, vas, vzs = [_data[k] for k in ['phis', 'vas', 'vzs']] # fit rho = p3_atm.get_rho(h) K = 2.*dm.P.m*dm.P.g/rho/dm.P.Sref H,Y = [],[] for iphi, phi in enumerate(phis): for iva, va in enumerate(vas): CL = K/va**2 #H = np.array([[va**3/K, K/va] for va in vas]) H.append([va/CL, va*CL/np.cos(phi)**2]) Y.append(vzs[iva, iphi]) H=np.array(H) Y=np.array(Y) CD0, B = np.dot(np.linalg.pinv(H), Y) #pdb.set_trace() print('K {} CD0 {} B {}'.format(K, CD0, B)) plt.subplot(1,2,1) for iva, va in enumerate(vas): plt.plot(np.rad2deg(phis), vzs[iva,:], label='va {}'.format(va)) p3_pu.decorate(plt.gca(), title='zd', xlab='phi (deg)', ylab='m/s', legend=True) plt.subplot(1,2,2) for iphi, phi in enumerate(phis): plt.plot(vas, vzs[:,iphi], label='phi {}'.format(np.rad2deg(phi))) p3_pu.decorate(plt.gca(), title='zd', xlab='va (m/s)', ylab='m/s', legend=True) vzs2 = np.zeros((len(vas), len(phis))) for iphi, phi in enumerate(phis): for iva, va in enumerate(vas): CL = K/va**2 vzs2[iva, iphi] = va*(CD0/CL+B*CL/np.cos(phi)**2) fig = plt.figure() ax = fig.gca(projection='3d') _phis, _vas = np.meshgrid(phis, vas) surf = ax.plot_surface(_vas, np.rad2deg(phis), vzs) surf2 = ax.plot_surface(_vas, np.rad2deg(phis), vzs2) ax.set_xlabel('va (m/s)') ax.set_ylabel('phi (deg)') ax.set_zlabel('vz (m/s)') plt.show()
def trim(self, args={}, UengFun=Ueng_of_throttle, UsfcFun=Usfc_of_elevator, debug=False): #Xe = np.zeros(self.sv_size); Xe[self.sv_qi] = 1.; Xe[self.sv_xd] = 15. #Ue = np.zeros(self.iv_size) va, gamma, h = args.get('va', self.P.Vref), args.get('gamma', 0.), args.get('h', 0.) if debug: print("searching for constant path trajectory with") print(" h {:f} m".format(h)) print(" va {:f} m/s".format(va)) print(" gamma {:f} deg".format(np.rad2deg(gamma))) LOG.info(" Running trim for va {} gamma {} h {}".format(va, gamma, h)) rho = patm.get_rho(h) Pdyn, beta, omega = 0.5 * rho * va**2, 0, [0, 0, 0] def err_func(args): ele, throttle, alpha = args Ueng, Usfc = UengFun(throttle, self.P), UsfcFun(ele, self.P) eulers = [0, gamma + alpha, 0] Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, eulers, omega, Pdyn, Ueng, Usfc, self.P) f_weight_body = np.dot(pfw.earth_to_body(eulers), [0., 0., self.P.m * self.P.g]) Fb = Fb + f_weight_body Mb = get_moments_body(alpha, beta, eulers, omega, Pdyn, f_eng_body, Usfc, self.P) return [Fb[0], Fb[2], Mb[1]] sol = scipy.optimize.root(err_func, [0., 0.5, 0.], method='hybr') ele, throttle, alpha = sol.x #print sol LOG.info( " elevator {:.1f} deg throttle {:.1f} % alpha {:.1f} deg".format( np.rad2deg(ele), 100 * throttle, np.rad2deg(alpha))) if debug: print("""result: throttle : {:f} % elevator : {:f} deg angle of attack : {:f} deg""".format(100. * throttle, np.rad2deg(ele), np.rad2deg(alpha))) #Xe = [0, 0, -h, va*math.cos(alpha), 0, va*math.sin(alpha), 0, gamma+alpha, 0, 0, 0, 0] q = pal.quat_of_euler([0, gamma + alpha, 0]) Xe = [ 0, 0, -h, va * math.cos(gamma), 0, va * math.sin(gamma), q[0], q[1], q[2], q[3], 0, 0, 0 ] Ue = UengFun(throttle, self.P) + UsfcFun(ele, self.P) return Xe, Ue
def trim_aero_cst_throttle(self, h=0., va=12., throttle=0., flaps=0., report=True, debug=False, atm=None): if report: print("searching (aero) for constant throttle trajectory with") print(" h {:.1f} m".format(h)) print(" va {:.2f} m/s".format(va)) print(" throttle {:.1f} %".format(100 * throttle)) print(" flap {:.1f} deg".format(np.rad2deg(flaps))) beta = 0. rho = p3_atm.get_rho(h) Pdyn = 0.5 * rho * va**2 X_rvel_body = [0, 0, 0] def err_func(args): gamma, elevator, alpha = args Ueng, Usfc = [throttle], [0, elevator, 0, flaps] f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P) eul = phi, theta, psi = [0, gamma + alpha, 0] earth_to_body_R = p3_alg.rmat_of_euler(eul) f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g]) forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P) m_body = m_aero_body + m_eng_body return np.linalg.norm(np.concatenate((forces_body, m_body))) p0 = [0., np.deg2rad(-2.), np.deg2rad(1.)] gamma_e, ele_e, alpha_e = scipy.optimize.fmin_powell(err_func, p0, disp=debug, ftol=1e-9) if report: print("""result: gamma : {:.2f} deg elevator : {:.2f} deg angle of attack : {:.2f} deg""".format(np.rad2deg(gamma_e), np.rad2deg(ele_e), np.rad2deg(alpha_e))) return gamma_e, ele_e, alpha_e
def get_f_eng_body(h, va, U, P): """ return propulsion forces expressed in body frame """ rho = p3_atm.get_rho(h) thrusts = [ thrust_of_throttle(U[i], P.fmaxs[i], rho, P.rhois[i], P.nrhos[i], va, P.Vis[i], P.nVs[i]) for i in range(P.eng_nb) ] f_engines_body = np.array([ np.dot(P.eng_to_body[i], np.array([thrusts[i], 0., 0.])) for i in range(P.eng_nb) ]) return f_engines_body
def cont_dyn(self, X, t, U): Ueng, Usfc = U[:self.P.eng_nb], U[ self.P. eng_nb:] # engines and control surfaces parts of input vector rho = patm.get_rho(-X[self.sv_z]) Pdyn = 0.5 * rho * X[self.sv_v]**2 (va, alpha, beta), euler, omega = X[self.sv_slice_vaero], X[ self.sv_slice_eul], X[self.sv_slice_rvel] 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) #pdb.set_trace() #print Usfc, Mb return pat_dyn.SolidDM3.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def dyn(self, X, t, U, wind_ned=[0, 0, 0]): Ueng = U[self.P.u_slice_eng()] # engines part of input vector Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector X_pos = X[self.sv_slice_pos] # X_vel = X[self.sv_slice_vel] # X_euler = X[self.sv_slice_eul] # euler angles X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities earth_to_body_R = p3_alg.rmat_of_euler(X_euler) va, alpha, beta = p3_fr.vel_world_to_aero_eul(X_vel, X_euler, wind_ned) h = -X[self.sv_z] rho = p3_atm.get_rho(h) Pdyn = 0.5 * rho * va**2 f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P) f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g]) forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P) moments_body = m_aero_body + m_eng_body if 1: _U = np.concatenate((forces_body, moments_body)) return p3_fr.SixDOFEuclidianEuler.cont_dyn(X, t, _U, self.P) else: Xdot = np.zeros(self.sv_size) # Position kinematics Xdot[self.sv_slice_pos] = X[self.sv_slice_vel] # Newton for forces in world frame forces_world = np.dot(earth_to_body_R.T, forces_body) Xdot[self.sv_slice_vel] = 1. / self.P.m * forces_world # Orientation kinematics Xdot[self.sv_slice_eul] = p3_alg.euler_derivatives( X_euler, X_rvel_body) # Newton for moments in body frame raccel_body = np.dot( self.P.invI, moments_body - np.cross(X_rvel_body, np.dot(self.P.I, X_rvel_body))) Xdot[self.sv_slice_rvel] = raccel_body return Xdot
def test0(): h = 0 aero_vel = va, alpha, beta = [10, 0, 0] euler, rvel = [0., 0, 0], [0.1, 0, 0] Usfc = [0, 0, 0, 0] Ueng = [0.1] p_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml') P = p3_fwparam.Param(p_filename) rho = p3_atm.get_rho(h) Pdyn = 0.5*rho*va**2 X = np.concatenate(([0, 0, -h], aero_vel, euler, rvel)) print('f_eng new: {}'.format(p3_fw_aero.get_f_eng_body(h, va, Ueng, P))) print('f_eng old: {}'.format(p1_fw_dyn.get_f_eng_body(X, Ueng, P))) print('new: {}'.format(p1_fw_dyn.get_m_aero_body2(va, alpha, beta, rvel, Usfc, P, Pdyn))) print('old {}'.format(p1_fw_dyn.get_m_aero_body(X, Usfc, P, Pdyn)))
def trim(self, args={}, UengFun=Ueng_of_throttle, UsfcFun=Usfc_of_elevator, debug=False): va, gamma, h = args.get('va', self.P.Vref), args.get('gamma', 0), args.get('h', 0) if debug: print("searching for constant path trajectory with") print(" h {:f} m".format(h)) print(" va {:f} m/s".format(va)) print(" gamma {:f} deg".format(np.rad2deg(gamma))) rho = patm.get_rho(h) Pdyn, beta, omega = 0.5 * rho * va**2, 0, [0, 0, 0] def err_func(args): ele, throttle, alpha = args Ueng, Usfc = UengFun(throttle, self.P), UsfcFun(ele, self.P) eulers = [0, gamma + alpha, 0] Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, eulers, omega, Pdyn, Ueng, Usfc, self.P) f_weight_body = np.dot(pfw.earth_to_body(eulers), [0., 0., self.P.m * self.P.g]) Fb = Fb + f_weight_body Mb = get_moments_body(alpha, beta, eulers, omega, Pdyn, f_eng_body, Usfc, self.P) return [Fb[0], Fb[2], Mb[1]] sol = scipy.optimize.root(err_func, [0., 0.5, 0.], method='hybr') ele, throttle, alpha = sol.x if debug: print("""result: throttle : {:f} % elevator : {:f} deg angle of attack : {:f} deg""".format(100. * throttle, np.rad2deg(ele), np.rad2deg(alpha))) Ue = np.zeros(self.P.input_nb) Ue[:self.P.eng_nb] = UengFun(throttle, self.P) Ue[self.P.eng_nb:self.P.input_nb] = UsfcFun(ele, self.P) Xe = [0., 0., -h, va, alpha, beta, 0., gamma + alpha, 0., 0., 0., 0.] return Xe, Ue
def fit_va(atm, dm, h=0): vas, vzs = np.arange(6, 20, 1.), [] # compute simulator trims for va in vas: Xae_e, Uae_e = dm.trim({'h':h, 'va':va, 'throttle':0}, report=False, debug=False) vzs.append(-Xae_e[p3_fr.SixDOFEuclidianEuler.sv_zd]) # fit polynomial rho = p3_atm.get_rho(h) K = 2.*dm.P.m*dm.P.g/rho/dm.P.Sref H = np.array([[va**3/K, K/va] for va in vas]) CD0, B = np.dot(np.linalg.pinv(H), vzs) print(f'K {K} CD0 {CD0} B {B}') # display simulator points and polynomial model vas2 = np.arange(6, 20, 0.1) vzs2 = [va**3/K*CD0+K/va*B for va in vas2] plt.plot(vas, vzs, '*') plt.plot(vas2, vzs2) p3_pu.decorate(plt.gca(), xlab='va (m/s)', ylab='vz (m/s)') plt.show()
def trim_aero_banked_cst_throttle(self, h=0., va=12., throttle=0., flaps=0., phi=0., report=True, debug=False, atm=None): beta = 0. rho = p3_atm.get_rho(h) Pdyn = 0.5 * rho * va**2 psi, psid = 0., self.P.g * np.tan(phi) / va def err_func(args): theta, elevator, aileron, rudder, alpha = args Ueng, Usfc = [throttle], [aileron, elevator, rudder] if self.P.sfc_nb >= 4: Usfc.append(flaps) X_rvel_body = p, q, r = 0., np.sin(phi) * np.cos( theta) * psid, np.cos(phi) * np.cos(theta) * psid #X_rvel_body = p, q, r = 0, np.sin(phi)*np.cos(theta)*va/R, np.cos(phi)*np.cos(theta)*va/R wind_ned = [0, 0, 0] earth_to_body_R = p3_alg.rmat_of_euler([phi, theta, psi]) ivel_b = p3_fr.vel_aero_to_body_R([va, alpha, beta], earth_to_body_R, wind_ned) f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P) #f_body = f_aero_body + np.sum(f_eng_body, axis=0) #f_ned = np.dot(earth_to_body_R.T, f_body).squeeze() #f_ned[2] += self.P.m*self.P.g #f_ned[1] -= self.P.m*va*psid f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g]) f_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body uvwd = 1 / self.P.m * f_body - np.cross(X_rvel_body, ivel_b) m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body, Usfc, self.P, Pdyn) m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P) m_body = m_aero_body + m_eng_body #pdb.set_trace() #pqrd = np.dot(self.P.invI, m_body - np.cross(X_rvel_body, np.dot(self.P.I, X_rvel_body))) pqrd = m_body - np.cross(X_rvel_body, np.dot( self.P.I, X_rvel_body)) foo = np.concatenate((uvwd, pqrd)) #return np.linalg.norm(np.concatenate((f_ned, m_body))) #print foo return np.linalg.norm(foo) #+100.*abs(aileron) p0 = [0., np.deg2rad(-2.), np.deg2rad(-1.), np.deg2rad(2.), 0.] theta, elevator, aileron, rudder, alpha = scipy.optimize.fmin_powell( err_func, p0, disp=debug, ftol=1e-12) gamma = theta - alpha if report: print("""result: gamma : {:.2f} deg theta : {:.2f} deg elevator : {:.2f} deg aileron : {:.2f} deg rudder : {:.2f} deg angle of attack : {:.2f} deg""".format(np.rad2deg(gamma), np.rad2deg(theta), np.rad2deg(elevator), np.rad2deg(aileron), np.rad2deg(rudder), np.rad2deg(alpha))) return gamma, elevator, aileron, rudder, alpha