Esempio n. 1
0
def speed_up(function, parameters, backend=u'clang'):
    str_params = [str(x) for x in parameters]
    try:
        f = ca.Function('f', [Matrix(parameters)], [ca.densify(function)])
    except:
        f = ca.Function('f', [Matrix(parameters)], ca.densify(function))
    return CompiledFunction(str_params, f, 0, function.shape)
Esempio n. 2
0
def writeObjective(ocp, out0, exportName):
    dae = ocp.dae

    # first make out not a function of xDot or z
    inputs0 = [dae.xDotVec(), dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec()]
    outputFun0 = C.SXFunction(inputs0, [out0])

    (xDotDict, zDict) = dae.solveForXDotAndZ()
    xDot = C.veccat([xDotDict[name] for name in dae.xNames()])
    z    = C.veccat([zDict[name] for name in dae.zNames()])

    # plug in xdot, z solution to outputs fun
    outputFun0.init()
    [out] = outputFun0.eval([xDot, dae.xVec(), z, dae.uVec(), dae.pVec()])

    # make sure each element in the output is only a function of x or u, not both
    testSeparation(dae,out,exportName)

    # make new SXFunction that is only fcn of [x, u, p]
    if exportName == 'lsqExtern':
        inputs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec()])
        outs = C.veccat( [ out, C.jacobian(out,dae.xVec()).T, C.jacobian(out,dae.uVec()).T ] )
        outputFun = C.SXFunction([inputs], [C.densify(outs)])
        outputFun.init()
        assert len(outputFun.getFree()) == 0, 'the "impossible" happened >_<'
    elif exportName == 'lsqEndTermExtern':
        inputs = C.veccat([dae.xVec(), dae.pVec()])
        outs = C.veccat( [ out, C.jacobian(out,dae.xVec()).T ] )
        outputFun = C.SXFunction([inputs], [C.densify(outs)])
        outputFun.init()
        assert len(outputFun.getFree()) == 0, 'lsqEndTermExtern cannot be a function of controls u, saw: '+str(outputFun.getFree())
    else:
        raise Exception('unrecognized name "'+exportName+'"')

    return codegen.writeCCode(outputFun,exportName)
Esempio n. 3
0
    def __init__(self, ocp,
                 lqrDae = None,
                 ocpOptions=None,
                 integratorOptions=None,
                 codegenOptions=None,
                 phase1Options=None):
        assert isinstance(ocp, Mpc), "MpcRT must be given an Mpc object, you gave: "+str(type(ocp))

        # call the parent init
        OcpRT.__init__(self, ocp,
                       ocpOptions=ocpOptions,
                       integratorOptions=integratorOptions,
                       codegenOptions=codegenOptions,
                       phase1Options=phase1Options)

        # set up measurement functions
        self._yxFun = C.SXFunction([ocp.dae.xVec(),ocp.dae.pVec()], [C.densify(self.ocp.yx)])
        self._yuFun = C.SXFunction([ocp.dae.uVec(),ocp.dae.pVec()], [C.densify(self.ocp.yu)])
        self._yxFun.init()
        self._yuFun.init()
        
        if lqrDae is None:
            self._lqrDae = self.ocp.dae
        else:
            self._lqrDae = lqrDae
        self._integratorLQR  = rawe.RtIntegrator(self._lqrDae, ts=self.ocp.ts, options=integratorOptions)
Esempio n. 4
0
    def __init__(self,
                 ocp,
                 lqrDae,
                 ocpOptions=None,
                 integratorOptions=None,
                 codegenOptions=None,
                 phase1Options=None):
        assert isinstance(
            ocp, Mpc), "MpcRT must be given an Mpc object, you gave: " + str(
                type(ocp))

        # call the parent init
        OcpRT.__init__(self,
                       ocp,
                       ocpOptions=ocpOptions,
                       integratorOptions=integratorOptions,
                       codegenOptions=codegenOptions,
                       phase1Options=phase1Options)

        # set up measurement functions
        self._yxFun = C.SXFunction([ocp.dae.xVec()], [C.densify(self.ocp.yx)])
        self._yuFun = C.SXFunction([ocp.dae.uVec()], [C.densify(self.ocp.yu)])
        self._yxFun.init()
        self._yuFun.init()

        self._lqrDae = lqrDae
        self._integratorLQR = rawe.RtIntegrator(self._lqrDae,
                                                ts=self.ocp.ts,
                                                options=integratorOptions)
Esempio n. 5
0
    def _state_init(self, m0, S0, L0):
        m0 = self.x(m0[:])
        S0 = self.x.squared(ca.densify(S0))
        L0 = self.x.squared(ca.densify(L0))

        x0 = self._draw_initial_state(m0, S0)

        b0 = self.b()
        b0['m'] = m0
        b0['S'] = S0

        eb0 = self.eb()
        eb0['m'] = m0
        eb0['S'] = S0
        eb0['L'] = L0

        return [x0, m0, S0, L0, b0, eb0]
Esempio n. 6
0
def generateCModel(dae,timeScaling,measurements):
    xdot = C.veccat([dae.ddt(name) for name in dae.xNames()])
    inputs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), xdot])
    jacobian_inputs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), xdot])
    f = dae.getResidual()

    # dae residual
    rhs = C.SXFunction( [inputs], [f] )
    rhs.init()
    # handle time scaling
    [f] = rhs.eval([C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), xdot/timeScaling])])
    rhs = C.SXFunction( [inputs], [C.densify(f)] )
    rhs.init()
    rhsString = codegen.writeCCode(rhs, 'rhs')

    # dae residual jacobian
    jf = C.veccat( [ C.jacobian(f,jacobian_inputs).T ] )
    rhsJacob = C.SXFunction( [inputs], [C.densify(jf)] )
    rhsJacob.init()
    rhsJacobString = codegen.writeCCode(rhsJacob, 'rhsJacob')

    ret = {'rhs':rhs,
           'rhsJacob':rhsJacob,
           'rhsFile':rhsString,
           'rhsJacobFile':rhsJacobString}

    if measurements is not None:
        # measurements
        measurementsFun = C.SXFunction( [inputs], [measurements] )
        measurementsFun.init()
        [measurements] = measurementsFun.eval([C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), xdot/timeScaling])])
        measurementsFun = C.SXFunction( [inputs], [C.densify(measurements)] )
        measurementsFun.init()
        measurementsString = codegen.writeCCode(measurementsFun, 'measurements')
        ret['measurements'] = measurementsFun
        ret['measurementsFile'] = measurementsString

        # measurements jacobian
        jo = C.veccat( [ C.jacobian(measurements,jacobian_inputs).T ] )
        measurementsJacobFun = C.SXFunction( [inputs], [C.densify(jo)] )
        measurementsJacobFun.init()
        measurementsJacobString = codegen.writeCCode(measurementsJacobFun, 'measurementsJacob')
        ret['measurementsJacob'] = measurementsJacobFun
        ret['measurementsJacobFile'] = measurementsJacobString

    return ret
Esempio n. 7
0
def phase1src(dae, options, measurements):
    ret = '''\
#include <string>
#include <iostream>

#include <acado_code_generation.hpp>
#include <acado/utils/acado_types.hpp>

extern "C"{
  int export_integrator( const char * genPath);
}

using namespace std;
USING_NAMESPACE_ACADO

int export_integrator( const char * genPath)
{
  const double timestep = 1.0;
  const int numIntervals = 1;
  SIMexport sim(numIntervals, timestep);

  // set INTEGRATOR_TYPE
  sim.set( INTEGRATOR_TYPE, %(INTEGRATOR_TYPE)s);

  // set NUM_INTEGRATOR_STEPS
  sim.set( NUM_INTEGRATOR_STEPS, %(NUM_INTEGRATOR_STEPS)s );

  // 0 == rhs()
  sim.setModel( "model", "rhs", "rhsJacob" );
  sim.setDimensions( %(nx)d, %(nx)d, %(nz)d, %(nup)d );
''' % {
        'INTEGRATOR_TYPE': options['INTEGRATOR_TYPE'],
        'NUM_INTEGRATOR_STEPS': options['NUM_INTEGRATOR_STEPS'],
        'nx': len(dae.xNames()),
        'nz': len(dae.zNames()),
        'nup': len(dae.uNames()) + len(dae.pNames())
    }

    if measurements is not None:
        ret += '''
  // set MEASUREMENT_GRID
  sim.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );

  // set output/measurements
  sim.addOutput( "measurements", "measurementsJacob", %(outputDimension)d );
  Vector Meas(1);
  Meas(0) = 1;
  sim.setMeasurements( Meas );
''' % {
            'outputDimension': C.densify(measurements).size()
        }
    ret += '''
  sim.set( GENERATE_MAKE_FILE, false );

  return sim.exportCode(genPath);
}
'''
    return ret
Esempio n. 8
0
 def set_initial_state(self, x0, m0, S0):
     self.x0 = self.x(x0)
     self.m0 = self.x(m0[:])
     self.S0 = self.x.squared(ca.densify(S0))
     self.b0['m'] = self.m0
     self.b0['S'] = self.S0
     self.eb0['m'] = self.m0
     self.eb0['S'] = self.S0
     self.n = self._estimate_simulation_duration()
Esempio n. 9
0
    def __init__(self, ocp,
                 ocpOptions=None,
                 integratorOptions=None,
                 codegenOptions=None,
                 phase1Options=None):
        assert isinstance(ocp, Mhe), "MheRT must be given an Mhe object, you gave: "+str(type(ocp))

        # call the parent init
        OcpRT.__init__(self, ocp,
                       ocpOptions=ocpOptions,
                       integratorOptions=integratorOptions,
                       codegenOptions=codegenOptions,
                       phase1Options=phase1Options,
                       integratorMeasurements=C.veccat([ocp.yx,ocp.yu]))

        # set up measurement functions
        self._yxFun = C.SXFunction([ocp.dae.xVec(),ocp.dae.pVec()], [C.densify(self.ocp.yx)])
        self._yuFun = C.SXFunction([ocp.dae.uVec(),ocp.dae.pVec()], [C.densify(self.ocp.yu)])
        self._yxFun.init()
        self._yuFun.init()
Esempio n. 10
0
    def __init__(self, ocp,
                 ocpOptions=None,
                 integratorOptions=None,
                 codegenOptions=None,
                 phase1Options=None):
        assert isinstance(ocp, Mhe), "MheRT must be given an Mhe object, you gave: "+str(type(ocp))

        # call the parent init
        OcpRT.__init__(self, ocp,
                       ocpOptions=ocpOptions,
                       integratorOptions=integratorOptions,
                       codegenOptions=codegenOptions,
                       phase1Options=phase1Options,
                       integratorMeasurements=C.veccat([ocp.yx,ocp.yu]))

        # set up measurement functions
        self._yxFun = C.SXFunction([ocp.dae.xVec()], [C.densify(self.ocp.yx)])
        self._yuFun = C.SXFunction([ocp.dae.uVec()], [C.densify(self.ocp.yu)])
        self._yxFun.init()
        self._yuFun.init()
Esempio n. 11
0
def phase1src(dae,options,measurements):
    ret = '''\
#include <string>
#include <iostream>

#include <acado_code_generation.hpp>
#include <acado/utils/acado_types.hpp>

extern "C"{
  int export_integrator( const char * genPath);
}

using namespace std;
USING_NAMESPACE_ACADO

int export_integrator( const char * genPath)
{
  const double timestep = 1.0;
  const int numIntervals = 1;
  SIMexport sim(numIntervals, timestep);

  // set INTEGRATOR_TYPE
  sim.set( INTEGRATOR_TYPE, %(INTEGRATOR_TYPE)s);

  // set NUM_INTEGRATOR_STEPS
  sim.set( NUM_INTEGRATOR_STEPS, %(NUM_INTEGRATOR_STEPS)s );

  // 0 == rhs()
  sim.setModel( "model", "rhs", "rhsJacob" );
  sim.setDimensions( %(nx)d, %(nx)d, %(nz)d, %(nup)d );
''' % {'INTEGRATOR_TYPE': options['INTEGRATOR_TYPE'],
       'NUM_INTEGRATOR_STEPS': options['NUM_INTEGRATOR_STEPS'],
       'nx':len(dae.xNames()),
       'nz':len(dae.zNames()),
       'nup':len(dae.uNames())+len(dae.pNames())}

    if measurements is not None:
        ret += '''
  // set MEASUREMENT_GRID
  sim.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );

  // set output/measurements
  sim.addOutput( "measurements", "measurementsJacob", %(outputDimension)d );
  Vector Meas(1);
  Meas(0) = 1;
  sim.setMeasurements( Meas );
''' % {'outputDimension':C.densify(measurements).size()}
    ret +='''
  sim.set( GENERATE_MAKE_FILE, false );

  return sim.exportCode(genPath);
}
'''
    return ret
Esempio n. 12
0
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
def phase1src(dae,options,measurements):
    ret = '''\
#include <string>
#include <iostream>

#include <acado_code_generation.hpp>

extern "C"{
  int export_integrator( const char * genPath);
}

using namespace std;
USING_NAMESPACE_ACADO

int export_integrator( const char * genPath)
{
    string path( genPath );
    string _stdout = path + "/_stdout.txt";
    
    std::ofstream out( _stdout.c_str() );
    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buf
    std::cout.rdbuf(out.rdbuf()); // redirect std::cout to the text file

    Logger::instance().setLogLevel( LVL_DEBUG );

  const double timestep = 1.0;
  const int numIntervals = 1;
  SIMexport sim(numIntervals, timestep);

  // set INTEGRATOR_TYPE
  sim.set( INTEGRATOR_TYPE, %(INTEGRATOR_TYPE)s);

  // set NUM_INTEGRATOR_STEPS
  sim.set( NUM_INTEGRATOR_STEPS, %(NUM_INTEGRATOR_STEPS)s );
  
  sim.set( DYNAMIC_SENSITIVITY, %(dynamic_sensitivity)s );

  // 0 == rhs()
  sim.setModel( "model", "rhs", "rhsJacob" );
  sim.setDimensions( %(nx)d, %(nx)d, %(nz)d, %(nu)d, %(nod)d, 0 );
''' % {'INTEGRATOR_TYPE': options['INTEGRATOR_TYPE'],
       'NUM_INTEGRATOR_STEPS': options['NUM_INTEGRATOR_STEPS'],
       'nx': len(dae.xNames()),
       'nz': len(dae.zNames()),
       'nu': len(dae.uNames()),
       'nod': len(dae.pNames()),
       'dynamic_sensitivity': options['DYNAMIC_SENSITIVITY']}

    if measurements is not None:
        ret += '''
  // set MEASUREMENT_GRID
  // sim.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );

  // set output/measurements
  DVector Meas( 1 );
  Meas( 0 ) = 1;
  sim.addOutput("measurements", "measurementsJacob", %(outputDimension)d, Meas);
''' % {'outputDimension':C.densify(measurements).size()}
    ret +='''
  sim.set( GENERATE_MAKE_FILE, false );
    
    returnValue status = sim.exportCode(genPath);
    
    std::cout.rdbuf(coutbuf); //reset to standard output again

    return status;
}
'''
    return ret
Esempio n. 14
0
# %% =========================================================================
#                            Initial conditions
# ============================================================================

# State
m0 = ca.DMatrix([0.0, 0.0, 0.0, 5.0, 4.0, 25.0, 15.0, 0.0, ca.pi])
S0 = ca.diagcat([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1e-2]) * 0.25
L0 = ca.DMatrix.eye(nx) * 1e-3
L0[-1, -1] = 1e-5

mu0 = np.array(m0).ravel()  # to get samples from normal distribution

b0 = belief()
b0["m"] = m0
b0["S"] = ca.densify(S0)

eb0 = ext_belief()
eb0["m"] = m0
eb0["S"] = ca.densify(S0)
eb0["L"] = ca.densify(L0)

# Covariances
Q = ca.DMatrix.eye(nx) * 1e-3  # does not change
Q[-1, -1] = 1e-5

# Nominal controls
u_ = control.repeated(ca.DMatrix.zeros(nu, N_sim))
u_[:, "v"] = 5.0
u_[:, "w"] = -0.2
Esempio n. 15
0
def solve_chain_mass_nmpc_qp(num_masses, num_intervals):
    time_step = 0.1

    u, y, dot_y = get_chain_mass_ode(num_masses)

    init_state = get_equilibrium_states(num_masses, y, dot_y, [0, 1, 0])
    ref_state = get_equilibrium_states(num_masses, y, dot_y, [1, 0, 0])

    # NMPC formulation:

    num_states = y.shape[0]
    num_controls = 3

    weights_states = numpy.ones(y.shape)
    weights_states[0:3 * num_masses] = 1e2
    weights_states[3 * num_masses:6 * num_masses] = 1e0
    weights_states[-3:] = 1e2

    weights_controls = numpy.ones((num_controls, 1)) * 1e-2

    # states 1..N
    states = casadi.SX.sym('states', num_states, num_intervals)
    # controls 0..N-1
    controls = casadi.SX.sym('controls', num_controls, num_intervals)

    weights_states_sqrt = numpy.sqrt(weights_states)
    weights_controls_sqrt = numpy.sqrt(weights_controls)

    objective_residuals = []
    ref_control = numpy.zeros(u.shape)
    for node in range(num_intervals):
        objective_residuals.append(
            (states[:, node] - ref_state) * weights_states_sqrt)
        objective_residuals.append(
            (controls[:, node] - ref_control) * weights_controls_sqrt)

    objective_residuals = casadi.veccat(*objective_residuals)

    ode = casadi.Function('ode', [u, y], [dot_y])

    rk4_k1 = ode(u, y)
    rk4_k2 = ode(u, y + time_step / 2.0 * rk4_k1)
    rk4_k3 = ode(u, y + time_step / 2.0 * rk4_k2)
    rk4_k4 = ode(u, y + time_step * rk4_k3)
    final_y = y + time_step / 6.0 * (rk4_k1 + 2 * rk4_k2 + 2 * rk4_k3 + rk4_k4)
    integrate = casadi.Function('integrate', [u, y], [final_y])

    states_for_integration = casadi.horzcat(init_state, states[:, :-1])
    integrated_states = integrate.map(num_intervals)(controls,
                                                     states_for_integration)
    equality_constraints = states - integrated_states
    equality_constraints = casadi.veccat(equality_constraints)

    # Prepare and condense the underlying QP.
    states_vec = casadi.veccat(states)
    controls_vec = casadi.veccat(controls)

    jac_obj_residuals_wrt_states = casadi.jacobian(objective_residuals,
                                                   states_vec)
    jac_obj_residuals_wrt_controls = casadi.jacobian(objective_residuals,
                                                     controls_vec)

    jac_eq_constraints_wrt_states = casadi.jacobian(equality_constraints,
                                                    states_vec)
    jac_eq_constraints_wrt_controls = casadi.jacobian(equality_constraints,
                                                      controls_vec)

    qp_h = jac_obj_residuals_wrt_controls.T @ jac_obj_residuals_wrt_controls

    delta_x_contrib = casadi.solve(jac_eq_constraints_wrt_states,
                                   jac_eq_constraints_wrt_controls)
    delta_x_qp_contrib = -jac_obj_residuals_wrt_states @ delta_x_contrib

    qp_h += delta_x_qp_contrib.T @ delta_x_qp_contrib

    qp_g = (jac_obj_residuals_wrt_controls +
            delta_x_qp_contrib).T @ objective_residuals

    states_lbx = numpy.zeros(y.shape)
    states_ubx = numpy.zeros(y.shape)

    states_lbx.fill(-numpy.inf)
    for m in range(num_masses):
        states_lbx[3 * m + 1] = -0.01
    states_ubx.fill(numpy.inf)

    qp_lb_a = []
    qp_ub_a = []

    for _ in range(num_intervals):
        qp_lb_a.append(states_lbx)
        qp_ub_a.append(states_ubx)
    qp_lb_a = numpy.concatenate(qp_lb_a, axis=0)
    qp_ub_a = numpy.concatenate(qp_ub_a, axis=0)

    init_states = numpy.concatenate([init_state for _ in range(num_intervals)],
                                    axis=0)
    delta_x_bound_contrib = casadi.solve(jac_eq_constraints_wrt_states,
                                         equality_constraints) + init_states

    qp_lb_a -= delta_x_bound_contrib
    qp_ub_a -= delta_x_bound_contrib

    qp_a = -delta_x_contrib

    qp_fcn = casadi.Function('qp_h_fcn', [controls_vec, states_vec],
                             [qp_h, qp_g, qp_a, qp_lb_a, qp_ub_a])

    init_controls = numpy.zeros(controls_vec.shape)
    qp_h_eval, qp_g_eval, qp_a_eval, qp_lb_a_eval, qp_ub_a_eval = qp_fcn(
        init_controls, init_states)

    qp_lbx = -numpy.ones(qp_g.shape)
    qp_ubx = numpy.ones(qp_g.shape)

    # Reduce the number of rows of the A-matrix to the minimum.
    qp_a_indices = []
    for el in range(qp_lb_a.shape[0]):
        if qp_lb_a_eval[el] <= -numpy.inf and qp_ub_a_eval[el] >= numpy.inf:
            continue
        qp_a_indices.append(el)

    qp_lb_a_eval = qp_lb_a_eval[qp_a_indices]
    qp_ub_a_eval = qp_ub_a_eval[qp_a_indices]
    qp_a_eval = qp_a_eval[qp_a_indices, :]

    qp_x = casadi.SX.sym('qp_x', *qp_g.shape)
    qp = {
        'x': qp_x,
        'f': 0.5 * qp_x.T @ qp_h_eval @ qp_x + qp_x.T @ qp_g_eval,
        'g': casadi.densify(qp_a_eval @ qp_x)
    }

    qp_solver = casadi.qpsol('qp_solver', 'qpoases', qp)

    sol = qp_solver(lbx=qp_lbx, ubx=qp_ubx, lbg=qp_lb_a_eval, ubg=qp_ub_a_eval)

    x_opt = numpy.asarray(sol['x']).flatten()
    y_opt = numpy.asarray(-casadi.vertcat(sol['lam_x'], sol['lam_g']))
    f_opt = sol['f']

    num_variables = qp_x.shape[0]
    num_constraints = qp_a_eval.shape[0]

    qp_h_flat = numpy.asarray(qp_h_eval).flatten()
    qp_g_flat = numpy.asarray(qp_g_eval).flatten()
    qp_a_flat = numpy.asarray(qp_a_eval).flatten()
    qp_lb_a_flat = numpy.asarray(qp_lb_a_eval).flatten()
    qp_ub_a_flat = numpy.asarray(qp_ub_a_eval).flatten()

    return (num_variables, num_constraints, qp_h_flat, qp_g_flat, qp_a_flat,
            qp_lbx, qp_ubx, qp_lb_a_flat, qp_ub_a_flat, x_opt, y_opt, f_opt)
Esempio n. 16
0
def generateCModel(dae, timeScaling, measurements):
    xdot = C.veccat([dae.ddt(name) for name in dae.xNames()])
    inputs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), xdot])
    jacobian_inputs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), xdot])
    f = dae.getResidual()

    # dae residual
    rhs = C.SXFunction([inputs], [f])
    rhs.init()
    # handle time scaling
    [f] = rhs.eval([
        C.veccat([
            dae.xVec(),
            dae.zVec(),
            dae.uVec(),
            dae.pVec(), xdot / timeScaling
        ])
    ])
    rhs = C.SXFunction([inputs], [C.densify(f)])
    rhs.init()
    rhsString = codegen.writeCCode(rhs, 'rhs')

    # dae residual jacobian
    jf = C.veccat([C.jacobian(f, jacobian_inputs).T])
    rhsJacob = C.SXFunction([inputs], [C.densify(jf)])
    rhsJacob.init()
    rhsJacobString = codegen.writeCCode(rhsJacob, 'rhsJacob')

    ret = {
        'rhs': rhs,
        'rhsJacob': rhsJacob,
        'rhsFile': rhsString,
        'rhsJacobFile': rhsJacobString
    }

    if measurements is not None:
        # measurements
        measurementsFun = C.SXFunction([inputs], [measurements])
        measurementsFun.init()
        [measurements] = measurementsFun.eval([
            C.veccat([
                dae.xVec(),
                dae.zVec(),
                dae.uVec(),
                dae.pVec(), xdot / timeScaling
            ])
        ])
        measurementsFun = C.SXFunction([inputs], [C.densify(measurements)])
        measurementsFun.init()
        measurementsString = codegen.writeCCode(measurementsFun,
                                                'measurements')
        ret['measurements'] = measurementsFun
        ret['measurementsFile'] = measurementsString

        # measurements jacobian
        jo = C.veccat([C.jacobian(measurements, jacobian_inputs).T])
        measurementsJacobFun = C.SXFunction([inputs], [C.densify(jo)])
        measurementsJacobFun.init()
        measurementsJacobString = codegen.writeCCode(measurementsJacobFun,
                                                     'measurementsJacob')
        ret['measurementsJacob'] = measurementsJacobFun
        ret['measurementsJacobFile'] = measurementsJacobString

    return ret
Esempio n. 17
0
#                            Initial conditions
# ============================================================================

# State
m0 = ca.DMatrix([0., 0., 5., 4.,
                 15., 0., ca.pi/2])
S0 = ca.diagcat([1., 1., 1., 1.,
                 1., 1., 1e-2]) * 0.25
L0 = ca.DMatrix.eye(nx) * 1e-3
L0[-1,-1] = 1e-5

mu0 = np.array(m0).ravel() # to get samples from normal distribution

b0 = belief()
b0['m'] = m0
b0['S'] = ca.densify(S0)

eb0 = ext_belief()
eb0['m'] = m0
eb0['S'] = ca.densify(S0)
eb0['L'] = ca.densify(L0)

# Covariances
Q  = ca.DMatrix.eye(nx) * 1e-3   # does not change
Q[-1,-1] = 1e-5

# Nominal controls
u_ = control.repeated(ca.DMatrix.zeros(nu,N_sim))
u_[:,'v'] = 5.
u_[:,'w'] = 0.2
Esempio n. 18
0
def phase1src(dae, options, measurements):
    ret = '''\
#include <string>
#include <iostream>

#include <acado_code_generation.hpp>

extern "C"{
  int export_integrator( const char * genPath);
}

using namespace std;
USING_NAMESPACE_ACADO

int export_integrator( const char * genPath)
{
    string path( genPath );
    string _stdout = path + "/_stdout.txt";
    
    std::ofstream out( _stdout.c_str() );
    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buf
    std::cout.rdbuf(out.rdbuf()); // redirect std::cout to the text file

    Logger::instance().setLogLevel( LVL_DEBUG );

  const double timestep = 1.0;
  const int numIntervals = 1;
  SIMexport sim(numIntervals, timestep);

  // set INTEGRATOR_TYPE
  sim.set( INTEGRATOR_TYPE, %(INTEGRATOR_TYPE)s);

  // set NUM_INTEGRATOR_STEPS
  sim.set( NUM_INTEGRATOR_STEPS, %(NUM_INTEGRATOR_STEPS)s );
  
  sim.set( DYNAMIC_SENSITIVITY, %(dynamic_sensitivity)s );

  // 0 == rhs()
  sim.setModel( "model", "rhs", "rhsJacob" );
  sim.setDimensions( %(nx)d, %(nx)d, %(nz)d, %(nu)d, %(nod)d, 0 );
''' % {
        'INTEGRATOR_TYPE': options['INTEGRATOR_TYPE'],
        'NUM_INTEGRATOR_STEPS': options['NUM_INTEGRATOR_STEPS'],
        'nx': len(dae.xNames()),
        'nz': len(dae.zNames()),
        'nu': len(dae.uNames()),
        'nod': len(dae.pNames()),
        'dynamic_sensitivity': options['DYNAMIC_SENSITIVITY']
    }

    if measurements is not None:
        ret += '''
  // set MEASUREMENT_GRID
  // sim.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );

  // set output/measurements
  DVector Meas( 1 );
  Meas( 0 ) = 1;
  sim.addOutput("measurements", "measurementsJacob", %(outputDimension)d, Meas);
''' % {
            'outputDimension': C.densify(measurements).size()
        }
    ret += '''
  sim.set( GENERATE_MAKE_FILE, false );
    
    returnValue status = sim.exportCode(genPath);
    
    std::cout.rdbuf(coutbuf); //reset to standard output again

    return status;
}
'''
    return ret