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 cont_dyn(cls, X, t, U, P, wind_ned=[0, 0, 0], add_weight=False): Fb, Mb = U[:3], U[3:] X_pos = X[cls.sv_slice_pos] # ned pos X_avel = va, alpha, beta = X[cls.sv_slice_vaero] # airvel, alpha, beta X_euler = X[cls.sv_slice_eul] # euler angles X_rvel_body = X[cls.sv_slice_rvel] # body rotational velocities earth_to_body_R = p3_alg.rmat_of_euler(X_euler) p_w, v_aero, e_w2b, om_b = X[cls.sv_slice_pos], X[ cls.sv_slice_vaero], X[cls.sv_slice_eul], X[cls.sv_slice_rvel] waccel_body = [0, 0, 0] # np.dot(earth_to_body, waccel_earth) ivel_world = vel_aero_to_world_euler(X_avel, X_euler, wind_ned) ivel_body = np.dot(earth_to_body_R, ivel_world) avel_aero = [X[cls.sv_va], 0., 0.] _R_aero_to_body = R_aero_to_body(alpha, beta) avel_body = np.dot(_R_aero_to_body, avel_aero) Xd = np.zeros(cls.sv_size) # Translational kinematics Xd[cls.sv_slice_pos] = ivel_world # Translational dynamics iaccel_body = 1. / P.m * Fb - np.cross(X_rvel_body, ivel_body) aaccel_body = iaccel_body - waccel_body Xd[cls.sv_va] = np.inner(avel_body, aaccel_body) / X[cls.sv_va] (avx, avy, avz), (aax, aay, aaz) = avel_body, aaccel_body Xd[cls.sv_alpha] = (avx * aaz - avz * aax) / (avx**2 + avz**2) Xd[cls.sv_beta] = (X[cls.sv_va] * aay - avy * Xd[cls.sv_va] ) / X[cls.sv_va] / math.sqrt(avx**2 + aaz**2) # Euler angles kinematics Xd[cls.sv_slice_eul] = p3_alg.euler_derivatives(X_euler, X_rvel_body) # Rotational dynamics Xd[cls.sv_slice_rvel] = np.dot( P.invI, Mb - np.cross(X_rvel_body, np.dot(P.I, X_rvel_body))) return Xd
def __init__(self): p3_rpu.PeriodicNode.__init__(self, 'ros_pat_publish_transform') pos_ned = [0, 0, -1] self.T_w2b = np.eye(4) dm_ae = p1_fw_dyn.DynamicModel() Xe, Ue = dm_ae.trim({ 'h': 0, 'va': 8, 'gamma': 0 }, report=True, debug=True) va, alpha, beta = Xe[p3_fr.SixDOFAeroEuler.sv_slice_vaero] #va, alpha, beta = 10, np.deg2rad(3), np.deg2rad(0) phi, theta, psi = Xe[p3_fr.SixDOFAeroEuler.sv_slice_eul] #phi, theta, psi = np.deg2rad(0), np.deg2rad(20), np.deg2rad(0) p3_u.set_rot(self.T_w2b, p3_al.rmat_of_euler([phi, theta, psi])) self.T_b2w = np.linalg.inv(self.T_w2b) p3_u.set_trans(self.T_b2w, pos_ned) self.T_a2b = np.eye(4) p3_u.set_rot(self.T_a2b, p3_fr.R_aero_to_body(alpha, beta)) self.T_b2a = self.T_a2b #np.linalg.inv(self.T_a2b) # FIXME self.tf_pub = p3_rpu.TransformPublisher() self.marker_pub = p3_rpu.PoseArrayPublisher(dae='glider.dae')
def _trim(self): self.Xe, self.Ue = self.dm.trim(self.trim_args, report=True, debug=False) self.T_w2b = np.eye(4) phi, theta, psi = self.Xe[p3_fr.SixDOFAeroEuler.sv_slice_eul] p3_u.set_rot(self.T_w2b, p3_al.rmat_of_euler([phi, theta, psi])) self.T_b2w = np.linalg.inv(self.T_w2b) self.T_a2b = np.eye(4) va, self.alpha, beta = self.Xe[p3_fr.SixDOFAeroEuler.sv_slice_vaero] self.gamma = theta - self.alpha p3_u.set_rot(self.T_a2b, p3_fr.R_aero_to_body(self.alpha, beta)) self.dail, self.dele, self.dflap = 0, self.Ue[self.dm.iv_de()], self.Ue[self.dm.iv_df()]
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 _compute_roll_setpoint(self, t, X, traj): self.b2c_ned = self.carrot - self.pos R_w2b = p3_alg.rmat_of_euler(X[p3_fr.SixDOFAeroEuler.sv_slice_eul]) self.b2c_b = np.dot(R_w2b, self.b2c_ned) phi, theta = X[p3_fr.SixDOFAeroEuler.sv_phi], X[ p3_fr.SixDOFAeroEuler.sv_theta] if 0: self.R = (np.linalg.norm(self.b2c_b)**2) / (2 * self.b2c_b[1]) # R.g.tan(phi) = v^2 phi_sp = np.arctan(v**2 / self.R / 9.81) else: self.R = 0 err_psi = X[p3_fr.SixDOFAeroEuler.sv_psi] - np.arctan2( self.b2c_ned[1], self.b2c_ned[0]) err_psi = p3_u.norm_mpi_pi(err_psi) phi_sp = -0.75 * err_psi self.phi_sp = np.clip(phi_sp, -self.max_phi, self.max_phi) return phi_sp
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)))
def cont_dyn(cls, X, t, U, P, add_weight=False): Fb, Mb = U[:3], U[3:] Xd = np.zeros(cls.sv_size) p_w, ivel_b, e_w2b, rvel_b = X[cls.sv_slice_pos], X[ cls.sv_slice_vel], X[cls.sv_slice_eul], X[cls.sv_slice_rvel] R_b2w = p3_alg.rmat_of_euler(e_w2b).T # Translational kinematics Xd[cls.sv_slice_pos] = vel_body_to_world_R(ivel_b, R_b2w) # Newton for forces # FIXME why 2*np.cross? Xd[cls.sv_slice_vel] = 1 / P.m * Fb - 2. * np.cross( rvel_b, ivel_b) ### CHECK with silvia # do we add weight or not??? #if add_weight: Xd[cls.sv_slice_vel] += 1./P.m*(np.array([0, 0, P.m*P.g])) # Rotational kinematics Xd[cls.sv_slice_eul] = p3_alg.euler_derivatives(e_w2b, rvel_b) # Newton for moments Xd[cls.sv_slice_rvel] = np.dot( P.invI, Mb - np.cross(rvel_b, np.dot(P.I, rvel_b))) return Xd
def cont_dyn(cls, X, t, U, P, add_weight=False): Fb, Mb = U[:3], U[3:] Xd = np.zeros(cls.sv_size) p_w, v_w, e_w2b, om_b = X[cls.sv_slice_pos], X[cls.sv_slice_vel], X[ cls.sv_slice_eul], X[cls.sv_slice_rvel] # Translational kinematics Xd[cls.sv_slice_pos] = v_w # Newton for forces R_b2w = p3_alg.rmat_of_euler(e_w2b).T Xd[cls.sv_slice_vel] = 1. / P.m * (np.dot(R_b2w, Fb)) # do we add weight or not??? if add_weight: Xd[cls.sv_slice_vel] += 1. / P.m * (np.array([0, 0, P.m * P.g])) # Rotational kinematics Xd[cls.sv_slice_eul] = p3_alg.euler_derivatives(e_w2b, om_b) # Newton for moments Xd[cls.sv_slice_rvel] = np.dot(P.invI, Mb - np.cross(om_b, np.dot(P.I, om_b))) return Xd
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)
def _update_byproducts(self): self.T_w2b[:3, :3] = p3_alg.rmat_of_euler(self.X[self.sv_slice_eul]).T self.T_w2b[:3, 3] = self.X[self.sv_slice_pos]
def vel_body_to_world_euler(ivel_body, eul): body_to_world_R = p3_alg.rmat_of_euler(eul).T return vel_body_to_world_R(ivel_body, body_to_world_R)
def vel_world_to_body_eul(ivel_world, eul): world_to_body_R = p3_alg.rmat_of_euler(eul) return vel_world_to_body_R(ivel_world, world_to_body_R)
def vel_aero_to_world_euler(avel_aero, eul, wind_ned): ''' ''' body_to_world_R = p3_alg.rmat_of_euler(eul).T return vel_aero_to_world_R(avel_aero, body_to_world_R, wind_ned)