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
