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)
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)
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)
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)
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]
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
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
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()
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()
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()
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
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
# %% ========================================================================= # 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
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)
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
# 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
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
