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