コード例 #1
0
ファイル: legacy_6dof.py プロジェクト: poine/pat
    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)
コード例 #2
0
ファイル: frames.py プロジェクト: poine/pat
    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
コード例 #3
0
    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')
コード例 #4
0
ファイル: glider_trim_node.py プロジェクト: poine/pat
 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()]
コード例 #5
0
ファイル: legacy_6dof.py プロジェクト: poine/pat
    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
コード例 #6
0
ファイル: guidance.py プロジェクト: poine/pat
 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
コード例 #7
0
ファイル: legacy_6dof.py プロジェクト: poine/pat
 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)))
コード例 #8
0
ファイル: frames.py プロジェクト: poine/pat
 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
コード例 #9
0
ファイル: frames.py プロジェクト: poine/pat
 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
コード例 #10
0
ファイル: legacy_6dof.py プロジェクト: poine/pat
        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)
コード例 #11
0
ファイル: legacy_6dof.py プロジェクト: poine/pat
 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]
コード例 #12
0
ファイル: frames.py プロジェクト: poine/pat
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)
コード例 #13
0
ファイル: frames.py プロジェクト: poine/pat
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)
コード例 #14
0
ファイル: frames.py プロジェクト: poine/pat
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)
コード例 #15
0
ファイル: frames.py プロジェクト: poine/pat
def vel_world_to_aero_eul(ivel_world, eul, wind_ned):
    '''
    '''
    world_to_body_R = p3_alg.rmat_of_euler(eul)
    return vel_world_to_aero_R(ivel_world, world_to_body_R, wind_ned)