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)