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"
