def _setupDynamicsConstraints(self, endTime, traj): # Todo: add parallelization # Todo: get endTime right g = [] nicp = 1 deg = 4 p = self._dvMap.pVec() for k in range(self.nk): newton = Newton(LagrangePoly, self.dae, 1, nicp, deg, 'RADAU') newton.setupStuff(endTime) X0_i = self._dvMap.xVec(k) U_i = self._U[k, :].T # guess if traj is None: newton.isolver.setOutput(1, 0) else: X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \ for name in traj.dvMap._xNames]) Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \ for name in traj.dvMap._zNames]) newton.isolver.setOutput(C.veccat([X, Z]), 0) _, Xf_i = newton.isolver.call([X0_i, U_i, p]) X0_i_plus = self._dvMap.xVec(k + 1) g.append(Xf_i - X0_i_plus) return g
def advanceState(x): axes = js.getAxes() torque = axes[0] u = C.DMatrix([torque, 0.4]) f.setInput(x, C.INTEGRATOR_X0) f.setInput(u, C.INTEGRATOR_P) f.evaluate() return (C.DMatrix(f.output()), u)
def computeOutputs(self, x, u): self._outputsFun.setInput(x, 0) self._outputsFun.setInput(u, 1) self._outputsFun.evaluate() outs = [ C.DMatrix(self._outputsFun.output(k)) for k in range(self._outputsFun.getNumOutputs()) ] return numpy.squeeze(numpy.array(C.veccat(outs)))
def advanceState(): js = sim.handleInput() # saving/loading fstButton = 13 for k in range(0, 4): if k + fstButton + 4 in js['buttonsDown'] and k in sim._saves: print "loading save #" + str(k + 1) sim.load(k) sim.default = k if k + fstButton in js['buttonsDown']: if k == 0: print "can't override save0" else: print "making save #" + str(k + 1) sim.save(k) sim.default = k if 5 in js['buttonsDown']: k = sim.default print "loading save #" + str(k + 1) sim.load(k) # play replay if 3 in js['buttonsDown']: sim.playReplay(communicator) sim.loadDefault() aileron = -js['axes'][0] * 0.05 elevator = js['axes'][1] * 0.2 rudder = -js['axes'][2] * 0.15 tc = 600 * (1 - js['axes'][6]) wind_x = 5 * (1 - js['axes'][7]) wind_x = 0 x = sim.getCurrentState().x u = C.DMatrix([tc, aileron, elevator, rudder]) p = C.DMatrix([wind_x]) f.setInput(x, C.INTEGRATOR_X0) f.setInput(C.veccat([u, p]), C.INTEGRATOR_P) f.evaluate() xNext = C.DMatrix(f.output()) return ((x, u, p), xNext)
def _calc_der_vals(self): # Derivatives of all basis polynomials at all interpolation points der_vals = casadi.DMatrix(self.n + 1, self.n + 1) for j in xrange(self.n + 1): for k in xrange(self.n + 1): der_vals[j, k] = lagrange_derivative_eval(self.p, j, self.p[k]) # Store derivative values as data attribute self.der_vals = der_vals
def getSteadyState( self ): guessVec = C.DMatrix([self.guess[n] for n in self.dvsNames] + [self.dotGuess[n] for n in self.dae.xNames()]) boundsVec = [self.bounds[n] for n in self.dvsNames] + [self.dotBounds[n] for n in self.dae.xNames()] self.solver.setInput(self.lbg, 'lbg') self.solver.setInput(self.ubg, 'ubg') self.solver.setInput(guessVec, 'x0') lb, ub = zip(*boundsVec) self.solver.setInput(C.DMatrix(lb), 'lbx') self.solver.setInput(C.DMatrix(ub), 'ubx') self.solver.solve() ret = self.solver.getStat('return_status') assert ret in ['Solve_Succeeded', 'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret xOpt = self.solver.output('x') k = 0 sol = {} for name in self.dvsNames: sol[name] = xOpt[k].at(0) k += 1 dotSol = {} for name in self.dae.xNames(): dotSol[name] = xOpt[k].at(0) k += 1 if self.robustVersion is True: self.zFcn.setInput(C.DMatrix([sol[ n ] for n in self.dvsNames]), 0) self.zFcn.evaluate() zEval = self.zFcn.output( 0 ) for k, name in enumerate( self.dae.zNames() ): sol[ name ] = zEval[ k ].at( 0 ) # Just a quick check for name in self.dae.xNames(): if abs( dotSol[ name ] ) > 1e-8 and name not in ["sin_delta", "cos_delta"]: print "Warning: dotSol[ %s ] = %5.3f" % (name, dotSol[ name ]) return sol, dotSol
def getOutputs(self, x, u, p): if self.outputsFun0 == None: return {} (xVec, uVec, pVec) = vectorizeXUP(x, u, p, self.dae) self.outputsFun0.setInput(xVec, 0) self.outputsFun0.setInput(uVec, 1) self.outputsFun0.setInput(pVec, 2) self.outputsFun0.evaluate() ret = {} for k, name in enumerate(self.outputs0names): ret[name] = maybeToScalar(C.DMatrix(self.outputsFun0.output(k))) return ret
def step(self, x, u, p): (xVec, uVec, pVec) = vectorizeXUP(x, u, p, self.dae) self.integrator.setInput(xVec, C.INTEGRATOR_X0) self.integrator.setInput(C.veccat([uVec, pVec]), C.INTEGRATOR_P) self.integrator.evaluate() xNext = C.DMatrix(self.integrator.output()) if type(x) == dict: ret = {} for k, name in enumerate(self.dae.xNames()): ret[name] = xNext[k].at(0) else: ret = xNext return ret
def qpsolve(H, g, lbx, ubx, A=NP.zeros((0, 0)), lba=NP.zeros(0), uba=NP.zeros(0)): # Convert to CasADi types H = C.DMatrix(H) g = C.DMatrix(g) lbx = C.DMatrix(lbx) ubx = C.DMatrix(ubx) A = C.DMatrix(A) A = A.reshape((A.size1(), H.size1())) # Make sure matching dimensions lba = C.DMatrix(lba) uba = C.DMatrix(uba) # QP structure qp = C.qpStruct(h=H.sparsity(), a=A.sparsity()) # Create CasADi solver instance if False: solver = C.QpSolver("qpoases", qp) else: solver = C.QpSolver("nlp", qp) solver.setOption("nlp_solver", "ipopt") # Set options #solver.setOption("option_name", ...) # Initialize the solver solver.init() # Pass problem data solver.setInput(H, "h") solver.setInput(g, "g") solver.setInput(A, "a") solver.setInput(lbx, "lbx") solver.setInput(ubx, "ubx") solver.setInput(lba, "lba") solver.setInput(uba, "uba") # Solver the QP solver.evaluate() # Return the solution return NP.array(solver.getOutput("x"))
# # You should have received a copy of the GNU Lesser General Public # License along with CasADi; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA # # import casadi as ca import numpy as np import matplotlib.pyplot as plt nk = 100 # Control discretization tf = 10.0 # End time coll = False # Use collocation integrator # initial and target points x0 = ca.DMatrix([0, np.pi / 10, 0, 0]) x_target = ca.DMatrix([0, np.pi, 0, 0]) # control limitations u_min = -40. u_max = 40. x_min = -10. x_max = 10. # constants g = 9.81 mc = 1.0 mp = 1. l = 1. mip = 1. mic = 5.
# ---------------------------------------------------------------------------- # Parameters # ---------------------------------------------------------------------------- # Simulation t_sim = 2.0 # simulation time n_sim = 20 # time steps dt = t_sim / n_sim # time-step length t = np.linspace(0, t_sim, n_sim + 1) # Model nx = 3 nu = 2 # Initial condition x0 = ca.DMatrix([10, 6, ca.pi]) # Identity matrix Inx = ca.DMatrix.eye(nx) # ---------------------------------------------------------------------------- # Dynamics # ---------------------------------------------------------------------------- # Continuous dynamics dt_sym = ca.SX.sym('dt') state = cat.struct_symSX(['x', 'y', 'phi']) control = cat.struct_symSX(['v', 'w']) rhs = cat.struct_SX(state) rhs['x'] = control['v'] * ca.cos(state['phi']) rhs['y'] = control['v'] * ca.sin(state['phi'])
def check_and_set_initials(self, \ uN = None, \ pinit = None, \ xinit = None): ''' :param tbd: tbd :type tbd: tbd Define structures for the initial values for the several variables that build up the parameter estimation problem, and prepare the values provided with the arguments properly. Afterwards, the values are stored inside the class variable `Varsinit``. ''' # Define structures for initial values from the original # variable struct of the problem # Set controls values # (only if the number of controls is not 0, else set them nothing) if not self.nu == 0: if uN is None: uN = np.zeros((self.nu, self.nsteps)) uN = np.atleast_2d(uN) if uN.shape == (self.nsteps, self.nu): uN = uN.T if not uN.shape == (self.nu, self.nsteps): raise ValueError( \ "Wrong dimension for control values uN.") self.uN = uN else: self.uN = ca.DMatrix(0, self.nsteps) # Set initials for the parameters if pinit is None: pinit = np.zeros(self.np) pinit = np.atleast_1d(np.squeeze(pinit)) if not pinit.shape == (self.np,): raise ValueError( \ "Wrong dimension for argument pinit.") self.Pinit = pinit # If it's a dynamic problem, set initials and bounds for the states if type(self.system) is not systems.BasicSystem: if xinit is None: xinit = np.zeros((self.nx, self.nsteps + 1)) xinit = np.atleast_2d(xinit) if xinit.shape == (self.nsteps + 1, self.nx): xinit = xinit.T if not xinit.shape == (self.nx, self.nsteps + 1): raise ValueError( \ "Wrong dimension for argument xinit.") self.Xinit = ca.repmat(xinit[:,:-1], self.ntauroot+1, 1) self.XFinit = xinit[:,-1] else: self.Xinit = ca.DMatrix(0, 0) self.XFinit = ca.DMatrix(0, 0) self.Vinit = np.zeros(self.V.shape) self.EPS_Einit = np.zeros(self.EPS_E.shape) self.EPS_Uinit = np.zeros(self.EPS_U.shape)
def __init__(self, system = None, \ tu = None, uN = None, \ ty = None, yN = None, pinit = None, \ xinit = None, \ scheme = "radau", \ order = 3): self.tstart_setup = time.time() SetupsBaseClass.__init__(self) if not type(system) is systems.ExplODE: raise TypeError("Setup-method " + self.__class__.__name__ + \ " not allowed for system of type " + str(type(system)) + ".") self.system = system # Dimensions self.nx = system.x.shape[0] self.nu = system.u.shape[0] self.np = system.p.shape[0] self.neps_e = system.eps_e.shape[0] self.neps_u = system.eps_u.shape[0] self.nphi = system.phi.shape[0] if np.atleast_2d(tu).shape[0] == 1: self.tu = np.asarray(tu) elif np.atleast_2d(tu).shape[1] == 1: self.tu = np.squeeze(np.atleast_2d(tu).T) else: raise ValueError("Invalid dimension for argument tu.") if ty == None: self.ty = self.tu elif np.atleast_2d(ty).shape[0] == 1: self.ty = np.asarray(ty) elif np.atleast_2d(ty).shape[1] == 1: self.ty = np.squeeze(np.atleast_2d(ty).T) else: raise ValueError("Invalid dimension for argument ty.") self.nsteps = self.tu.shape[0] - 1 self.scheme = scheme self.order = order self.tauroot = ca.collocationPoints(order, scheme) # Degree of interpolating polynomial self.ntauroot = len(self.tauroot) - 1 # Define the optimization variables self.P = ca.MX.sym("P", self.np) self.X = ca.MX.sym("X", (self.nx * (self.ntauroot+1)), self.nsteps) self.XF = ca.MX.sym("XF", self.nx) self.V = ca.MX.sym("V", self.nphi, self.nsteps+1) if self.neps_e != 0: self.EPS_E = ca.MX.sym("EPS_E", \ (self.neps_e * self.ntauroot), self.nsteps) else: self.EPS_E = ca.DMatrix(0, self.nsteps) if self.neps_u != 0: self.EPS_U = ca.MX.sym("EPS_U", \ (self.neps_u * self.ntauroot), self.nsteps) else: self.EPS_U = ca.DMatrix(0, self.nsteps) # Define bounds and initial values self.check_and_set_initials( \ uN = uN, \ pinit = pinit, \ xinit = xinit) # Set tp the collocation coefficients # Coefficients of the collocation equation self.C = np.zeros((self.ntauroot + 1, self.ntauroot + 1)) # Coefficients of the continuity equation self.D = np.zeros(self.ntauroot + 1) # Dimensionless time inside one control interval tau = ca.SX.sym("tau") # Construct the matrix T that contains all collocation time points self.T = np.zeros((self.nsteps, self.ntauroot + 1)) for k in range(self.nsteps): for j in range(self.ntauroot + 1): self.T[k,j] = self.tu[k] + \ (self.tu[k+1] - self.tu[k]) * self.tauroot[j] self.T = self.T.T # For all collocation points self.lfcns = [] for j in range(self.ntauroot + 1): # Construct Lagrange polynomials to get the polynomial basis # at the collocation point L = 1 for r in range(self.ntauroot + 1): if r != j: L *= (tau - self.tauroot[r]) / \ (self.tauroot[j] - self.tauroot[r]) lfcn = ca.SXFunction("lfcn", [tau],[L]) # Evaluate the polynomial at the final time to get the # coefficients of the continuity equation [self.D[j]] = lfcn([1]) # Evaluate the time derivative of the polynomial at all # collocation points to get the coefficients of the # collocation equation tfcn = lfcn.tangent() for r in range(self.ntauroot + 1): self.C[j,r] = tfcn([self.tauroot[r]])[0] self.lfcns.append(lfcn) # Initialize phiN self.phiN = [] # Initialize measurement function phifcn = ca.MXFunction("phifcn", \ [system.t, system.u, system.x, system.eps_u, system.p], \ [system.phi]) # Initialzie setup of g self.g = [] # Initialize ODE right-hand-side ffcn = ca.MXFunction("ffcn", \ [system.t, system.u, system.x, system.eps_e, system.eps_u, \ system.p], [system.f]) # Collect information for measurement function # Structs to hold variables for later mapped evaluation Tphi = [] Uphi = [] Xphi = [] EPS_Uphi = [] for k in range(self.nsteps): hk = self.tu[k + 1] - self.tu[k] t_meas = self.ty[np.where(np.logical_and( \ self.ty >= self.tu[k], self.ty < self.tu[k + 1]))] for t_meas_j in t_meas: Uphi.append(self.uN[:, k]) EPS_Uphi.append(self.EPS_U[:self.neps_u, k]) if t_meas_j == self.tu[k]: Tphi.append(self.tu[k]) Xphi.append(self.X[:self.nx, k]) else: tau = (t_meas_j - self.tu[k]) / hk x_temp = 0 for r in range(self.ntauroot + 1): x_temp += self.lfcns[r]([tau])[0] * \ self.X[r*self.nx : (r+1) * self.nx, k] Tphi.append(t_meas_j) Xphi.append(x_temp) if self.tu[-1] in self.ty: Tphi.append(self.tu[-1]) Uphi.append(self.uN[:,-1]) Xphi.append(self.XF) EPS_Uphi.append(self.EPS_U[:self.neps_u,-1]) # Mapped calculation of the collocation equations # Collocation nodes hc = ca.MX.sym("hc", 1) tc = ca.MX.sym("tc", self.ntauroot) xc = ca.MX.sym("xc", self.nx * (self.ntauroot+1)) eps_ec = ca.MX.sym("eps_ec", self.neps_e * self.ntauroot) eps_uc = ca.MX.sym("eps_uc", self.neps_u * self.ntauroot) coleqn = ca.vertcat([ \ hc * ffcn([tc[j-1], \ system.u, \ xc[j*self.nx : (j+1)*self.nx], \ eps_ec[(j-1)*self.neps_e : j*self.neps_e], \ eps_uc[(j-1)*self.neps_u : j*self.neps_u], \ system.p])[0] - \ sum([self.C[r,j] * xc[r*self.nx : (r+1)*self.nx] \ for r in range(self.ntauroot + 1)]) \ for j in range(1, self.ntauroot + 1)]) coleqnfcn = ca.MXFunction("coleqnfcn", \ [hc, tc, system.u, xc, eps_ec, eps_uc, system.p], [coleqn]) coleqnfcn = coleqnfcn.expand() [gcol] = coleqnfcn.map([ \ np.atleast_2d((self.tu[1:] - self.tu[:-1])), self.T[1:,:], \ self.uN, self.X, self.EPS_E, self.EPS_U, self.P]) # Continuity nodes xnext = ca.MX.sym("xnext", self.nx) conteqn = xnext - sum([self.D[r] * xc[r*self.nx : (r+1)*self.nx] \ for r in range(self.ntauroot + 1)]) conteqnfcn = ca.MXFunction("conteqnfcn", [xnext, xc], [conteqn]) conteqnfcn = conteqnfcn.expand() [gcont] = conteqnfcn.map([ \ ca.horzcat([self.X[:self.nx, 1:], self.XF]), self.X]) # Stack equality constraints together self.g = ca.veccat([gcol, gcont]) # Evaluation of the measurement function [self.phiN] = phifcn.map( \ [ca.horzcat(k) for k in Tphi, Uphi, Xphi, EPS_Uphi] + \ [self.P]) # self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN]) self.tend_setup = time.time() self.duration_setup = self.tend_setup - self.tstart_setup print('Initialization of ExplODE system sucessful.')
def __init__(self, system = None, \ tu = None, uN = None, \ pinit = None): SetupsBaseClass.__init__(self) if not type(system) is systems.BasicSystem: raise TypeError("Setup-method " + self.__class__.__name__ + \ " not allowed for system of type " + str(type(system)) + ".") self.system = system # Dimensions self.nu = system.u.shape[0] self.np = system.p.shape[0] self.nphi = system.phi.shape[0] if np.atleast_2d(tu).shape[0] == 1: self.tu = np.asarray(tu) elif np.atleast_2d(tu).shape[1] == 1: self.tu = np.squeeze(np.atleast_2d(tu).T) else: raise ValueError("Invalid dimension for argument tu.") self.nsteps = tu.shape[0] # Define the struct holding the variables self.P = ca.MX.sym("P", self.np) self.X = ca.DMatrix(0, self.nsteps) self.XF = ca.DMatrix(0, self.nsteps) self.V = ca.MX.sym("V", self.nphi, self.nsteps) self.EPS_E = ca.DMatrix(0, self.nsteps) self.EPS_U = ca.DMatrix(0, self.nsteps) # Set bounds and initial values self.check_and_set_initials( \ uN = uN, pinit = pinit) # Set up phiN self.phiN = [] phifcn = ca.MXFunction("phifcn", \ [system.t, system.u, system.p], [system.phi]) for k in range(self.nsteps): self.phiN.append(phifcn([self.tu[k], \ self.uN[:, k], self.P])[0]) self.phiN = ca.vertcat(self.phiN) # self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN]) # Set up g # TODO! Can/should/must gfcn depend on uN and/or t? gfcn = ca.MXFunction("gfcn", [system.p], [system.g]) self.g = gfcn.call([self.P])[0] self.tend_setup = time.time() self.duration_setup = self.tend_setup - self.tstart_setup print('Initialization of BasicSystem system sucessful.')
xSim = np.copy(x0) + ca.vertcat([np.random.normal() * scaleX[k] / 10 for k in range(ocp.x.size())]) zSim = np.copy(z0) xSh.append(xSim) xh = [] uh = [] yh = [] yPh = [] Juh_NMPC = [] monitoredOlgah = [] monitoredModelicah = [] Obj_total = 0 Objh_NMPC = [] ObjTotalh = [] simTime = [] xh.append(ca.DMatrix(x0)) ULog = None monitor = True if monitor: measurementsEst = ca.vertcat([ocp.variable(varName).v for varName in measurementsMonitorModelica]) measure_funcEst = ocp.beq(measurementsEst) HEst = ca.SXFunction(ca.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t), [measure_funcEst]) HEst.init() if openLoop or controlOlga: # testing state estimation on if controlOlga and not openLoop: y0Olga = Olga.OLGA_read_tags(da, measurementTagsOlga) extendedKalman = True # u0 = ca.vertcat([ocp.variable(xit.getName()).initialGuess.getValue() for xit in ocp.u])
def runSolver(self, U, trajTrue=None): # make sure all bounds are set (xMissing, pMissing) = self._guessMap.getMissing() msg = [] for name in xMissing: msg.append("you forgot to set a guess for \"" + name + "\" at timesteps: " + str(xMissing[name])) for name in pMissing: msg.append("you forgot to set a guess for \"" + name + "\"") if len(msg) > 0: raise ValueError('\n'.join(msg)) lbx, ubx = zip(*(self._boundMap.vectorize())) xk = C.DMatrix(list(self._guessMap.vectorize())) for k in range(100): ############# plot stuff ############### print "iteration: ", k # import nmheMaps # xOpt = np.array(xk).squeeze() # traj = nmheMaps.VectorizedReadOnlyNmheMap(self.dae,self.nk,xOpt) # # xsT = np.array([trajTrue.lookup('x',timestep=kk) for kk in range(self.nk+1)] ) # ysT = np.array([trajTrue.lookup('y',timestep=kk) for kk in range(self.nk+1)] ) # zsT = np.array([trajTrue.lookup('z',timestep=kk) for kk in range(self.nk+1)] ) # # xs = np.array([traj.lookup('x',timestep=kk) for kk in range(self.nk+1)] ) # ys = np.array([traj.lookup('y',timestep=kk) for kk in range(self.nk+1)] ) # zs = np.array([traj.lookup('z',timestep=kk) for kk in range(self.nk+1)] ) # # outputMap = nmheMaps.NmheOutputMap(self._outputMapGenerator, xOpt, U) # c = np.array([outputMap.lookup('c',timestep=kk) for kk in range(self.nk)]) # cdot = np.array([outputMap.lookup('cdot',timestep=kk) for kk in range(self.nk)]) # # figure() # title(str(float(k))) # subplot(3,2,1) # plot(xs) # plot(xsT) # ylabel('x '+str(k)) # # subplot(3,2,3) # plot(ys) # plot(ysT) # ylabel('y '+str(k)) # # subplot(3,2,5) # plot(zs) # plot(zsT) # ylabel('z '+str(k)) # ## subplot(2,2,2) ## plot(dxs,-dzs) ## ylabel('vel') ## axis('equal') # # subplot(3,2,2) # plot(c) # ylabel('c') # # subplot(3,2,4) # plot(cdot) # ylabel('cdot') # ########################################## self.masterFun.setInput(xk, 0) self.masterFun.setInput(U, 1) t0 = time.time() try: self.masterFun.evaluate() except RuntimeError as e: print "ERRRRRRRRRRRRROR" show() raise e t1 = time.time() masterFunTime = (t1 - t0) * 1000 hessL = self.masterFun.output(0) gradF = self.masterFun.output(1) g = self.masterFun.output(2) jacobG = self.masterFun.output(3) f = self.masterFun.output(4) self.qp.setInput(0, C.QP_X_INIT) self.qp.setInput(hessL, C.QP_H) self.qp.setInput(jacobG, C.QP_A) self.qp.setInput(gradF, C.QP_G) assert all((lbx - xk) <= 0), "lower bounds violation" assert all((ubx - xk) >= 0), "upper bounds violation" self.qp.setInput(lbx - xk, C.QP_LBX) self.qp.setInput(ubx - xk, C.QP_UBX) self.qp.setInput(self.glb - g, C.QP_LBA) self.qp.setInput(self.gub - g, C.QP_UBA) t0 = time.time() self.qp.evaluate() t1 = time.time() # print "gradF: ",gradF # print 'dim(jacobG): "gra # print "rank: ",np.linalg.matrix_rank(jacobG) print "masterFun delta time: %.3f ms" % masterFunTime print "f: ", f, '\tmax constraint: ', max(C.fabs(g)) print "qp delta time: %.3f ms" % ((t1 - t0) * 1000) print "" deltaX = self.qp.output(C.QP_PRIMAL) # import scipy.io # scipy.io.savemat('hessL.mat',{'hessL':np.array(hessL), # 'gradF':np.array(gradF), # 'x0':0*np.array(deltaX), # 'xopt':np.array(deltaX), # 'lbx':np.array(lbx-xk), # 'ubx':np.array(ubx-xk), # 'jacobG':np.array(jacobG), # 'lba':np.array(self.glb-g), # 'uba':np.array(self.gub-g)}) # import sys; sys.exit() # print deltaX xk += deltaX
import joy #tc0 = 2*389.970797939731 x0 = C.DMatrix([ -10 # position , 0, 15, 1 # DCM , 0, 0, 0, 1, 0, 0, 0, 1, 20.0 # vel , 0.0, 0.0, 0 # ang vel , 0, 0 ]) ts = 0.02 sim = simutils.Sim(ts=ts, sloMoFactor=4,
def getSteadyState(dae, r0, v0): # make steady state model g = Constraints() g.add(dae.getResidual(), '==', 0, tag=('dae residual', None)) def constrainInvariantErrs(): R_n2b = dae['R_n2b'] makeOrthonormal(g, R_n2b) g.add(dae['c'], '==', 0, tag=('c(0)==0', None)) g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0', None)) constrainInvariantErrs() # constrain airspeed g.add(dae['airspeed'], '>=', v0, tag=('airspeed fixed', None)) g.addBnds(dae['alpha_deg'], (4, 10), tag=('alpha', None)) g.addBnds(dae['beta_deg'], (-10, 10), tag=('beta', None)) dvs = C.veccat( [dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()]) # ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])]) obj = 0 obj += (dae['cL'] - 0.5)**2 obj += dae.ddt('w_bn_b_x')**2 obj += dae.ddt('w_bn_b_y')**2 obj += dae.ddt('w_bn_b_z')**2 ffcn = C.SXFunction([dvs], [obj]) gfcn = C.SXFunction([dvs], [g.getG()]) ffcn.init() gfcn.init() guess = { 'x': r0, 'y': 0, 'z': -1, 'r': r0, 'dr': 0, 'e11': 0, 'e12': -1, 'e13': 0, 'e21': 0, 'e22': 0, 'e23': 1, 'e31': -1, 'e32': 0, 'e33': 0, 'dx': 0, 'dy': -20, 'dz': 0, 'w_bn_b_x': 0, 'w_bn_b_y': 0, 'w_bn_b_z': 0, 'aileron': 0, 'elevator': 0, 'rudder': 0, 'daileron': 0, 'delevator': 0, 'drudder': 0, 'nu': 300, 'motor_torque': 10, 'dmotor_torque': 0, 'ddr': 0, 'dddr': 0.0, 'w0': 10.0 } dotGuess = { 'x': 0, 'y': -20, 'z': 0, 'dx': 0, 'dy': 0, 'dz': 0, 'r': 0, 'dr': 0, 'e11': 0, 'e12': 0, 'e13': 0, 'e21': 0, 'e22': 0, 'e23': 0, 'e31': 0, 'e32': 0, 'e33': 0, 'w_bn_b_x': 0, 'w_bn_b_y': 0, 'w_bn_b_z': 0, 'aileron': 0, 'elevator': 0, 'rudder': 0, 'ddr': 0 } guessVec = C.DMatrix([ guess[n] for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames() ] + [dotGuess[n] for n in dae.xNames()]) bounds = { 'x': (0.01, r0 * 2), 'y': (0, 0), 'z': (-r0 * 0.2, -r0 * 0.2), 'dx': (0, 0), 'dy': (-50, 0), 'dz': (0, 0), 'r': (r0, r0), 'dr': (0, 0), 'ddr': (0, 0), 'e11': (-0.5, 0.5), 'e12': (-1.5, -0.5), 'e13': (-0.5, 0.5), 'e21': (-0.5, 0.5), 'e22': (-0.5, 0.5), 'e23': (0.5, 1.5), 'e31': (-1.5, -0.5), 'e32': (-0.5, 0.5), 'e33': (-0.5, 0.5), 'w_bn_b_x': (0, 0), 'w_bn_b_y': (0, 0), 'w_bn_b_z': (0, 0), # 'aileron':(-0.2,0.2),'elevator':(-0.2,0.2),'rudder':(-0.2,0.2), 'aileron': (0, 0), 'elevator': (0, 0), 'rudder': (0, 0), 'daileron': (0, 0), 'delevator': (0, 0), 'drudder': (0, 0), 'nu': (0, 3000), 'dddr': (0, 0), 'w0': (10, 10) } dotBounds = { 'dz': (-C.inf, 0) } #'dx':(-500,-500),'dy':(-500,500),'dz':(-500,500), # 'w_bn_b_x':(0,0),'w_bn_b_y':(0,0),'w_bn_b_z':(0,0), for name in dae.xNames(): if name not in dotBounds: dotBounds[name] = (-C.inf, C.inf) boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \ [dotBounds[n] for n in dae.xNames()] # gfcn.setInput(guessVec) # gfcn.evaluate() # ret = gfcn.output() # for k,v in enumerate(ret): # if math.isnan(v): # print 'index ',k,' is nan: ',g._tags[k] # import sys; sys.exit() # context = zmq.Context(1) # publisher = context.socket(zmq.PUB) # publisher.bind("tcp://*:5563") # class MyCallback: # def __init__(self): # import rawekite.kiteproto as kiteproto # import rawekite.kite_pb2 as kite_pb2 # self.kiteproto = kiteproto # self.kite_pb2 = kite_pb2 # self.iter = 0 # def __call__(self,f,*args): # x = C.DMatrix(f.input('x')) # sol = {} # for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()): # sol[name] = x[k].at(0) # lookup = lambda name: sol[name] # kp = self.kiteproto.toKiteProto(lookup, # lineAlpha=0.2) # mc = self.kite_pb2.MultiCarousel() # mc.horizon.extend([kp]) # mc.messages.append("iter: "+str(self.iter)) # self.iter += 1 # publisher.send_multipart(["multi-carousel", mc.SerializeToString()]) solver = C.IpoptSolver(ffcn, gfcn) def addCallback(): nd = len(boundsVec) nc = g.getLb().size() c = C.PyFunction( MyCallback(), C.nlpsolverOut(x=C.sp_dense(nd, 1), f=C.sp_dense(1, 1), lam_x=C.sp_dense(nd, 1), lam_p=C.sp_dense(0, 1), lam_g=C.sp_dense(nc, 1), g=C.sp_dense(nc, 1)), [C.sp_dense(1, 1)]) c.init() solver.setOption("iteration_callback", c) # addCallback() solver.setOption('max_iter', 10000) solver.setOption('expand', True) # solver.setOption('tol',1e-14) # solver.setOption('suppress_all_output','yes') # solver.setOption('print_time',False) solver.init() solver.setInput(g.getLb(), 'lbg') solver.setInput(g.getUb(), 'ubg') #guessVec = numpy.load('steady_state_guess.npy') solver.setInput(guessVec, 'x0') lb, ub = zip(*boundsVec) solver.setInput(C.DMatrix(lb), 'lbx') solver.setInput(C.DMatrix(ub), 'ubx') solver.solve() ret = solver.getStat('return_status') assert ret in ['Solve_Succeeded', 'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret # publisher.close() # context.destroy() xOpt = solver.output('x') #numpy.save('steady_state_guess',numpy.squeeze(numpy.array(xOpt))) k = 0 sol = {} for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames(): sol[name] = xOpt[k].at(0) k += 1 # print name+':\t',sol[name] dotSol = {} for name in dae.xNames(): dotSol[name] = xOpt[k].at(0) k += 1 # print 'DDT('+name+'):\t',dotSol[name] return sol, dotSol
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2 # obj += (nmhe('z',timestep=k) - xTraj[k][1])**2 # obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2 # obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2 # obj += 1e-8*nmhe('x',timestep=k)**2 # obj += 1e-8*nmhe('z',timestep=k)**2 # obj += 1e-8*nmhe('dx',timestep=k)**2 # obj += 1e-8*nmhe('dz',timestep=k)**2 # obj += 1e-8*nmhe('m')**2 mhe.setObj(obj) # nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000) # get u uTraj = C.DMatrix([[traj.lookup(name, timestep=k) for k in range(nk)] for name in dae.uNames()]).T endTime = traj.tgrid[1, 0, 0] - traj.tgrid[0, 0, 0] mhe.makeSolver(endTime, traj=traj) mhe.runSolver(uTraj, traj) # # callback function # class MyCallback: # def __init__(self): # self.iter = 0 # def __call__(self,f,*args): # self.iter = self.iter + 1 # xOpt = numpy.array(f.input(C.NLP_X_OPT)) # # traj = trajectory.Trajectory(ocp,xOpt) # # kiteProtos = []
# Start Simulator da = Olga.OLGA_connect(serverIP=serverIP, modelPrefix=model) y0Olga = Olga.OLGA_read_tags(da, measurementTagsOlga) xSh = [] xh = [] uh = [] yh = [] yPh = [] Juh_NMPC = [] monitoredOlgah = [] monitoredModelicah = [] Objh_NMPC = [] simTime = [] ObjTotalh = [] xh.append(ca.DMatrix(x0)) ULog = None monitor = True if monitor: measurementsEst = ca.vertcat([ocp.variable(varName).v for varName in measurementsMonitorModelica]) measure_funcEst = ocp.beq(measurementsEst) HEst = ca.SXFunction(ca.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t), [measure_funcEst]) HEst.init() extendedKalman = True # u0 = ca.vertcat([ocp.variable(xit.getName()).initialGuess.getValue() for xit in ocp.u]) # x0,z0,y0 = daeModel.findSteadyState(u0,None,None,0,consList=['net.w1.z1','net.w2.z1','net.p.z']) KF.setState(x0, z0)
def run_simulation(self, \ x0 = None, tsim = None, usim = None, psim = None, method = "rk"): r''' :param x0: initial value for the states :math:`x_0 \in \mathbb{R}^{n_x}` :type x0: list, numpy,ndarray, casadi.DMatrix :param tsim: optional, switching time points for the controls :math:`t_{sim} \in \mathbb{R}^{L}` to be used for the simulation :type tsim: list, numpy,ndarray, casadi.DMatrix :param usim: optional, control values :math:`u_{sim} \in \mathbb{R}^{n_u \times L}` to be used for the simulation :type usim: list, numpy,ndarray, casadi.DMatrix :param psim: optional, parameter set :math:`p_{sim} \in \mathbb{R}^{n_p}` to be used for the simulation :type psim: list, numpy,ndarray, casadi.DMatrix :param method: optional, CasADi integrator to be used for the simulation :type method: str This function performs a simulation of the system for a given parameter set :math:`p_{sim}`, starting from a user-provided initial value for the states :math:`x_0`. If the argument ``psim`` is not specified, the estimated parameter set :math:`\hat{p}` is used. For this, a parameter estimation using :func:`run_parameter_estimation()` has to be done beforehand, of course. By default, the switching time points for the controls :math:`t_u` and the corresponding controls :math:`u_N` will be used for simulation. If desired, other time points :math:`t_{sim}` and corresponding controls :math:`u_{sim}` can be passed to the function. For the moment, the function can only be used for systems of type :class:`pecas.systems.ExplODE`. ''' intro.pecas_intro() print('\n' + 27 * '-' + \ ' PECas system simulation ' + 26 * '-') print('\nPerforming system simulation, this might take some time ...') if not type(self.pesetup.system) is systems.ExplODE: raise NotImplementedError("Until now, this function can only " + \ "be used for systems of type ExplODE.") if x0 == None: raise ValueError("You have to provide an initial value x0 " + \ "to run the simulation.") x0 = np.squeeze(np.asarray(x0)) if np.atleast_1d(x0).shape[0] != self.pesetup.nx: raise ValueError("Wrong dimension for initial value x0.") if tsim == None: tsim = self.pesetup.tu if usim == None: usim = self.pesetup.uN if psim == None: try: psim = self.phat except AttributeError: errmsg = ''' You have to either perform a parameter estimation beforehand to obtain a parameter set that can be used for simulation, or you have to provide a parameter set in the argument psim. ''' raise AttributeError(errmsg) else: if not np.atleast_1d(np.squeeze(psim)).shape[0] == self.pesetup.np: raise ValueError("Wrong dimension for parameter set psim.") fp = ca.MXFunction("fp", \ [self.pesetup.system.t, self.pesetup.system.u, \ self.pesetup.system.x, self.pesetup.system.eps_e, \ self.pesetup.system.eps_u, self.pesetup.system.p], \ [self.pesetup.system.f]) fpeval = fp([\ self.pesetup.system.t, self.pesetup.system.u, \ self.pesetup.system.x, np.zeros(self.pesetup.neps_e), \ np.zeros(self.pesetup.neps_u), psim])[0] fsim = ca.MXFunction("fsim", \ ca.daeIn(t = self.pesetup.system.t, \ x = self.pesetup.system.x, \ p = self.pesetup.system.u), \ ca.daeOut(ode = fpeval)) Xsim = [] Xsim.append(x0) u0 = ca.DMatrix() for k, e in enumerate(tsim[:-1]): try: integrator = ca.Integrator("integrator", method, \ fsim, {"t0": e, "tf": tsim[k+1]}) except RuntimeError as err: errmsg = ''' It seems like you want to use an integration method that is not currently supported by CasADi. Please refer to the CasADi documentation for a list of supported integrators, or use the default RK4-method by not setting the method-argument of the function. ''' raise RuntimeError(errmsg) if not self.pesetup.nu == 0: u0 = usim[:, k] Xk_end = itemgetter('xf')(integrator({'x0': x0, 'p': u0})) Xsim.append(Xk_end) x0 = Xk_end self.Xsim = ca.horzcat(Xsim) print( \ '''System simulation finished.''')
t = time() dummy = c.mul(x.T, x) print "CasADi pure = %.4f s" % (time() - t) X = MX("X", x.sparsity()) t = time() f = MXFunction([X], [c.mul(X.T, X)]) f.init() print "CasADi MX wrapped init overhead = %.4f s" % (time() - t) f.setInput(x) t = time() f.evaluate() print "CasADi MX wrapped = %.4f s" % (time() - t) t = time() # Create the sparsity pattern for the matrix-matrix product spres = x.sparsity().patternProduct(x.sparsity()) print "CasADi generating procuct sparsity pattern = %.4f s" % (time() - t) t = time() # Create the return object with correct sparsity ret = c.DMatrix(spres) print "CasADi allocating resulting = %.4f s" % (time() - t) t = time() # Carry out the matrix product c.DMatrix.mul_no_alloc(x, x, ret) print "CasADi actual multiplication = %.4f s" % (time() - t)
# along with PECas. If not, see <http://www.gnu.org/licenses/>. # This example is an adapted version of the system identification example # included in CasADi, for the original file see: # https://github.com/casadi/casadi/blob/master/docs/examples/python/sysid.py import casadi as ca import pylab as pl import pecas import os N = 1000 fs = 610.1 p_true = ca.DMatrix([5.625e-6, 2.3e-4, 1, 4.69]) p_guess = ca.DMatrix([5, 3, 1, 5]) scale = ca.vertcat([1e-6, 1e-4, 1, 1]) x = ca.MX.sym("x", 2) u = ca.MX.sym("u", 1) p = ca.MX.sym("p", 4) f = ca.vertcat([ x[1], \ (u - scale[3] * p[3] * x[0]**3 - scale[2] * p[2] * x[0] - \ scale[1] * p[1] * x[1]) / (scale[0] * p[0]), \ ]) phi = x
yN = y_randn, wv = wv) lsqpe_test.run_parameter_estimation() p_test.append(lsqpe_test.phat) p_mean = pl.mean(p_test) p_std = pl.std(p_test, ddof=0) lsqpe_test.compute_covariance_matrix() # Generate report print("\np_mean = " + str(ca.DMatrix(p_mean))) print("phat_last_exp = " + str(ca.DMatrix(lsqpe_test.phat))) print("\np_sd = " + str(ca.DMatrix(p_std))) print("sd_from_covmat = " + str(ca.diag(ca.sqrt(lsqpe_test.Covp)))) print("beta = " + str(lsqpe_test.beta)) print("\ndelta_abs_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \ ca.diag(ca.sqrt(lsqpe_test.Covp))))) print("delta_rel_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \ ca.diag(ca.sqrt(lsqpe_test.Covp))) / ca.DMatrix(p_std))) fname = os.path.basename(__file__)[:-3] + ".rst" report = open(fname, "w")
import pickle import rawe import rawekite x0 = C.DMatrix( [ 1.154244772411 , -0.103540608242 , -0.347959211327 , 0.124930983341 , 0.991534857363 , 0.035367725910 , 0.316039689643 , -0.073559821379 , 0.945889986864 , 0.940484536806 , -0.106993361072 , -0.322554269411 , 0.000000000000 , 0.000000000000 , 0.000000000000 , 0.137035790811 , 3.664945343102 , -1.249768772258 , 3.874600000000 , 0.0 ]) x0=C.veccat([x0,C.sqrt(C.sumAll(x0[0:2]*x0[0:2])),0,0,0]) def setupOcp(dae,conf,nk=50,nicp=1,deg=4): ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg) ocp.setupCollocation(ocp.lookup('endTime'))
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2 # obj += (nmhe('z',timestep=k) - xTraj[k][1])**2 # obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2 # obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2 # obj += 1e-8*nmhe('x',timestep=k)**2 # obj += 1e-8*nmhe('z',timestep=k)**2 # obj += 1e-8*nmhe('dx',timestep=k)**2 # obj += 1e-8*nmhe('dz',timestep=k)**2 # obj += 1e-8*nmhe('m')**2 # nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000) nmhe.setObj(obj) uTraj = C.DMatrix(np.concatenate(uTraj)) nmhe.makeSolver(dt) nmhe.runSolver(uTraj) ## newton.isolver.input(0).set(x0) # newton.isolver.setInput(x0,0) # newton.isolver.setInput(0,1) # torque # newton.isolver.setInput(0.3,2) # mass # newton.isolver.evaluate() # newton.isolver.evaluate() # # j = newton.isolver.jacobian(0,1) # j.init() # j.setInput(x0,0) # j.setInput(0,1) # j.setInput(0.3,2)
cY += conf['cY_rudder'] * dae['rudder'] cD += conf['cD_rudder2'] * dae['rudder'] * dae['rudder'] + conf[ 'cD_B_rudder'] * beta * dae['rudder'] + conf['cD_rudder'] * dae[ 'rudder'] dae['cL'] = cL dae['cD'] = cD dae['cY'] = cY dae['cD_tether'] = 0.25 * dae['r'] * 0.001 / sref dae['L_over_D'] = cL / cD cD = dae['cD'] + dae['cD_tether'] dae['L_over_D_with_tether'] = cL / cD ######## moment coefficients ####### # offset dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0]) # with roll rates # non-dimensionalized angular velocity w_bn_b_hat = C.veccat([ 0.5 * conf['bref'] / dae['airspeed'] * w_bn_b[0], 0.5 * conf['cref'] / dae['airspeed'] * w_bn_b[1], 0.5 * conf['bref'] / dae['airspeed'] * w_bn_b[2] ]) momentCoeffs_pqr = C.mul( C.vertcat([ C.horzcat([conf['cl_p'], conf['cl_q'], conf['cl_r']]), C.horzcat([conf['cm_p'], conf['cm_q'], conf['cm_r']]), C.horzcat([conf['cn_p'], conf['cn_q'], conf['cn_r']]) ]), w_bn_b_hat) dae['momentCoeffs_pqr'] = momentCoeffs_pqr
ocp.makeSemiExplicit() ocp.eliminateIndependentParameters() ocp.eliminateDependentParameters() ocp.eliminateDependentParameterInterdependencies() ocp.eliminateAlgebraic() DT = 10 # for simulation and kalman filtering scaleXModelica = ca.vertcat( [ocp.variable(ocp.x[k].getName()).nominal for k in range(ocp.x.size())]) scaleZModelica = ca.vertcat( [ocp.variable(ocp.z[k].getName()).nominal for k in range(ocp.z.size())]) scaleUModelica = ca.vertcat( [ocp.variable(ocp.u[k].getName()).nominal for k in range(ocp.u.size())]) u = ca.DMatrix([1.2963, 1.3245, 70000, 200000, 200000]) daeModel = DaeModel.DaeModel() daeModel.init(ocp, DT, measurementTagsModelica) x0, z0, y_0 = daeModel.findSteadyState( u, None, None, 0, consList=['net.w1.z1', 'net.w2.z1', 'net.p.z']) uOpt, xOpt, zOpt, yOpt = daeModel.findOptimalPoint( u, x0=x0, z0=z0, simCount=0, consList=['net.w1.z1', 'net.w2.z1', 'net.p.z']) x0 = xOpt
## Start Simulator openLoop = False controlOlga = False xh = [] uh = [] yh = [] yPh = [] simTime = [] Juh_NMPC = [] monitoredSimh = [] monitoredModelicah = [] Objh_NMPC = [] ObjTotalh = [] xh.append(ca.DMatrix(x0)) ULog = None monitor = True if monitor: measurementsEst = ca.vertcat([ocp.variable(varName).v for varName in measurementsMonitorModelica]) measure_funcEst = ocp.beq(measurementsEst) HEst = ca.SXFunction(ca.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t), [measure_funcEst]) HEst.init() extendedKalman = True KF.setState(x0, z0) x_hat = KF.getState() # z_hat= ca.vertcat([ocp.variable(ocp.z[k].getName()).start for k in range(ocp.z.size())])
# You should have received a copy of the GNU Lesser General Public License # along with PECas. If not, see <http://www.gnu.org/licenses/>. # This example is an adapted version of the system identification example # included in CasADi, for the original file see: # https://github.com/casadi/casadi/blob/master/docs/examples/python/sysid.py import pylab as pl import casadi as ca import pecas N = 10000 fs = 610.1 p_true = ca.DMatrix([5.625e-6, 2.3e-4, 1, 4.69]) p_guess = ca.DMatrix([5, 3, 1, 5]) scale = ca.vertcat([1e-6, 1e-4, 1, 1]) x = ca.MX.sym("x", 2) u = ca.MX.sym("u", 1) p = ca.MX.sym("p", 4) f = ca.vertcat([ x[1], \ (u - scale[3] * p[3] * x[0]**3 - scale[2] * p[2] * x[0] - \ scale[1] * p[1] * x[1]) / (scale[0] * p[0]), \ ]) phi = x