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)