def stage_loss(self, states, inputs):
dx = states[:int(self.Nx * 0.5)] + states[
int(self.Nx * 0.5):self.Nx] - self.ss_states[:int(self.Nx * 0.5)]
du = inputs - self.ss_inputs
return mpc.mtimes(dx.T, self.Q, dx) + mpc.mtimes(du.T, self.R, du)
def stage_loss(self, states, inputs, parameter):
dx = states[:int(self.Nx * 0.5)] + states[
int(self.Nx * 0.5):self.Nx] - self.ss_states[:int(self.Nx * 0.5)]
du = inputs - self.ss_inputs
x_cost = mpc.mtimes(dx.T, self.Q, dx)
u_cost = mpc.mtimes(du.T, self.R, du)
return (x_cost + u_cost) * self.gamma**parameter
def stage_loss(self, states, inputs, parameter):
# First part is actual state, 2nd part is offset free control
dx = states[:int(self.Nx * 0.5)] + states[
int(self.Nx * 0.5):self.Nx] - self.ss_states[:int(self.Nx * 0.5)]
du = inputs - self.ss_inputs
x_cost = mpc.mtimes(dx.T, self.Q, dx)
u_cost = mpc.mtimes(du.T, self.R, du)
return (x_cost + u_cost) * self.gamma**parameter
def output_loss(self, states, inputs, parameter):
x = states[:int(self.Nx * 0.5)] + states[int(self.Nx * 0.5):self.Nx]
# y = np.array([(0.776 * x[0] - 0.9 * x[2]), (0.6055 * x[1] - 1.3472 * x[3])]) - np.array([100, 0])
y = np.array([
(0.776 * x[0] - 0.9 * x[2]),
]) - np.array([100])
du = inputs - self.ss_inputs
y_cost = mpc.mtimes(y.T, self.Q, y)
u_cost = mpc.mtimes(du.T, self.R, du)
return (y_cost + u_cost) * self.gamma**parameter
def terminal_loss(self, states):
# First term is real states, 2nd term is the output disturbance correction.
dx = states[:int(self.Nx * 0.5)] + states[
int(self.Nx * 0.5):self.Nx] - self.ss_states[:int(self.Nx * 0.5)]
return mpc.mtimes(dx.T, self.P, dx)
def _init_controller(self, args_mpc_functions=None):
"""
Controller initialization.
:return:
"""
# Init weights for cost function
self._init_weights()
# Init lambda functions to get delta_x & delta_u
self.x_diff = lambda x: x[:4] - self.target[:4]
self.x_diff = lambda x: np.array([
x[0] - self.target[0], x[1] - self.target[1],
np.sqrt(x[2]**2) - np.sqrt(self.target[2]**2), x[3] - self.target[3
]
])
self.u_diff = lambda x, u: u - x[-2:]
# Defining lambda functions for alpha & dxi for system equation
self.alpha = lambda u: np.arctan(
(self._lr / (self._lf + self._lr)) * np.tan(u[1]))
if args_mpc_functions is None:
args_mpc_functions = {
'sys':
lambda x, u: np.array([
self._dt *
(x[3] * np.cos(x[2] + self.alpha(u))) + x[0], # X
self._dt *
(x[3] * np.sin(x[2] + self.alpha(u))) + x[1], # Y
self._dt *
((x[3] / self._lr) * np.sin(self.alpha(u))) + x[2], # psi
self._dt * (u[0]) + x[3], # velocity
u[0], # u0
u[1] # u1
]),
'lfunc':
lambda x, u: mpc.mtimes(
self.x_diff(x).T, self._Qn, self._Q, self.x_diff(x)) + mpc.
mtimes(u.T, self._Rn, self._R, u) + mpc.mtimes(
self.u_diff(x, u).T, self._Sn, self._S, self.u_diff(x, u))
}
self._sys = args_mpc_functions.get('sys')
self._lfunc = args_mpc_functions.get('lfunc')
self._controller = True
def _init_controller(self, args_mpc_functions=None):
super(CurvMPCController, self)._init_controller()
# Defining lambda function for delta_x
self.x_diff = lambda x: np.array([x[3], x[5], x[6]]) - self.target
# Defining lambda function for delta_xi
self.dxi = lambda x, u: (1 / (1 - self._kr * x[5])) * x[3] * np.cos(x[
6] + self.alpha(u))
if args_mpc_functions is None:
args_mpc_functions = {
'sys':
lambda x, u: np.array([
self._dt *
(x[3] * np.cos(x[2] + self.alpha(u))) + x[0], # X
self._dt *
(x[3] * np.sin(x[2] + self.alpha(u))) + x[1], # Y
self._dt *
((x[3] / self._lr) * np.sin(self.alpha(u))) + x[2], # psi
self._dt * (u[0]) + x[3], # velocity
self._dt * self.dxi(x, u) + x[4], # xi
self._dt *
(x[3] * np.sin(x[6] + self.alpha(u))) + x[5], # eta
self._dt * ((x[3] / self._lr) * np.sin(self.alpha(u)) -
self._kr * self.dxi(x, u)) + x[6], # theta
u[0], # u0
u[1], # u1
# self._kr, # kappa
]),
'lfunc':
lambda x, u: mpc.mtimes(
self.x_diff(x).T, self._Qn, self._Q, self.x_diff(x)) + mpc.
mtimes(u.T, self._Rn, self._R, u) + mpc.mtimes(
self.u_diff(x, u).T, self._Sn, self._S, self.u_diff(x, u))
}
self._sys = args_mpc_functions.get('sys')
self._lfunc = args_mpc_functions.get('lfunc')
def calculate_curvature_func_args(self, log=False):
"""
Function to calculate the curvature function arguments.
This is done by fitting all reference waypoint from the waypoint buffer to 3th-polynomial function.
:return: [p0, p1, p2, p3] of the 3th polynomial.
"""
kappa_log = dict()
# Current waypoint as reference point to center all waypoints
ref_point = np.array(
[self.wp1.transform.location.x, self.wp1.transform.location.y])
# Rotation matrix to align current waypoint to x-axis
angle_wp = get_wp_angle(self.wp1, self.wp2)
R = rotmat(angle_wp)
# Getting reduced waypoint matrix
wp_mat = np.zeros([self._buffer_size, 2])
wp_mat_0 = np.zeros([self._buffer_size,
2]) # saving original locations of wps for debug
counter_mat = 0
for i in range(self._buffer_size):
wp = self._waypoint_buffer[i][0]
loc = get_localization_from_waypoint(wp)
point = np.array([loc.x, loc.y]) # init point
point = point.T - ref_point.T # center traj to origin
point = mpc.mtimes(
R, point) # rotation of point ot allign with x-axis
wp_mat_0[counter_mat] = np.array([loc.x, loc.y])
wp_mat[counter_mat] = point
counter_mat += 1
# Getting optimal parameters for 3th polynomial by fitting the a curve
# Doc: https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.curve_fit.html
p_opt, _ = curve_fit(polynomial3, wp_mat[:, 0], wp_mat[:, 1])
# Logging all necessary information
if log:
kappa_log = {
'wp_mat': [wp_mat],
'wp_mat_0': [wp_mat_0],
'refernce_point': [ref_point],
'rotations_mat': [R],
'angle_wp': [angle_wp],
'p_opt': [p_opt]
}
print(kappa_log)
return p_opt, kappa_log
def terminal_loss(self, states):
dx = states[:self.Nx] + states[self.Nx:self.Nx*2] - self.ss_states
return mpc.mtimes(dx.T, self.P, dx)
def _stage_cost(self, w, v):
"""Stage cost in moving horizon estimation."""
return mpc.mtimes(w.T, self.Qwinv, w) + mpc.mtimes(v.T, self.Rvinv, v)
def stage_loss(self, states, inputs):
dx = states[:self.Nx] + states[self.Nx:self.Nx * 2] - self.ss_states
du = inputs - self.ss_inputs
return mpc.mtimes(dx.T, self.Q, dx) + mpc.mtimes(du.T, self.R, du)
def stage_loss(self, states, inputs):
dx = states[:self.Nx] - self.ss_states[:self.Nx]
du = inputs[:self.Nu] - self.ss_inputs[:self.Nu]
return mpc.mtimes(dx.T, self.Q, dx) + mpc.mtimes(du.T, self.R, du)
def _prior_cost(self, x, xprior):
"""Prior cost in moving horizon estimation."""
dx = x - xprior
return mpc.mtimes(dx.T, self.P0inv, dx)
def lfunc(x,u,x_sp):
return mpc.mtimes((x-x_sp).T,Q,(x-x_sp)) + mpc.mtimes(u.T,R,u)
def Pffunc(x,x_sp):
return mpc.mtimes((x-x_sp).T,Qn,(x-x_sp))
def lxfunc(x):
return mpc.mtimes(x.T, linalg.inv(P), x)
def costtogo(x,xsp):
# Deviation variables.
dx = x - xsp
# Calculate cost to go.
return mpc.mtimes(dx.T,10*Q,dx)
def _stage_cost(self, x, u, xs, us):
"""Stage cost in moving horizon estimation."""
dx = x - xs
du = u - us
return mpc.mtimes(dx.T, self.Q, dx) + mpc.mtimes(du.T, self.R, du)
def _stage_cost(self, ys, us, ysp, usp):
""" The stage cost for the target selector."""
dy = ys - ysp
du = us - usp
return mpc.mtimes(dy.T, self.Qs, dy) + mpc.mtimes(du.T, self.Rs, du)
def lfunc(x, u):
return mpc.mtimes((x - x_ref).T, Q, (x - x_ref)) + mpc.mtimes(u.T, R, u)
def ekf_continuous_discrete(f,
h,
x_hat,
u,
w,
y,
P,
Q,
R,
Nw,
Delta,
f_jacx=None,
f_jacw=None,
h_jacx=None):
"""
This ekf is discrete-time to continuous-time EKF. That is, it can be used to
ODE models.
f and h should be casadi functions. f must be discrete-time. P, Q, and R
are the prior, state disturbance, and measurement noise covariances. Note
that f must be f(x,u,w) and h must be h(x).
The value of x that should be fed is xhat(k | k-1), and the value of P
should be P(k | k-1). xhat will be updated to xhat(k | k) and then advanced
to xhat(k+1 | k), while P will be updated to P(k | k) and then advanced to
P(k+1 | k). The return values are a list as follows
[P(k+1 | k), xhat(k+1 | k), P(k | k), xhat(k | k)]
Depending on your specific application, you will only be interested in
some of these values.
"""
# Check jacobians.
if f_jacx is None:
f_jacx = f.jacobian(0)
if f_jacw is None:
f_jacw = f.jacobian(2)
if h_jacx is None:
h_jacx = h.jacobian(0)
P_k_kminus = P # Pmp1_cov is the covariance matrix P(k|k-1)
xekf_k_kminus = x_hat # This is the estimate xhat(k | k-1)
C = np.array(h_jacx(xekf_k_kminus)
[0]) # Linearize the output equations to obtain C matrix
# update step
Lk = mpc.mtimes(
P_k_kminus, np.transpose(C),
np.linalg.inv(np.dot(np.dot(C, P_k_kminus), np.transpose(C)) +
R)) # L = Pc(:,:,i)*C'/(C*Pc(:,:,i)*C'+Rc)
xekf = xekf_k_kminus + np.dot(
Lk,
(y - np.dot(C, xekf_k_kminus))) ## update current estimate xhat(k|k)
P_cov = P_k_kminus - mpc.mtimes(
Lk, C,
P_k_kminus) ## update current estimate of P(k|k) based on P(k|k-1)
# predict step
A = np.array(f_jacx(xekf, u, np.zeros((Nw, 1)))[0])
B = np.array(f_jacw(xekf, u, np.zeros((Nw, 1)))[0])
[Ad, Bd] = c2d(A, B, Delta, Bp=None, f=None, asdict=False)
P_kplus_k = mpc.mtimes(Ad, P_cov,
np.transpose(Ad)) + Q # update matrix P (k+1|k)
return [P_kplus_k, P, xekf]
def lfunc(x, u):
return mpc.mtimes(x.T, Q, x) + mpc.mtimes(u.T, R, u)
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 _terminal_cost(self, x, xs):
"""Prior cost in moving horizon estimation."""
dx = x - xs
return mpc.mtimes(dx.T, self.P, dx)
def Pffunc(x):
return mpc.mtimes((x - x_ref).T, Qn, (x - x_ref))
])
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 terminal_loss(self, states):
dx = states[:int(self.Nx * 0.5)] + states[
int(self.Nx * 0.5):self.Nx] - self.ss_states[:int(self.Nx * 0.5)]
return mpc.mtimes(dx.T, self.P, dx)
def lfunc(x, u):
return mpc.mtimes((x - M_XREF).T, Q, (x - M_XREF)) + mpc.mtimes(
u.T, R, u) # - casadi.log( - 0.2**2 +(x[0]-x[4])**2 + (x[1]-x[5])**2)
def stagecost(x,u,xsp,usp,Q,R):
# Return deviation variables.
dx = x - xsp
du = u - usp
# Calculate stage cost.
return mpc.mtimes(dx.T,Q,dx) + mpc.mtimes(du.T,R,du)
def lfunc(w, v):
return mpc.mtimes(w.T, linalg.inv(Q), w) + mpc.mtimes(
v.T, linalg.inv(R), v)
