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([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.dense(outs)]) outputFun.init() assert outputFun.getFree().shape[0] == 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.dense(outs)]) outputFun.init() assert outputFun.getFree().shape[0] == 0, 'lsqEndTermExtern cannot be a function of controls u, saw: '+str(outputFun.getFree()) else: raise Exception('unrecognized name "'+exportName+'"') return codegen.writeCCode(outputFun,exportName)
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([C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), xdot/timeScaling])]) rhs = C.SXFunction( [inputs], [C.dense(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.dense(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([C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), xdot/timeScaling])]) measurementsFun = C.SXFunction( [inputs], [C.dense(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.dense(jo)] ) measurementsJacobFun.init() measurementsJacobString = codegen.writeCCode(measurementsJacobFun, 'measurementsJacob') ret['measurementsJacob'] = measurementsJacobFun ret['measurementsJacobFile'] = measurementsJacobString return ret
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.dense(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