コード例 #1
0
 def get_steady_state_guess():
     ddelta = nTurns*2*pi/(ocp._guess.lookup('endTime'))
     from rawekite.carouselSteadyState import getSteadyState
     conf_ss = makeConf()
     steadyState,_ = getSteadyState(carousel_dae.makeDae(conf_ss), None, ddelta, lineRadiusGuess, {})
     return steadyState
コード例 #2
0
# Reference parameters
refP = {'r0':2,
        'ddelta0':4}

# utility function
def getDeltaRange(delta0, kRange):
    return numpy.array([delta0 + k*Ts*refP['ddelta0'] for k in kRange])
def getDeltaRangeMhe(delta0):
    return getDeltaRange(delta0, range(-MHE.mheHorizonN, 1))
def getDeltaRangeMpc(delta0):
    return getDeltaRange(delta0, range(0, NMPC.mpcHorizonN + 1))

# Create a sim and initalize it
sim = rawe.RtIntegrator(daeSim, ts=Ts, options=MHE.mheIntOpts)
steadyState,_ = getSteadyState(daeSim, conf, refP['ddelta0'], refP['r0'])

sim.x = steadyState
sim.z = steadyState
sim.u = steadyState
sim.p = steadyState

####### initialize MHE #########
# x/z/u
for k,name in enumerate(mheRT.ocp.dae.xNames()):
    mheRT.x[:,k] = steadyState[name]
    if name == 'sin_delta':
        mheRT.x[:,k] = numpy.sin(getDeltaRangeMhe(0))
    if name == 'cos_delta':
        mheRT.x[:,k] = numpy.cos(getDeltaRangeMhe(0))
for k,name in enumerate(mheRT.ocp.dae.zNames()):
コード例 #3
0
ファイル: carouselsim.py プロジェクト: psinha/rawesome
import casadi as C

import time

import rawe
import rawekite
from rawekite.carouselSteadyState import getSteadyState

if __name__=='__main__':
    # create the model
    from highwind_carousel_conf import conf
    dae = rawe.models.carousel(conf)
    
    # compute the steady state
    steadyState, ssDot = getSteadyState(dae,conf,2*C.pi,1.2,-0.1)
    print steadyState
    print ssDot
    
    
    # create the sim
    dt = 0.01
    sim = rawe.sim.Sim(dae,dt)
    communicator = rawekite.communicator.Communicator()
#    js = joy.Joy()

    # set the initial state from steadyState
    x = {}
    for name in dae.xNames():
        x[name] = steadyState[name]
    u = {}
コード例 #4
0
ファイル: carouselsim.py プロジェクト: jgillis/rawesome
import casadi as C

import time

import rawe
import rawekite
from rawekite.carouselSteadyState import getSteadyState

if __name__=='__main__':
    # create the model
    from rawe.models.arianne_conf import makeConf
    conf = makeConf()
    dae = rawe.models.carousel(makeConf())
    
    # compute the steady state
    steadyState, ssDot = getSteadyState(dae,conf,2*C.pi,1.2,0.1)
    print steadyState
    print ssDot
    
    
    # create the sim
    dt = 0.01
    sim = rawe.sim.Sim(dae,dt)
#    communicator = rawekite.communicator.Communicator()
#    js = joy.Joy()

    # set the initial state from steadyState
    x = {}
    for name in dae.xNames():
        x[name] = steadyState[name]
    u = {}
コード例 #5
0
    #
    # Generate steady state for NMPC
    #
    
    # Cable length for steady state calculation
    steadyStateCableLength = 1.7
    # Speed for the steady state calculation
    steadyStateSpeed = -4.0

    # Reference parameters
    refP = {'r0': steadyStateCableLength,
            'ddelta0': steadyStateSpeed,
            }

    # Get the steady state
    steadyState, _ = getSteadyState(dae, conf, refP['ddelta0'], refP['r0'], verbose = False)

    # Write the steady state to the file
    names = {"x": dae.xNames(), "u": dae.uNames(), "z": dae.zNames()}
    for k, v in names.items():
        fw.write("const double ss_" + k + "[ " + str( len( v ) ) + " ] = {")
        fw.write(", ".join([repr( steadyState[ name ] ) + " /*" + name + "*/" for name in v]))
        fw.write("};\n\n")

    # Write bounds for control surfaces
    fw.write("// Plane's control surface limits [rad] \n")
    for name in ["aileron_bound", "elevator_bound"]:
        fw.write("#define " + name + " " + repr( conf[ name ] ) + "\n")
    
    fw.write("\n\n#endif // SIMULATOR_CONFIGURATION\n")
    fw.close()
コード例 #6
0
ファイル: lets_id_some_sys.py プロジェクト: ghorn/rawesome
 def get_steady_state_guess():
     from rawekite.carouselSteadyState import getSteadyState
     conf_ss = makeConf()
     steadyState,_ = getSteadyState(carousel_dae.makeDae(conf_ss), None, ddelta0, lineRadiusGuess, {})
     return steadyState
コード例 #7
0
ファイル: test_mhe.py プロジェクト: drewm1980/planepower
            'z':(-10,10),
            'aileron':(0,0),
            'elevator':(0,0),
            'rudder':(0,0),
            'flaps':(0,0),
            }
#ref_dict = {
#            'z':(0,0),
#            'aileron':(-1,1),
#            'elevator':(-1,1),
#            'rudder':(-1,1),
#            'flaps':(-1,1),
#            }

# Get the steady state
steadyState,dSS = getSteadyState(dae, conf, refP['ddelta0'], refP['r0'], ref_dict)

# Utility functions
def getDeltaRange(delta0, kRange):
    return numpy.array([delta0 + k*Ts*refP['ddelta0'] for k in kRange])
def getDeltaRangeMhe(delta0):
    return getDeltaRange(delta0, range(-MHE.mheHorizonN, 1))

# Starting angle, from measurements
delta0 = numpy.arctan2(measurements['sin_delta'][0], measurements['cos_delta'][0])

# Initialize differential state variables
for k, name in enumerate(mheRT.ocp.dae.xNames()):
    mheRT.x[:, k] = steadyState[name]
    
    # Rewrite angles again? Now with actual angles from measurements? 
コード例 #8
0
ファイル: generateNmpc.py プロジェクト: drewm1980/planepower
def generateReference(nmpc, conf, refCableLength, refSpeed, visualize = False):
    """
    A function for generation of reference around the steady state.
    
    For now, the goal is just to decrease Z slowly about <ddz> from the
    steady state specified by input arguments.
    """

    # Actuator limitations
    aileron_bound = conf['aileron_bound']
    elevator_bound = conf['elevator_bound']
    
    #
    # Configuration for steady state computation
    #

    r0 = refCableLength
    omega0 = refSpeed

    guess = {'x':r0, 'y':0, 'z':0,
             'r':r0, 'dr':0,
             'e11':0, 'e12':-1, 'e13':0,
             'e21':0, 'e22':0, 'e23':1,
             'e31':-1, 'e32':0, 'e33':0,
             'dx':0, 'dy':0, 'dz':0,
             'w_bn_b_x':0, 'w_bn_b_y':omega0, 'w_bn_b_z':0,
             'ddelta':omega0,
             'cos_delta':1, 'sin_delta':0,
             'aileron':0, 'elevator':0, 'rudder':0, 'flaps':0,
             'daileron':0, 'delevator':0, 'drudder':0, 'dflaps':0,
             'nu':100, 'motor_torque':0,
             'dmotor_torque':0, 'ddr':0,
             'dddr':0.0, 'w0':0.0}
    
    dotGuess = {'x':0, 'y':0, 'z':0, 'dx':0, 'dy':0, 'dz':0,
                'r':0, 'dr':0,
                'e11':0, 'e12':0, 'e13':0,
                'e21':0, 'e22':0, 'e23':0,
                'e31':0, 'e32':0, 'e33':0,
                'w_bn_b_x':0, 'w_bn_b_y':0, 'w_bn_b_z':0,
                'ddelta':0,
                'cos_delta':0, 'sin_delta':omega0,
                'aileron':0, 'elevator':0, 'rudder':0, 'flaps':0,
                'motor_torque':0, 'ddr':0}

    bounds = {'x':(-1.1 * r0, 1.1 * r0), 'y':(-1.1 * r0, 1.1 * r0), 'z':(-1.1 * r0, 1.1 * r0),
             'dx':(0, 0), 'dy':(0, 0), 'dz':(0, 0),
             'r':(r0, r0), 'dr':(0, 0), 'ddr':(0, 0), 'dddr':(0, 0),
             'e11':(-2, 2), 'e12':(-2, 2), 'e13':(-2, 2),
             'e21':(-2, 2), 'e22':(-2, 2), 'e23':(-2, 2),
             'e31':(-2, 2), 'e32':(-2, 2), 'e33':(-2, 2),
             'w_bn_b_x':(-100, 100), 'w_bn_b_y':(-100, 100), 'w_bn_b_z':(-100, 100),
             'ddelta':(omega0, omega0),
             'cos_delta':(1, 1), 'sin_delta':(0, 0),
             
              'aileron':(-aileron_bound, aileron_bound), 'elevator':(-elevator_bound, elevator_bound), 'rudder':(-0.2, 0.2), 'flaps':(-0.2, 0.2),
             
             'daileron':(0, 0), 'delevator':(0, 0), 'drudder':(0, 0), 'dflaps':(0, 0),
              
             
             'motor_torque':(-1000, 1000), 'dmotor_torque':(0, 0),
             
             'w0':(0, 0),
             
             'nu':(0, 3000),
             }

    dotBounds = {'x':(-1, 1), 'y':(-1, 1), 'z':(-1, 1),
                 'dx':(0, 0), 'dy':(0, 0), 'dz':(0, 0),
                 
                 'r':(-0, 0), 'dr':(0, 0), 'ddr':(0, 0),
                 
                 'e11':(-50, 50), 'e12':(-50, 50), 'e13':(-50, 50),
                 'e21':(-50, 50), 'e22':(-50, 50), 'e23':(-50, 50),
                 'e31':(-50, 50), 'e32':(-50, 50), 'e33':(-50, 50),
                 
                 'w_bn_b_x':(0, 0), 'w_bn_b_y':(0, 0), 'w_bn_b_z':(0, 0),
                 
                 'ddelta':(0, 0),
                 
                 'cos_delta':(omega0 - 10, omega0 + 10), 'sin_delta':(omega0 - 10, omega0 + 10),
                 
                 'aileron':(-1, 1), 'elevator':(-1, 1), 'rudder':(-1, 1), 'flaps':(-1, 1),
                 
                 'motor_torque':(-1000, 1000)
                 }

    #
    # Generate the reference
    #
    ref = []
    
    # Generate the first reference
    pt = 1
    ss, dss = getSteadyState(nmpc.dae, conf, refSpeed, refCableLength,
                             guess = guess, dotGuess = dotGuess,
                             bounds = bounds, dotBounds = dotBounds,
                             verbose = False
                             )
    ref.append( ss )
    
    print repr( pt ) + ". point of the reference trajectory is generated."
    
    ssZ = ss[ "z" ]
    
    ddz = 0.025 # [m]
    x = np.linspace(-12 * 2, 12 * 2, num = 75)
    sszVec = ssZ - ddz * 1.0 / (1.0 + np.exp(-1.0 * x))
    for z in sszVec:
        rDict = {"z": (z, z)}
        ss, dss = getSteadyState(nmpc.dae, conf, refSpeed, refCableLength, ref_dict = rDict,
                                 guess = ss, dotGuess = dss,
                                 bounds = bounds, dotBounds = dotBounds,
                                 verbose = False
                                 )
        ref.append( ss )
        pt += 1
        print repr( pt ) + ". point of the reference trajectory is generated."
        
    refOut = {}
    for name in nmpc.dae.xNames() + nmpc.dae.zNames() + nmpc.dae.uNames() + nmpc.dae.pNames():
        points = [pts[ name ] for pts in ref]
        # Go away and return back...
        refOut[ name ] = np.concatenate((points, points[::-1]), axis = 0)
    refUpDown = ref + ref[::-1]
    
    #
    # Now prepare a string with references
    #
    
    import casadi as C
    yxFun = C.SXFunction([nmpc.dae.xVec()], [C.densify(nmpc.yx)])
    yuFun = C.SXFunction([nmpc.dae.uVec()], [C.densify(nmpc.yu)])
    yxFun.init()
    yuFun.init()
    
    refCode = []
    for r in refUpDown:
        _x = [r[ name ] for name in nmpc.dae.xNames()]
        _u = [r[ name ] for name in nmpc.dae.uNames()]
        
        yxFun.setInput(_x, 0)
        yxFun.evaluate()  
        
        yuFun.setInput(_u, 0)
        yuFun.evaluate()  
        
        _r = np.concatenate((np.squeeze(np.array( yxFun.output( 0 ) )),
                             np.squeeze(np.array( yuFun.output( 0 ) ))),
                             axis = 0)
        
        t = "{" + ", ".join([repr( el ) for el in _r]) + "}"
        refCode.append( t )
    
    refs = ",\n".join( refCode )
    
    code = """\
#define REF_NUM_POINTS %(nRef)d

static const double references[ %(nRef)d ][ %(yDim)d ] =
{ 
%(refs)s
};
""" % {"yDim": nmpc.yx.shape[ 0 ] + nmpc.yu.shape[ 0 ],
       "nRef": len( refUpDown ),
       "refs": refs}

    if visualize is True:
        import matplotlib.pyplot as plt
        fig = plt.figure()
        plt.title("x, y, z")
        plt.subplot(3, 1, 1)
        plt.plot(ref["x"])
        plt.subplot(3, 1, 2)
        plt.plot(ref["y"])
        plt.subplot(3, 1, 3)
        plt.plot(ref["z"])
    
        fig = plt.figure()
        plt.title("dx, dy, dz")
        plt.subplot(3, 1, 1)
        plt.plot(ref["dx"])
        plt.subplot(3, 1, 2)
        plt.plot(ref["dy"])
        plt.subplot(3, 1, 3)
        plt.plot(ref["dz"])
    
        fig = plt.figure()
        plt.title("DCM")
        for k, name in enumerate(["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"]):
            plt.subplot(3, 3, k + 1)
            plt.plot(ref[ name ])
        
        fig = plt.figure()
        for k, name in enumerate( ["aileron", "elevator"] ):
            plt.subplot(2, 1, k + 1)
            plt.plot(ref[ name ])
    
        plt.show()
    
    return refOut, code
コード例 #9
0
ファイル: generateNmpc.py プロジェクト: drewm1980/planepower
        fw.write("#define offset_" + str( name ) + " " + str( yOffset ) + "\n")
        yOffset += nmpc[ name ].shape[ 0 ]
    for name in nmpc.yuNames:
        fw.write("#define offset_" + str( name ) + " " + str( yOffset ) + "\n")
        yOffset += nmpc[ name ].shape[ 0 ]
    fw.write("\n\n")
    
    #
    # Generate steady state for NMPC
    #

    # Reference parameters
    refP = {'r0': measCableLength, 'ddelta0': steadyStateSpeed}

    # Get the steady state
    steadyState, dSS = getSteadyState(nmpc.dae, conf, refP['ddelta0'], refP['r0'], verbose = False)

    # Write the steady state to the file
    names = {"x": nmpc.dae.xNames(), "u": nmpc.dae.uNames(), "z": nmpc.dae.zNames()}
    for k, v in names.items():
        fw.write("const double ss_" + k + "[ " + str( len( v ) ) + " ] = {")
        fw.write(", ".join([repr( steadyState[ name ] ) + " /*" + name + "*/" for name in v]))
        fw.write("};\n\n")

    #
    # Generate reference for the NMPC
    #
    _, code = generateReference(nmpc, conf, measCableLength, steadyStateSpeed)
    fw.write( code )
    
    fw.write("#endif // NMPC_CONFIGURATION\n")