def main(): pend_model = pm.pendulum_model() updated_sys = _adjointSystem(pend_model) t = np.linspace(0, 5, 501) r = 0.2 * np.ones(np.size(t)) yout, tout, xout = lsim(updated_sys, r, t) plt.plot(tout, yout[:, 0], tout, yout[:, 1]) plt.show() return
def main(): nk = 15 print "creating model" dae = pendulum_model.pendulum_model() dae.addP('endTime') print "setting up OCP" ocp = Coll(dae, nk=nk,nicp=1,deg=4) # constrain invariants def invariantErrs(): f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()] , [dae.output('invariants')] ) f.setOption('name','invariant errors') f.init() return f [c0] = invariantErrs().call([ocp.xVec(0),ocp.uVec(0),ocp.pVec()]) ocp.constrain(c0,'==',0) # bounds r = 0.3 ocp.bound('x',(-0.5,0.5)) ocp.bound('z',(-0.5,0.5)) ocp.bound('dx',(-5,5)) ocp.bound('dz',(-5,5)) ocp.bound('torque',(-50,50)) ocp.bound('m',(0.3,0.5)) ocp.bound('endTime',(0.01,1.5)) # boundary conditions ocp.bound('x',(r,r),timestep=0) ocp.bound('z',(0,0),timestep=0) ocp.bound('x',(0,0),timestep=-1) ocp.bound('z',(-r*1.5,-r/2),timestep=-1) ocp.bound('dx',(0,0),timestep=0) ocp.bound('dz',(0,0),timestep=0) ocp.bound('dx',(0,0),timestep=-1) ocp.bound('dz',(0,0),timestep=-1) # make the solver ocp.setObjective(ocp.lookup('endTime')) context = zmq.Context(1) publisher = context.socket(zmq.PUB) publisher.bind("tcp://*:5563") # callback function class MyCallback: def __init__(self): self.iter = 0 def __call__(self,f,*args): xOpt = numpy.array(f.input(C.NLP_X_OPT)) self.iter = self.iter + 1 xup = ocp.devectorize(xOpt)['vardict'] po = kite_pb2.PendulumOpt() po.x.extend(list(xup['x'])) po.z.extend(list(xup['z'])) po.endTime = xup['endTime'] po.iters = self.iter publisher.send_multipart(["pendulum-opt", po.SerializeToString()]) # solver solverOptions = [ ("linear_solver","ma27") # , ("derivative_test","first-order") , ("expand_f",True) , ("expand_g",True) , ("generate_hessian",True) # , ("max_iter",1000) , ("tol",1e-4) ] constraintFunOptions = [('numeric_jacobian',False)] # initial conditions ocp.guessX([r,0,0,0]) ocp.guess('torque',0) ocp.guess('m',0) ocp.guess('endTime',0.3) ocp.setupCollocation( ocp.lookup('endTime') ) ocp.setupSolver( solverOpts=solverOptions, constraintFunOpts=constraintFunOptions, callback=MyCallback() ) opt = ocp.solve() # Plot the results plt.figure(1) plt.clf() legend = [] for name in ocp.dae.xNames(): legend.append(name) plt.plot(opt['tgrid'],opt['vardict'][name])#,'--') for name in ocp.dae.uNames(): legend.append(name) plt.plot(opt['tgrid'],opt['vardict'][name]/20)#,'--') plt.title("pendulum swingup optimization") plt.xlabel('time') plt.legend(legend) plt.grid() plt.show()
def main(): nSteps = 15 print "creating model" dae = pendulum_model.pendulum_model(nSteps=nSteps) print "setting up OCP" ocp = MultipleShootingStage(dae, nSteps) # make the integrator print "setting up dynamics constraints" integratorOptions = [ ("reltol",1e-7) , ("abstol",1e-9) , ("t0",0) , ("tf",1) , ('name','integrator') , ("linear_solver_creator",C.CSparse) # , ("linear_solver_creator",C.LapackLUDense) , ("linear_solver","user_defined") ] ocp.setIdasIntegrator(integratorOptions) # constrain invariants def invariantErrs(): f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()] , [dae.output('invariants')] ) f.setOption('name','invariant errors') f.init() return f [c0] = invariantErrs().call([ocp.states[:,0],ocp.actions[:,0],ocp.params]) ocp.addConstraint(c0,'==',0) # bounds r = 0.3 ocp.setBound('x',(-0.5,0.5)) ocp.setBound('z',(-0.5,0.5)) ocp.setBound('dx',(-5,5)) ocp.setBound('dz',(-5,5)) ocp.setBound('torque',(-50,50)) ocp.setBound('m',(0.3,0.5)) ocp.setBound('endTime',(0.01,5.0)) # boundary conditions ocp.setBound('x',(r,r),timestep=0) ocp.setBound('z',(0,0),timestep=0) ocp.setBound('x',(0,0),timestep=nSteps-1) ocp.setBound('z',(-r*1.5,-r/2),timestep=nSteps-1) ocp.setBound('dx',(0,0),timestep=0) ocp.setBound('dz',(0,0),timestep=0) ocp.setBound('dx',(0,0),timestep=nSteps-1) ocp.setBound('dz',(0,0),timestep=nSteps-1) # make the solver ocp.setObjective(ocp.lookup('endTime')) context = zmq.Context(1) publisher = context.socket(zmq.PUB) publisher.bind("tcp://*:5563") # callback function class MyCallback: def __init__(self): self.iter = 0 def __call__(self,f,*args): xOpt = f.input(C.NLP_X_OPT) self.iter = self.iter + 1 xup = ocp.devectorize(xOpt) po = kite_pb2.PendulumOpt() po.x.extend(list(xup['x'])) po.z.extend(list(xup['z'])) po.endTime = xup['endTime'] po.iters = self.iter publisher.send_multipart(["pendulum-opt", po.SerializeToString()]) def makeCallback(): nd = ocp.getDesignVars().size() nc = ocp._constraints.getG().size() c = C.PyFunction( MyCallback(), C.nlpsolverOut(x_opt=C.sp_dense(nd,1), cost=C.sp_dense(1,1), lambda_x=C.sp_dense(nd,1), lambda_g = C.sp_dense(nc,1), g = C.sp_dense(nc,1) ), [C.sp_dense(1,1)] ) c.init() return c # solver solverOptions = [ ("iteration_callback",makeCallback()) # , ("derivative_test","first-order") , ("linear_solver","ma57") ] constraintFunOptions = [('numeric_jacobian',False)] ocp.setSolver( C.IpoptSolver, solverOptions=solverOptions, constraintFunOptions=constraintFunOptions ) # initial conditions ocp.setXGuess([r,0,0,0]) ocp.setGuess('torque',0) ocp.setGuess('m',0.4) ocp.setGuess('endTime',0.5) #ocp.setBounds() # solve! opt = ocp.devectorize(ocp.solve()) # Plot the results plt.figure(1) plt.clf() time = numpy.linspace(0,opt['endTime'],opt['x'].size()) plt.plot(time, opt['x'], '--') plt.plot(time, opt['z'], '-') plt.plot(time, opt['dx'], '-.') plt.plot(time, opt['dz'], '--') time = numpy.linspace(0,opt['endTime'],opt['torque'].size()) plt.plot(time,opt['torque']/20,'--') plt.title("pendulum swingup optimization") plt.xlabel('time') plt.legend(['x','z','vx','vz','torque']) plt.grid() plt.show()
cs.kiteDcm.r31 = 0 cs.kiteDcm.r32 = 0 cs.kiteDcm.r33 = 1 cs.delta = 0 cs.ddelta = 0 cs.tc = u.at(0) cs.u1 = 0 cs.u2 = u.at(1) cs.wind_x = 0 return cs if __name__=='__main__': print "creating model" (dae, others) = pendulum_model.pendulum_model() dae.init() # # compile model code # print "generating model code" # t0 = time.time() # dae.generateCode("dae.c") # print "took "+str(time.time()-t0)+" seconds to generate code" # t0 = time.time() # os.system("gcc -fPIC -O2 -shared dae.c -o dae.so") # print "took "+str(time.time()-t0)+" seconds to compile code" # dae_ext = C.ExternalFunction("./dae.so") # dae_ext.init() # dae = dae_ext print "creating integrator"