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],
