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 _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_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 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
u_ub = np.array([4., 1.0]) u_lb = np.array([-4., -1.0]) 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))}, ) Nlin = {"t": Nt, "x": Nx, "u": Nu} Nnonlin = Nlin.copy() Nnonlin["c"] = 2 # Use collocation to discretize. solver = mpc.nmpc(f=ode_casadi, N=Nnonlin, Delta=Delta, **commonargs) # This function will be called every time a new scan message is # published. def odom_callback(odom_msg): # Save a global reference to the most recent sensor state so that # it can be accessed in the main control loop. # (The global keyword prevents the creation of a local variable here.) global ODOM ODOM = odom_msg # This is the 'main' def start():
"Delta": Delta, "x0" : x0, "lb" : lb, "ub" : ub, "p" : p, "verbosity" : 0, "Pf" : Pf, "l" : l, "sp" : sp, "uprev" : us, "funcargs" : {"l" : largs}, "extrapar" : {"Q" : Q, "R" : R}, # In case we want to tune online. } solvers = {} # solvers["lmpc"] = mpc.nmpc(f=Flinear,guess=guesslin,**nmpc_commonargs) solvers["nmpc"] = mpc.nmpc(f=Fnonlinear,guess=guessnonlin,**nmpc_commonargs) # Also build steady-state target finders. contVars = [0,2] #sstarg_commonargs = { # "N" : N, # "lb" : {"u" : np.tile(us - umax, (1,1))}, # "ub" : {"u" : np.tile(us + umax, (1,1))}, # "verbosity" : 0, ## "h" : h, # "p" : np.array([ds]), #} #sstargs = {} # sstargs["lmpc"] = mpc.sstarg(f=Flinear,**sstarg_commonargs) # sstargs["nmpc"] = mpc.sstarg(f=Fnonlinear,**sstarg_commonargs)
ef = mpc.getCasadiFunc(terminalconstraint, [Nx * Nrobots], ["x"], "ef") Nef = 3 funcargs = {"f": ["x", "u"], "e": ["x", "u"], "l": ["x", "u"], "ef": ["x"]} # Build controller and adjust some ipopt options. N = {"x": Nx * Nrobots, "u": Nu * Nrobots, "e": Ne, "t": Nt, "ef": Nef} controller = mpc.nmpc(f_casadi, l, N, x0, e=e, ef=ef, lb=lb, ub=ub, funcargs=funcargs, Pf=Pf, verbosity=0, casaditype="SX") controller.initialize(solveroptions=dict(max_iter=5000)) huskies = [] for h_idx in range(Nrobots): r_idx = h_idx * Nx x0_husky = M_X0[h_idx] huskies.append(utils.create_robot_controller(h_idx, Delta, x0_husky)) # Now ready for simulation. X = np.zeros((Nsim + 1, Nx * Nrobots)) X[0, :] = x0
controller = [] N = {"x": Nx * Nrobots, "u": Nu + Nslack, "e": Ne, "ef": Nef, "t": Nt, "p": Np} Xk_DATA = np.zeros((Nx * Nrobots, )) U_DATA = np.zeros((Nt, Np)) for i in range(Nrobots): controller.append( mpc.nmpc(f_casadi[i], l[i], N, x0, e=e[i], ef=ef[i], lb=lb, ub=ub, funcargs=funcargs, Pf=Pf[i], verbosity=0, casaditype="SX", p=U_DATA)) controller[i].initialize(solveroptions=dict(max_iter=5000)) # Now ready for simulation. X = [] U = [] for i in range(Nrobots): X.append(np.zeros((Nsim + 1, Nx * Nrobots))) X[i][0, :] = x0
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)