def __call__(self,f,*args):
self.iter = self.iter + 1
xOpt = numpy.array(f.input(C.NLP_X_OPT))
opt = ocp.devectorize(xOpt)
xup = opt['vardict']
kiteProtos = []
for k in range(0,nk):
j = nicp*(deg+1)*k
kiteProtos.append( kiteproto.toKiteProto(C.DMatrix(opt['x'][:,j]),
C.DMatrix(opt['u'][:,j]),
C.DMatrix(opt['p']),
conf['kite']['zt'],
conf['carousel']['rArm'],
zeroDelta=True) )
# kiteProtos = [kiteproto.toKiteProto(C.DMatrix(opt['x'][:,k]),C.DMatrix(opt['u'][:,k]),C.DMatrix(opt['p']), conf['kite']['zt'], conf['carousel']['rArm'], zeroDelta=True) for k in range(opt['x'].shape[1])]
mc = kite_pb2.MultiCarousel()
mc.css.extend(list(kiteProtos))
mc.messages.append("endTime: "+str(xup['endTime']))
mc.messages.append("w0: "+str(xup['w0']))
mc.messages.append("iter: "+str(self.iter))
# bounds feedback
# lbx = ocp.solver.input(C.NLP_LBX)
# ubx = ocp.solver.input(C.NLP_UBX)
# violations = boundsFeedback(xOpt,lbx,ubx,ocp.bndtags,tolerance=1e-9)
# for name in violations:
# violmsg = "violation!: "+name+": "+str(violations[name])
# mc.messages.append(violmsg)
publisher.send_multipart(["multi-carousel", mc.SerializeToString()])
def sendKite(self,x,u,p,outs,conf,otherMessages=[]):
pb = kiteproto.toKiteProto(dict(x.items()+u.items()+p.items()+outs.items()).__getitem__,
lineAlpha=0.2)
for name in outs:
v = outs[name]
if type(v) is float:
pb.messages.append(name+": "+str(v))
if len(otherMessages)>0:
pb.messages.append("-------------------------")
for om in otherMessages:
pb.messages.append(om)
self.publisher.send_multipart(["carousel", pb.SerializeToString()])
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.horizon.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 __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.horizon.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 normalCallback(traj,myiter,ocp,conf,showAllPoints=False):
kiteProtos = []
for k in range(0,ocp.nk):
for nicpIdx in range(0,ocp.nicp):
if showAllPoints:
degIdxRange = range(ocp.deg+1)
else:
degIdxRange = [0]
for degIdx in degIdxRange:
lookup = lambda name: traj.lookup(name,timestep=k,nicpIdx=nicpIdx,degIdx=degIdx)
kiteProtos.append( kiteproto.toKiteProto(lookup,
lineAlpha=0.2) )
t2 = time.time()
mc = kite_pb2.MultiCarousel()
mc.horizon.extend(list(kiteProtos))
mc.messages.append("w0: "+str(traj.lookup('w0')))
mc.messages.append("iter: "+str(myiter))
mc.messages.append("endTime: "+str(traj.lookup('endTime')))
mc.messages.append("average power: "+str(traj.lookup('quadrature energy',timestep=-1)/traj.lookup('endTime'))+" W")
return mc.SerializeToString()
print "creating model..."
dae = carouselmodel.model(conf,extraParams=['endTime'])
print "setting up ocp..."
ocp = setupOcp(dae,conf,publisher,nk=50)
xOpt = None
for w0 in [10]:
ocp.bound('w0',(w0,w0),force=True)
opt = ocp.solve(xInit=xOpt)
xup = opt['vardict']
xOpt = opt['X_OPT']
for k in range(0,ocp.nk):
j = ocp.nicp*(ocp.deg+1)*k
oldKites.append( kiteproto.toKiteProto(C.DMatrix(opt['x'][:,j]),C.DMatrix(opt['u'][:,j]),C.DMatrix(opt['p']), conf['kite']['zt'], conf['carousel']['rArm']) )
print "optimal power: "+str(opt['vardict']['energy'][-1]/opt['vardict']['endTime'])
# Plot the results
ocp.plot(['x','y','z'],opt)
ocp.plot(['aileron','elevator'],opt,title='control surface inputs')
ocp.plot(['tc'],opt,title='motor inputs (tc)')
ocp.plot(['ddr'],opt,title='winch accel (ddr)')
ocp.plot(['c','cdot','cddot'],opt,title="invariants")
ocp.plot('airspeed',opt)
ocp.plot(['alpha(deg)','beta(deg)','alphaTail(deg)','betaTail(deg)'],opt)
ocp.plot('cL',opt)
ocp.plot('cD',opt)
ocp.plot('L/D',opt)
ocp.plot('motor power',opt)
# ocp.plot('winch power',opt)
import kiteproto
import joy
class Communicator:
def __init__(self, fOutputs, outputNames):
self.context = zmq.Context(1)
self.publisher = self.context.socket(zmq.PUB)
self.publisher.bind("tcp://*:5563")
self.fOutputs = fOutputs
self.outputNames = outputNames
def sendKite(self, sim, (x, z, u, p), zt, rArm, w0, otherMessages=[]):
assert isinstance(otherMessages, list)
pb = kiteproto.toKiteProto(x, u, p, zt, rArm, w0=w0)
pb.messages.append("sloMoFactor: " + str(sim.sloMoFactor))
pb.messages.append("-------------------------")
self.fOutputs.setInput(x, 0)
self.fOutputs.setInput(z, 1)
self.fOutputs.setInput(u, 2)
self.fOutputs.setInput(p, 3)
self.fOutputs.evaluate()
for k, n in enumerate(self.outputNames):
pb.messages.append(n + ": " + str(self.fOutputs.output(k)))
if len(otherMessages) > 0:
pb.messages.append("-------------------------")
for om in otherMessages:
assert isinstance(om, str)
pb.messages.append(om)
