Example #1
0
def main():
    nSteps = 15

    print "creating model"
    dae = models.carousel(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.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 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()
Example #2
0
def main():
    nSteps = 15

    print "creating model"
    dae = models.carousel(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.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 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()
Example #3
0
def main():
    nSteps = 15

    print "creating model"
    dae = models.pendulum(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 = rawe.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()
Example #4
0
def main():
    nSteps = 15

    print "creating model"
    dae = models.pendulum(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 = rawe.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()