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()
def main():
nSteps = 15
print "creating model"
dae = model.model(zt,rArm,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():
dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')])
, C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')])
, C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')])
] ).trans()
err = C.mul(dcm.trans(), dcm)
dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ])
f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
, [dae.output('c'),dae.output('cdot'),dcmErr]
)
f.setOption('name','invariant errors')
f.init()
return f
[c0,cdot0,dcmError0] = invariantErrs().call([ocp.states[:,0],ocp.actions[:,0],ocp.params])
ocp.addConstraint(c0,'==',0)
ocp.addConstraint(cdot0,'==',0)
ocp.addConstraint(dcmError0,'==',0)
# bounds
ocp.setBound('aileron',(-0.04,0.04))
ocp.setBound('elevator',(-0.1,0.1))
ocp.setBound('x',(0,4))
ocp.setBound('y',(-3,3))
ocp.setBound('z',(-2,3))
ocp.setBound('r',(1,2))
ocp.setBound('dr',(-1,1))
ocp.setBound('ddr',(0,0))
ocp.setBound('r',(1.2,1.2),timestep=0)
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.setBound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.setBound(d,(-50,50))
for w in ['w1','w2','w3']:
ocp.setBound(w,(-8*pi,8*pi))
ocp.setBound('delta',(-0.01,1.01*2*pi))
ocp.setBound('ddelta',(-pi/4,8*pi))
ocp.setBound('tc',(-200,600))
# ocp.setBound('tc',(389.970797939731,389.970797939731))
ocp.setBound('endTime',(0.5,2.0))
# ocp.setBound('endTime',(1.6336935276077966,1.6336935276077966))
ocp.setBound('w0',(10,10))
# boundary conditions
ocp.setBound('delta',(0,0),timestep=0)
ocp.setBound('delta',(2*pi,2*pi),timestep=nSteps-1)
# make it periodic
ocp.addConstraint(ocp.states[:18,0],'==',ocp.states[:18,-1])
ocp.addConstraint(ocp.states[19,0],'==',ocp.states[19,-1])
ocp.addConstraint(ocp.actions[:,0],'==',ocp.actions[:,-1])
# make the solver
# objective function
tc0 = 390
obj = (C.sumAll(ocp.actions[0:2,:]*ocp.actions[0:2,:]) + 1e-10*C.sumAll((ocp.actions[2,:]-tc0)*(ocp.actions[2,:]-tc0)))*ocp.lookup('endTime')
ocp.setObjective(obj)
# zero mq setup
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):
self.iter = self.iter + 1
xOpt = f.input(C.NLP_X_OPT)
xs,us,p = ocp.getTimestepsFromDvs(xOpt)
kiteProtos = [kiteproto.toKiteProto(xs[k],us[k],p,rArm,zt) for k in range(0,nSteps)]
mc = kite_pb2.KiteOpt()
mc.css.extend(list(kiteProtos))
xup = ocp.devectorize(xOpt)
mc.messages.append("endTime: "+str(xup['endTime']))
mc.messages.append("w0: "+str(xup['w0']))
mc.messages.append("iter: "+str(self.iter))
publisher.send_multipart(["multi-carousel", mc.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())
, ("linear_solver","ma57")
# , ("max_iter",5)
]
ocp.setSolver( C.IpoptSolver, solverOptions=solverOptions )
#ocp.setBounds()
# initial conditions
ocp.setXGuess(x0)
for k in range(0,nSteps):
val = 2*pi*k/(nSteps-1)
ocp.setGuess('delta',val,timestep=k,quiet=True)
ocp.setGuess('aileron',0)
ocp.setGuess('elevator',0)
ocp.setGuess('tc',389.970797939731)
ocp.setGuess('endTime',1.6336935276077966)
ocp.setGuess('ddr',0)
ocp.setGuess('w0',0)
ocp.solve()
