def __init__(self, dae, N=None, ts=None, yxNames=None, yuNames=None):
Ocp.__init__(self, dae, N=N, ts=ts)
self.hashPrefix = 'mhe'
if yxNames is None:
yxNames = dae.xNames()
if yuNames is None:
yuNames = dae.uNames()
if not isinstance(yxNames, list):
raise Exception("If you decide to provide measurements, "+\
"you have to provide them as a list of strings")
if not isinstance(yuNames, list):
raise Exception("If you decide to provide end measurements, "+\
"you have to provide them as a list of strings")
self._yxNames = yxNames
self._yuNames = yuNames
self._yx = C.veccat([self[n] for n in self.yxNames])
self._yu = C.veccat([self[n] for n in self.yuNames])
assert not C.dependsOn(
self.yx, self.dae.uVec()), "error: x measurement depends on u"
assert not C.dependsOn(
self.yu, self.dae.xVec()), "error: u measurement depends on x"
Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu]))
Ocp.minimizeLsqEndTerm(self, self.yx)
def _get_subst_set(self, **kwargs):
subst_from = []
subst_to = []
if "t" in kwargs:
subst_from.append(self.t)
subst_to.append(kwargs["t"])
if "x" in kwargs:
subst_from.append(self.x)
subst_to.append(kwargs["x"])
if "z" in kwargs:
subst_from.append(self.z)
subst_to.append(kwargs["z"])
if "xq" in kwargs:
subst_from.append(self.xq)
subst_to.append(kwargs["xq"])
if "u" in kwargs:
subst_from.append(self.u)
subst_to.append(kwargs["u"])
if "p" in kwargs and self.parameters['']:
p = veccat(*self.parameters[''])
subst_from.append(p)
subst_to.append(kwargs["p"])
if "p_control" in kwargs and self.parameters['control']:
p = veccat(*self.parameters['control'])
subst_from.append(p)
subst_to.append(kwargs["p_control"])
if "v" in kwargs and self.variables['']:
v = veccat(*self.variables[''])
subst_from.append(v)
subst_to.append(kwargs["v"])
if "v_control" in kwargs and self.variables['control']:
v = veccat(*self.variables['control'])
subst_from.append(v)
subst_to.append(kwargs["v_control"])
return (subst_from, subst_to)
def initial_guess(self, t, tf_init, params):
x_guess = self.xd()
inclination = 30.0 * ca.pi / 180.0 # the other angle than the one you're thinking of
dcmInclination = np.array(
[[ca.cos(inclination), 0.0, -ca.sin(inclination)], [0.0, 1.0, 0.0],
[ca.sin(inclination), 0.0,
ca.cos(inclination)]])
dtheta = 2.0 * ca.pi / tf_init
theta = t * dtheta
r = 0.25 * params['l']
angle = ca.SX.sym('angle')
x_cir = ca.sqrt(params['l']**2 - r**2)
y_cir = r * ca.cos(angle)
z_cir = r * ca.sin(angle)
init_pos_fun = ca.Function(
'init_pos', [angle],
[ca.mtimes(dcmInclination, ca.veccat(x_cir, y_cir, z_cir))])
init_vel_fun = init_pos_fun.jacobian()
ret = {}
ret['q'] = init_pos_fun(theta)
ret['dq'] = init_vel_fun(theta, 0) * dtheta
ret['w'] = ca.veccat(0.0, 0.0, dtheta)
norm_vel = ca.norm_2(ret['dq'])
norm_pos = ca.norm_2(ret['q'])
R0 = ret['dq'] / norm_vel
R2 = ret['q'] / norm_pos
R1 = ca.cross(ret['q'] / norm_pos, ret['dq'] / norm_vel)
ret['R'] = ca.vertcat(R0.T, R1.T, R2.T).T
return ret
def _ode(self):
ode = veccat(*[self._state_der[k] for k in self.states])
quad = veccat(*[self._state_der[k] for k in self.qstates])
alg = veccat(*self._alg)
return Function('ode', [self.x, self.u, self.z, self.p, self.t],
[ode, alg, quad], ["x", "u", "z", "p", "t"],
["ode", "alg", "quad"])
def _create_belief_nonlinear_constraints(model, V):
"""Non-linear constraints for planning"""
bk = cat.struct_SX(model.b)
bk['S'] = model.b0['S']
g, lbg, ubg = [], [], []
for k in range(model.n):
# Belief propagation
bk['m'] = V['X', k]
[bk_next] = model.BF([bk, V['U', k]])
bk_next = model.b(bk_next)
# Multiple shooting
g.append(bk_next['m'] - V['X', k+1])
lbg.append(ca.DMatrix.zeros(model.nx))
ubg.append(ca.DMatrix.zeros(model.nx))
# Control constraints
constraint_k = model._set_constraint(V, k)
g.append(constraint_k)
lbg.append(-ca.inf)
ubg.append(0)
# Advance time
bk = bk_next
g = ca.veccat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
return [g, lbg, ubg]
def getSteadyStateNlpFunctions(self, dae, ref_dict = {}):
# make steady state model
g = Constraints()
g.add(dae.getResidual(), '==', 0, tag = ('dae residual', None))
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
self.makeOrthonormal(g, R_c2b)
g.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
g.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
constrainInvariantErrs()
# Rotational velocity time derivative
# NOTE: In the following constraint omega0 was hard-coded.
g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
("Rotational velocities", None))
for name in ['alpha_deg', 'beta_deg', 'cL']:
if name in ref_dict:
g.addBnds(dae[name], ref_dict[name], tag = (name, None))
dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
obj = 0
# for name in ['aileron', 'elevator', 'rudder', 'flaps']:
for name in dae.uNames():
if name in dae:
obj += dae[ name ] ** 2
return dvs, obj, g.getG(), g.getLb(), g.getUb()
def __init__(self, dae, N=None, ts=None, yxNames=None, yuNames=None):
Ocp.__init__(self, dae, N=N, ts=ts)
self.hashPrefix = "mhe"
if yxNames is None:
yxNames = dae.xNames()
if yuNames is None:
yuNames = dae.uNames()
if not isinstance(yxNames, list):
raise Exception("If you decide to provide measurements, " + "you have to provide them as a list of strings")
if not isinstance(yuNames, list):
raise Exception(
"If you decide to provide end measurements, " + "you have to provide them as a list of strings"
)
self._yxNames = yxNames
self._yuNames = yuNames
self._yx = C.veccat([self[n] for n in self.yxNames])
self._yu = C.veccat([self[n] for n in self.yuNames])
assert not C.dependsOn(self.yx, self.dae.uVec()), "error: x measurement depends on u"
assert not C.dependsOn(self.yu, self.dae.xVec()), "error: u measurement depends on x"
Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu]))
Ocp.minimizeLsqEndTerm(self, self.yx)
def get_chain_mass_ode(num_masses):
x = casadi.SX.sym('x', 3, num_masses)
dot_x = casadi.SX.sym('dot_x', 3, num_masses)
x_end = casadi.SX.sym('x_end', 3)
u = casadi.SX.sym('u', 3)
y = casadi.veccat(x, dot_x, x_end)
l = 0.033
d = 1.0
m = 0.03
g = [0, 0, -9.81]
def get_f(x_i, x_ip1):
x_diff = x_ip1 - x_i
return d * (1 - l / casadi.norm_2(x_diff)) * x_diff
ddot_x = casadi.SX.zeros(3, num_masses)
for i in range(0, num_masses):
if i == 0:
x_real_end = x[:, i + 1] if num_masses > 1 else x_end
ddot_x[:, i] = 1. / m * (get_f(x[:, i], x_real_end) -
get_f(CHAIN_X0, x[:, i])) + g
elif i < num_masses - 1:
ddot_x[:, i] = 1. / m * (get_f(x[:, i], x[:, i + 1]) -
get_f(x[:, i - 1], x[:, i])) + g
else:
ddot_x[:, i] = 1. / m * (get_f(x[:, i], x_end) -
get_f(x[:, i - 1], x[:, i])) + g
dot_y = casadi.veccat(dot_x, ddot_x, u)
assert y.shape == dot_y.shape
return u, y, dot_y
def initial_residual_function(self):
if hasattr(self, '_states_vector'):
return ca.Function('initial_residual', [self.time, self._states_vector, self._der_states_vector,
self._alg_states_vector, self._inputs_vector, ca.veccat(*self._symbols(self.constants)),
ca.veccat(*self._symbols(self.parameters))], [ca.veccat(*self.initial_equations)] if len(self.initial_equations) > 0 else [])
else:
return ca.Function('initial_residual', [self.time, ca.veccat(*self._symbols(self.states)), ca.veccat(*self._symbols(self.der_states)),
ca.veccat(*self._symbols(self.alg_states)), ca.veccat(*self._symbols(self.inputs)), ca.veccat(*self._symbols(self.constants)),
ca.veccat(*self._symbols(self.parameters))], [ca.veccat(*self.initial_equations)] if len(self.initial_equations) > 0 else [])
def variable_metadata_function(self):
in_var = ca.veccat(*self._symbols(self.parameters))
out = []
is_affine = True
zero, one = ca.MX(0), ca.MX(
1) # Recycle these common nodes as much as possible.
for variable_list in [
self.states, self.alg_states, self.inputs, self.parameters,
self.constants
]:
attribute_lists = [[] for i in range(len(ast.Symbol.ATTRIBUTES))]
for variable in variable_list:
for attribute_list_index, attribute in enumerate(
ast.Symbol.ATTRIBUTES):
value = ca.MX(getattr(variable, attribute))
if value.is_zero():
value = zero
elif value.is_one():
value = one
value = value if value.numel() != 1 else ca.repmat(
value, *variable.symbol.size())
attribute_lists[attribute_list_index].append(value)
expr = ca.horzcat(*[
ca.veccat(*attribute_list)
for attribute_list in attribute_lists
])
if len(self.parameters) > 0 and isinstance(expr, ca.MX):
f = ca.Function('f', [in_var], [expr])
contains_if_else = ca.OP_IF_ELSE_ZERO in [
f.instruction_id(k) for k in range(f.n_instructions())
]
zero_hessian = ca.jacobian(ca.jacobian(expr, in_var),
in_var).is_zero()
if contains_if_else or not zero_hessian:
is_affine = False
out.append(expr)
if len(self.parameters) > 0 and is_affine:
# Rebuild variable metadata as a single affine expression, if all
# subexpressions are affine.
in_var_ = ca.MX.sym('in_var', in_var.shape)
out_ = []
for o in out:
Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)])
bf = ca.Function('bf', [in_var], [o])
A = Af(0)
A = ca.sparsify(A)
b = bf(0)
b = ca.sparsify(b)
o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b
out_.append(o_)
out = out_
in_var = in_var_
return ca.Function('variable_metadata', [in_var], out)
def generateCModel(dae,ag):
writer = AlgorithmWriter()
inputs = C.veccat([ag['x'], ag['z'], ag['u'], ag['p'], ag['xdot']])
# dae residual
f = ag['f']
rhs = C.SXFunction( [inputs], [f] )
rhs.init()
# handle time scaling
[f] = rhs.eval([C.veccat([ag['x'], ag['z'], ag['u'], ag['p'], ag['xdot']/ag['timeScaling']])])
rhs = C.SXFunction( [inputs], [f] )
rhs.init()
rhs_string = [writer.writePrototype('rhs')]
rhs_string.extend(writer.convertAlgorithm(rhs))
rhs_string.append('}')
# dae residual jacobian
jf = C.veccat( [ C.jacobian(f,inputs).T ] )
rhs_jacob = C.SXFunction( [inputs], [jf] )
rhs_jacob.init()
rhs_jacob_string = [writer.writePrototype('rhs_jac')]
rhs_jacob_string.extend(writer.convertAlgorithm(rhs_jacob))
rhs_jacob_string.append('}')
# outputs
o = C.veccat( [dae[outname] for outname in dae.outputNames()] )
outputs = C.SXFunction( [inputs], [o] )
outputs.init()
outputs_string = [writer.writePrototype('out')]
outputs_string.extend(writer.convertAlgorithm(outputs))
outputs_string.append('}')
# outputs jacobian
jo = C.veccat( [ C.jacobian(o,inputs).T ] )
outputs_jacob = C.SXFunction( [inputs], [jo] )
outputs_jacob.init()
outputs_jacob_string = [writer.writePrototype('out_jac')]
outputs_jacob_string.extend(writer.convertAlgorithm(outputs_jacob))
outputs_jacob_string.append('}')
# model file
modelFile = ['#include "acado.h"']
modelFile.append('')
modelFile.extend(rhs_string)
modelFile.append('')
modelFile.extend(rhs_jacob_string)
modelFile.append('')
modelFile.append('')
modelFile.extend(outputs_string)
modelFile.append('')
modelFile.extend(outputs_jacob_string)
return {'modelFile':'\n'.join(modelFile),
'rhs':rhs,
'rhsJacob':rhs_jacob}
def dynamics(self, aero, params):
ScalePower = params['ScalePower']
scale = 1000.
xd = self.xd
u = self.u
xa = self.xa
p = self.p
puni = self.puni
l = self.l
m = params['mK'] + 1. / 3 * params['mT'] #mass of kite and tether
g = params['g'] # gravity
A = params['sref'] # area of Kite
J = params['J'] # Kite Inertia
xddot = ca.struct_symSX(
[ca.entry('d' + name, shape=xd[name].shape) for name in xd.keys()])
[q, dq, R, w, coeff, E, Drag] = xd[...]
(Fa, M, F_tether_scaled, F_drag, F_gravity,
Tether_drag), outputs = aero(xd, xa, p, puni, l, params)
wx = skew(w[0], w[1],
w[2]) # creating a skrew matrix of the angular velocities
# Rdot = R*w (whereas w is in the skrew symmetric form)
Rconstraint = ca.reshape(xddot['dR'] - ca.mtimes(xd['R'], wx), 9,
1) # J*wdot +w x J*w = T
TorqueEq = ca.mtimes(
J, xddot['dw']) + (ca.cross(w.T,
ca.mtimes(J, w).T).T - scale *
(1 - puni['gam']) * u['T']) - puni['gam'] * M
DragForce = -Drag * R[0:3]
# ------------------------------------------------------------------------------------
# DYNAMICS of the system - Create a structure for the Differential-Algebraic Equation
# ------------------------------------------------------------------------------------
res = ca.vertcat(
xddot['dq'] - xd['dq'],\
xddot['dcoeff'] - u['dcoeff'],\
xddot['dDrag'] - u['dDrag'],\
m*(xddot['ddq'][0] + F_tether_scaled[0] - A*(1-puni['gam'])*u['u',0]) - puni['gam']*Fa[0] - F_drag[0] - Tether_drag[0], \
m*(xddot['ddq'][1] + F_tether_scaled[1] - A*(1-puni['gam'])*u['u',1]) - puni['gam']*Fa[1] - F_drag[1] - Tether_drag[1], \
m*(xddot['ddq'][2] + F_tether_scaled[2] - A*(1-puni['gam'])*u['u',2]) - puni['gam']*Fa[2] - F_drag[2] - Tether_drag[2]+ F_gravity , \
xddot['dE'] - ScalePower*ca.mtimes(F_drag.T,dq)
)
res = ca.veccat(res, Rconstraint, TorqueEq)
res = ca.veccat(res,
ca.sum1(xd['q'] * xddot['ddq']) + ca.sum1(xd['dq']**2))
# --- System dynamics function (implicit formulation)
dynamics = ca.Function('dynamics', [xd, xddot, xa, u, p, puni, l],
[res])
return dynamics
def __init__(self, dae, N=None, ts=None):
Ocp.__init__(self, dae, N=N, ts=ts)
self.hashPrefix = "mpc"
self._yxNames = dae.xNames()
self._yuNames = dae.uNames()
self._yx = C.veccat([self[n] for n in self.yxNames])
self._yu = C.veccat([self[n] for n in self.yuNames])
Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu]))
Ocp.minimizeLsqEndTerm(self, self.yx)
def __init__(self, dae, N=None, ts=None):
Ocp.__init__(self, dae, N=N, ts=ts)
self.hashPrefix = 'mpc'
self._yxNames = dae.xNames()
self._yuNames = dae.uNames()
self._yx = C.veccat([self[n] for n in self.yxNames])
self._yu = C.veccat([self[n] for n in self.yuNames])
Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu]))
Ocp.minimizeLsqEndTerm(self, self.yx)
def getOutputs(self, x, u, p):
xVec = C.veccat([getitemMsg(x,name,"the states") for name in self.dae.xNames()])
uVec = C.veccat([getitemMsg(u,name,"the controls") for name in self.dae.uNames()])
pVec = C.veccat([getitemMsg(p,name,"the parameters") for name in self.dae.pNames()])
self.outputsFun0.setInput(xVec, 0)
self.outputsFun0.setInput(uVec, 1)
self.outputsFun0.setInput(pVec, 2)
self.outputsFun0.evaluate()
ret = {}
for k,name in enumerate(self.outputs0names):
ret[name] = maybeToScalar(C.DMatrix(self.outputsFun0.output(k)))
return ret
def step(self, x, u, p):
xVec = C.veccat([getitemMsg(x,name,"the states") for name in self.dae.xNames()])
uVec = C.veccat([getitemMsg(u,name,"the controls") for name in self.dae.uNames()])
pVec = C.veccat([getitemMsg(p,name,"the parameters") for name in self.dae.pNames()])
self.integrator.setInput(xVec,C.INTEGRATOR_X0)
self.integrator.setInput(C.veccat([uVec,pVec]),C.INTEGRATOR_P)
self.integrator.evaluate()
xNext = C.DMatrix(self.integrator.output())
ret = {}
for k,name in enumerate(self.dae.xNames()):
ret[name] = xNext[k].at(0)
return ret
def pendulum_model(nSteps=None):
dae = Dae()
dae.addZ( [ "ddx"
, "ddz"
, "tau"
] )
dae.addX( [ "x"
, "z"
, "dx"
, "dz"
] )
dae.addU( [ "torque"
] )
dae.addP( [ "m"
] )
r = 0.3
dae.stateDotDummy = C.veccat( [C.ssym(name+"DotDummy") for name in dae._xNames] )
scaledStateDotDummy = dae.stateDotDummy
if nSteps is not None:
endTime = dae.addP('endTime')
scaledStateDotDummy = dae.stateDotDummy/(endTime/(nSteps-1))
xDotDummy = scaledStateDotDummy[0]
zDotDummy = scaledStateDotDummy[1]
dxDotDummy = scaledStateDotDummy[2]
dzDotDummy = scaledStateDotDummy[3]
ode = [ dae.x('dx') - xDotDummy
, dae.x('dz') - zDotDummy
, dae.z('ddx') - dxDotDummy
, dae.z('ddz') - dzDotDummy
]
fx = dae.u('torque')*dae.x('z')
fz = -dae.u('torque')*dae.x('x') + dae.p('m')*9.8
alg = [ dae.p('m')*dae.z('ddx') + dae.x('x')*dae.z('tau') - fx
, dae.p('m')*dae.z('ddz') + dae.x('z')*dae.z('tau') - fz
, dae.x('x')*dae.z('ddx') + dae.x('z')*dae.z('ddz') + (dae.x('dx')*dae.x('dx') + dae.x('dz')*dae.x('dz')) ]
c = [ dae.x('x')*dae.x('x') + dae.x('z')*dae.x('z') - r*r
, dae.x('dx')*dae.x('x')* + dae.x('dz')*dae.x('z')
]
dae.addOutput('invariants',C.veccat(c))
dae.setAlgRes( alg )
dae.setOdeRes( ode )
return dae
def variable_metadata_function(self):
in_var = ca.veccat(*self._symbols(self.parameters))
out = []
is_affine = True
zero, one = ca.MX(0), ca.MX(1) # Recycle these common nodes as much as possible.
for variable_list in [self.states, self.alg_states, self.inputs, self.parameters, self.constants]:
attribute_lists = [[] for i in range(len(CASADI_ATTRIBUTES))]
for variable in variable_list:
for attribute_list_index, attribute in enumerate(CASADI_ATTRIBUTES):
value = ca.MX(getattr(variable, attribute))
if value.is_zero():
value = zero
elif value.is_one():
value = one
value = value if value.numel() != 1 else ca.repmat(value, *variable.symbol.size())
attribute_lists[attribute_list_index].append(value)
expr = ca.horzcat(*[ca.veccat(*attribute_list) for attribute_list in attribute_lists])
if len(self.parameters) > 0 and isinstance(expr, ca.MX):
f = ca.Function('f', [in_var], [expr])
# NOTE: This is not a complete list of operations that can be
# handled in an affine expression. That said, it should
# capture the most common ways variable attributes are
# expressed as a function of parameters.
allowed_ops = {ca.OP_INPUT, ca.OP_OUTPUT, ca.OP_CONST,
ca.OP_SUB, ca.OP_ADD, ca.OP_SUB, ca.OP_MUL, ca.OP_DIV, ca.OP_NEG}
f_ops = {f.instruction_id(k) for k in range(f.n_instructions())}
contains_unallowed_ops = not f_ops.issubset(allowed_ops)
zero_hessian = ca.jacobian(ca.jacobian(expr, in_var), in_var).is_zero()
if contains_unallowed_ops or not zero_hessian:
is_affine = False
out.append(expr)
if len(self.parameters) > 0 and is_affine:
# Rebuild variable metadata as a single affine expression, if all
# subexpressions are affine.
in_var_ = ca.MX.sym('in_var', in_var.shape)
out_ = []
for o in out:
Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)])
bf = ca.Function('bf', [in_var], [o])
A = Af(0)
A = ca.sparsify(A)
b = bf(0)
b = ca.sparsify(b)
o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b
out_.append(o_)
out = out_
in_var = in_var_
return self._expand_mx_func(ca.Function('variable_metadata', [in_var], out))
def invariantErrs():
dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')])
, C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')])
, C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')])
] ).trans()
err = C.mul(dcm.trans(), dcm)
dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ])
f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
, [dae.output('c'),dae.output('cdot'),dcmErr]
)
f.setOption('name','invariant errors')
f.init()
return f
def _plot_arrows_3D(name, ax, x, y, phi, psi):
x = ca.veccat(x)
y = ca.veccat(y)
z = ca.DMatrix.zeros(x.size())
phi = ca.veccat(phi)
psi = ca.veccat(psi)
x_vec = ca.cos(psi) * ca.cos(phi)
y_vec = ca.cos(psi) * ca.sin(phi)
z_vec = ca.sin(psi)
ax.quiver(x + x_vec, y + y_vec, z + z_vec,
x_vec, y_vec, z_vec,
color='r', alpha=0.8)
return [Patch(color='red', label=name)]
def invariantErrs():
dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')])
, C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')])
, C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')])
] ).trans()
err = C.mul(dcm.trans(), dcm)
dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ])
f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
, [dae.output('c'),dae.output('cdot'),dcmErr]
)
f.setOption('name','invariant errors')
f.init()
return f
def setQuadratureDdt(self,quadratureStateName,quadratureStateDotName,lookup,lagrangePoly,h,symbolicDvs):
if quadratureStateName in self._quadratures:
raise ValueError(quadratureStateName+" is not unique")
qStates = np.resize(np.array([None],dtype=C.MX),(self._nk+1,self._nicp,self._deg+1))
# set the dimension of the initial quadrature state correctly, and make sure the derivative name is valid
try:
qStates[0,0,0] = 0*lookup(quadratureStateDotName,timestep=0,nicpIdx=0,degIdx=1)
except ValueError:
raise ValueError('quadrature derivative name \"'+quadratureStateDotName+
'\" is not a valid x/z/u/p/output')
ldInv = np.linalg.inv(lagrangePoly.lDotAtTauRoot[1:,1:])
ld0 = lagrangePoly.lDotAtTauRoot[1:,0]
l1 = lagrangePoly.lAtOne
# print -C.mul(C.mul(ldInv, ld0).T, l1[1:]) + l1[0]
breakQuadratureIntervals = True
if breakQuadratureIntervals:
for k in range(self._nk):
for i in range(self._nicp):
dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j)
for j in range(1,self._deg+1)])
Qs = C.mul( ldInv, dQs)
for j in range(1,self._deg+1):
qStates[k,i,j] = qStates[k,i,0] + Qs[j-1]
m = C.mul( Qs.T, l1[1:])
if i < self._nicp - 1:
qStates[k,i+1,0] = qStates[k,i,0] + m
else:
qStates[k+1,0,0] = qStates[k,i,0] + m
else:
for k in range(self._nk):
for i in range(self._nicp):
dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j)
for j in range(1,self._deg+1)])
Qs = C.mul( ldInv, dQs - ld0*qStates[k,i,0] )
for j in range(1,self._deg+1):
qStates[k,i,j] = Qs[j-1]
m = C.veccat( [qStates[k,i,0], Qs] )
m = C.mul( m.T, l1)
if i < self._nicp - 1:
qStates[k,i+1,0] = m
else:
qStates[k+1,0,0] = m
self._quadratures[quadratureStateName] = qStates
self._setupQuadratureFunctions(symbolicDvs)
def plot_arrows3D(name, ax, x, y, phi, theta):
x = ca.veccat(x)
y = ca.veccat(y)
z = ca.DMatrix.zeros(x.size())
phi = ca.veccat(phi)
theta = ca.veccat(theta)
x_vec = ca.cos(theta)*ca.cos(phi)
y_vec = ca.cos(theta)*ca.sin(phi)
z_vec = ca.sin(theta)
length = 2.0
ax.quiver(x+length*x_vec, y+length*y_vec, z+length*z_vec,
x_vec, y_vec, z_vec,
length=length, arrow_length_ratio=0.5)
return [Patch(color='red', label=name)]
def delay_arguments_function(self):
# We cannot assume that we can ca.horzcat/vertcat all delay arguments
# and expressions due to shape differences, so instead we flatten our
# delay expressions and durations into list, i.e. [delay_expr_1,
# delay_duration_1, delay_expr_2, delay_duration_2, ...].
out_arguments = list(itertools.chain.from_iterable(self.delay_arguments))
if hasattr(self, '_states_vector'):
return self._expand_mx_func(ca.Function(
'delay_arguments',
[self.time,
self._states_vector,
self._der_states_vector,
self._alg_states_vector,
self._inputs_vector,
ca.veccat(*self._symbols(self.constants)),
ca.veccat(*self._symbols(self.parameters))],
out_arguments))
else:
return self._expand_mx_func(ca.Function(
'delay_arguments',
[self.time,
ca.veccat(*self._symbols(self.states)),
ca.veccat(*self._symbols(self.der_states)),
ca.veccat(*self._symbols(self.alg_states)),
ca.veccat(*self._symbols(self.inputs)),
ca.veccat(*self._symbols(self.constants)),
ca.veccat(*self._symbols(self.parameters))],
out_arguments))
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state["phi"]), ca.sin(state["phi"])])
r = ca.veccat([state["x_b"] - state["x_c"], state["y_b"] - state["y_c"]])
r_cos_theta = ca.mul(d.T, r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz, nz))
R["x_b", "x_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
R["y_b", "y_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction("Observation covariance", [state], [R])
def delay_arguments_function(self):
if hasattr(self, '_states_vector'):
return self._expand_mx_func(
ca.Function('delay_arguments', [
self.time, self._states_vector, self._der_states_vector,
self._alg_states_vector, self._inputs_vector,
ca.veccat(*self._symbols(self.constants)),
ca.veccat(*self._symbols(self.parameters))
], [
ca.horzcat(delay_argument.expr, delay_argument.duration)
for delay_argument in self.delay_arguments
] if len(self.delay_arguments) > 0 else []))
else:
return self._expand_mx_func(
ca.Function('delay_arguments', [
self.time,
ca.veccat(*self._symbols(self.states)),
ca.veccat(*self._symbols(self.der_states)),
ca.veccat(*self._symbols(self.alg_states)),
ca.veccat(*self._symbols(self.inputs)),
ca.veccat(*self._symbols(self.constants)),
ca.veccat(*self._symbols(self.parameters))
], [
ca.horzcat(delay_argument.expr, delay_argument.duration)
for delay_argument in self.delay_arguments
] if len(self.delay_arguments) > 0 else []))
def vectorizeXUP(x,u,p,dae):
if type(x) == dict:
xVec = C.veccat([getitemMsg(x,name,"the states") for name in dae.xNames()])
else:
xVec = x
if type(u) == dict:
uVec = C.veccat([getitemMsg(u,name,"the controls") for name in dae.uNames()])
else:
uVec = u
if type(p) == dict:
pVec = C.veccat([getitemMsg(p,name,"the parameters") for name in dae.pNames()])
else:
pVec = p
return (xVec,uVec,pVec)
def sxFun(self):
self._freeze('sxFun()')
algRes = None
if hasattr(self,'_algRes'):
algRes = self._algRes
odeRes = self._odeRes
if isinstance(odeRes,list):
odeRes = C.veccat(odeRes)
if isinstance(algRes,list):
algRes = C.veccat(algRes)
return C.SXFunction( C.daeIn( x=self.xVec(), z=self.zVec(), p=C.veccat([self.uVec(),self.pVec()]), xdot=self.stateDotDummy ),
C.daeOut( alg=algRes, ode=odeRes) )
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state['phi']), ca.sin(state['phi'])])
r = ca.veccat([state['x_b']-state['x_c'], state['y_b']-state['y_c']])
r_cos_theta = ca.mul(d.T,r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz,nz))
R['x_b','x_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
R['y_b','y_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction('Observation covariance',[state],[R])
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.blockcat([[ conf['j1'], 0, conf['j31']],
[ 0, conf['j2'], 0],
[conf['j31'], 0, conf['j3']]])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.blockcat([[mm00,mm01,mm02],
[mm10,mm11,mm12],
[mm20,mm21,mm22]])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.vertcat([C.horzcat([ conf['j1'], 0, conf['j31']]),
C.horzcat([ 0, conf['j2'], 0]),
C.horzcat([conf['j31'], 0, conf['j3']])])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.vertcat([C.horzcat([mm00,mm01,mm02]),
C.horzcat([mm10,mm11,mm12]),
C.horzcat([mm20,mm21,mm22])])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def computeOutputs(self, x, u):
self._outputsFun.setInput(x,0)
self._outputsFun.setInput(u,1)
self._outputsFun.evaluate()
outs = [C.DMatrix(self._outputsFun.output(k))
for k in range(self._outputsFun.getNumOutputs())]
return numpy.squeeze(numpy.array(C.veccat(outs)))
def __init__(self, dae, ts, measurements=None, options=RtIntegratorOptions()):
self._dae = dae
self._ts = ts
if measurements is None:
self._measurements = measurements
else:
if isinstance(measurements,list):
measurements = C.veccat(measurements)
self._measurements = measurements
# setup outputs function
self._outputsFun = self._dae.outputsFunWithSolve()
(integratorLib, modelLib, rtModelGen) = exportIntegrator(self._dae, ts, options, self._measurements)
self._integratorLib = integratorLib
self._modelLib = modelLib
self._rtModelGen = rtModelGen
self._initIntegrator = 1
nx = len( self._dae.xNames() )
nz = len( self._dae.zNames() )
nu = len( self._dae.uNames() )
np = len( self._dae.pNames() )
print "creating outputs function"
(fAll, (f0,outputs0names)) = self._dae.outputsFun()
self.outputsFunAll = fAll
self.outputsFun0 = f0
self.outputs0names = outputs0names
self.xNames = self._dae.xNames()
self.uNames = self._dae.uNames()
self.outputNames = self._dae.outputNames()
# self.uNames = dae.uNames()
listOut=[]
for n in self.outputNames: listOut.append([])
self._log = {'x':[],'u':[],'y':[],'yN':[],'outputs':dict(zip(self.outputNames,listOut))}
# [ x z d(x,z)/dx d(x,z)/d(u,p) u p]
self.x = numpy.zeros( nx )
self.z = numpy.zeros( nz )
self.u = numpy.zeros( nu )
self.p = numpy.zeros( np )
self.dx1_dx0 = numpy.zeros( (nx, nx) )
self.dz0_dx0 = numpy.zeros( (nz, nx) )
self.dx1_du = numpy.zeros( (nx, nu) )
self.dx1_dp = numpy.zeros( (nx, np) )
self.dz0_du = numpy.zeros( (nz, nu) )
self.dz0_dp = numpy.zeros( (nz, np) )
self._dx1z0_dx0 = numpy.zeros( (nx+nz, nx) )
self._dx1z0_dup = numpy.zeros( (nx+nz, nu+np) )
if self._measurements is not None:
nh = self._measurements.size()
self.h = numpy.zeros( nh )
self.dh_dx0 = numpy.zeros( (nh, nx) )
self.dh_du = numpy.zeros( (nh, nu) )
self.dh_dp = numpy.zeros( (nh, np) )
def constrainInvariantErrs():
dcm = ocp.lookup('dcm',timestep=0)
err = C.mul(dcm.T,dcm)
ocp.constrain( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
("initial dcm orthonormal",None))
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('c(0)==0',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('cdot(0)==0',None))
def get_primitives(el, *args):
"""
Out of a list of expression, retrieves all primitive expressions
The result is sorted into a dictionary with the key originating
from the 'shorthand' class attribute of OptimzationObject subclasses
"""
if len(args) == 0:
dep = True
else:
dep = args[0]
try:
vars = C.symvar(C.veccat(*el))
except:
vars = {}
syms = dict()
for i, v in enumerate(vars):
mymapping = OptimizationObject.mapping
vobj = mymapping[hash(v)]
symkey = vobj.shorthand
if symkey in syms:
syms[symkey] = syms[symkey] + [vobj]
else:
syms[symkey] = [vobj]
return syms
def _setupDynamicsConstraints(self,endTime,traj):
# Todo: add parallelization
# Todo: get endTime right
g = []
nicp = 1
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly,self.dae,1,nicp,deg,'RADAU')
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._U[k,:].T
# guess
if traj is None:
newton.isolver.setOutput(1,0)
else:
X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._xNames])
Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._zNames])
newton.isolver.setOutput(C.veccat([X,Z]),0)
_, Xf_i = newton.isolver.call([X0_i,U_i,p])
X0_i_plus = self._dvMap.xVec(k+1)
g.append(Xf_i-X0_i_plus)
return g
def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def _setupDynamicsConstraints(self, endTime, traj):
# Todo: add parallelization
# Todo: get endTime right
g = []
nicp = 1
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly, self.dae, 1, nicp, deg, 'RADAU')
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._U[k, :].T
# guess
if traj is None:
newton.isolver.setOutput(1, 0)
else:
X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._xNames])
Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._zNames])
newton.isolver.setOutput(C.veccat([X, Z]), 0)
_, Xf_i = newton.isolver.call([X0_i, U_i, p])
X0_i_plus = self._dvMap.xVec(k + 1)
g.append(Xf_i - X0_i_plus)
return g
def setupImplicitFunction(operAlpha, An, geom):
#setup function to obtain B and RHS for the Fourier series calc
def getBandRHS(alphaiLoc):
alphaLoc = geom.alphaGeometric(operAlpha) - alphaiLoc
clLoc = geom.clLoc(alphaLoc)
RHS = clLoc*geom.chordLoc
B = numpy.sin(numpy.outer(geom.thetaLoc, geom.sumN))*(4.0*geom.bref)
return (B,RHS)
alphaiLoc = []
for i in range(geom.n):
alphaiLoc.append(
C.inner_prod(geom.sumN*numpy.sin(geom.sumN*geom.thetaLoc[i])/numpy.sin(geom.thetaLoc[i]), An))
alphaiLoc = C.veccat(alphaiLoc)
(B,RHS) = getBandRHS(alphaiLoc)
makeMeZero = RHS - C.mul(B, An)
CL = An[0]*numpy.pi*geom.AR
CDi = 0.0
for i in range(geom.n):
k = geom.sumN[i]
CDi += k * An[i]**2 / An[0]**2
CDi *= numpy.pi*geom.AR*An[0]**2
return (makeMeZero, alphaiLoc, CL, CDi)
def constrainInvariantErrs():
dcm = dae['dcm']
err = C.mul(dcm.T,dcm)
g.add( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
("initial dcm orthonormal",None))
g.add(dae['c'], '==', 0, tag=('c(0)==0',None))
g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0',None))
def maybeAddBoxConstraint():
inputs = C.veccat([self.dae.xVec(), self.dae.uVec()])
# make sure only x and u are in rhs,lhs
rml = rhs - lhs
f = C.SXFunction([inputs], [rml])
f.init()
if len(f.getFree()) != 0:
return
# take jacobian of rhs-lhs
jac = C.jacobian(rml, inputs)
# fail if any jacobian element is not constant
coeffs = {}
for j in range(inputs.size()):
if not jac[0, j].toScalar().isZero():
if not jac[0, j].toScalar().isConstant():
return
coeffs[j] = jac[0, j]
if len(coeffs) == 0:
raise Exception("constraint has no design variables in it")
if len(coeffs) > 1:
self.debug(
"found linear constraint that is not box constraint")
return
# alright, we've found a box constraint!
j = coeffs.keys()[0]
coeff = coeffs[j]
name = (self.dae.xNames() + self.dae.uNames())[j]
[f0] = f.eval([0 * inputs])
# if we just divided by a negative number (coeff), flip the comparison
if not coeff.toScalar().isNonNegative():
# lhs `cmp` rhs
# 0 `cmp` rhs - lhs
# 0 `cmp` coeff*x + f0
# -f0 `cmp` coeff*x
# -f0/coeff `FLIP(cmp)` x
if comparison == '>=':
newComparison = '<='
elif comparison == '<=':
newComparison = '>='
else:
newComparison = comparison
else:
newComparison = comparison
c = -f0 / coeff
self.debug('found linear constraint: ' + str(c) + ' ' +
newComparison + ' ' + name)
if newComparison == '==':
self._bound(name, c, 'equality', when=when)
elif newComparison == '<=':
self._bound(name, c, 'lower', when=when)
elif newComparison == '>=':
self._bound(name, c, 'upper', when=when)
else:
raise Exception('the "impossible" happened, comparison "' +
str(comparison) + "\" not in ['==','>=','<=']")
return 'found box constraint'
def minimizeLsqEndTerm(self, obj):
if isinstance(obj, list):
obj = C.veccat(obj)
C.makeDense(obj)
shape = obj.shape
assert shape[0] == 1 or shape[1] == 1, "objective cannot be matrix, got shape: " + str(shape)
assert not hasattr(self, "_minLsqEndTerm"), "you can only call minimizeLsqEndTerm once"
self._minLsqEndTerm = obj
def __minimizeLsqEndTerm(self, obj):
if isinstance(obj, list):
obj = C.veccat(obj)
obj.makeDense()
shape = obj.shape
assert shape[0] == 1 or shape[1] == 1, 'objective cannot be matrix, got shape: '+str(shape)
assert not hasattr(self, '_minLsqEndTerm'), 'you can only call __minimizeLsqEndTerm once'
self._minLsqEndTerm = obj
def maybeAddBoxConstraint():
inputs = C.veccat([self.dae.xVec(),self.dae.uVec()])
# make sure only x and u are in rhs,lhs
rml = rhs-lhs
f = C.SXFunction([inputs],[rml])
f.init()
if f.getFree().shape[0] != 0:
return
# take jacobian of rhs-lhs
jac = C.jacobian(rml,inputs)
# fail if any jacobian element is not constant
coeffs = {}
for j in range(inputs.size()):
if jac.hasNZ(0,j):
if not jac[0,j].isConstant():
return
coeffs[j] = jac[0,j]
if len(coeffs) == 0:
raise Exception("constraint has no design variables in it")
if len(coeffs) > 1:
self.debug("found linear constraint that is not box constraint")
return
# alright, we've found a box constraint!
j = coeffs.keys()[0]
coeff = coeffs[j]
name = (self.dae.xNames()+self.dae.uNames())[j]
[f0] = f([0*inputs])
# if we just divided by a negative number (coeff), flip the comparison
#if not coeff.toScalar().isNonNegative():
if not coeff>=0.0:
# lhs `cmp` rhs
# 0 `cmp` rhs - lhs
# 0 `cmp` coeff*x + f0
# -f0 `cmp` coeff*x
# -f0/coeff `FLIP(cmp)` x
if comparison == '>=':
newComparison = '<='
elif comparison == '<=':
newComparison = '>='
else:
newComparison = comparison
else:
newComparison = comparison
c = -f0/coeff
self.debug('found linear constraint: '+str(c)+' '+newComparison+' '+name)
if newComparison == '==':
self._bound( name, c, 'equality', when=when)
elif newComparison == '<=':
self._bound( name, c, 'lower', when=when)
elif newComparison == '>=':
self._bound( name, c, 'upper', when=when)
else:
raise Exception('the "impossible" happened, comparison "'+str(comparison)+
"\" not in ['==','>=','<=']")
return 'found box constraint'
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()])
assert len(dae.pNames(
)) == 0, "parameters not supported right now in ocp export, sorry"
# 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 = dae.xVec()
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 computeOutputs(self, x, u):
self._outputsFun.setInput(x, 0)
self._outputsFun.setInput(u, 1)
self._outputsFun.evaluate()
outs = [
C.DMatrix(self._outputsFun.output(k))
for k in range(self._outputsFun.getNumOutputs())
]
return numpy.squeeze(numpy.array(C.veccat(outs)))
def _create_nonlinear_constraints(model, V):
g, lbg, ubg = [], [], []
for k in range(model.n):
# Multiple shooting
[xk_next] = model.F([V['X', k], V['U', k]])
g.append(xk_next - V['X', k+1])
lbg.append(ca.DMatrix.zeros(model.nx))
ubg.append(ca.DMatrix.zeros(model.nx))
# Control constraints
constraint_k = model._set_constraint(V, k)
g.append(constraint_k)
lbg.append(-ca.inf)
ubg.append(0)
g = ca.veccat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
return [g, lbg, ubg]
def add_constraints(self, stage, opti):
# Obtain the discretised system
F = self.discrete_system(stage)
if stage.is_free_time():
opti.subject_to(self.T >= 0)
self.xk = []
self.q = 0
# we only save polynomal coeffs for runge-kutta4
if stage._method.intg == 'rk':
self.poly_coeff = []
else:
self.poly_coeff = None
for k in range(self.N):
FF = F(x0=self.X[k],
u=self.U[k],
t0=self.control_grid[k],
T=self.control_grid[k + 1] - self.control_grid[k],
p=vertcat(veccat(*self.P), self.get_p_control_at(stage, k)))
# Dynamic constraints a.k.a. gap-closing constraints
opti.subject_to(self.X[k + 1] == FF["xf"])
# Save intermediate info
poly_coeff_temp = FF["poly_coeff"]
xk_temp = FF["Xi"]
self.q = self.q + FF["qf"]
# we cannot return a list from a casadi function
self.xk.extend([xk_temp[:, i] for i in range(self.M)])
if self.poly_coeff is not None:
self.poly_coeff.extend(
horzsplit(poly_coeff_temp,
poly_coeff_temp.shape[1] // self.M))
for l in range(self.M):
for c, meta, _ in stage._constraints["integrator"]:
opti.subject_to(self.eval_at_integrator(stage, c, k, l),
meta=meta)
for c, meta, _ in stage._constraints["inf"]:
self.add_inf_constraints(stage, opti, c, k, l, meta)
for c, meta, _ in stage._constraints[
"control"]: # for each constraint expression
opti.subject_to(self.eval_at_control(stage, c, k), meta=meta)
for c, meta, _ in stage._constraints["control"] + stage._constraints[
"integrator"]: # for each constraint expression
# Add it to the optimizer, but first make x,u concrete.
opti.subject_to(self.eval_at_control(stage, c, -1), meta=meta)
self.xk.append(self.X[-1])
for c, meta, _ in stage._constraints[
"point"]: # Append boundary conditions to the end
opti.subject_to(self.eval(stage, c), meta=meta)
def setResidual(self,res):
'''
set the dae implicit residual vector f
where f(x,xdot,z,u,p) == 0
'''
if hasattr(self,'_residual'):
raise ValueError('residual already set')
if isinstance(res,list):
res = C.veccat(res)
self._residual = res
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
self.makeOrthonormal(ginv, R_c2b)
ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
ginv.add(di, '==', 0, tag = ('delta invariant', None))
ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
("Rotational velocities", None))
def minimizeLsq(self, obj):
if isinstance(obj, list):
obj = C.veccat(obj)
C.makeDense(obj)
shape = obj.shape
assert shape[0] == 1 or shape[
1] == 1, 'objective cannot be matrix, got shape: ' + str(shape)
assert not hasattr(self,
'_minLsq'), 'you can only call minimizeLsq once'
self._minLsq = obj
def constrainInvariantErrs():
dcm = mhe.lookup('dcm', timestep=0)
err = C.mul(dcm.T, dcm)
mhe.constrain(
C.veccat([
err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1],
err[0, 2], err[1, 2]
]), '==', 0)
mhe.constrain(mhe.lookup('c', timestep=0), '==', 0)
mhe.constrain(mhe.lookup('cdot', timestep=0), '==', 0)
def _makeImplicitFunction(self):
ffcn = self._makeResidualFunction()
x0 = C.ssym('x0', self.dae.xVec().size())
X = C.ssym('X', self.dae.xVec().size(), self.deg * self.nicp)
Z = C.ssym('Z', self.dae.zVec().size(), self.deg * self.nicp)
u = C.ssym('u', self.dae.uVec().size())
p = C.ssym('p', self.dae.pVec().size())
constraints = []
############################################################
ndiff = self.dae.xVec().size()
x0_ = x0
for nicpIdx in range(self.nicp):
X_ = X[:, nicpIdx * self.deg:(nicpIdx + 1) * self.deg]
Z_ = Z[:, nicpIdx * self.deg:(nicpIdx + 1) * self.deg]
for j in range(1, self.deg + 1):
# Get an expression for the state derivative at the collocation point
xp_jk = self.lagrangePoly.lDotAtTauRoot[j, 0] * x0_
for j2 in range(1, self.deg + 1):
# get the time derivative of the differential states (eq 10.19b)
xp_jk += self.lagrangePoly.lDotAtTauRoot[j,
j2] * X_[:,
j2 - 1]
# Add collocation equations to the NLP
[fk] = ffcn.eval(
[xp_jk / self.h, X_[:, j - 1], Z_[:, j - 1], u, p])
# impose system dynamics (for the differential states (eq 10.19b))
constraints.append(fk[:ndiff])
# impose system dynamics (for the algebraic states (eq 10.19b))
constraints.append(fk[ndiff:])
# Get an expression for the state at the end of the finite element
xf = self.lagrangePoly.lAtOne[0] * x0_
for j in range(1, self.deg + 1):
xf += self.lagrangePoly.lAtOne[j] * X_[:, j - 1]
x0_ = xf
ifcn = C.SXFunction([C.veccat([X, Z]), x0, u, p],
[C.veccat(constraints), xf])
ifcn.init()
return ifcn
def _initialize_ekf_for_arrival_cost_update(self):
self.hfcn = ca.Function("h", [self.x, self.c], [self.h])
self.H = self.hfcn.jac()
ode = {"x": self.x, "p": ca.veccat(self.c, self.u, self.b, self.w), \
"ode": self.f}
self.phi = ca.integrator("integrator", "cvodes", ode, \
{"t0": 0.0, "tf": self._timing.dt_day})
self.Phi = self.phi.jac()
def makeNmpc(dae, N, dt):
mpc = rawe.Ocp(dae, N=N, ts=dt)
ocpOpts = rawe.OcpExportOptions()
ocpOpts['HESSIAN_APPROXIMATION'] = 'GAUSS_NEWTON'
ocpOpts['DISCRETIZATION_TYPE'] = 'MULTIPLE_SHOOTING'
ocpOpts['QP_SOLVER'] = 'QP_QPOASES'
ocpOpts['HOTSTART_QP'] = False
ocpOpts['SPARSE_QP_SOLUTION'] = 'CONDENSING'
# ocpOpts['SPARSE_QP_SOLUTION'] = 'FULL_CONDENSING_U2'
# ocpOpts['AX_NUM_QP_ITERATIONS'] = '30'
ocpOpts['FIX_INITIAL_STATE'] = True
mpc.minimizeLsq(C.veccat([mpc['x'], mpc['v'], mpc['u']]))
mpc.minimizeLsqEndTerm(C.veccat([mpc['x'], mpc['v']]))
cgOpts = {'CXX': 'g++', 'CC': 'gcc'}
return rawe.OcpRT(mpc,
ocpOptions=ocpOpts,
integratorOptions=intOpts,
codegenOptions=cgOpts)
def writeDimensions(topname, dae, measX, measU, mheHorizN, mpcHorizN):
nMeasX = C.veccat([dae[n] for n in measX]).numel()
nMeasU = C.veccat([dae[n] for n in measU]).numel()
nOutputs = C.veccat([dae[n] for n in dae.outputNames()]).numel()
ret = []
ret.append('#ifndef __' + topname + '_DIMENSIONS_H__')
ret.append('#define __' + topname + '_DIMENSIONS_H__')
ret.append('\n')
ret.append('#define NUM_DIFFSTATES ' + str(len(dae.xNames())))
ret.append('#define NUM_ALGVARS ' + str(len(dae.zNames())))
ret.append('#define NUM_CONTROLS ' + str(len(dae.uNames())))
ret.append('#define NUM_PARAMETERS ' + str(len(dae.pNames())))
ret.append('#define NUM_OUTPUTS ' + repr(nOutputs))
ret.append('#define NUM_MEASUREMENTS_X ' + repr(nMeasX))
ret.append('#define NUM_MEASUREMENTS_U ' + repr(nMeasU))
ret.append('#define NUM_MHE_HORIZON ' + str(mheHorizN))
ret.append('#define NUM_MPC_HORIZON ' + str(mpcHorizN))
ret.append('\n')
ret.append('#endif // __' + topname + '_DIMENSIONS_H__')
return '\n'.join(ret)
