def _create_ARC_model(self, numstates=1):
""" Create model with quadrature for amplitude sensitivities
numstates might allow us to calculate entire sARC at once, but
now will use seed method. """
# Allocate symbolic vectors for the model
dphidx = cs.ssym('dphidx', numstates)
t = self.model.inputSX(cs.DAE_T) # time
xd = self.model.inputSX(cs.DAE_X) # differential state
s = cs.ssym("s", self.NEQ, numstates) # sensitivities
p = cs.vertcat([self.model.inputSX(2), dphidx]) # parameters
# Symbolic function (from input model)
ode_rhs = self.model.outputSX()
f_tilde = self.y0[-1]*ode_rhs/(2*np.pi)
# symbolic jacobians
jac_x = self.model.jac(cs.DAE_X, cs.DAE_X)
sens_rhs = jac_x.mul(s)
quad = cs.ssym('q', self.NEQ, numstates)
for i in xrange(numstates):
quad[:,i] = 2*(s[:,i] - dphidx[i]*f_tilde)*(xd - self.avg)
shape = (self.NEQ*numstates, 1)
x = cs.vertcat([xd, s.reshape(shape)])
ode = cs.vertcat([ode_rhs, sens_rhs.reshape(shape)])
ffcn = cs.SXFunction(cs.daeIn(t=t, x=x, p=p),
cs.daeOut(ode=ode, quad=quad))
return ffcn
def model():
# Time Variable
t = cs.ssym("t")
# Variable Assignments
X = cs.ssym("X")
Y = cs.ssym("Y")
y = cs.vertcat([X, Y])
# Parameter Assignments
u = cs.ssym("u")
ode = [[]] * NEQ
ode[0] = u * (X - (X**3 / 3.) - Y)
ode[1] = X / u
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=cs.vertcat([
u,
])), cs.daeOut(ode=ode))
fn.setOption("name", "Van der Pol oscillator")
return fn
def model():
""" Create an ODE casadi SXFunction """
# State Variables
Y1 = cs.ssym("Y1")
Y2 = cs.ssym("Y2")
Y3 = cs.ssym("Y3")
y = cs.vertcat([Y1, Y2, Y3])
# Parameters
c1x1 = cs.ssym("c1x1")
c2 = cs.ssym("c2")
c3x2 = cs.ssym("c3x2")
c4 = cs.ssym("c4")
c5x3 = cs.ssym("c5x3")
symparamset = cs.vertcat([c1x1, c2, c3x2, c4, c5x3])
# Time
t = cs.ssym("t")
# ODES
ode = [[]] * NEQ
ode[0] = c1x1 * Y2 - c2 * Y1 * Y2 + c3x2 * Y1 - c4 * Y1**2
ode[1] = -c1x1 * Y2 - c2 * Y1 * Y2 + c5x3 * Y3
ode[2] = c3x2 * Y1 - c5x3 * Y3
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=symparamset), cs.daeOut(ode=ode))
fn.setOption("name", "Oregonator")
return fn
def model():
# Variable Assignments
X = cs.ssym("X")
Y = cs.ssym("Y")
y = cs.vertcat([X,Y]) # vector version of y
# Parameter Assignments
P = cs.ssym("P")
kt = cs.ssym("kt")
kd = cs.ssym("kd")
a0 = cs.ssym("a0")
a1 = cs.ssym("a1")
a2 = cs.ssym("a2")
symparamset = cs.vertcat([P, kt, kd, a0, a1, a2])
# Time Variable (typically unused for autonomous odes)
t = cs.ssym("t")
ode = [[]]*NEQ
ode[0] = 1 / (1 + Y**P) - X
ode[1] = kt*X - kd*Y - Y/(a0 + a1*Y + a2*Y**2)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=symparamset),
cs.daeOut(ode=ode))
fn.setOption("name","Simple Hysteresis model")
return fn
def solve(self):
"""
define extra variables
homotopy parameters >= 0!!
"""
if not self.prob['s']:
self.set_grid()
N = self.options['N']
Nc = self.options['Nc']
self.prob['vars'] = [cas.ssym("b", N + 1, self.sys.order),
cas.ssym("h", Nc, self.h.size1())]
# Vectorize variables
V = cas.vertcat([
cas.vec(self.prob['vars'][0]),
cas.vec(self.prob['vars'][1])
])
self._make_constraints()
self._make_objective()
self._h_init()
con = cas.SXFunction([V], [self.prob['con'][0]])
obj = cas.SXFunction([V], [self.prob['obj']])
if self.options.get('solver') == 'Ipopt':
solver = cas.IpoptSolver(obj, con)
else:
print """Other solver than Ipopt are currently not supported,
switching to Ipopt"""
solver = cas.IpoptSolver(obj, con)
for option, value in self.options.iteritems():
if solver.hasOption(option):
solver.setOption(option, value)
solver.init()
# Setting constraints
solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg")
solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg")
solver.setInput(
cas.vertcat([
[np.inf] * self.sys.order * (N + 1),
[1] * self.h.size1() * Nc
]), "ubx"
)
solver.setInput(
cas.vertcat([
[0] * (N + 1),
[-np.inf] * (self.sys.order - 1) * (N + 1),
[0] * self.h.size1() * Nc
]), "lbx"
)
solver.solve()
self.prob['solver'] = solver
self._get_solution()
def model():
#======================================================================
# Variable Assignments
#======================================================================
m1 = cs.ssym("m1")
m2 = cs.ssym("m2")
m3 = cs.ssym("m3")
y = cs.vertcat([m1, m2, m3])
#======================================================================
# Parameter Assignments
#======================================================================
alpha1 = cs.ssym("alpha1")
alpha2 = cs.ssym("alpha2")
alpha3 = cs.ssym("alpha3")
n = cs.ssym("n")
symparamset = cs.vertcat([alpha1, alpha2, alpha3, n])
# Time Variable
t = cs.ssym("t")
ode = [[]] * NEQ
ode[0] = alpha1 / (1. + m2**n) - m1
ode[1] = alpha2 / (1. + m3**n) - m2
ode[2] = alpha3 / (1. + m1**n) - m3
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=symparamset), cs.daeOut(ode=ode))
fn.setOption("name", "Small repressilator")
return fn
def __init__(self, ny, order):
self.order = order
self.ny = ny
self.y = cas.ssym("y", ny, self.order + 1)
self.x = dict()
self._nx = 0
self._dy = cas.horzcat([self.y[:, 1:], cas.SXMatrix.zeros(ny, 1)])
def __init__(self, ny, order):
self.order = order
self.ny = ny
self.y = cas.ssym("y", ny, self.order + 1)
self.x = dict()
self._nx = 0
self._dy = cas.horzcat([self.y[:, 1:], cas.SXMatrix.zeros(ny, 1)])
def _mkLagrangePolynomials(self):
# Collocation point
tau = CS.ssym("_tau")
# lagrange polynomials
self.lfcns = []
# lagrange polynomials evaluated at collocation points
self.lAtOne = np.zeros(self.deg+1)
# derivative of lagrange polynomials evaluated at collocation points
self.lDotAtTauRoot = np.zeros((self.deg+1,self.deg+1))
# For all collocation points: eq 10.4 or 10.17 in Biegler's book
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
for j in range(self.deg+1):
L = 1
for k in range(self.deg+1):
if k != j:
L *= (tau-self.tau_root[k])/(self.tau_root[j]-self.tau_root[k])
lfcn = CS.SXFunction([tau],[L])
lfcn.init()
self.lfcns.append(lfcn)
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn.setInput(1.0)
lfcn.evaluate()
self.lAtOne[j] = lfcn.output()
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the collocation equation
for k in range(self.deg+1):
lfcn.setInput(self.tau_root[k])
lfcn.setFwdSeed(1.0)
lfcn.evaluate(1,0)
self.lDotAtTauRoot[k,j] = lfcn.fwdSens()
def __init__(self, sys):
self.sys = sys
self.s = cas.ssym("s", self.sys.order + 1)
self.path = cas.SXMatrix.nan(self.sys.y.shape[0], self.sys.order + 1)
self.constraints = []
self.objective = {'Lagrange': [], 'Mayer': []}
self.options = {
'N': 199,
'Ts': 0.01,
'solver': 'Ipopt',
'tol': 1e-6,
'max_iter': 100,
'plot': True,
'reg': 1e-20,
'bc': False
}
self.sol = {
's': [],
't': [],
'states': [],
'inputs': [],
'diagnostics': 0
}
self.prob = {
'var': [],
'con': [],
'obj': [],
'solver': [],
'x_init': None,
's': [],
'path': []
}
def transcription_function(self, weight):
"""
Returns a function that can be used with Collocation's
'minimize_f', such that total transcription is minimized with
constant weight 'weight'
"""
x = cs.vertcat(self.states)
p = cs.vertcat(self.params)
txn = [
rxn.rate for rxn in self.reactions if type(rxn) is TranscriptionRxn
]
txn = weight * cs.sumAll(cs.vertcat(txn))
t = cs.ssym('t')
fn = cs.SXFunction([x, p], [txn])
fn.init()
def return_func(x, p):
""" Function to return for Collocation """
try:
fn.setInput(x, 0)
fn.setInput(p, 1)
fn.evaluate()
return float(fn.output().toArray())
except Exception:
return fn.call([x, p])[0]
return return_func
def inverse_parameterization(R, parameterizationFunction):
angle1 = ssym('angle1')
angle2 = ssym('angle2')
angle3 = ssym('angle3')
Restimated = parameterizationFunction(angle1,angle2,angle3)
E = R - Restimated;
e = casadi.sumAll(E*E)
f = SXFunction([casadi.vertcat([angle1,angle2,angle3])], [e])
f.init()
anglerange = numpy.linspace(-1*numpy.pi,1*numpy.pi,25)
best_eTemp = 9999999999999
best_angle1 = numpy.nan
best_angle2 = numpy.nan
best_angle3 = numpy.nan
for i in xrange(len(anglerange)):
for j in xrange(len(anglerange)):
for k in xrange(len(anglerange)):
r = anglerange[i]
p = anglerange[j]
y = anglerange[k]
f.setInput([r,p,y])
f.evaluate()
eTemp = f.output(0)
if eTemp<best_eTemp:
best_eTemp = eTemp
best_angle1 = r
best_angle2 = p
best_angle3 = y
if 0:
V = msym("V",(3,1))
fval = f.call([V])
fmx = MXFunction([V],fval)
nlp_solver = IpoptSolver(fmx)
else:
nlp_solver = IpoptSolver(f)
nlp_solver.setOption('generate_hessian',True)
nlp_solver.setOption('expand_f',True)
nlp_solver.setOption('expand_g',True)
nlp_solver.init()
nlp_solver.setInput([best_angle1,best_angle2,best_angle3],NLP_X_INIT)
nlp_solver.solve()
sol = nlp_solver.output(NLP_X_OPT).toArray()
sol = nlp_solver.output(NLP_X_OPT).toArray()
return sol
def model():
""" Create an ODE casadi SXFunction """
# State Variables
Y1 = cs.ssym("Y1")
Y2 = cs.ssym("Y2")
y = cs.vertcat([Y1, Y2])
# Parameters
c1x1 = cs.ssym("c1x1")
c2x2 = cs.ssym("c2x2")
c3 = cs.ssym("c3")
c4 = cs.ssym("c4")
symparamset = cs.vertcat([c1x1, c2x2, c3, c4])
# Time
t = cs.ssym("t")
# ODES
ode = [[]] * NEQ
ode[0] = c1x1 - c2x2 * Y1 + (c3 / 2.) * (Y1**2) * Y2 - c4 * Y1
ode[1] = c2x2 * Y1 - (c3 / 2.) * (Y1**2) * Y2
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=symparamset), cs.daeOut(ode=ode))
fn.setOption("name", "Brusselator")
return fn
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 __init__(self, ssOcps):
if not isinstance(ssOcps, list):
ssOcps = [ssOcps]
self.ssOcps = {}
for ssOcp in ssOcps:
self.ssOcps[ssOcp.ode.name] = ssOcps
self.bigBigN = sum([ssOcp._getBigN() for ssOcp in self.ssOcps.values()])
self.designVariables = C.ssym('designVariables', self.bigBigN)
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 __init__(self, ssOcps):
if not isinstance(ssOcps, list):
ssOcps = [ssOcps]
self.ssOcps = {}
for ssOcp in ssOcps:
self.ssOcps[ssOcp.ode.name] = ssOcps
self.bigBigN = sum(
[ssOcp._getBigN() for ssOcp in self.ssOcps.values()])
self.designVariables = C.ssym('designVariables', self.bigBigN)
def ddt(self,name):
'''
get the time derivative of a differential state
'''
if name not in self._xNames:
raise ValueError("unrecognized state \""+name+"\"")
try:
return self._dummyDdtMap[name]
except KeyError:
self._dummyDdtMap[name] = C.ssym('_DotDummy_'+name)
return self.ddt(name)
def test_R_from_roll_pitch_yaw():
# Use it twice for different sets of angles
r1= ssym('roll')
p1= ssym('pitch')
y1= ssym('yaw')
r2= ssym('roll')
p2= ssym('pitch')
y2= ssym('yaw')
R1 = R_from_roll_pitch_yaw_symbolic(r1, p1, y1)
R2 = R_from_roll_pitch_yaw_symbolic(r2, p2, y2)
# Still building up everything symbolic as Matrix<SX>
R = R1*R2
#from casadi.tools.graph import dotdraw
#dotdraw(R)
# As one of the last steps, convert to SXFunction for code generation, optimization, etc...
v = SXMatrix(6,1)
v[0,0] = r1
v[1,0] = p1
v[2,0] = y1
v[3,0] = r2
v[4,0] = p2
v[5,0] = y2
f_RPY = SXFunction([v],[R])
f_RPY.init()
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 set_parameter(self, parameter_str, val):
"""
Remove a parameter (parameter_str, string) from the equations by
setting it to a fixed value (val, float), then removing it from
the self.params list. Must be called after build_odes and before
build_model.
"""
parameter, index = self.params.lookup(parameter_str)
self.params.pop(index)
for i, ode in enumerate(self.odes):
self.odes[i] = cs.substitute(ode, parameter, cs.ssym(val))
def _addVar(self,name,namelist):
self.assertXzupNotFrozen()
if isinstance(name,list):
return [self._addVar(n,namelist) for n in name]
assert(isinstance(name,str))
self.assertUniqueName(name)
namelist.append(name)
ret = C.ssym(name)
self._syms[name] = ret
return ret
def create_integrator_rk4():
u = C.SX("u") # control for one segment
k = C.SX("k") # spring constant
b = C.SX("b") # viscous damping coefficient
# initial state
s0 = C.SX("s0") # initial position
v0 = C.SX("v0") # initial speed
t0 = C.SX("t0") # initial time
tf = C.SX("tf") # final time
m = 1.0 # mass
# apply fixed step rk4
def eoms(x_, scalar, kn, t_):
s_ = x_[0] + scalar * kn[0]
v_ = x_[1] + scalar * kn[1]
return [v_, (u - k * s_ - b * v_) / m]
dt = tf - t0 # time step
x0 = [s0, v0]
k1 = eoms(x0, 0, [0, 0], t0)
k2 = eoms(x0, 0.5 * dt, k1, t0 + 0.5 * dt)
k3 = eoms(x0, 0.5 * dt, k2, t0 + 0.5 * dt)
k4 = eoms(x0, dt, k3, t0 + dt)
s = s0 + dt * (k1[0] + 2 * k2[0] + 2 * k3[0] + k4[0]) / 6
v = v0 + dt * (k1[1] + 2 * k2[1] + 2 * k3[1] + k4[1]) / 6
# State vector
x = [s, v]
# Input to the integrator function being created
ivpsol_in = C.INTEGRATOR_NUM_IN * [[]]
ivpsol_in[C.INTEGRATOR_T0] = [t0]
ivpsol_in[C.INTEGRATOR_TF] = [tf]
ivpsol_in[C.INTEGRATOR_X0] = x0
ivpsol_in[C.INTEGRATOR_P] = [u, k, b]
# Create a dummy state derivative vector
xp0 = C.ssym("x", len(x)).data()
ivpsol_in[C.INTEGRATOR_XP0] = xp0
# Create the explicit rk4 integrator
integrator = C.SXFunction(ivpsol_in, [x])
return integrator
def create_integrator_rk4():
u = C.SX("u") # control for one segment
k = C.SX("k") # spring constant
b = C.SX("b") # viscous damping coefficient
# initial state
s0 = C.SX("s0") # initial position
v0 = C.SX("v0") # initial speed
t0 = C.SX("t0") # initial time
tf = C.SX("tf") # final time
m = 1.0 # mass
# apply fixed step rk4
def eoms(x_, scalar, kn, t_):
s_ = x_[0] + scalar*kn[0]
v_ = x_[1] + scalar*kn[1]
return [v_, (u - k*s_ - b*v_)/m]
dt = tf-t0 # time step
x0 = [s0, v0]
k1 = eoms( x0, 0, [0,0], t0 );
k2 = eoms( x0, 0.5*dt, k1, t0 + 0.5*dt );
k3 = eoms( x0, 0.5*dt, k2, t0 + 0.5*dt );
k4 = eoms( x0, dt, k3, t0 + dt );
s = s0 + dt*(k1[0] + 2*k2[0] + 2*k3[0] + k4[0])/6
v = v0 + dt*(k1[1] + 2*k2[1] + 2*k3[1] + k4[1])/6
# State vector
x = [s,v]
# Input to the integrator function being created
ivpsol_in = C.INTEGRATOR_NUM_IN * [[]]
ivpsol_in[C.INTEGRATOR_T0] = [t0]
ivpsol_in[C.INTEGRATOR_TF] = [tf]
ivpsol_in[C.INTEGRATOR_X0] = x0
ivpsol_in[C.INTEGRATOR_P] = [u,k,b]
# Create a dummy state derivative vector
xp0 = C.ssym("x",len(x)).data()
ivpsol_in[C.INTEGRATOR_XP0] = xp0
# Create the explicit rk4 integrator
integrator = C.SXFunction(ivpsol_in,[x])
return integrator
def discretize(self, N):
if self.ode.locked:
errStr = "Ode "+self.name+" has already been discretized and is in read-only mode"
raise ValueError(errStr)
self.ode.locked = True
self.N = N
#self.designVariables = C.MX('designVariables', self._getBigN())
self.designVariables = C.ssym('designVariables', self._getBigN())
self.lb = [-1e-15 for k in range(self._getBigN())]
self.ub = [ 1e-15 for k in range(self._getBigN())]
self.guess = [ 0 for k in range(self._getBigN())]
def _addVar(self,name,namelist,namedict):
self.assertNotFrozen()
if isinstance(name,list):
return [self._addVar(n,namelist,namedict) for n in name]
assert(isinstance(name,str))
if name in namedict:
raise KeyError(name+' is already in in '+str(namelist))
self.assertUniqueName(name)
namelist.append(name)
namedict[name] = C.ssym(name)
return namedict[name]
def discretize(self, N):
if self.ode.locked:
errStr = "Ode " + self.name + " has already been discretized and is in read-only mode"
raise ValueError(errStr)
self.ode.locked = True
self.N = N
#self.designVariables = C.MX('designVariables', self._getBigN())
self.designVariables = C.ssym('designVariables', self._getBigN())
self.lb = [-1e-15 for k in range(self._getBigN())]
self.ub = [1e-15 for k in range(self._getBigN())]
self.guess = [0 for k in range(self._getBigN())]
def ODEModel():
# Variable Assignments
X = cs.ssym("X")
Y = cs.ssym("Y")
sys = cs.vertcat([X, Y]) # vector version of y
# Parameter Assignments
P = cs.ssym("P")
kt = cs.ssym("kt")
kd = cs.ssym("kd")
a0 = cs.ssym("a0")
a1 = cs.ssym("a1")
a2 = cs.ssym("a2")
kdx = cs.ssym("kdx")
paramset = cs.vertcat([P, kt, kd, a0, a1, a2, kdx])
#================================================================================
# Model Equations
#================================================================================
ode = [[]] * EqCount
# MRNA Species
t = cs.ssym("t")
ode = [[]] * EqCount
ode[0] = 1 / (1 + Y**P) - kdx * X
ode[1] = kt * X - kd * Y - Y / (a0 + a1 * Y + a2 * Y**2)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=sys, p=paramset), cs.daeOut(ode=ode))
fn.setOption("name", "2state")
return fn
def phase_of_point(self, point, error=False, tol=1E-3):
""" Finds the phase at which the distance from the point to the
limit cycle is minimized. phi=0 corresponds to the definition of
y0, returns the phase and the minimum distance to the limit
cycle """
point = np.asarray(point)
for i in xrange(100):
dist = cs.ssym("dist")
x = self.model.inputSX(cs.DAE_X)
ode = self.model.outputSX()
dist_ode = cs.sumAll(2*(x - point)*ode)
cat_x = cs.vertcat([x, dist])
cat_ode = cs.vertcat([ode, dist_ode])
dist_model = cs.SXFunction(
cs.daeIn(t=self.model.inputSX(cs.DAE_T), x=cat_x,
p=self.model.inputSX(cs.DAE_P)),
cs.daeOut(ode=cat_ode))
dist_model.setOption("name","distance model")
dist_0 = ((self.y0[:-1] - point)**2).sum()
cat_y0 = np.hstack([self.y0[:-1], dist_0, self.y0[-1]])
roots_class = pBase(dist_model, self.paramset, cat_y0)
# roots_class.solveBVP()
roots_class.roots()
phase = self._t_to_phi(roots_class.Tmin[-1])
distance = roots_class.Ymin[-1]
if distance < tol: return phase, distance
intr = cs.CVodesIntegrator(self.model)
intr.setOption("abstol", self.intoptions['transabstol'])
intr.setOption("reltol", self.intoptions['transreltol'])
intr.setOption("tf", self.y0[-1])
intr.setOption("max_num_steps",
self.intoptions['transmaxnumsteps'])
intr.setOption("disable_internal_warnings", True)
intr.init()
intr.setInput(point, cs.INTEGRATOR_X0)
intr.setInput(self.paramset, cs.INTEGRATOR_P)
intr.evaluate()
point = intr.output().toArray().flatten()
raise RuntimeError("Point failed to converge to limit cycle")
def solve(self):
"""Solve the optimal control problem
solve() first check for steady state feasibility and defines the
optimal control problem. After solving, the instance variable sol can
be used to examine the solution.
TODO: Add support for other solvers
TODO: version bump
"""
if len(self.prob['s']) == 0:
self.set_grid()
# Check feasibility
# self.check_ss_feasibility()
# Construct optimization problem
self.prob['solver'] = None
N = self.options['N']
self.prob['vars'] = cas.ssym("b", N + 1, self.sys.order)
V = cas.vec(self.prob['vars'])
self._make_objective()
self._make_constraints()
con = cas.SXFunction([V], [self.prob['con'][0]])
obj = cas.SXFunction([V], [self.prob['obj']])
if self.options.get('solver') == 'Ipopt':
solver = cas.IpoptSolver(obj, con)
else:
print """Other solver than Ipopt are currently not supported,
switching to Ipopt"""
solver = cas.IpoptSolver(obj, con)
for option, value in self.options.iteritems():
if solver.hasOption(option):
solver.setOption(option, value)
solver.init()
# Setting constraints
solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg")
solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg")
solver.setInput([np.inf] * self.sys.order * (N + 1), "ubx")
solver.setInput(
cas.vertcat((
[0] * (N + 1),
(self.sys.order - 1) * (N + 1) * [-np.inf])),
"lbx")
solver.solve()
self.prob['solver'] = solver
self._get_solution()
def __init__(self, sys):
self.sys = sys
self.s = cas.ssym("s", self.sys.order + 1)
self.path = cas.SXMatrix.nan(self.sys.y.shape[0], self.sys.order + 1)
self.constraints = []
self.objective = {'Lagrange': [], 'Mayer': []}
self.options = {
'N': 199, 'Ts': 0.01, 'solver': 'Ipopt', 'tol': 1e-6,
'max_iter': 100, 'plot': True, 'reg': 1e-20, 'bc': False
}
self.sol = {
's': [], 't': [], 'states': [], 'inputs': [], 'diagnostics': 0
}
self.prob = {
'var': [], 'con': [], 'obj': [], 'solver': [], 'x_init': None,
's': [], 'path': []
}
def _reform_model(self):
"""
Reform self.model to conform to the standard (implicit) form
used by the rest of the collocation calculations.
"""
#
# Allocate symbolic vectors for the model
t = self.model.inputSX(cs.DAE_T) # time
xd = self.model.inputSX(cs.DAE_X) # differential state
xddot = cs.ssym("xd", self.NEQ) # differential state dt
p = self.model.inputSX(2) # parameters
# Symbolic function (from input model)
ode_rhs = self.model.outputSX()
ode = xddot[:self.NEQ] - ode_rhs
self.rfmod = cs.SXFunction([t, xddot, xd, p], [ode])
self.rfmod.init()
def solve(self):
"""Solve the optimal control problem
solve() first check for steady state feasibility and defines the
optimal control problem. After solving, the instance variable sol can
be used to examine the solution.
TODO: Add support for other solvers
TODO: version bump
"""
if len(self.prob['s']) == 0:
self.set_grid()
# Check feasibility
# self.check_ss_feasibility()
# Construct optimization problem
self.prob['solver'] = None
N = self.options['N']
self.prob['vars'] = cas.ssym("b", N + 1, self.sys.order)
V = cas.vec(self.prob['vars'])
self._make_objective()
self._make_constraints()
con = cas.SXFunction([V], [self.prob['con'][0]])
obj = cas.SXFunction([V], [self.prob['obj']])
if self.options.get('solver') == 'Ipopt':
solver = cas.IpoptSolver(obj, con)
else:
print """Other solver than Ipopt are currently not supported,
switching to Ipopt"""
solver = cas.IpoptSolver(obj, con)
for option, value in self.options.iteritems():
if solver.hasOption(option):
solver.setOption(option, value)
solver.init()
# Setting constraints
solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg")
solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg")
solver.setInput([np.inf] * self.sys.order * (N + 1), "ubx")
solver.setInput(
cas.vertcat(
([0] * (N + 1), (self.sys.order - 1) * (N + 1) * [-np.inf])),
"lbx")
solver.solve()
self.prob['solver'] = solver
self._get_solution()
def setupCoeffs(deg=None,collPoly=None,nk=None,h=None):
assert(deg is not None)
assert(collPoly is not None)
assert(nk is not None)
assert(h is not None)
collocation_points = mkCollocationPoints()
assert( collPoly in collocation_points )
# Coefficients of the collocation equation
C = np.zeros((deg+1,deg+1))
# Coefficients of the continuity equation
D = np.zeros(deg+1)
# Collocation point
tau = CS.ssym("__collocation_tau__")
# All collocation time points
tau_root = collocation_points[collPoly][deg]
# T = np.zeros((nk,deg+1))
# for i in range(nk):
# for j in range(deg+1):
# T[i][j] = h*(i + tau_root[j])
# For all collocation points: eq 10.4 or 10.17 in Biegler's book
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
for j in range(deg+1):
L = 1
for j2 in range(deg+1):
if j2 != j:
L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2])
lfcn = CS.SXFunction([tau],[L])
lfcn.init()
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn.setInput(1.0)
lfcn.evaluate()
D[j] = lfcn.output()
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
for j2 in range(deg+1):
lfcn.setInput(tau_root[j2])
lfcn.setFwdSeed(1.0)
lfcn.evaluate(1,0)
C[j][j2] = lfcn.fwdSens()
return (C,D,tau,tau_root)
def build_model(self):
"""
Takes inputs from self.states, self.params and self.odes to
build a casadi SXFunction model in self.model. Also calculates
self.NP and self.NEQ
"""
x = cs.vertcat(self.states)
p = cs.vertcat(self.params)
ode = cs.vertcat(self.odes)
t = cs.ssym('t')
fn = cs.SXFunction(cs.daeIn(t=t, x=x, p=p), cs.daeOut(ode=ode))
self.model = fn
self.NP = len(self.params)
self.NEQ = len(self.states)
def modifiedModel(self):
"""
Creates a new casadi model with period as a parameter, such that
the model has an oscillatory period of 1. Necessary for the
exact determinination of the period and initial conditions
through the BVP method. (see Wilkins et. al. 2009 SIAM J of Sci
Comp)
"""
pSX = self.model.inputSX(cs.DAE_P)
T = cs.ssym("T")
pTSX = cs.vertcat([pSX, T])
self.modlT = cs.SXFunction(
cs.daeIn(t=self.model.inputSX(cs.DAE_T),
x=self.model.inputSX(cs.DAE_X), p=pTSX),
cs.daeOut(ode=cs.vertcat([self.model.outputSX()*T])))
self.modlT.setOption("name","T-shifted model")
def set_path(self, paths):
"""The path is defined as the convex combination of the paths in paths.
Args:
paths (list of lists of SXMatrix): The path is taken as the
convex combination of the paths in paths.
Example:
The path is defined as the convex combination of
(s, 0.5*s) and (2, 2*s):
>>> P.set_path([(P.s[0], 0.5 * P.s[0]), [P.s[0], 2 * P.s[0]]])
"""
l = len(paths)
self.h = cas.ssym("h", l, self.sys.order + 1)
self.path[:, 0] = np.sum(cas.SXMatrix(paths) * cas.horzcat([self.h[:, 0]] * len(paths[0])), axis=0)
dot_s = cas.vertcat([self.s[1:], 0])
dot_h = cas.horzcat([self.h[:, 1:], cas.SXMatrix.zeros(l, 1)])
for i in range(1, self.sys.order + 1): # Chainrule
self.path[:, i] = (cas.mul(cas.jacobian(self.path[:, i - 1], self.s), dot_s) +
sum([cas.mul(cas.jacobian(self.path[:, i - 1], self.h[j, :]), dot_h[j, :].trans()) for j in range(l)]) * self.s[1])
def model():
""" Deterministic model """
# State Variables
X1 = cs.ssym('X1')
X2 = cs.ssym('X2')
X3 = cs.ssym('X3')
y = cs.vertcat([X1, X2, X3])
# Parameters
h = cs.ssym('h')
a = cs.ssym('a')
i = cs.ssym('i')
r = cs.ssym('r')
symparamset = cs.vertcat([h, a, i, r])
# Time
t = cs.ssym('t')
# ODES
ode = [[]]*NEQ
ode[0] = 1./(1. + X3**h) - a*X1
ode[1] = X1 - i*X2
ode[2] = X2 - r*X3
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=symparamset),
cs.daeOut(ode=ode))
fn.setOption("name", "Goodwin")
return fn
def ReformModel(self):
"""
Reform self.model to conform to the standard (implicit) form
used by the rest of the collocation calculations. Should also
append sensitivity states (dy/dp_i) given as a list to the end
of the ode model.
Monodromy - calculate a symbolic monodromy matrix by integrating
dy/dy_0,i
"""
T = cs.ssym("T") # Period
if self.NLPdata['PRC']:
self.NVAR = 2 * self.NEQ
s = cs.ssym("s", self.NEQ, 1) # sensitivities
jac_x = self.model.jac(cs.DAE_X, cs.DAE_X)
prc_rhs = T * jac_x.T.mul(s)
else:
self.NVAR = self.NEQ
s = cs.ssym("s", 0, 1) # sensitivities
prc_rhs = 0
# Allocate symbolic vectors for the model
t = self.model.inputSX(cs.DAE_T) # time
u = cs.ssym("u", 0, 1) # control (empty)
xd_in = self.model.inputSX(cs.DAE_X) # differential state
xa = cs.ssym("xa", 0, 1) # algebraic state (empty)
xddot = cs.ssym("xd", self.NVAR) # differential state dt
p = self.model.inputSX(2) # parameters
# Symbolic function (from input model)
ode_rhs = T * self.model.outputSX()
# symbolic jacobians
ode = xddot[:self.NEQ] - ode_rhs
sens = xddot[self.NEQ:] - prc_rhs
xd = cs.vertcat([xd_in, s])
tot = cs.vertcat([ode, sens])
p_in = cs.vertcat([p, T])
self.rfmod = cs.SXFunction([t, xddot, xd, xa, u, p_in], [tot])
self.rfmod.init()
def _make_path(self):
"""Rewrite the path as a function of the optimization variables.
Substitutes the time derivatives of s in the expression of the path by
expressions that are function of b and its path derivatives by
repeatedly applying the chainrule
Returns:
* SXMatrix. The substituted path
* SXMatrix. b and the path derivatives
* SXMatrix. The derivatives of s as a function of b
"""
b = cas.ssym("b", self.sys.order)
db = cas.vertcat((b[1:], 0))
Ds = cas.SXMatrix.nan(self.sys.order) # Time derivatives of s
Ds[0] = cas.sqrt(b[0])
Ds[1] = b[1] / 2
# Apply chainrule for finding higher order derivatives
for i in range(1, self.sys.order - 1):
Ds[i + 1] = (cas.mul(cas.jacobian(Ds[i], b), db) * self.s[1] +
cas.jacobian(Ds[i], self.s[1]) * Ds[1])
Ds = cas.substitute(Ds, self.s[1], cas.sqrt(b[0]))
return cas.substitute(self.path, self.s[1:], Ds), b, Ds
def setTimeInterval(self, t0, tf):
self.t0 = t0
self.tf = tf
#self.ssStates = C.msym("ssStates", self.ode._Nx(), self.N)
self.ssStates = C.ssym("ssStates", self.ode._Nx(), self.N)
self.ssStates[:, 0] = self._getState0Vec()
# dynamics constraint
dt = (self.tf - self.t0) / (self.N - 1)
for k in range(self.N - 1):
x0 = self._getStateVec(k)
u0 = self._getActionVec(k)
u1 = self._getActionVec(k + 1)
p = self._getParamVec()
t0 = k * dt
t1 = (k + 1) * dt
self.ssStates[:, k + 1] = self.ode.rk4Step(x0, u0, u1, p, t0, t1)
def setTimeInterval(self, t0, tf):
self.t0 = t0
self.tf = tf
#self.ssStates = C.msym("ssStates", self.ode._Nx(), self.N)
self.ssStates = C.ssym("ssStates", self.ode._Nx(), self.N)
self.ssStates[:,0] = self._getState0Vec()
# dynamics constraint
dt = (self.tf - self.t0)/(self.N - 1)
for k in range(self.N-1):
x0 = self._getStateVec(k)
u0 = self._getActionVec(k)
u1 = self._getActionVec(k+1)
p = self._getParamVec()
t0 = k*dt
t1 = (k+1)*dt
self.ssStates[:,k+1] = self.ode.rk4Step( x0, u0, u1, p, t0, t1)
def _make_path(self):
"""Rewrite the path as a function of the optimization variables.
Substitutes the time derivatives of s in the expression of the path by
expressions that are function of b and its path derivatives by
repeatedly applying the chainrule
Returns:
* SXMatrix. The substituted path
* SXMatrix. b and the path derivatives
* SXMatrix. The derivatives of s as a function of b
"""
b = cas.ssym("b", self.sys.order)
db = cas.vertcat((b[1:], 0))
Ds = cas.SXMatrix.nan(self.sys.order) # Time derivatives of s
Ds[0] = cas.sqrt(b[0])
Ds[1] = b[1] / 2
# Apply chainrule for finding higher order derivatives
for i in range(1, self.sys.order - 1):
Ds[i + 1] = (cas.mul(cas.jacobian(Ds[i], b), db) * self.s[1] +
cas.jacobian(Ds[i], self.s[1]) * Ds[1])
Ds = cas.substitute(Ds, self.s[1], cas.sqrt(b[0]))
return cas.substitute(self.path, self.s[1:], Ds), b, Ds
def kiss_model_ode():
# For two oscillators
e1 = cs.ssym('e1')
tht1 = cs.ssym('tht1')
e2 = cs.ssym('e2')
tht2 = cs.ssym('tht2')
state_set = cs.vertcat([e1, tht1, e2, tht2])
# Parameter Assignments
A1Rind1 = cs.ssym('A1Rind1')
A2Rind2 = cs.ssym('A2Rind2')
V = cs.ssym('V')
Ch = cs.ssym('Ch')
a = cs.ssym('a')
b = cs.ssym('b')
c = cs.ssym('c')
gam1 = cs.ssym('gam1')
gam2 = cs.ssym('gam2')
AR = cs.ssym('AR')
# AR is coupling strength, = 50.5 for model in paper
param_set = cs.vertcat([A1Rind1, A2Rind2, V, Ch, a, b, c, gam1, gam2, AR])
# Time
t = cs.ssym('t')
ode = [[]] * neq
# oscillator 1
ode[0] = (V+100*np.sin(10*t)-e1)/A1Rind1 - \
( Ch*cs.exp(0.5*e1)/(1+Ch*cs.exp(e1)) + a*cs.exp(e1) )*(1-tht1) -\
(e1-e2)/AR
ode[1] = (1/gam1)*(\
(cs.exp(0.5*e1)/(1+Ch*cs.exp(e1)))*(1-tht1) -\
b*Ch*cs.exp(2*e1)*tht1/(c*Ch+cs.exp(e1))\
)
# oscillator 2
ode[2] = (V-e2-np.sin(freq/(2*np.pi)*t))/A2Rind2 - \
( Ch*cs.exp(0.5*e2)/(1+Ch*cs.exp(e2)) + a*cs.exp(e2) )*(1-tht2) -\
(e2-e1)/AR
ode[3] = (1/gam1)*(\
(cs.exp(0.5*e2)/(1+Ch*cs.exp(e2)))*(1-tht2) -\
b*Ch*cs.exp(2*e2)*tht2/(c*Ch+cs.exp(e2))\
)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=state_set, p=param_set),
cs.daeOut(ode=ode))
fn.setOption("name", "kiss_oscillator_2011")
return fn
def freeModel(endTimeSteps=None):
stateNames = [ "x" # state 0
, "y" # state 1
, "z" # state 2
, "e11" # state 3
, "e12" # state 4
, "e13" # state 5
, "e21" # state 6
, "e22" # state 7
, "e23" # state 8
, "e31" # state 9
, "e32" # state 10
, "e33" # state 11
, "dx" # state 12
, "dy" # state 13
, "dz" # state 14
, "w1" # state 15
, "w2" # state 16
, "w3" # state 17
]
uNames = [ "tc"
, "aileron"
, "elevator"
, "rudder"
# , 'ddr'
]
pNames = [ "wind_x"
]
uSyms = [C.ssym(name) for name in uNames]
uDict = dict(zip(uNames,uSyms))
uVec = C.veccat( uSyms )
pSyms = [C.ssym(name) for name in pNames]
pDict = dict(zip(pNames,pSyms))
pVec = C.veccat( pSyms )
stateSyms = [C.ssym(name) for name in stateNames]
stateDict = dict(zip(stateNames,stateSyms))
stateVec = C.veccat( stateSyms )
dx = stateDict['dx']
dy = stateDict['dy']
dz = stateDict['dz']
outputs = {}
outputs['aileron(deg)']=uDict['aileron']*180/C.pi
outputs['elevator(deg)']=uDict['elevator']*180/C.pi
outputs['rudder(deg)']=uDict['rudder']*180/C.pi
(massMatrix, rhs, dRexp) = modelInteg(stateDict, uDict, pDict, outputs)
zVec = C.solve(massMatrix, rhs)
ddX = zVec[0:3]
dw = zVec[3:6]
ode = C.veccat( [ C.veccat([dx,dy,dz])
, dRexp.trans().reshape([9,1])
, ddX
, dw
] )
if endTimeSteps is not None:
endTime,nSteps = endTimeSteps
pNames.append("endTime")
pDict["endTime"] = endTime
pVec = C.veccat([pVec,endTime])
ode = ode#/(endTime/(nSteps-1))
dae = C.SXFunction( C.daeIn( x=stateVec, p=C.veccat([uVec,pVec])),
C.daeOut( ode=ode))
return (dae, {'xVec':stateVec,'xDict':stateDict,'xNames':stateNames,
'uVec':uVec,'uNames':uNames,
'pVec':pVec,'pNames':pNames}, outputs)
def model():
# Variable Assignments
y1 = cs.ssym('y1')
y2 = cs.ssym('y2')
y3 = cs.ssym('y3')
y4 = cs.ssym('y4')
y5 = cs.ssym('y5')
y6 = cs.ssym('y6')
y7 = cs.ssym('y7')
y = cs.vertcat([y1, y2, y3, y4, y5, y6, y7])
# Parameter Assignments
v1b = cs.ssym("v1b")
k1b = cs.ssym("k1b")
k1i = cs.ssym("k1i")
c = cs.ssym("c")
p = cs.ssym("p")
k1d = cs.ssym("k1d")
k2b = cs.ssym("k2b")
q = cs.ssym("q")
k2d = cs.ssym("k2d")
k2t = cs.ssym("k2t")
k3t = cs.ssym("k3t")
k3d = cs.ssym("k3d")
v4b = cs.ssym("v4b")
k4b = cs.ssym("k4b")
r = cs.ssym("r")
k4d = cs.ssym("k4d")
k5b = cs.ssym("k5b")
k5d = cs.ssym("k5d")
k5t = cs.ssym("k5t")
k6t = cs.ssym("k6t")
k6d = cs.ssym("k6d")
k6a = cs.ssym("k6a")
k7a = cs.ssym("k7a")
k7d = cs.ssym("k7d")
symparamset = cs.vertcat([
v1b, k1b, k1i, c, p, k1d, k2b, q, k2d, k2t, k3t, k3d, v4b, k4b, r, k4d,
k5b, k5d, k5t, k6t, k6d, k6a, k7a, k7d
])
# Time Variable
t = cs.ssym("t")
ode = [[]] * NEQ
f1 = v1b * (y7 + c) / (k1b * (1 + (y3 / k1i)**p) + y7 + c)
f2 = v4b * y3**r / (k4b**r + y3**r) # Sabine had k4b**r, but Geier didn't
ode[0] = f1 - k1d * y1
ode[1] = k2b * y1**q - k2d * y2 - k2t * y2 + k3t * y3
ode[2] = k2t * y2 - k3t * y3 - k3d * y3
ode[3] = f2 - k4d * y4
ode[4] = k5b * y4 - k5d * y5 - k5t * y5 + k6t * y6
ode[5] = k5t * y5 - k6t * y6 - k6d * y6 + k7a * y7 - k6a * y6
ode[6] = k6a * y6 - k7a * y7 - k7d * y7
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=symparamset), cs.daeOut(ode=ode))
fn.setOption("name", "Sabine Model (from stephanie)")
return fn
import casadi
from casadi import ssym, SXFunction
from casadi import sin, cos, fabs
import numpy as np
import pylab
from collections import namedtuple
OptimizationProblem = namedtuple('OptimizationProblem','u NU fc t1 t2')
problems = [] # A list of optimization problems
u = ssym("u") # Variable that is optimized
t1 = 0.0; t2 = 1.0;
# Toy example
def fc(u,t):
assert u.numel()==1
return u*sin(.3*t) - cos(.4*t)
problems.append( OptimizationProblem(u=u,fc=fc,t1=t1,t2=t2, NU=1) )
from signals import fy_sin,fx_polybasis
N = 7
NU = N+1
u = casadi.ssym('u',NU)
def fc2(u,t):
return fx_polybasis(u,t) - fy_sin(t)
problems.append( OptimizationProblem(u=u,fc=fc2,t1=t1,t2=t2, NU=NU) )
p = problems[0]
u,fc,t1,t2 = p.u,p.fc,p.t1,p.t2
def FullModel(Stoch=False):
# Variable Assignments
X = cs.ssym("X")
Y = cs.ssym("Y")
sys = cs.vertcat([X, Y]) # vector version of y
# Parameter Assignments
P = cs.ssym("P")
kt = cs.ssym("kt")
kd = cs.ssym("kd")
a0 = cs.ssym("a0")
a1 = cs.ssym("a1")
a2 = cs.ssym("a2")
kdx = cs.ssym("kdx")
paramset = cs.vertcat([P, kt, kd, a0, a1, a2, kdx])
# Time
t = cs.ssym("t")
ode = [[]] * EqCount
ode[0] = 1 / (1 + Y**P) - kdx * X
ode[1] = kt * X - kd * Y - Y / (a0 + a1 * Y + a2 * Y**2)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=sys, p=paramset), cs.daeOut(ode=ode))
fn.setOption("name", "2state")
#==================================================================
# Stochastic Model Portion
#==================================================================
if Stoch == True:
print 'Now converting model to StochKit XML format...'
#Converts concentration to population
y0in_pop = (vol * y0in).astype(int)
#collects state and parameter array to be converted to species and parameter objects
species_array = [
fn.inputSX(cs.DAE_X)[i].getDescription() for i in xrange(EqCount)
]
param_array = [
fn.inputSX(cs.DAE_P)[i].getDescription()
for i in xrange(ParamCount)
]
#Names model
SSAmodel = stk.StochKitModel(name=modelversion)
#creates SSAmodel class object
SSA_builder = mb.SSA_builder(species_array, param_array, y0in_pop,
param, SSAmodel, vol)
# REACTIONS
SSA_builder.SSA_tyson_x('x prod nonlinear term', 'X', 'Y', 'P')
SSA_builder.SSA_MA_deg('x degradation', 'X', 'kdx')
SSA_builder.SSA_MA_tln('y creation', 'Y', 'kt', 'X')
SSA_builder.SSA_MA_deg('y degradation, linear', 'Y', 'kd')
SSA_builder.SSA_tyson_y('y nonlinear term', 'Y', 'a0', 'a1', 'a2')
# END REACTIONS
state_names = [
fn.inputSX(cs.DAE_X)[i].getDescription() for i in xrange(EqCount)
]
param_names = [
fn.inputSX(cs.DAE_P)[i].getDescription() for i in xrange(ParamCount)
]
return fn, SSAmodel, state_names, param_names
def ODEmodel():
#==================================================================
#State variable definitions
#==================================================================
p = cs.ssym("p")
c1 = cs.ssym("c1")
c2 = cs.ssym("c2")
vip = cs.ssym("vip")
P = cs.ssym("P")
C1 = cs.ssym("C1")
C2 = cs.ssym("C2")
eVIP = cs.ssym("eVIP")
C1P = cs.ssym("C1P")
C2P = cs.ssym("C2P")
CREB = cs.ssym("CREB")
#for Casadi
y = cs.vertcat([p, c1, c2, vip, P, C1, C2, eVIP, C1P, C2P, CREB])
# Time Variable
t = cs.ssym("t")
#===================================================================
#Parameter definitions
#===================================================================
#mRNA
vtpp = cs.ssym("vtpp")
vtpr = cs.ssym("vtpr")
vtc1r = cs.ssym("vtc1r")
vtc2r = cs.ssym("vtc2r")
knpr = cs.ssym("knpr")
kncr = cs.ssym("kncr")
vtvr = cs.ssym("vtvr")
knvr = cs.ssym("knvr")
vdp = cs.ssym("vdp") #degradation of per
vdc1 = cs.ssym("vdc1") #degradation of cry1
vdc2 = cs.ssym("vdc2") #degradation of cry2
kdp = cs.ssym("kdp") #degradation of per
kdc = cs.ssym("kdc") #degradation of cry1 and 2
vdv = cs.ssym("vdv") #degradation of vip
kdv = cs.ssym("kdv") #degradation of vip
#Proteins
vdP = cs.ssym("vdP") #degradation of Per
vdC1 = cs.ssym("vdC1") #degradation of Cry1
vdC2 = cs.ssym("vdC2") #degradation of Cry2
vdC1n = cs.ssym(
"vdC1n"
) #degradation of Cry1-Per or Cry2-Per complex into nothing (nuclear)
vdC2n = cs.ssym("vdC2n")
kdP = cs.ssym("kdP") #degradation of Per
kdC = cs.ssym("kdC") #degradation of Cry1 & Cry2
kdCn = cs.ssym(
"kdCn"
) #degradation of Cry1-Per or Cry2-Per complex into nothing (nuclear)
vaCP = cs.ssym("vaCP") #formation of Cry1-Per or Cry2-Per complex
vdCP = cs.ssym(
'vdCP'
) #degradation of Cry1-Per or Cry2-Per into components in ctyoplasm
ktlnp = cs.ssym('ktlnp') #translation of per
ktlnc = cs.ssym('ktlnc')
#Signalling
vdVIP = cs.ssym("vdVIP") ### NOT USED
kdVIP = cs.ssym("kdVIP")
vdCREB = cs.ssym("vdCREB")
kdCREB = cs.ssym("kdCREB")
ktlnv = cs.ssym("ktlnv")
vgpka = cs.ssym("vgpka")
kgpka = cs.ssym("kgpka")
kcouple = cs.ssym(
"kcouple") #necessary for stochastic model, does not need to be fit
paramset = cs.vertcat([
vtpr, vtc1r, vtc2r, knpr, kncr, vdp, vdc1, vdc2, kdp, kdc, vdP, kdP,
vdC1, vdC2, kdC, vdC1n, vdC2n, kdCn, vaCP, vdCP, ktlnp, vtpp, vtvr,
knvr, vdv, kdv, vdVIP, kdVIP, vdCREB, kdCREB, ktlnv, vgpka, kgpka,
ktlnc, kcouple
])
#===================================================================
# Model Equations
#===================================================================
ode = [[]] * EqCount
# MRNA Species
ode[0] = mb.prtranscription2(CREB, C1P, C2P, vtpr, vtpp,
knpr) - mb.michaelisMenten(p, vdp, kdp)
ode[1] = mb.rtranscription(C1P, C2P, vtc1r, kncr, 1) - mb.michaelisMenten(
c1, vdc1, kdc)
ode[2] = mb.rtranscription(C1P, C2P, vtc2r, kncr, 1) - mb.michaelisMenten(
c2, vdc2, kdc)
ode[3] = mb.rtranscription(C1P, C2P, vtvr, knvr, 1) - mb.michaelisMenten(
vip, vdv, kdv)
# Free Proteins
ode[4] = mb.translation(p, ktlnp) - mb.michaelisMenten(
P, vdP, kdP) - mb.Complexing(vaCP, C1, P, vdCP, C1P) - mb.Complexing(
vaCP, C2, P, vdCP, C2P)
ode[5] = mb.translation(c1, ktlnc) - mb.michaelisMenten(
C1, vdC1, kdC) - mb.Complexing(vaCP, C1, P, vdCP, C1P)
ode[6] = mb.translation(c2, ktlnc) - mb.michaelisMenten(
C2, vdC2, kdC) - mb.Complexing(vaCP, C2, P, vdCP, C2P)
ode[7] = mb.translation(vip, ktlnv) + mb.lineardeg(kdVIP, eVIP)
# PER/CRY Cytoplasm Complexes in the Nucleus
ode[8] = mb.Complexing(vaCP, C1, P, vdCP, C1P) - mb.sharedDegradation(
C1P, C2P, vdC1n, kdCn)
ode[9] = mb.Complexing(vaCP, C2, P, vdCP, C2P) - mb.sharedDegradation(
C2P, C1P, vdC2n, kdCn)
#Sgnaling Pathway
ode[10] = mb.ptranscription(eVIP, vgpka, kgpka, 1) - mb.michaelisMenten(
CREB, vdCREB, kdCREB)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=paramset), cs.daeOut(ode=ode))
fn.setOption("name", "SDS16")
return fn
plt.plot(opt['tgrid'][:-1],y)
else:
raise ValueError("unrecognized name: \""+name+"\"")
if isinstance(title,str):
plt.title(title)
plt.xlabel('time')
plt.legend(legend)
plt.grid()
if __name__=='__main__':
dae = Dae()
# -----------------------------------------------------------------------------
# Model setup
# -----------------------------------------------------------------------------
# Declare variables (use scalar graph)
t = CS.ssym("t") # time
u = dae.addU("u") # control
xd = CS.veccat(dae.addX(["x0","x1","x2"])) # differential state
# xa = CS.ssym("xa",0,1) # algebraic state
# xddot = CS.ssym("xdot",3) # differential state time derivative
# p = CS.ssym("p",0,1) # parameters
dae.addOutput('u^2',u*u)
dae.stateDotDummy = CS.veccat( [CS.ssym(name+"DotDummy") for name in dae._xNames] )
# ODE right hand side function
rhs = CS.vertcat([(1 - xd[1]*xd[1])*xd[0] - xd[1] + u, \
xd[0], \
xd[0]*xd[0] + xd[1]*xd[1] + u*u])
dae.setOdeRes( rhs - dae.stateDotDummy )
nicp_ = 1 # Number of (intermediate) collocation points per control interval
import camera_model
from camera_model import triangulate, single_camera_intrinsics
from load_calibration_sequences import point_correspondences as point_correspondences_numpy;
point_correspondences = casadi.SXMatrix(point_correspondences_numpy)
# For first attempt, only perform bundle adjustment for extrinsics.
from load_calibration_data_casadi import T as T_initial
from load_calibration_data_casadi import Tinv as Tinv_initial
from load_calibration_data_casadi import f1
from load_calibration_data_casadi import c1
from load_calibration_data_casadi import f2
from load_calibration_data_casadi import c2
# The extrinsics will be free parameters
roll = ssym('roll')
pitch = ssym('pitch')
yaw = ssym('yaw')
tx = ssym('tx')
ty = ssym('ty')
tz = ssym('tz')
t = casadi.vertcat([tx,ty,tz])
# Build expression for the camera pair extrinsics in terms of roll, pitch, and yaw
R = R_from_roll_pitch_yaw(roll, pitch, yaw)
T = pack_se3(R,t)
Tinv = invert_se3(T)
# Compute initial values for roll, pitch, yaw
R_initial,t_initial = unpack_se3(T_initial)
roll_initial,yaw_initial,pitch_initial = roll_pitch_yaw_from_R(R_initial)
sin,cos,arctan2, solve, vertcat,\
jacobianTimesVector, sqrt
import numpy
import casadi_conveniences
from casadi_conveniences import *
from rotation_matrices import Rx,Ry,Rz,Tx,Ty,Tz,unskew
####################
# This script is for investigating whether several
# carousel-like mechanical systems are open loop stable.
####################
# Note, in this file we use x-y "math" coordinates, rather
# than North-East-Down "aero" coordinates
t = casadi.ssym('t')
R = ssym('R') # length of carousel arm
r = ssym('r') # length of tether
ddelta = ssym('ddelta') # Rotational speed of the arm [rad/s]
delta = ddelta*t # The carousel angle, counter-clockwise,
# is positive when viewed from above
# Horizontal theta angle of the tether behind the arm
# a.k.a. theta
theta = ssym('theta')
dtheta = ssym('dtheta')
ddtheta = ssym('ddtheta')
# Our generalized coordinate is theta, i.e. the azimuth angle referenced
# to world frame x
def LLT_solver(operAlphaDegLst, geom):
operAlphaLst = [numpy.radians(x) for x in operAlphaDegLst]
operCLLst = []
operCDiLst = []
#
#Now let's iterate over the Alphas and solve LLT equations to obtain lift for each one
#
alpha = C.ssym('alpha')
An = C.ssym('A',geom.n)
(makeMeZero, alphaiLoc, _, _) = setupImplicitFunction(alpha, An, geom)
# make the solver
f = C.SXFunction([An,alpha], [makeMeZero])
f.init()
solver = C.NLPImplicitSolver(f)
solver.setOption('nlp_solver', C.IpoptSolver)
#solver.setOption('nlp_solver_options',{'suppress_all_output':'yes','print_time':False})
solver.init()
for operAlpha in operAlphaLst:
print 'Current Alpha = ', numpy.degrees(operAlpha)
guess = numpy.zeros(geom.n)
guess[0] = 0.01
solver.setOutput(guess,0)
solver.setInput(operAlpha)
solver.evaluate()
iterAn = numpy.squeeze(numpy.array(solver.output()))
#Compute CL/CDi
CL = iterAn[0]*numpy.pi*geom.AR
CD0 = 0
if iterAn[0] != 0:
for i in range(geom.n):
k = geom.sumN[i]
CD0 += k*iterAn[i]**2/(iterAn[0]**2)
CDi = numpy.pi*geom.AR*iterAn[0]**2*CD0
operCLLst.append(CL)
operCDiLst.append(CDi)
operCLLst = numpy.array(operCLLst)
operCDiLst = numpy.array(operCDiLst)
#
# plot everything
#
pylab.plot(numpy.degrees(operAlphaLst),operCLLst,'r',numpy.degrees(operAlphaLst), numpy.array(operAlphaLst)*2*numpy.pi*10.0/12.0)
pylab.xlabel('Alpha')
pylab.ylabel('CL')
pylab.legend(['llt CL','flat plate CL'])
pylab.figure()
pylab.plot(numpy.degrees(operAlphaLst),operCDiLst,'r',numpy.degrees(operAlphaLst),operCLLst[:]**2/(numpy.pi*geom.AR),'b')
pylab.legend(['llt CDi','flat plate CDi'])
pylab.xlabel('Alpha')
pylab.ylabel('CDi')
pylab.figure()
pylab.plot(operCDiLst,operCLLst)
pylab.xlabel('CDi')
pylab.ylabel('CL')
pylab.show()
#!/usr/bin/env python
import numpy
from casadi import ssym, mul
import casadi
from rotation_matrices import pack_se3, vectorize_se3
from camera_model import triangulate
from register_three_points_casadi import register_three_points_casadi
from load_calibration_data_casadi import f1,c1,f2,c2,RPC1,RPC2,T,Tinv,m1_body,m2_body,m3_body
from simple_codegen import generateSimpleCode
c1m1u = ssym('c1m1u') # Marker 1, camera 1, u image coordinate
c1m1v = ssym('c1m1v')
c1m2u = ssym('c1m2u')
c1m2v = ssym('c1m2v')
c1m3u = ssym('c1m3u')
c1m3v = ssym('c1m3v')
c2m1u = ssym('c2m1u')
c2m1v = ssym('c2m1v')
c2m2u = ssym('c2m2u')
c2m2v = ssym('c2m2v')
c2m3u = ssym('c2m3u')
c2m3v = ssym('c2m3v')
markers = [c1m1u, c1m1v, c1m2u, c1m2v, c1m3u, c1m3v,
c2m1u, c2m1v, c2m2u, c2m2v, c2m3u, c2m3v]
markers = casadi.vertcat(markers)
c1m1 = casadi.vertcat([c1m1u,c1m1v])
definition=proto + '\n{\n' + body + '}\n'
sq_inline = "inline double sq(double x){return x*x;}\n"
sign_inline = "inline double sign(double x){ return x<0 ? -1 : x>0 ? 1 : x;}\n"
code=warnstring+'\n'+sq_inline+sign_inline+'\n'+docstring+'\n'+declaration+definition
import os
f = open(filename,'wb');
f.write(code);
f.close();
return code
if __name__=='__main__':
# A Simple Example Function
a = ssym('a')
b = ssym('b')
c = ssym('c')
d = ssym('d')
f = (a+b)/c*d
g = f*f*f
h = f*c
i1 = vertcat([a,b])
i2 = vertcat([c,d])
o1 = vertcat([f,c])
o2 = vertcat([g])
F = casadi.SXFunction([i1,i2],[o1,o2])
F.init()
F.generateCode('joel_style.c')
code = generateSimpleCode(F,
def sympy2casadi(node):
sympy_syms = node.free_symbols # a python set of sympy Symbols
# A dictionary matching symbol names to instances of casadi Matrix<SXElement>
casadi_syms = {sym.name:casadi.ssym(sym.name) for sym in sympy_syms}
return traverse(node, casadi_syms, node)
def ODEmodel():
#==================================================================
#State variable definitions
#==================================================================
M = cs.ssym("M")
Pc = cs.ssym("Pc")
Pn = cs.ssym("Pn")
#for Casadi
y = cs.vertcat([M, Pc, Pn])
# Time Variable
t = cs.ssym("t")
#===================================================================
#Parameter definitions
#===================================================================
vs0 = cs.ssym('vs0')
light = cs.ssym('light')
alocal = cs.ssym('alocal')
couplingStrength = cs.ssym('couplingStrength')
n = cs.ssym('n')
vm = cs.ssym('vm')
k1 = cs.ssym('k1')
km = cs.ssym('km')
ks = cs.ssym('ks')
vd = cs.ssym('vd')
kd = cs.ssym('kd')
k1_ = cs.ssym('k1_')
k2_ = cs.ssym('k2_')
paramset = cs.vertcat([vs0, light, alocal, couplingStrength,
n, vm, k1, km,
ks, vd, kd, k1_,
k2_])
#===================================================================
# Model Equations
#===================================================================
ode = [[]]*EqCount
def pm_prod(Pn, K, v0, n, light, a, M, Mi):
vs = v0+light+a*(M-Mi)
return (vs*K**n)/(K**n + Pn**n)
def pm_deg(M, Km, vm):
return vm*M/(Km+M)
def Pc_prod(ks, M):
return ks*M
def Pc_deg(Pc, K, v):
return v*Pc/(K+Pc)
def Pc_comp(k1,k2,Pc,Pn):
return -k1*Pc + k2*Pn
def Pn_comp(k1,k2,Pc,Pn):
return k1*Pc - k1*Pn
#Rxns
ode[0] = (pm_prod(Pn, k1, vs0, n, light, alocal, M, M) - pm_deg(M,km,vm))
ode[1] = Pc_prod(ks,M) - Pc_deg(Pc,kd,vd) + Pc_comp(k1_,k2_,Pc,Pn)
ode[2] = Pn_comp(k1_,k2_,Pc,Pn)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=paramset),
cs.daeOut(ode=ode))
fn.setOption("name","SDS16")
return fn
#!/usr/bin/env python
import numpy
import casadi
from casadi import ssym, mul, SXMatrix
from rotation_matrices import pack_se3, vectorize_se3, unvectorize_se3, append_one
from camera_model import triangulate, project
from register_three_points_casadi import register_three_points_casadi
from load_calibration_data_casadi import *
from simple_codegen import generateSimpleCode
# Vector of uninitialized casadi variables for the pose input
pose = ssym('pose',12,1)
# Return a casadi expression that computes the markers from the pose.
def markers_from_pose_casadi(pose):
T_body_to_anchor = unvectorize_se3(pose)
markers_body = [m1_body, m2_body, m3_body]
markers_body = map(append_one, markers_body)
markers = SXMatrix.zeros(12,1)
RPCinvs = [RPC1inv, RPC2inv] # These map points in arm frames to camera frames
fs = [f1,f2]
cs = [c1,c2]
for ci in xrange(2):
for mi in xrange(3):
m_anchor = mul(T_body_to_anchor, markers_body[mi])
m_cam = mul(RPCinvs[ci],m_anchor)
uv = project(fs[ci],cs[ci],m_cam)
starti = ci*6+mi*2
markers[starti:starti+2,0] = uv
def model(conf, nSteps=None, extraParams=[]):
dae = Dae()
for ep in extraParams:
dae.addP(ep)
dae.addZ(["ddx", "ddy", "ddz", "dw1", "dw2", "dw3", "nu"])
dae.addX(
[
"x", # state 0
"y", # state 1
"z", # state 2
"e11", # state 3
"e12", # state 4
"e13", # state 5
"e21", # state 6
"e22", # state 7
"e23", # state 8
"e31", # state 9
"e32", # state 10
"e33", # state 11
"dx", # state 12
"dy", # state 13
"dz", # state 14
"w1", # state 15
"w2", # state 16
"w3", # state 17
"r", # state 20
"dr", # state 21
"energy", # state 22
]
)
dae.addU(["aileron", "elevator", "ddr"])
dae.addP(["w0"])
dae.addOutput("r", dae.x("r"))
dae.addOutput("dr", dae.x("dr"))
dae.addOutput("aileron(deg)", dae.u("aileron") * 180 / C.pi)
dae.addOutput("elevator(deg)", dae.u("elevator") * 180 / C.pi)
dae.addOutput("winch force", dae.x("r") * dae.z("nu"))
dae.addOutput("winch power", dae.x("r") * dae.x("dr") * dae.z("nu"))
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat(
[
C.veccat(dae.x(["dx", "dy", "dz"])),
dRexp.trans().reshape([9, 1]),
C.veccat(dae.z(["ddx", "ddy", "ddz"])),
C.veccat(dae.z(["dw1", "dw2", "dw3"])),
dae.x("dr"),
dae.u("ddr"),
dae.output("winch power"),
]
)
if nSteps is not None:
dae.addP("endTime")
dae.stateDotDummy = C.veccat([C.ssym(name + "DotDummy") for name in dae.xNames()])
scaledStateDotDummy = dae.stateDotDummy
if nSteps is not None:
scaledStateDotDummy = dae.stateDotDummy / (dae.p("endTime") / (nSteps - 1))
dae.setOdeRes(ode - scaledStateDotDummy)
dae.setAlgRes(C.mul(massMatrix, dae.zVec()) - rhs)
return dae
