f.setInput(opt['zPlt'][:,k],1)
f.setInput(opt['u'][:,k],2)
f.setInput(opt['p'],3)
f.evaluate()
y.append(CS.DMatrix(f.output(0)))
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, \
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 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
