def _setup_nonlinear_regulator(self): """ Construct a MHE solver.""" N = dict(x=self.Nx, u=self.Nu, t=self.N) funcargs = dict(f=["x", "u", "ds"], l=["x", "u", "xs", "us"], Pf=["x", "xs"]) extrapar = dict(xs=self.xs[-1], us=self.us[-1], ds=self.ds[-1]) l = mpc.getCasadiFunc(self._stage_cost, [self.Nx, self.Nu, self.Nx, self.Nu], funcargs["l"]) Pf = mpc.getCasadiFunc(self._terminal_cost, [self.Nx, self.Nx], funcargs["Pf"]) lb = dict(u=self.ulb.T) ub = dict(u=self.uub.T) self.nonlinear_regulator = mpc.nmpc(f=self.fxud, l=l, Pf=Pf, N=N, lb=lb, ub=ub, extrapar=extrapar, funcargs=funcargs, x0=self.x0[-1].squeeze(axis=-1)) self.nonlinear_regulator.par["xs"] = self.xs * (self.N + 1) self.nonlinear_regulator.par["us"] = self.us * self.N self.nonlinear_regulator.par["ds"] = self.ds * self.N self.nonlinear_regulator.solve() breakpoint() self.useq.append(np.asarray(self.nonlinear_regulator.var["u"]))
def get_mhe_models_and_matrices(fxud, hxd, sample_time, num_rk4_discretization_steps, Nx, Nu, Nd, Nmhe, Qwx, Qwd, Rv, xs, us, ds, ys): """ Get the models, proir estimates and data, and the penalty matrices to setup an MHE solver.""" # Prior estimates and data. xprior = np.concatenate((xs, ds), axis=0) xprior = np.repeat(xprior.T, Nmhe, axis=0) u = np.repeat(us, Nmhe, axis=0) y = np.repeat(ys, Nmhe + 1, axis=0) # Penalty matrices. Qwxinv = np.linalg.inv(Qwx) Qwdinv = np.linalg.inv(Qwd) Qwinv = scipy.linalg.block_diag(Qwxinv, Qwdinv) P0inv = Qwinv Rvinv = np.linalg.inv(Rv) # Get the augmented models. fxuw = mpc.getCasadiFunc(NonlinearMPCController.mhe_state_space_model( fxud, Nx, Nd), [Nx + Nd, Nu], ["x", "u"], rk4=True, Delta=sample_time, M=num_rk4_discretization_steps) hx = mpc.getCasadiFunc( NonlinearMPCController.mhe_measurement_model(hxd, Nx), [Nx + Nd], ["x"]) # Return the required quantities for MHE. return (fxuw, hx, P0inv, Qwinv, Rvinv, xprior, u, y)
def get_mpc_controller(self, ode_casadi, delta, x0, random_guess=True, c=5, verbosity=0, upper_bound=1.2, lower_bound=0.8): loss = mpc.getCasadiFunc(self.stage_loss, [self.Nx * 2, self.Nu], ["x", "u"], funcname="loss") pf = mpc.getCasadiFunc(self.terminal_loss, [self.Nx * 2], ["x"], funcname="pf") if random_guess is True: guess = {"x": np.random.uniform(5, 10, self.Nx), "u": np.random.uniform(5, 10, self.Nu)} else: guess = {"x": np.concatenate([self.ss_states, [0, 0, 0]]), "u": self.ss_inputs} # Set "c" above 1 to use collocation contargs = dict( N={"t": self.Nt, "x": self.Nx * 2, "u": self.Nu, "c": c}, verbosity=verbosity, l=loss, x0=x0, Pf=pf, # Upper and Lower Confidence Bound ub={"u": upper_bound * self.ss_inputs, "x": upper_bound * np.concatenate([self.ss_states, [inf, inf, inf]])}, lb={"u": lower_bound * self.ss_inputs, "x": lower_bound * np.concatenate([self.ss_states, [-inf, -inf, -inf]])}, guess=guess ) # Create MPC controller mpc_controller = mpc.nmpc(f=ode_casadi, Delta=delta, **contargs) return mpc_controller
def __init__(self, nsim, delta=1, nx=3, nu=2, nt=10, f0=0.1, t0=350, c0=1, r=0.219, k0=7.2e10, er_ratio=8750, u=54.94, rho=1000, cp=0.239, dh=-5e4, xs=np.array([0.878, 324.5, 0.659]), us=np.array([300, 0.1]), x0=np.array([1, 310, 0.659])): self.Delta = delta self.Nsim = nsim self.Nx = nx self.Nu = nu self.Nt = nt self.F0 = f0 self.T0 = t0 self.c0 = c0 self.r = r self.k0 = k0 self.er_ratio = er_ratio self.U = u self.rho = rho self.Cp = cp self.dH = dh self.xs = xs self.us = us self.xsp = np.zeros([self.Nsim + 1, self.Nx]) self.xsp[0, :] = xs self.x0 = x0 self.x = np.zeros([self.Nsim + 1, self.Nx]) self.x[0, :] = self.x0 self.u = np.zeros([self.Nsim + 1, self.Nu]) self.u[0, :] = [300, 0.1] self.u[:, 1] = 0.1 self.cstr_sim = mpc.DiscreteSimulator(self.ode, self.Delta, [self.Nx, self.Nu], ["x", "u"]) self.cstr_ode = mpc.getCasadiFunc(self.ode, [self.Nx, self.Nu], ["x", "u"], funcname="odef") self.cstr_ode_control = mpc.getCasadiFunc(self.ode_control, [self.Nx * 2, self.Nu], ["x", "u"], funcname="odef")
def get_mpc_controller(self, ode_casadi, delta, x0, random_guess=False, c=3, verbosity=0, upper_bound=1.8, lower_bound=0.2, uprev=5): # A very sketchy way to find the amount of time varying parameters if self.parameter.shape == (self.parameter.shape[0], ): shape = 1 else: shape = self.parameter.shape[1] loss = mpc.getCasadiFunc(self.stage_loss, [self.Nx, self.Nu, shape, 1], ["x", "u", "p", "Du"], funcname="loss") # pf = mpc.getCasadiFunc(self.terminal_loss, [self.Nx], ["x"], funcname="pf") # Define where the function args should be. More precisely, we must put p in the loss function. funcargs = {"f": ["x", "u"], "l": ["x", "u", "p", "Du"]} if random_guess is True: guess = { "x": np.random.uniform(5, 10, self.Nx), "u": np.random.uniform(5, 10, self.Nu) } else: guess = {"x": self.ss_states, "u": self.ss_inputs} # Set "c" above 1 to use collocation, parameter is same size as Nx, therefore automatically adjusts contargs = dict( N={ "t": self.Nt, "x": self.Nx, "u": self.Nu, "c": c, "p": 1 }, verbosity=verbosity, l=loss, x0=x0, p=self.parameter, uprev=uprev, # Upper and Lower State Constraints # ub={"u": upper_bound * self.ss_inputs, # "x": upper_bound * np.concatenate([self.ss_states[0:int(self.Nx * 0.5)], [np.inf]])}, # lb={"u": lower_bound * self.ss_inputs, # "x": lower_bound * np.concatenate([self.ss_states[0:int(self.Nx * 0.5)], [-np.inf]])}, guess=guess, funcargs=funcargs) # Create MPC controller mpc_controller = mpc.nmpc(f=ode_casadi, Delta=delta, **contargs) return mpc_controller
def create_robot_controller(idx,Delta,x0): print idx, x0 Nx = 3 Nu = 2 # Define model. def ode(x,u): return np.array([u[0]*np.cos(x[2]), u[0]*np.sin(x[2]), u[1]]) # Then get nonlinear casadi functions and the linearization. ode_casadi = mpc.getCasadiFunc(ode, [Nx,Nu], ["x","u"], funcname="f") Q = np.array([[15.0, 0.0, 0.0], [0.0, 15.0, 0.0], [0.0, 0.0, 0.01]]) Qn = np.array([[150.0, 0.0, 0.0], [0.0, 150.0, 0.0], [0.0, 0.0, 0.01]]) R = np.array([[5.0, 0.0], [0.0, 5.0]]) def lfunc(x,u,x_sp): return mpc.mtimes((x-x_sp).T,Q,(x-x_sp)) + mpc.mtimes(u.T,R,u) l = mpc.getCasadiFunc(lfunc, [Nx,Nu,Nx], ["x","u","x_sp"], funcname="l") def Pffunc(x,x_sp): return mpc.mtimes((x-x_sp).T,Qn,(x-x_sp)) Pf = mpc.getCasadiFunc(Pffunc, [Nx,Nx], ["x","x_sp"], funcname="Pf") # Make optimizers. Note that the linear and nonlinear solvers have some common # arguments, so we collect those below. Nt = 5 x_sp = np.zeros((Nt+1,Nx)) sp = dict(x=x_sp) u_ub = np.array([4, 1]) u_lb = np.array([-4, -1]) commonargs = dict( verbosity=0, l=l, x0=x0, Pf=Pf, lb={"u" : np.tile(u_lb,(Nt,1))}, ub={"u" : np.tile(u_ub,(Nt,1))}, funcargs = {"l":["x","u","x_sp"], "Pf":["x","x_sp"]} ) Nlin = {"t":Nt, "x":Nx, "u":Nu, "x_sp":Nx} Nnonlin = Nlin.copy() Nnonlin["c"] = 2 # Use collocation to discretize. solver = mpc.nmpc(f=ode_casadi,N=Nnonlin,sp=sp,Delta=Delta,**commonargs) solver.fixvar('x',0,x0) return dict(x0=x0,solver=solver,Nt=Nt,Nx=Nx,Nu=Nu)
def __init__(self, nsim, x0=np.array([3]), u0=np.array([3, 6]), xs=np.array([4.5]), us=np.array([5.5, 8]), step_size=0.2, control=False, q_cost=1, r_cost=0.5, random_seed=1): self.Nsim = nsim self.x0 = x0 self.u0 = u0 self.xs = xs self.us = us self.step_size = step_size self.t = np.linspace(0, nsim * self.step_size, nsim + 1) self.control = control # Model Parameters if self.control: self.Nx = 2 # Because of offset free control else: self.Nx = 1 self.Nu = 2 self.action_space = Box(low=np.array([-12, -12]), high=np.array([12, 12])) self.Q = q_cost self.R = r_cost * np.eye(self.Nu) self.A = np.array([-3]) self.B = np.array([1, 1]) self.C = np.array([1]) self.D = 0 # State and Input Trajectories self.x = np.zeros([nsim + 1, self.Nx]) self.u = np.zeros([nsim + 1, self.Nu]) self.x[0, :] = x0 self.u[0, :] = u0 # Build the CasaDI functions self.system_sim = mpc.DiscreteSimulator(self.ode, self.step_size, [self.Nx, self.Nu], ["x", "u"]) self.system_ode = mpc.getCasadiFunc(self.ode, [self.Nx, self.Nu], ["x", "u"], funcname="odef") # Set-point trajectories self.xsp = np.zeros([self.Nsim + 1, self.Nx]) self.xsp[0, :] = self.xs self.usp = np.zeros([self.Nsim + 1, self.Nu]) self.usp[0, :] = self.us # Seed the system for reproducability random.seed(random_seed) np.random.seed(random_seed)
def __init__(self, nsim, delta=1, nx=3, nu=2, nt=10, f0=0.1, t0=350, c0=1, r=0.219, k0=7.2e10, er_ratio=8750, u=54.94, rho=1000, cp=0.239, dh=-5e4, xs=np.array([0.894, 309.8, 2]), us=np.array([300, 0.1]), x0=np.array([1, 310, 2]), control=False): self.Delta = delta self.Nsim = nsim self.Nx = nx self.Nu = nu self.Nt = nt self.F0 = f0 self.T0 = t0 self.c0 = c0 self.r = r self.k0 = k0 self.er_ratio = er_ratio self.U = u self.rho = rho self.Cp = cp self.dH = dh self.xs = xs self.us = us self.xsp = np.zeros([self.Nsim + 1, self.Nx]) self.xsp[0, :] = xs self.x0 = x0 self.x = np.zeros([self.Nsim + 1, self.Nx]) self.x[0, :] = self.x0 self.u = np.zeros([self.Nsim + 1, self.Nu]) self.u[0, :] = us self.control = control self.observation_space = np.zeros(nx) self.action_space = Box(low=np.array([-0, -0.01]), high=np.array([0, 0.01])) self.cstr_sim = mpc.DiscreteSimulator(self.ode, self.Delta, [self.Nx, self.Nu], ["x", "u"]) self.cstr_ode = mpc.getCasadiFunc(self.ode, [self.Nx, self.Nu], ["x", "u"], funcname="odef")
def _setup_moving_horizon_estimator(self): """ Construct a MHE solver.""" N = dict(x=self.Nx, u=self.Nu, y=self.Ny, t=self.N) funcargs = dict(f=["x", "u"], h=["x"], l=["w", "v"], lx=["x", "x0bar"]) l = mpc.getCasadiFunc(self._stage_cost, [N["x"], N["y"]], funcargs["l"]) lx = mpc.getCasadiFunc(self._prior_cost, [N["x"], N["x"]], funcargs["lx"]) self.moving_horizon_estimator = mpc.nmhe(f=self.fxuw, h=self.hx, wAdditive=True, N=N, l=l, lx=lx, u=self.u, y=self.y, funcargs=funcargs, x0bar=self.xhat[0], verbosity=0) self.moving_horizon_estimator.solve() self.xhat.append(self.moving_horizon_estimator.var["x"][-1])
def setup_target_selector(fxud, hxd, sample_time, num_rk4_discretization_steps, Nx, Nu, Nd, Ny, ys, us, ds, Rs, Qs, ulb, uub): """ Setup the target selector for the MPC controller.""" fxud = mpc.getCasadiFunc(fxud, [Nx, Nu, Nd], ["x", "u", "ds"], rk4=True, Delta=sample_time, M=num_rk4_discretization_steps) hxd = mpc.getCasadiFunc(hxd, [Nx, Nd], ["x", "ds"]) return NonlinearTargetSelector(fxud=fxud, hxd=hxd, Nx=Nx, Nu=Nu, Nd=Nd, Ny=Ny, ys=ys, us=us, ds=ds, Rs=Rs, Qs=Qs, ulb=ulb, uub=uub)
def setup_regulator(fxud, hxd, sample_time, num_rk4_discretization_steps, Nmpc, Nx, Nu, Nd, xs, us, ds, Q, R, P, ulb, uub): """ Augment the system for rate of change penalty and build the regulator.""" fxud = mpc.getCasadiFunc(fxud, [Nx, Nu, Nd], ["x", "u", "ds"], rk4=True, Delta=sample_time, M=num_rk4_discretization_steps) hxd = mpc.getCasadiFunc(hxd, [Nx, Nd], ["x", "ds"]) return NonlinearMPCRegulator(fxud=fxud, hxd=hxd, xs=xs, us=us, ds=ds, Nx=Nx, Nu=Nu, Nd=Nd, N=Nmpc, Q=Q, R=R, P=P, ulb=ulb, uub=uub)
def __init__(self, *, fxup, hx, Rv, Nx, Nu, Np, Ny, sample_time, x0): # Set attributes. self.fxup = mpc.DiscreteSimulator(fxup, sample_time, [Nx, Nu, Np], ["x", "u", "p"]) self.hx = mpc.getCasadiFunc(hx, [Nx], ["x"], funcname="hx") (self.Nx, self.Nu, self.Ny, self.Np) = (Nx, Nu, Ny, Np) self.measurement_noise_std = np.sqrt(np.diag(Rv)[:, np.newaxis]) self.sample_time = sample_time # Create lists to save data. self.x = [x0] self.u = [] self.p = [] self.y = [ np.asarray(self.hx(x0)) + self.measurement_noise_std * np.random.randn(self.Ny, 1) ] self.t = [0.]
def _get_linearized_model(*, parameters): """ Return a linear model for use in MPC for the cstrs plant.""" cstrs_ode = lambda x, u, p: _cstrs_ode(x, u, p, parameters) cstrs_ode_casadi = mpc.getCasadiFunc( cstrs_ode, [parameters['Nx'], parameters['Nu'], parameters['Np']], ["x", "u", "p"], funcname="cstrs") linear_model = mpc.util.getLinearizedModel(cstrs_ode_casadi, [ np.zeros((parameters['Nx'], 1)), np.zeros((parameters['Nu'], 1)), np.zeros((parameters['Np'], 1)) ], ["A", "B", "Bp"], Delta=parameters['sample_time']) # Do some scaling. yscale = parameters['yscale'] C = np.diag(1 / yscale.squeeze()) @ parameters['C'] # Return the matrices. return (linear_model['A'], linear_model['B'], C, linear_model['Bp'])
def __init__(self, nsim, delta=1, nx=3, nu=2, nt=10, f0=0.1, t0=350, c0=1, r=0.219, k0=7.2e10, er_ratio=8750, u=54.94, rho=1000, cp=0.239, dh=-5e4, xs=np.array([0.878, 324.5, 0.659]), us=np.array([300, 0.1]), x0=np.array([1, 310, 0.659]), control=False, q_cost=0.1, r_cost=0.1): self.Delta = delta self.Nsim = nsim self.Nx = nx self.Nu = nu self.Nt = nt self.F0 = f0 self.T0 = t0 self.c0 = c0 self.r = r self.k0 = k0 self.er_ratio = er_ratio self.U = u self.rho = rho self.Cp = cp self.dH = dh self.xs = xs self.us = us self.xsp = np.zeros([self.Nsim + 1, self.Nx]) self.xsp[0, :] = xs self.x0 = x0 self.x = np.zeros([self.Nsim + 1, self.Nx]) self.x[0, :] = self.x0 self.u = np.zeros([self.Nsim + 1, self.Nu]) self.u[0, :] = [300, 0.1] self.u[:, 1] = 0.1 self.control = control self.observation_space = np.zeros(nx) self.action_space = Box(low=-2.5, high=2.5, shape=(1, )) self.cstr_sim = mpc.DiscreteSimulator(self.ode, self.Delta, [self.Nx, self.Nu], ["x", "u"]) self.cstr_ode = mpc.getCasadiFunc(self.ode, [self.Nx, self.Nu], ["x", "u"], funcname="odef") # Cost function Q and R tuning parameters self.q_cost = q_cost self.r_cost = r_cost
def _setup_target_selector(self): """ Construct the target selector object for this class.""" N = dict(x=self.Nx, u=self.Nu, d=self.Nd, y=self.Ny) funcargs = dict(f=["x", "u", "ds"], h=["x", "ds"], phi=["y", "u", "ysp", "usp"]) extrapar = dict(ysp=self.ysp[-1], usp=self.usp[-1], ds=self.ds[-1]) phi = mpc.getCasadiFunc(self._stage_cost, [N["y"], N["u"], N["y"], N["u"]], funcargs["phi"]) lb = dict(u=self.ulb.T) ub = dict(u=self.uub.T) self.target_selector = mpc.sstarg(f=self.fxud, h=self.hxd, phi=phi, N=N, lb=lb, ub=ub, extrapar=extrapar, funcargs=funcargs, verbosity=0) self.target_selector.solve() self.xs.append(self.target_selector.var["x"][0]) self.us.append(self.target_selector.var["u"][0])
e = [] ef = [] l = [] Pf = [] for i in range(Nrobots): other_robots_list = range(Nrobots) other_robots_list.remove(i) Am_cont = generate_matrix_A(Acont, Nrobots) Bm_cont = generate_matrix_B([i], Bcont, Nx, Nu + Nslack, Nrobots) Cm_cont = generate_matrix_C(other_robots_list, Bcont, Nx, Np, Nrobots) # Discretize. (A, B, C, _) = mpc.util.c2d(Am_cont, Bm_cont, Delta, Bp=Cm_cont) f_casadi.append( mpc.getCasadiFunc( lambda x, u, p: mpc.mtimes(A, x) + mpc.mtimes(B, u) + mpc.mtimes( C, p), [Nx * Nrobots, Nu + Nslack, Np], ["x", "u", "p"], "f")) Al.append(A) Bl.append(B) Cl.append(C) def nlcon(x, u): x_ = x[i * Nx + 0] y_ = x[i * Nx + 1] dist_con = [] for j in Rlist: if j != i: dist_con.append(r_dist**2 - (x_ - x[j * Nx + 0])**2 - (y_ - x[j * Nx + 1])**2 + u[(Nu) + len(dist_con)])
def __init__(self, nsim, model_type='SISO', x0=np.array([0.5]), u0=np.array([1]), xs=np.array([5]), us=np.array([10]), step_size=0.1, control=False, q_cost=1, r_cost=0.5, random_seed=1): self.Nsim = nsim self.x0 = x0 self.u0 = u0 self.xs = xs self.us = us self.model_type = model_type self.step_size = step_size self.t = np.linspace(0, nsim * self.step_size, nsim + 1) self.control = control # Initialization Characteristics if model_type == "SISO": if self.control is True: self.Nx = 2 else: self.Nx = 1 self.Nu = 1 self.action_space = Box(low=-5, high=5, shape=(1, )) self.Q = q_cost self.R = r_cost # Model parameters self.A = np.array([-4]) self.B = np.array([2]) self.C = np.array([1]) self.D = 0 elif model_type == "MIMO": if self.control is True: self.Nx = 4 else: self.Nx = 2 self.Nu = 2 self.action_space = Box(low=np.array([-5, -5]), high=np.array([5, 5])) self.Q = q_cost * np.eye(self.Nx) self.R = r_cost * np.eye(self.Nu) # Model parameters self.A = np.array([[-3, -2], [0, -3]]) self.B = np.array([[4, 0], [0, 2]]) self.C = np.array([1, 1]) self.D = 0 else: raise ValueError("Model type not specified properly") self.observation_space = np.zeros(self.Nx) # State and input trajectories self.x = np.zeros([nsim + 1, self.Nx]) self.u = np.zeros([nsim + 1, self.Nu]) self.x[0, :] = x0 self.u[0, :] = u0 self.xsp = np.zeros([self.Nsim + 1, self.Nx]) self.xsp[0, :] = self.xs self.usp = np.zeros([self.Nsim + 1, self.Nu]) self.usp[0, :] = self.us # Build the CasaDI functions self.system_sim = mpc.DiscreteSimulator(self.ode, self.step_size, [self.Nx, self.Nu], ["x", "u"]) self.system_ode = mpc.getCasadiFunc(self.ode, [self.Nx, self.Nu], ["x", "u"], funcname="odef") # Seed the system for reproducability random.seed(random_seed) np.random.seed(random_seed)
def mpc_control(self, waypoint, target_speed=None, solve_nmpc=True, manual=False, last_u=None, log=False, debug=True): """ Execute one step of control to reach given waypoint as closely as possible with a given target speed. :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: distance (in meters) to the waypoint """ start_time = time.time() # Init controller if necessary if not self._controller: self._init_controller() # Getting initial state x0 = self.state # Updating the last applied control if last_u: self._last_control = last_u # Updating the target speed if target_speed: self._target_speed = target_speed # Updating target location if isinstance(waypoint, carla.Waypoint): self._target = waypoint # Load Model f = mpc.getCasadiFunc(self._sys, [self._Nx, self._Nu], ["x", "u"], "f") # Bounds on u. lb = {"u": np.array([-5, -0.5 - 0.1])} ub = {"u": np.array([5, 0.5 + 0.1])} upperbx = 10000 * np.ones((self._Nt + 1, self._Nx)) # define stage cost l = mpc.getCasadiFunc(self._lfunc, [self._Nx, self._Nu], ["x", "u"], "l") # Make optimizers funcargs = {"f": ["x", "u"], "l": ["x", "u"]} # Define pointer # Setting verbosity level of casADI solver output verbs = 0 if debug else 0 # Build controller and adjust some ipopt options. N = {"x": self._Nx, "u": self._Nu, "t": self._Nt} solver = mpc.nmpc(f, l, N, x0, lb, ub, uprev=self._last_control, funcargs=funcargs, Pf=None, verbosity=verbs) # Fix initial state. solver.fixvar("x", 0, x0) # Solve nlp. solver.solve() x = np.squeeze(solver.var["x", 1]) u = np.squeeze(solver.var["u", 0, :]) if debug: print("---- print us -------") print(np.squeeze(solver.var["u", 0, :])) print(np.squeeze(solver.var["u", 1, :])) print(np.squeeze(solver.var["u", 2, :])) print(np.squeeze(solver.var["u", 3, :])) print(np.squeeze(solver.var["u", 4, :])) print("---- print prediction states -------") print(np.squeeze(solver.var["x", 0])) print(np.squeeze(solver.var["x", 1])) print(np.squeeze(solver.var["x", 2])) print(np.squeeze(solver.var["x", 3])) print(np.squeeze(solver.var["x", 4])) print("-----General state information ------") print("Current state: \n", self.state) print("Target: \n", self.target) print("Delta state: \n", self.x_diff(self.state)) # Loads predicted input and states into variables which can be given out with print u_data1 = np.squeeze(solver.var["u", :, 0]) u_data2 = np.squeeze(solver.var["u", :, 1]) u_log = np.array([u_data1, u_data2]) t_log = 0 x_log = np.zeros((self._Nt, self._Nx)) for t_log in range(self._Nt): x_log[t_log, :] = np.squeeze(solver.var["x", t_log]) # reset last control self._last_control = u if debug and abs(self._last_control[1]) > 0.2: loc = carla.Location(x_log[self._Nt - 1, 0], x_log[self._Nt - 1 - 1, 1]) wp.draw_vehicle_bounding_box(self._world, loc, x_log[-1, 2]) if log: return converting_mpc_u_to_control( u, debug ), self.state, self._last_control, u_log, x_log, time.time( ) - start_time else: return converting_mpc_u_to_control(u, debug)
F0 = d[0] # Now create the ODE. rate = k0*c*np.exp(-E/T) dxdt = np.array([ F0*(c0 - c)/(np.pi*r**2*h) - rate, F0*(T0 - T)/(np.pi*r**2*h) - dH/(rho*Cp)*rate + 2*U/(r*rho*Cp)*(Tc - T), (F0 - F)/(np.pi*r**2) ]) return dxdt # Turn into casadi function and simulator. ode_casadi = mpc.getCasadiFunc(ode,[Nx,Nu,Nd],["x","u","d"],funcname="ode") ode_rk4_casadi = mpc.getCasadiFunc(ode,[Nx,Nu,Nd],["x","u","d"], funcname="ode_rk4",rk4=False,Delta=Delta) cstr = mpc.DiscreteSimulator(ode, Delta, [Nx,Nu,Nd], ["x","u","d"]) # Steady-state values. cs = .878 Ts = 324.5 hs = .659 Fs = .1 Tcs = 300 F0s = .1 # Update the steady-state values a few times to make sure they don't move. for i in range(10): [cs,Ts,hs] = cstr.sim([cs,Ts,hs],[Tcs,Fs],[F0s]).tolist()
#=================================================================# # test if the model works well #xs = np.array([0.878,324.5,0.659]) #us = np.array([300,0.1]) #derivative = cstr_xy_ode(xs,us) #measure = measurement_xy_model(xs) # Make a simulator. model_cstr_casadi = mpc.DiscreteSimulator(cstr_xy_ode_scale, Delta, [Nx, Nu, Nw], ["x", "u", "w"]) # Convert continuous-time f to explicit discrete-time F with RK4. F = mpc.getCasadiFunc(cstr_xy_ode_scale, [Nx, Nu, Nw], ["x", "u", "w"], "F", rk4=True, Delta=Delta, M=1) H = mpc.getCasadiFunc(measurement_xy_model, [Nx], ["x"], "H") # Define stage costs. def lfunc(w, v): return mpc.mtimes(w.T, linalg.inv(Q), w) + mpc.mtimes( v.T, linalg.inv(R), v) l = mpc.getCasadiFunc(lfunc, [Nw, Nv], ["w", "v"], "l") def lxfunc(x):
def __init__(self, nsim, x0=np.array([65.13, 42.55, 0.0, 0.0]), u0=np.array([3.9, 3.9, 0.0, 0.0]), xs=np.array([261.78, 171.18, 111.43, 76.62]), us=np.array([15.7, 15.7, 5.337, 5.337]), step_size=1, control=False, q_cost=1, r_cost=0.5, random_seed=1): # Initial conditions and other required parameters self.Nsim = nsim self.x0 = x0 self.u0 = u0 self.xs = xs self.us = us self.step_size = step_size self.t = np.linspace(0, nsim * self.step_size, nsim + 1) self.control = control # Model Parameters if self.control: self.Nx = 8 else: self.Nx = 4 # Double Nu to account for time delay self.Nu = int(self.u0.shape[0]) self.action_space = Box(low=np.array([-5]), high=np.array([5])) self.observation_space = np.zeros(self.Nx) self.Q = q_cost * np.eye(self.Nx) self.R = r_cost * np.eye(self.Nu) # State space model self.A = np.array([[-0.0629, 0, 0, 0], [0, -0.0963, 0, 0], [0, 0, -0.05, 0], [0, 0, 0, -0.0729]]) self.B = np.array([[1, 0], [1, 0], [0, 1], [0, 1]]) self.C = np.array([[0.7665, 0, -0.9, 0], [0, 0.6055, 0, -1.3472]]) self.D = 0 # State and input trajectories self.x = np.zeros([nsim + 1, self.Nx]) self.u = np.zeros([nsim + 1, int(self.Nu)]) self.x[0:self.Nx, :] = x0 self.u[0:self.Nx, :] = u0 # Set initial conditions for the first # Output trajectory self.y = np.zeros([nsim + 1, 2]) # Build the CasaDI functions self.system_sim = mpc.DiscreteSimulator(self.ode, self.step_size, [self.Nx, self.Nu], ["x", "u"]) self.system_ode = mpc.getCasadiFunc(self.ode, [self.Nx, self.Nu], ["x", "u"], funcname="odef") # Set-point trajectories self.xsp = np.zeros([self.Nsim + 1, self.Nx]) self.xsp[0, :] = self.xs self.usp = np.zeros([self.Nsim + 1, int(self.Nu)]) self.usp[0, :] = self.us # Seed the system for reproducability random.seed(random_seed) np.random.seed(random_seed)
t2 = -1.0 if t2 < -1.0 else t2 pitch = -math.asin(t2) roll = -math.atan2(t3, t4) yaw = -math.atan2(t1, t0) return (roll, pitch, yaw) # Define model. def ode(x, u): # return np.array([-x[0] - (1+u[0])*x[1], (1+u[0])*x[0] + x[1] + u[1]]) return np.array([u[0] * np.cos(x[2]), u[0] * np.sin(x[2]), u[1]]) # Then get nonlinear casadi functions and the linearization. ode_casadi = mpc.getCasadiFunc(ode, [Nx, Nu], ["x", "u"], funcname="f") Q = np.array([[15.0, 0.0, 0.0], [0.0, 15.0, 0.0], [0.0, 0.0, 0.01]]) Qn = np.array([[150.0, 0.0, 0.0], [0.0, 150.0, 0.0], [0.0, 0.0, 0.01]]) R = np.array([[25.0, 0.0], [0.0, 25.0]]) if len(sys.argv) == 3: x_ref = np.array([float(sys.argv[1]), float(sys.argv[2]), 0]) else: x_ref = np.array([10, 10, 0]) # Define stage cost and terminal weight. def lfunc(x, u): return mpc.mtimes((x - x_ref).T, Q, (x - x_ref)) + mpc.mtimes(u.T, R, u)
]) Bcont = np.array([ [0, 0, 0], [0, 0, 0], [1, 0, 0], [0, 1, 0], ]) # Build the matrix for the centralized MPC Am_cont = generate_matrix_A(Acont, Nrobots) Bm_cont = generate_matrix_A( Bcont, Nrobots) #C(range[Nrobots], Bcont, Nx, Nu, Nrobots) # Discretize. (A, B) = mpc.util.c2d(Am_cont, Bm_cont, Delta) f_casadi = mpc.getCasadiFunc(lambda x, u: mpc.mtimes(A, x) + mpc.mtimes(B, u), [Nx * Nrobots, Nu * Nrobots], ["x", "u"], "f") Al = A Bl = B r = .25 # m = 3 # centers = np.linspace(0,xmax,m+1) # centers = list(.5*(centers[1:] + centers[:-1])) centers = [1] holes = [(p, r) for p in itertools.product(centers, centers)] Ne = 3 def nlcon(x, u):
def get_mpc_controller(self, ode_casadi, delta, x0, random_guess=False, c=3, verbosity=0, upper_bound=1.8, lower_bound=0.2): # A very sketchy way to find the amount of time varying parameters if self.parameter.shape == (self.parameter.shape[0], ): shape = 1 else: shape = self.parameter.shape[1] loss = mpc.getCasadiFunc(self.stage_loss, [self.Nx, self.Nu, shape], ["x", "u", "p"], funcname="loss") # pf = mpc.getCasadiFunc(self.terminal_loss, [self.Nx], ["x"], funcname="pf") # Define where the function args should be. More precisely, we must put p in the loss function. funcargs = {"f": ["x", "u"], "l": ["x", "u", "p"]} if random_guess is True: guess = { "x": np.random.uniform(5, 10, self.Nx), "u": np.random.uniform(5, 10, self.Nu) } else: guess = {"x": self.ss_states, "u": self.ss_inputs} # Set "c" above 1 to use collocation contargs = dict( N={ "t": self.Nt, "x": self.Nx, "u": self.Nu, "c": c, "p": 1 }, verbosity=verbosity, l=loss, x0=x0, p=self.parameter, # Upper and Lower State Constraints ub={ "u": self.upp_u_const, "x": self.upp_x_const }, lb={ "u": self.low_u_const, "x": self.low_x_const }, guess=guess, funcargs=funcargs) # Create MPC controller mpc_controller = mpc.nmpc(f=ode_casadi, Delta=delta, **contargs) return mpc_controller
]) Bcont = np.array([ [0,0,0], [0,0,0], [1,0,0], [0,1,0], ]) # Build the matrix for the centralized MPC Am_cont = generate_matrix_A(Acont, Nrobots) Bm_cont = generate_matrix_A(Bcont, Nrobots) #C(range[Nrobots], Bcont, Nx, Nu, Nrobots) # Discretize. (A,B) = mpc.util.c2d(Am_cont, Bm_cont, Delta) f_casadi = mpc.getCasadiFunc(lambda x,u: mpc.mtimes(A,x) + mpc.mtimes(B,u), [Nx*Nrobots,Nu*Nrobots],["x","u"], "f") Al = A Bl = B r = .25 # m = 3 # centers = np.linspace(0,xmax,m+1) # centers = list(.5*(centers[1:] + centers[:-1])) centers = [1] holes = [(p,r) for p in itertools.product(centers,centers)] Ne=3 def nlcon(x,u): # [x1, x2] = x[0:2] # Doesn't work in Casadi 3.0 return np.array([r_dist**2 -(x[0]-x[4])**2 - (x[1]-x[5])**2 + u[2],