示例#1
0
def pendulumModel(nSteps=None):
    dae = Dae()
    dae.addZ( [ "tau"
              ] )
    dae.addX( [ "x"
              , "z"
              , "dx"
              , "dz"
              ] )
    dae.addU( [ "torque"
              ] )
    dae.addP( [ "m"
              ] )

    r = 0.3

    ddx = dae.ddt('dx')
    ddz = dae.ddt('dz')

    ode = [ dae['dx'] - dae.ddt('x')
          , dae['dz'] - dae.ddt('z')
#          , dae['ddx'] - dae.ddt('dx')
#          , dae['ddz'] - dae.ddt('dz')
          ]

    fx =  dae['torque']*dae['z']
    fz = -dae['torque']*dae['x'] + dae['m']*9.8

    alg = [ dae['m']*ddx + dae['x']*dae['tau'] - fx
          , dae['m']*ddz + dae['z']*dae['tau'] - fz
          , dae['x']*ddx + dae['z']*ddz + (dae['dx']*dae['dx'] + dae['dz']*dae['dz']) ]

    dae['c']    = dae['x']*dae['x'] + dae['z']*dae['z'] - r*r
    dae['cdot'] = dae['dx']*dae['x'] + dae['dz']*dae['z']

    dae.setResidual( [ode, alg] )

    return dae
示例#2
0
def carouselModel(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ("nu")
    dae.addX( [ "x"
              , "y"
              , "z"
              , "e11"
              , "e12"
              , "e13"
              , "e21"
              , "e22"
              , "e23"
              , "e31"
              , "e32"
              , "e33"
              , "dx"
              , "dy"
              , "dz"
              , "w_bn_b_x"
              , "w_bn_b_y"
              , "w_bn_b_z"
              , "ddelta"
              , "r"
              , "dr"
              , "aileron"
              , "elevator"
              , "motor_torque"
              , "ddr"
              ] )
    if 'cn_rudder' in conf:
        dae.addX('rudder')
        dae.addU('drudder')
    if 'cL_flaps' in conf:
        dae.addX('flaps')
        dae.addU('dflaps')
    if conf['delta_parameterization'] == 'linear':
        dae.addX('delta')
        dae['cos_delta'] = C.cos(dae['delta'])
        dae['sin_delta'] = C.sin(dae['delta'])
        dae_delta_residual = dae.ddt('delta') - dae['ddelta']

    elif conf['delta_parameterization'] == 'cos_sin':
        dae.addX("cos_delta")
        dae.addX("sin_delta")
        norm = dae['cos_delta']**2 + dae['sin_delta']**2

        if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
            pole_delta = 0.5    
        else:
            pole_delta = 0.0

        cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
        sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
        dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
                                       dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
    else:
        raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')

    dae.addU( [ "daileron"
              , "delevator"
              , "dmotor_torque"
              , 'dddr'
              ] )
    # add wind parameter if wind shear is in configuration
    if 'wind_model' in conf:
        if conf['wind_model']['name'] == 'hardcoded':
            dae['w0'] = conf['wind_model']['hardcoded_value']
        elif conf['wind_model']['name'] != 'random_walk':
            dae.addP( ['w0'] )

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in for plotting
    dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
    dae['aileron_deg'] = dae['aileron']*180/C.pi
    dae['elevator_deg'] = dae['elevator']*180/C.pi
    dae['daileron_deg_s'] = dae['daileron']*180/C.pi
    dae['delevator_deg_s'] = dae['delevator']*180/C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r']*dae['nu']

    # theoretical mechanical power
    dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']

    # carousel2 motor model from thesis data
    dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
    dae['torque'] = dae['tether_tension']*0.1
    dae['electrical_winch_power'] =  293.5816373499238 + \
                                     0.0003931623408*dae['rpm']*dae['rpm'] + \
                                     0.0665919381751*dae['torque']*dae['torque'] + \
                                     0.1078628659825*dae['rpm']*dae['torque']

    dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
                               [dae['e21'],dae['e22'],dae['e23']],
                               [dae['e31'],dae['e32'],dae['e33']]])
    
    dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
    dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
    dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi

    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
        RotPole = 0.5
    else:
        RotPole = 0.0
    Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )

    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
        dae_delta_residual,
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
#        dae.ddt('ddelta') - dae['dddelta'],
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('motor_torque') - dae['dmotor_torque'],
        dae.ddt('ddr') - dae['dddr']
        ])
    if 'rudder' in dae:
        ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
    if 'flaps' in dae:
        ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
        
    if 'useVirtualTorques' in conf:
        _v = conf[ 'useVirtualTorques' ]
        if isinstance(_v, str):
            _type = _v
            _which = True, True, True
        else:
            assert isinstance(_v, dict)
            _type = _v["type"]
            _which = _v["which"]
        
        if _type == 'random_walk':
            if _which[ 0 ]:
                ode = C.veccat([ode,
                                dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
            if _which[ 1 ]:
                ode = C.veccat([ode,
                                dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
            if _which[ 2 ]:
                ode = C.veccat([ode,
                                dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                ode = C.veccat([ode, dae.ddt('t1_disturbance')])
            if _which[ 1 ]:
                ode = C.veccat([ode, dae.ddt('t2_disturbance')])
            if _which[ 2 ]:
                ode = C.veccat([ode, dae.ddt('t3_disturbance')])
        
    if 'useVirtualForces' in conf:
        _v = conf[ 'useVirtualForces' ]
        if isinstance(_v, str):
            _type = _v
            _which = True, True, True
        else:
            assert isinstance(_v, dict)
            _type = _v["type"]
            _which = _v["which"]
        
        if _type is 'random_walk':

            if _which[ 0 ]:
                ode = C.veccat([ode,
                                dae.ddt('f1_disturbance') - dae['df1_disturbance']])
            if _which[ 1 ]:
                ode = C.veccat([ode,
                                dae.ddt('f2_disturbance') - dae['df2_disturbance']])
            if _which[ 2 ]:
                ode = C.veccat([ode,
                                dae.ddt('f3_disturbance') - dae['df3_disturbance']])
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                ode = C.veccat([ode, dae.ddt('f1_disturbance')])
            if _which[ 1 ]:
                ode = C.veccat([ode, dae.ddt('f2_disturbance')])
            if _which[ 2 ]:
                ode = C.veccat([ode, dae.ddt('f3_disturbance')])
        
    if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
        tau_wind = conf['wind_model']['tau_wind']
        
        ode = C.veccat([ode,
                        dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
                        , dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
                        , dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
                        ])

    if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
        cPole = 0.5
    else:
        cPole = 0.0
    rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']

    psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual([ode,alg])

    return dae
示例#3
0
# rawesome is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with rawesome.  If not, see <http://www.gnu.org/licenses/>.

import matplotlib.pyplot as plt

from rawe.dae import Dae
from rawe.collocation import Coll

if __name__ == "__main__":
    ######## make the Dae #######
    dae = Dae()

    [pos,vel] = dae.addX( ["pos","vel"] )
    dae.addX('abspos')
    force = dae.addU( "force" )
    
    # some extra outputs for the dae model
    dae['force_over_pos'] = force/pos

    # specify the ode residual
    mass = 1.0
    dae.setResidual([dae.ddt('pos') - vel,
                     dae.ddt('vel') - (force/mass - 3.0*pos - 0.2*vel)]
                    )

    ######## make the collocation scheme ########
示例#4
0
def carouselModel(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ("nu")
    dae.addX( [ "x"
              , "y"
              , "z"
              , "e11"
              , "e12"
              , "e13"
              , "e21"
              , "e22"
              , "e23"
              , "e31"
              , "e32"
              , "e33"
              , "dx"
              , "dy"
              , "dz"
              , "w_bn_b_x"
              , "w_bn_b_y"
              , "w_bn_b_z"
              , "ddelta"
              , "r"
              , "dr"
              , "aileron"
              , "elevator"
              , "motor_torque"
              , "ddr"
              ] )
    if 'cn_rudder' in conf:
        dae.addX('rudder')
        dae.addU('drudder')
    if 'cL_flaps' in conf:
        dae.addX('flaps')
        dae.addU('dflaps')
    if conf['delta_parameterization'] == 'linear':
        dae.addX('delta')
        dae['cos_delta'] = C.cos(dae['delta'])
        dae['sin_delta'] = C.sin(dae['delta'])
        dae_delta_residual = dae.ddt('delta') - dae['ddelta']

    elif conf['delta_parameterization'] == 'cos_sin':
        dae.addX("cos_delta")
        dae.addX("sin_delta")
        norm = dae['cos_delta']**2 + dae['sin_delta']**2

        if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
            pole_delta = 0.5    
        else:
            pole_delta = 0.0

        cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
        sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
        dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
                                       dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
    else:
        raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')

    if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True:
        dae.addP( [ "daileron",
                    "delevator",
                    "dmotor_torque",
                    'dddr' ] )
    else:
        dae.addU( [ "daileron",
                    "delevator",
                    "dmotor_torque",
                    'dddr' ] )
    
    # add wind parameter if wind shear is in configuration
    if 'wind_model' in conf:
        if conf['wind_model']['name'] == 'hardcoded':
            dae['w0'] = conf['wind_model']['hardcoded_value']
        elif conf['wind_model']['name'] != 'random_walk':
            dae.addP( ['w0'] )

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in for plotting
    dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
    dae['aileron_deg'] = dae['aileron']*180/C.pi
    dae['elevator_deg'] = dae['elevator']*180/C.pi
    dae['daileron_deg_s'] = dae['daileron']*180/C.pi
    dae['delevator_deg_s'] = dae['delevator']*180/C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r']*dae['nu']

    # theoretical mechanical power
    dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']

    # carousel2 motor model from thesis data
    dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
    dae['torque'] = dae['tether_tension']*0.1
    dae['electrical_winch_power'] =  293.5816373499238 + \
                                     0.0003931623408*dae['rpm']*dae['rpm'] + \
                                     0.0665919381751*dae['torque']*dae['torque'] + \
                                     0.1078628659825*dae['rpm']*dae['torque']

    dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
                               [dae['e21'],dae['e22'],dae['e23']],
                               [dae['e31'],dae['e32'],dae['e33']]])
    
    dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
    dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
    dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi

    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
        RotPole = 0.5
    else:
        RotPole = 0.0
    Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )

    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
        dae_delta_residual,
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
#        dae.ddt('ddelta') - dae['dddelta'],
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('motor_torque') - dae['dmotor_torque'],
        dae.ddt('ddr') - dae['dddr']
        ])
    if 'rudder' in dae:
        ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
    if 'flaps' in dae:
        ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
        
    if 'useVirtualTorques' in conf:
        _v = conf[ 'useVirtualTorques' ]
        if isinstance(_v, str):
            _type = _v
            _which = True, True, True
        else:
            assert isinstance(_v, dict)
            _type = _v["type"]
            _which = _v["which"]
        
        if _type == 'random_walk':
            if _which[ 0 ]:
                ode = C.veccat([ode,
                                dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
            if _which[ 1 ]:
                ode = C.veccat([ode,
                                dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
            if _which[ 2 ]:
                ode = C.veccat([ode,
                                dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                ode = C.veccat([ode, dae.ddt('t1_disturbance')])
            if _which[ 1 ]:
                ode = C.veccat([ode, dae.ddt('t2_disturbance')])
            if _which[ 2 ]:
                ode = C.veccat([ode, dae.ddt('t3_disturbance')])
        
    if 'useVirtualForces' in conf:
        _v = conf[ 'useVirtualForces' ]
        if isinstance(_v, str):
            _type = _v
            _which = True, True, True
        else:
            assert isinstance(_v, dict)
            _type = _v["type"]
            _which = _v["which"]
        
        if _type is 'random_walk':

            if _which[ 0 ]:
                ode = C.veccat([ode,
                                dae.ddt('f1_disturbance') - dae['df1_disturbance']])
            if _which[ 1 ]:
                ode = C.veccat([ode,
                                dae.ddt('f2_disturbance') - dae['df2_disturbance']])
            if _which[ 2 ]:
                ode = C.veccat([ode,
                                dae.ddt('f3_disturbance') - dae['df3_disturbance']])
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                ode = C.veccat([ode, dae.ddt('f1_disturbance')])
            if _which[ 1 ]:
                ode = C.veccat([ode, dae.ddt('f2_disturbance')])
            if _which[ 2 ]:
                ode = C.veccat([ode, dae.ddt('f3_disturbance')])
        
    if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
        tau_wind = conf['wind_model']['tau_wind']
        
        ode = C.veccat([ode,
                        dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
                        , dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
                        , dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
                        ])

    if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
        cPole = 0.5
    else:
        cPole = 0.0
    rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']

    psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual([ode,alg])

    return dae
示例#5
0
def pendulumModel(nSteps=None):
    dae = Dae()
    dae.addZ( [ "ddx"
              , "ddz"
              , "tau"
              ] )
    dae.addX( [ "x"
              , "z"
              , "dx"
              , "dz"
              ] )
    dae.addU( [ "torque"
              ] )
    dae.addP( [ "m"
              ] )

    r = 0.3
    
    ode = [ dae['dx'] - dae.ddt('x')
          , dae['dz'] - dae.ddt('z')
          , dae['ddx'] - dae.ddt('dx')
          , dae['ddz'] - dae.ddt('dz')
          ]

    fx =  dae['torque']*dae['z']
    fz = -dae['torque']*dae['x'] + dae['m']*9.8
    
    alg = [ dae['m']*dae['ddx'] + dae['x']*dae['tau'] - fx
          , dae['m']*dae['ddz'] + dae['z']*dae['tau'] - fz
          , dae['x']*dae['ddx'] + dae['z']*dae['ddz'] + (dae['dx']*dae['dx'] + dae['dz']*dae['dz']) ]

    dae['c']    = dae['x']*dae['x'] + dae['z']*dae['z'] - r*r
    dae['cdot'] = dae['dx']*dae['x'] + dae['dz']*dae['z']

    dae.setResidual( [ode,alg] )

    return dae
示例#6
0
import matplotlib.pyplot as plt

from rawe.dae import Dae
from rawe.collocation import Coll

if __name__ == "__main__":
    ######## make the Dae #######
    dae = Dae()

    [pos,vel,mass] = dae.addX( ["pos","vel","mass"] )
    thrust = dae.addU( "thrust" )
    
    # some extra outputs for the dae model
    dae['pos*vel'] = pos*vel
    dae['vel*vel'] = vel*vel

    # specify the ode residual
    dae.setResidual([dae.ddt('pos') - vel,
                     dae.ddt('vel') - thrust/mass,
                     dae.ddt('mass') - 0.1*thrust*thrust])

    ######## make the collocation scheme ########
    N = 100
    ocp = Coll(dae, nk=N, nicp=1, collPoly="RADAU", deg=4)

    endTime = 5
    ocp.setupCollocation( endTime )

    # bounds
    ocp.bound('thrust',(-1.3,0.9))
    ocp.bound('pos',(-10,10))
示例#7
0
def crosswind_model(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ('nu')
    dae.addX( ['r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z',
               'e11', 'e12', 'e13',
               'e21', 'e22', 'e23',
               'e31', 'e32', 'e33',
               'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
               'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z',
               'aileron', 'elevator', 'rudder', 'flaps', 'prop_drag'
           ])
    dae.addU( [ 'daileron'
              , 'delevator'
              , 'drudder'
              , 'dflaps'
              , 'dprop_drag'
              ] )
    dae.addP( ['r','w0'] )

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('v_bn_n_x')
    dae['ddy'] = dae.ddt('v_bn_n_y')
    dae['ddz'] = dae.ddt('v_bn_n_z')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in degrees for plotting
    dae['aileron_deg']     = dae['aileron']*180/C.pi
    dae['elevator_deg']    = dae['elevator']*180/C.pi
    dae['rudder_deg']      = dae['rudder']*180/C.pi
    dae['daileron_deg_s']  = dae['daileron']*180/C.pi
    dae['delevator_deg_s'] = dae['delevator']*180/C.pi
    dae['drudder_deg_s']   = dae['drudder']*180/C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r']*dae['nu']

    dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
                              C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
                              C.horzcat([dae['e31'], dae['e32'], dae['e33']])])

    # local wind
    dae['wind_at_altitude'] = get_wind(dae, conf)

    # wind velocity in NED
    dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])

    # body velocity in NED
    dae['v_bn_n'] = C.veccat( [ dae['v_bn_n_x'] , dae['v_bn_n_y'], dae['v_bn_n_z'] ] )

    # body velocity w.r.t. wind in NED
    v_bw_n =  dae['v_bn_n'] - dae['v_wn_n']

    # body velocity w.r.t. wind in body frame
    v_bw_b = C.mul( dae['R_n2b'], v_bw_n )

    # compute aerodynamic forces and moments
    (f1, f2, f3, t1, t2, t3) = \
        aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
                          dae['w_bn_b'],
                          (dae['e21'], dae['e22'], dae['e23']))
    dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed']
    dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
    f1 -= dae['prop_drag_vector'][0]
    f2 -= dae['prop_drag_vector'][1]
    f3 -= dae['prop_drag_vector'][2]

    # if we are running a homotopy,
    # add psudeo forces and moments as algebraic states
    if 'runHomotopy' in conf and conf['runHomotopy']:
        gamma_homotopy = dae.addP('gamma_homotopy')
        f1 = f1*gamma_homotopy + dae.addZ('f1_homotopy')*(1 - gamma_homotopy)
        f2 = f2*gamma_homotopy + dae.addZ('f2_homotopy')*(1 - gamma_homotopy)
        f3 = f3*gamma_homotopy + dae.addZ('f3_homotopy')*(1 - gamma_homotopy)
        t1 = t1*gamma_homotopy + dae.addZ('t1_homotopy')*(1 - gamma_homotopy)
        t2 = t2*gamma_homotopy + dae.addZ('t2_homotopy')*(1 - gamma_homotopy)
        t3 = t3*gamma_homotopy + dae.addZ('t3_homotopy')*(1 - gamma_homotopy)

    # derivative of dcm
    dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
    ddt_R_n2b = C.vertcat(\
        [C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
         C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
         C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])

    # get mass matrix, rhs
    (mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)

    # set up the residual
    ode = C.veccat([
        C.veccat([dae.ddt('r_n2b_n_'+name) - dae['v_bn_n_'+name] for name in ['x','y','z']]),
        C.veccat([ddt_R_n2b - dRexp]),
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('rudder') - dae['drudder'],
        dae.ddt('flaps') - dae['dflaps'],
        dae.ddt('prop_drag') - dae['dprop_drag']
        ])

    # acceleration for plotting
    ddx = dae['ddx']
    ddy = dae['ddy']
    ddz = dae['ddz']
    dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
    dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
    dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
    dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
       C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi

    # add local loyd's limit
    def addLoydsLimit():
        w = dae['wind_at_altitude']
        cL = dae['cL']
        cD = dae['cD'] + dae['cD_tether']
        rho = conf['rho']
        S = conf['sref']
        loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
        loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
        dae["loyds_limit"] = loyds
        dae["loyds_limit_exact"] = loyds2
    addLoydsLimit()

    psuedo_zvec = C.veccat([dae.ddt(name) for name in \
        ['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(mass_matrix, psuedo_zvec) - rhs
    dae.setResidual( [ode, alg] )

    return dae
示例#8
0
# rawesome is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with rawesome.  If not, see <http://www.gnu.org/licenses/>.

import matplotlib.pyplot as plt

from rawe.dae import Dae
from rawe.collocation import Coll

if __name__ == "__main__":
    ######## make the Dae #######
    dae = Dae()

    [pos,vel] = dae.addX( ["pos","vel"] )
    dae.addX('abspos')
    force = dae.addU( "force" )

    # some extra outputs for the dae model
    dae['force_over_pos'] = force/pos

    # specify the ode residual
    mass = 1.0
    dae.setResidual([dae.ddt('pos') - vel,
                     dae.ddt('vel') - (force/mass - 3.0*pos - 0.2*vel)]
                    )

    ######## make the collocation scheme ########
示例#9
0
def crosswind_model(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ('nu')
    dae.addX([
        'r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z', 'e11', 'e12', 'e13', 'e21',
        'e22', 'e23', 'e31', 'e32', 'e33', 'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
        'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z', 'aileron', 'elevator', 'rudder',
        'flaps', 'prop_drag'
    ])
    dae.addU(['daileron', 'delevator', 'drudder', 'dflaps', 'dprop_drag'])
    dae.addP(['r', 'w0'])

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('v_bn_n_x')
    dae['ddy'] = dae.ddt('v_bn_n_y')
    dae['ddz'] = dae.ddt('v_bn_n_z')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat(
        [dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in degrees for plotting
    dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
    dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
    dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
    dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
    dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
    dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r'] * dae['nu']

    dae['R_n2b'] = C.vertcat([
        C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
        C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
        C.horzcat([dae['e31'], dae['e32'], dae['e33']])
    ])

    # local wind
    dae['wind_at_altitude'] = get_wind(dae, conf)

    # wind velocity in NED
    dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])

    # body velocity in NED
    dae['v_bn_n'] = C.veccat(
        [dae['v_bn_n_x'], dae['v_bn_n_y'], dae['v_bn_n_z']])

    # body velocity w.r.t. wind in NED
    v_bw_n = dae['v_bn_n'] - dae['v_wn_n']

    # body velocity w.r.t. wind in body frame
    v_bw_b = C.mul(dae['R_n2b'], v_bw_n)

    # compute aerodynamic forces and moments
    (f1, f2, f3, t1, t2, t3) = \
        aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
                          dae['w_bn_b'],
                          (dae['e21'], dae['e22'], dae['e23']))
    dae['prop_drag_vector'] = dae['prop_drag'] * v_bw_n / dae['airspeed']
    dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
    f1 -= dae['prop_drag_vector'][0]
    f2 -= dae['prop_drag_vector'][1]
    f3 -= dae['prop_drag_vector'][2]

    # if we are running a homotopy,
    # add psudeo forces and moments as algebraic states
    if 'runHomotopy' in conf and conf['runHomotopy']:
        gamma_homotopy = dae.addP('gamma_homotopy')
        f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
                                                              gamma_homotopy)
        f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
                                                              gamma_homotopy)
        f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
                                                              gamma_homotopy)
        t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
                                                              gamma_homotopy)
        t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
                                                              gamma_homotopy)
        t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
                                                              gamma_homotopy)

    # derivative of dcm
    dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
    ddt_R_n2b = C.vertcat(\
        [C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
         C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
         C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])

    # get mass matrix, rhs
    (mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)

    # set up the residual
    ode = C.veccat([
        C.veccat([
            dae.ddt('r_n2b_n_' + name) - dae['v_bn_n_' + name]
            for name in ['x', 'y', 'z']
        ]),
        C.veccat([ddt_R_n2b - dRexp]),
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('rudder') - dae['drudder'],
        dae.ddt('flaps') - dae['dflaps'],
        dae.ddt('prop_drag') - dae['dprop_drag']
    ])

    # acceleration for plotting
    ddx = dae['ddx']
    ddy = dae['ddy']
    ddz = dae['ddz']
    dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
    dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
    dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
    dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
       C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi

    # add local loyd's limit
    def addLoydsLimit():
        w = dae['wind_at_altitude']
        cL = dae['cL']
        cD = dae['cD'] + dae['cD_tether']
        rho = conf['rho']
        S = conf['sref']
        loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
        loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
        dae["loyds_limit"] = loyds
        dae["loyds_limit_exact"] = loyds2

    addLoydsLimit()

    psuedo_zvec = C.veccat([dae.ddt(name) for name in \
        ['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(mass_matrix, psuedo_zvec) - rhs
    dae.setResidual([ode, alg])

    return dae
示例#10
0
def crosswindModel(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ("nu")
    dae.addX( [ "x"
              , "y"
              , "z"
              , "e11"
              , "e12"
              , "e13"
              , "e21"
              , "e22"
              , "e23"
              , "e31"
              , "e32"
              , "e33"
              , "dx"
              , "dy"
              , "dz"
              , "w_bn_b_x"
              , "w_bn_b_y"
              , "w_bn_b_z"
              , "r"
              , "dr"
              , 'ddr'
              , "aileron"
              , "elevator"
              , "rudder"
              , "flaps"
              ] )
    dae.addU( [ "daileron"
              , "delevator"
              , "drudder"
              , "dflaps"
              , 'dddr'
              ] )
    dae.addP( ['w0'] )

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in degrees for plotting
    dae['aileron_deg']     = dae['aileron']*180/C.pi
    dae['elevator_deg']    = dae['elevator']*180/C.pi
    dae['rudder_deg']      = dae['rudder']*180/C.pi
    dae['daileron_deg_s']  = dae['daileron']*180/C.pi
    dae['delevator_deg_s'] = dae['delevator']*180/C.pi
    dae['drudder_deg_s']   = dae['drudder']*180/C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r']*dae['nu']

    # theoretical mechanical power
    dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']

    # carousel2 motor model from thesis data
    dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
    dae['torque'] = dae['tether_tension']*0.1
    dae['electrical_winch_power'] =  293.5816373499238 + \
                                     0.0003931623408*dae['rpm']*dae['rpm'] + \
                                     0.0665919381751*dae['torque']*dae['torque'] + \
                                     0.1078628659825*dae['rpm']*dae['torque']

    dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
                              C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
                              C.horzcat([dae['e31'],dae['e32'],dae['e33']])])

    # get mass matrix, rhs
    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    # set up the residual
    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('ddr') - dae['dddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('rudder') - dae['drudder'],
        dae.ddt('flaps') - dae['dflaps']
        ])

    # acceleration for plotting
    ddx = dae['ddx']
    ddy = dae['ddy']
    ddz = dae['ddz']
    dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
    dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
    dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
    dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi

    # add local loyd's limit
    def addLoydsLimit():
        w = dae['wind_at_altitude']
        cL = dae['cL']
        cD = dae['cD'] + dae['cD_tether']
        rho = conf['rho']
        S = conf['sref']
        loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
        loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
        dae["loyds_limit"] = loyds
        dae["loyds_limit_exact"] = loyds2
        dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
        dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
    addLoydsLimit()

    psuedoZVec = C.veccat([dae.ddt(name) for name in \
                    ['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual( [ode, alg] )

    return dae
示例#11
0
def carouselModel(conf,nSteps=None,extraParams=[]):
    dae = Dae()
    for ep in extraParams:
        dae.addP(ep)
        
#    dae.addZ( [ "dddelta"
#              , "ddx"
#              , "ddy"
#              , "ddz"
#              , "dw1"
#              , "dw2"
#              , "dw3"
#              , "nu"
#              ] )
    dae.addZ("nu")
    dae.addX( [ "x"
              , "y"
              , "z"
              , "e11"
              , "e12"
              , "e13"
              , "e21"
              , "e22"
              , "e23"
              , "e31"
              , "e32"
              , "e33"
              , "dx"
              , "dy"
              , "dz"
              , "w1"
              , "w2"
              , "w3"
              , "ddelta"
              , "r"
              , "dr"
              , "aileron"
              , "elevator"
              ] )
    if conf['delta_parameterization'] == 'linear':
        dae.addX('delta')
        dae['cos_delta'] = C.cos(dae['delta'])
        dae['sin_delta'] = C.sin(dae['delta'])
        dae_delta_residual = dae.ddt('delta') - dae['ddelta'],

    elif conf['delta_parameterization'] == 'cos_sin':
        dae.addX("cos_delta")
        dae.addX("sin_delta")
        dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta']),
                                       dae.ddt('sin_delta') - dae['cos_delta']*dae['ddelta']])
    else:
        raise ValueErorr('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')

    dae.addU( [ "daileron"
              , "delevator"
              , "motor_torque"
              , 'ddr'
              ] )
    # add wind parameter if wind shear is in configuration
    if 'z0' in conf and 'zt_roughness' in conf:
        dae.addP( ['w0'] )

    
    dae['RPM'] = dae['ddelta']*60/(2*C.pi)
    dae['aileron(deg)'] = dae['aileron']*180/C.pi
    dae['elevator(deg)'] = dae['elevator']*180/C.pi
    dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
    dae['delevator(deg/s)'] = dae['delevator']*180/C.pi
    
    dae['motor power'] = dae['motor_torque']*dae['ddelta']

    dae['tether tension'] = dae['r']*dae['nu']
    dae['winch power'] = -dae['tether tension']*dae['dr']
    
    dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
                            C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
                            C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
    
    # line angle
    dae['cos(line angle)'] = \
      (dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi

    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
        dae_delta_residual,
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
#        dae.ddt('ddelta') - dae['dddelta'],
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator']
        ])

    if nSteps is not None:
        dae.addP('endTime')

    psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w1','w2','w3']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual([ode,alg])
    
    return dae
示例#12
0
def crosswindModel(conf):
    dae = Dae()

    dae.addZ("nu")
    dae.addX( [ "x"
              , "y"
              , "z"
              , "e11"
              , "e12"
              , "e13"
              , "e21"
              , "e22"
              , "e23"
              , "e31"
              , "e32"
              , "e33"
              , "dx"
              , "dy"
              , "dz"
              , "w1"
              , "w2"
              , "w3"
              , "aileron"
              , "elevator"
              ] )
    dae.addU( [ "daileron"
              , "delevator"
              , 'prop_drag'
              ] )
    dae.addP( ['r',
               'w0'] )
    
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['dw1'] = dae.ddt('w1')
    dae['dw2'] = dae.ddt('w2')
    dae['dw3'] = dae.ddt('w3')
    
    dae['aileron_deg'] = dae['aileron']*180/C.pi
    dae['elevator_deg'] = dae['elevator']*180/C.pi
    dae['daileron_deg_s'] = dae['daileron']*180/C.pi
    dae['delevator_deg_s'] = dae['delevator']*180/C.pi

    dae['tether_tension'] = dae['r']*dae['nu']

    dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
                            C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
                            C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
    
    (massMatrix, rhs, dRexp) = setupModel(dae, conf)
    
    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator']
        ])

    # acceleration
#    ddx = dae['ddx']
#    ddy = dae['ddy']
#    ddz = dae['ddz']
    ddx = dae.ddt('dx')
    ddy = dae.ddt('dy')
    ddz = dae.ddt('dz')
    dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
    dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
    dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
    dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)

    # line angle
    dae['cos_line_angle'] = \
      (dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi

    psuedoZVec = C.veccat([dae.ddt(name) for name in ['dx','dy','dz','w1','w2','w3']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual( [ode, alg] )
    
    return dae
示例#13
0
def crosswindModel(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ("nu")
    dae.addX([
        "x", "y", "z", "e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32",
        "e33", "dx", "dy", "dz", "w_bn_b_x", "w_bn_b_y", "w_bn_b_z", "r", "dr",
        'ddr', "aileron", "elevator", "rudder", "flaps"
    ])
    dae.addU(["daileron", "delevator", "drudder", "dflaps", 'dddr'])
    dae.addP(['w0'])

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat(
        [dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in degrees for plotting
    dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
    dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
    dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
    dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
    dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
    dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r'] * dae['nu']

    # theoretical mechanical power
    dae['mechanical_winch_power'] = -dae['tether_tension'] * dae['dr']

    # carousel2 motor model from thesis data
    dae['rpm'] = -dae['dr'] / 0.1 * 60 / (2 * C.pi)
    dae['torque'] = dae['tether_tension'] * 0.1
    dae['electrical_winch_power'] =  293.5816373499238 + \
                                     0.0003931623408*dae['rpm']*dae['rpm'] + \
                                     0.0665919381751*dae['torque']*dae['torque'] + \
                                     0.1078628659825*dae['rpm']*dae['torque']

    dae['R_n2b'] = C.vertcat([
        C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
        C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
        C.horzcat([dae['e31'], dae['e32'], dae['e33']])
    ])

    # get mass matrix, rhs
    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    # set up the residual
    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x', 'y', 'z']]) -
        C.veccat([dae['dx'], dae['dy'], dae['dz']]),
        C.veccat([
            dae.ddt(name) for name in
            ["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"]
        ]) - dRexp.trans().reshape([9, 1]),
        #        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
        #        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('ddr') - dae['dddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('rudder') - dae['drudder'],
        dae.ddt('flaps') - dae['dflaps']
    ])

    # acceleration for plotting
    ddx = dae['ddx']
    ddy = dae['ddy']
    ddz = dae['ddz']
    dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
    dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
    dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
    dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi

    # add local loyd's limit
    def addLoydsLimit():
        w = dae['wind_at_altitude']
        cL = dae['cL']
        cD = dae['cD'] + dae['cD_tether']
        rho = conf['rho']
        S = conf['sref']
        loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
        loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
        dae["loyds_limit"] = loyds
        dae["loyds_limit_exact"] = loyds2
        dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
        dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']

    addLoydsLimit()

    psuedoZVec = C.veccat([dae.ddt(name) for name in \
                    ['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual([ode, alg])

    return dae
示例#14
0
def crosswindModel(conf,extraParams=[]):
    dae = Dae()
    for ep in extraParams:
        dae.addP(ep)
        
#    dae.addZ( [ "ddx"
#              , "ddy"
#              , "ddz"
#              , "dw1"
#              , "dw2"
#              , "dw3"
#              , "nu"
#              ] )
    dae.addZ("nu")
    dae.addX( [ "x"   # state 0
              , "y"   # state 1
              , "z"   # state 2
              , "e11" # state 3
              , "e12" # state 4
              , "e13" # state 5
              , "e21" # state 6
              , "e22" # state 7
              , "e23" # state 8
              , "e31" # state 9
              , "e32" # state 10
              , "e33" # state 11
              , "dx"  # state 12
              , "dy"  # state 13
              , "dz"  # state 14
              , "w1"  # state 15
              , "w2"  # state 16
              , "w3"  # state 17
              , "r" # state 20
              , "dr" # state 21
              , "aileron"
              , "elevator"
              ] )
    dae.addU( [ "daileron"
              , "delevator"
              , 'ddr'
              ] )
    dae.addP( ['w0'] )
    
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['dw1'] = dae.ddt('w1')
    dae['dw2'] = dae.ddt('w2')
    dae['dw3'] = dae.ddt('w3')
    
    dae['aileron(deg)'] = dae['aileron']*180/C.pi
    dae['elevator(deg)'] = dae['elevator']*180/C.pi
    dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
    dae['delevator(deg/s)'] = dae['delevator']*180/C.pi

    dae['tether tension'] = dae['r']*dae['nu']
    dae['winch power'] = -dae['tether tension']*dae['dr']

    dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
                            C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
                            C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
    
    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    
    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator']
        ])

    # acceleration
#    ddx = dae['ddx']
#    ddy = dae['ddy']
#    ddz = dae['ddz']
    ddx = dae.ddt('dx')
    ddy = dae.ddt('dy')
    ddz = dae.ddt('dz')
    dae['accel (g)'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
    dae['accel without gravity (g)'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
    dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
    dae['accel without gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)

    # line angle
    dae['cos(line angle)'] = \
      (dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi

    # add local loyd's limit
    def addLoydsLimit():
        w = dae['wind at altitude']
        cL = dae['cL']
        cD = dae['cD']
        rho = conf['rho']
        S = conf['sref']
        loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
        loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
        dae["loyd's limit"] = loyds
        dae["loyd's limit (exact)"] = loyds2
        dae['-(winch power)'] = -dae['winch power']
    addLoydsLimit()
    
    psuedoZVec = C.veccat([dae.ddt(name) for name in ['dx','dy','dz','w1','w2','w3']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual( [ode, alg] )
    
    return dae