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