pl.ones(3), \ pl.zeros(N), \ ydata_noise[0,:].T ) sol = nlpsolver(x0=V0) p_est_single_shooting = sol["x"][:3] tstart_Sigma_p = time() J_s = ca.jacobian(r, V) F_s = ca.mtimes(J_s.T, J_s) beta = (ca.mtimes(r.T, r) / (r.numel() - V.numel())) Sigma_p_s = beta * ca.solve(F_s, ca.MX.eye(F_s.shape[0]), "csparse") beta_fcn = ca.Function("beta_fcn", [V], [beta]) print beta_fcn.call([sol["x"]])[0] Sigma_p_s_fcn = ca.Function("Sigma_p_s_fcn", \ [V] , [Sigma_p_s]) Cov_p = Sigma_p_s_fcn.call([sol["x"]])[0][:3, :3] tend_Sigma_p = time() time_covariance_matrix_evlaution.append(tend_Sigma_p - tstart_Sigma_p)
def rocket_equations(jit=True): x = ca.SX.sym('x', 14) u = ca.SX.sym('u', 4) p = ca.SX.sym('p', 16) t = ca.SX.sym('t') dt = ca.SX.sym('dt') # State: x # body frame: Forward, Right, Down omega_b = x[0:3] # inertial angular velocity expressed in body frame r_nb = x[3:7] # modified rodrigues parameters v_b = x[7:10] # inertial velocity expressed in body components p_n = x[10:13] # positon in nav frame m_fuel = x[13] # mass # Input: u m_dot = ca.if_else(m_fuel > 0, u[0], 0) fin = u_to_fin(u) # Parameters: p g = p[0] # gravity Jx = p[1] # moment of inertia Jy = p[2] Jz = p[3] Jxz = p[4] ve = p[5] l_fin = p[6] w_fin = p[7] CL_alpha = p[8] CL0 = p[9] CD0 = p[10] K = p[11] s_fin = p[12] rho = p[13] m_empty = p[14] l_motor = p[15] # Calculations m = m_empty + m_fuel J_b = ca.SX.zeros(3, 3) J_b[0, 0] = Jx + m_fuel * l_motor**2 J_b[1, 1] = Jy + m_fuel * l_motor**2 J_b[2, 2] = Jz J_b[0, 2] = J_b[2, 0] = Jxz C_nb = so3.Dcm.from_mrp(r_nb) g_n = ca.vertcat(0, 0, g) v_n = ca.mtimes(C_nb, v_b) # aerodynamics VT = ca.norm_2(v_b) q = 0.5 * rho * ca.dot(v_b, v_b) fins = { 'top': { 'fwd': [1, 0, 0], 'up': [0, 1, 0], 'angle': fin[0] }, 'left': { 'fwd': [1, 0, 0], 'up': [0, 0, -1], 'angle': fin[1] }, 'down': { 'fwd': [1, 0, 0], 'up': [0, -1, 0], 'angle': fin[2] }, 'right': { 'fwd': [1, 0, 0], 'up': [0, 0, 1], 'angle': fin[3] }, } rel_wind_dir = v_b / VT # build fin lift/drag forces vel_tol = 1e-3 FA_b = ca.vertcat(0, 0, 0) MA_b = ca.vertcat(0, 0, 0) for key, data in fins.items(): fwd = data['fwd'] up = data['up'] angle = data['angle'] U = ca.dot(fwd, v_b) W = ca.dot(up, v_b) side = ca.cross(fwd, up) alpha = ca.if_else(ca.fabs(U) > vel_tol, -ca.atan(W / U), 0) perp_wind_dir = ca.cross(side, rel_wind_dir) norm_perp = ca.norm_2(perp_wind_dir) perp_wind_dir = ca.if_else( ca.fabs(norm_perp) > vel_tol, perp_wind_dir / norm_perp, up) CL = CL0 + CL_alpha * (alpha + angle) CD = CD0 + K * (CL - CL0)**2 # model stall as no lift if above 23 deg. L = ca.if_else(ca.fabs(alpha) < 0.4, CL * q * s_fin, 0) D = CD * q * s_fin FAi_b = L * perp_wind_dir - D * rel_wind_dir FA_b += FAi_b MA_b += ca.cross(-l_fin * fwd - w_fin * side, FAi_b) FA_b = ca.if_else(ca.fabs(VT) > vel_tol, FA_b, ca.SX.zeros(3)) MA_b = ca.if_else(ca.fabs(VT) > vel_tol, MA_b, ca.SX.zeros(3)) # propulsion FP_b = ca.vertcat(m_dot * ve, 0, 0) # force and momental total F_b = FA_b + FP_b + ca.mtimes(C_nb.T, m * g_n) M_b = MA_b force_moment = ca.Function('force_moment', [x, u, p], [F_b, M_b], ['x', 'u', 'p'], ['F_b', 'M_b']) # right hand side rhs = ca.Function('rhs', [x, u, p], [ ca.vertcat( ca.mtimes(ca.inv(J_b), M_b - ca.cross(omega_b, ca.mtimes(J_b, omega_b))), so3.Mrp.kinematics(r_nb, omega_b), F_b / m - ca.cross(omega_b, v_b), ca.mtimes(C_nb, v_b), -m_dot) ], ['x', 'u', 'p'], ['rhs'], {'jit': jit}) # prediction t0 = ca.SX.sym('t0') h = ca.SX.sym('h') x0 = ca.SX.sym('x', 14) x1 = rk4(lambda t, x: rhs(x, u, p), t0, x0, h) x1[3:7] = so3.Mrp.shadow_if_necessary(x1[3:7]) predict = ca.Function('predict', [x0, u, p, t0, h], [x1], {'jit': jit}) def schedule(t, start, ty_pairs): val = start for ti, yi in ty_pairs: val = ca.if_else(t > ti, yi, val) return val # reference trajectory pitch_d = 1.0 euler = so3.Euler.from_mrp(r_nb) # roll, pitch, yaw pitch = euler[1] # control u_control = ca.SX.zeros(4) # these controls are just test controls to make sure the fins are working u_control[0] = 0.1 # mass flow rate u_control[1] = 0 u_control[2] = (pitch - 1) u_control[3] = 0 control = ca.Function('control', [x, p, t, dt], [u_control], ['x', 'p', 't', 'dt'], ['u']) # initialize pitch_deg = ca.SX.sym('pitch_deg') omega0_b = ca.vertcat(0, 0, 0) r0_nb = so3.Mrp.from_euler(ca.vertcat(0, pitch_deg * ca.pi / 180, 0)) v0_b = ca.vertcat(0, 0, 0) p0_n = ca.vertcat(0, 0, 0) m0_fuel = 0.8 # x: omega_b, r_nb, v_b, p_n, m_fuel x0 = ca.vertcat(omega0_b, r0_nb, v0_b, p0_n, m0_fuel) # g, Jx, Jy, Jz, Jxz, ve, l_fin, w_fin, CL_alpha, CL0, CD0, K, s, rho, m_emptpy, l_motor p0 = [ 9.8, 0.05, 1.0, 1.0, 0.0, 350, 1.0, 0.05, 2 * np.pi, 0, 0.01, 0.01, 0.05, 1.225, 0.2, 1.0 ] initialize = ca.Function('initialize', [pitch_deg], [x0, p0]) return { 'rhs': rhs, 'predict': predict, 'control': control, 'initialize': initialize, 'force_moment': force_moment, 'x': x, 'u': u, 'p': p } return rhs, x, u, p
def u2c2np(asd): return cs.Function("temp",[],[asd])()["o0"].toarray()
def euler_to_quaternion(angle): quaternion = biorbd.Quaternion.fromMatrix( biorbd.Rotation.fromEulerAngles(angle, "xyz")).to_mx() quaternion /= cas.norm_fro(quaternion) return cas.Function("euler_to_quaternion", [angle], [quaternion])
def createTimeContFun(ode, NX, NU): x = ca.SX.sym('x', NX) u = ca.SX.sym('u', NU) return ca.Function('timeContFun', [x, u], [ode(x, u)])
def MinCurvatureTrajectory(pts, nvecs, ws): """ This function uses optimisation to minimise the curvature of the path """ w_min = -ws[:, 0] * 0.9 w_max = ws[:, 1] * 0.9 th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))] N = len(pts) n_f_a = ca.MX.sym('n_f', N) n_f = ca.MX.sym('n_f', N - 1) th_f = ca.MX.sym('n_f', N - 1) x0_f = ca.MX.sym('x0_f', N - 1) x1_f = ca.MX.sym('x1_f', N - 1) y0_f = ca.MX.sym('y0_f', N - 1) y1_f = ca.MX.sym('y1_f', N - 1) th1_f = ca.MX.sym('y1_f', N - 1) th2_f = ca.MX.sym('y1_f', N - 1) th1_f1 = ca.MX.sym('y1_f', N - 2) th2_f1 = ca.MX.sym('y1_f', N - 2) o_x_s = ca.Function('o_x', [n_f], [pts[:-1, 0] + nvecs[:-1, 0] * n_f]) o_y_s = ca.Function('o_y', [n_f], [pts[:-1, 1] + nvecs[:-1, 1] * n_f]) o_x_e = ca.Function('o_x', [n_f], [pts[1:, 0] + nvecs[1:, 0] * n_f]) o_y_e = ca.Function('o_y', [n_f], [pts[1:, 1] + nvecs[1:, 1] * n_f]) dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f], [ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)]) track_length = ca.Function('length', [n_f_a], [ dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]), o_y_e(n_f_a[1:])) ]) real = ca.Function( 'real', [th1_f, th2_f], [ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)]) im = ca.Function( 'im', [th1_f, th2_f], [-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)]) sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f], [ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))]) get_th_n = ca.Function( 'gth', [th_f], [sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))]) d_n = ca.Function('d_n', [n_f_a, th_f], [track_length(n_f_a) / ca.tan(get_th_n(th_f))]) # objective real1 = ca.Function( 'real1', [th1_f1, th2_f1], [ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)]) im1 = ca.Function( 'im1', [th1_f1, th2_f1], [-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)]) sub_cmplx1 = ca.Function( 'a_cpx1', [th1_f1, th2_f1], [ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))]) # define symbols n = ca.MX.sym('n', N) th = ca.MX.sym('th', N - 1) nlp = {\ 'x': ca.vertcat(n, th), 'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])), # 'f': ca.sumsqr(track_length(n)), 'g': ca.vertcat( # dynamic constraints n[1:] - (n[:-1] + d_n(n, th)), # boundary constraints n[0], #th[0], n[-1], #th[-1], ) \ } # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}}) S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}}) ones = np.ones(N) n0 = ones * 0 th0 = [] for i in range(N - 1): th_00 = lib.get_bearing(pts[i, 0:2], pts[i + 1, 0:2]) th0.append(th_00) th0 = np.array(th0) x0 = ca.vertcat(n0, th0) lbx = list(w_min) + [-np.pi] * (N - 1) ubx = list(w_max) + [np.pi] * (N - 1) r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) x_opt = r['x'] n_set = np.array(x_opt[:N]) # thetas = np.array(x_opt[1*N:2*(N-1)]) return n_set
def _OptimProbl(self): # the symbolic optimization problem w = [] # Variáveis de otimização lbw = [] # Lower bound de w ubw = [] # Upper bound de w g = [] # Restrições não lineares lbg = [] # Lower bound de g ubg = [] # Upper bound de g N = self.N Ysp = self.Ysp # set-point syN = self.syN # Variáveis de folga na saída siN = self.siN # Variável de folga terminal X = self.X U = self.U X_pred = self.X_pred Y_pred = self.Y_pred U_pred = self.U_pred dU_pred = self.dU_pred J = self.J Pesos = self.Pesos ViN_ant = self.ViN_ant w += dU_pred for _ in range(0, N): # Adiciona duk nas variáveis de decisão lbw += [self.dulb] # bound inferior na variável ubw += [self.duub] # bound superior na variável # variáveis de folga w += [syN] lbw += [self.sylb] # bound inferior na variável ubw += [self.syub] # bound superior na variável w += [siN] lbw += [self.silb] # bound inferior na variável ubw += [self.siub] # bound superior na variável g += X_pred for _ in range(0, N): # Bounds em x lbg += [self.xlb] ubg += [self.xub] g += U_pred for _ in range(0,N): # Bounds em u lbg += [self.ulb] ubg += [self.uub] # Bounds em y (nenhuma) # estado terminal XN = self.X_pred[-1] # terminal state # Restrições terminais do ihmpc XiN = XN[self.nxs+self.nxd:self.nxs+self.nxd+self.nxi] res1 = XiN - siN XsN = XN[0:self.nxs] res2 = XsN - Ysp - syN # YN = Y_pred[-1] # res2 = YN - Ysp - syN # Restrição 1 g += [res1] lbg += [self.rilb] ubg += [self.riub] # Restrição 2 g += [res2] lbg += [self.rslb] ubg += [self.rsub] # restrições nos sub-objetivos l = len(self.V) for i in range(l): g += [self.V[i].V] lbg += [self.V[i].min] # in the case of ViN, min = ViN_ant ubg += [self.V[i].max] g = csd.vertcat(*g) w = csd.vertcat(*w) p = csd.vertcat(X, Ysp, U, *Pesos, *ViN_ant) lbw = csd.vertcat(*lbw) ubw = csd.vertcat(*ubw) lbg = csd.vertcat(*lbg) ubg = csd.vertcat(*ubg) X_pred = csd.vertcat(*X_pred) Y_pred = csd.vertcat(*Y_pred) U_pred = csd.vertcat(*U_pred) pred = csd.Function('pred', [X, U, csd.vertcat(*dU_pred)], [X_pred, Y_pred, U_pred], ['x0', 'u0', 'du_opt'], ['x_pred', 'y_pred', 'u_pred']) prob = {'f': J, 'x': w, 'g': g, 'p': p} bounds = {'lbw': lbw, 'ubw': ubw, 'lbg': lbg, 'ubg': ubg} return prob, bounds, pred
def _prepare_function_for_casadi(self): Qsym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.CoM = casadi.Function("CoM", [Qsym], [self.m.CoM(Qsym).to_mx()])
def formulateMPC(self): X = ca.MX.sym('X') Y = ca.MX.sym('Y') th = ca.MX.sym('th') self.e_c = ca.Function('e_c', [X, Y, th], [ ca.sin(self.Phi(th)) * (X - self.cs_x(th)) - ca.cos(self.Phi(th)) * (Y - self.cs_y(th)) ]) self.e_l = ca.Function('e_l', [X, Y, th], [ -ca.cos(self.Phi(th)) * (X - self.cs_x(th)) - ca.sin(self.Phi(th)) * (Y - self.cs_y(th)) ]) gp_in = ca.SX.sym('gp_in', 4, 1) mean = self.gp_dx.predict(gp_in)[0] f_mean_dx = ca.Function('f_mean_dx', [gp_in], [mean]) mean = self.gp_dy.predict(gp_in)[0] f_mean_dy = ca.Function('f_mean_dy', [gp_in], [mean]) gp_in2 = ca.SX.sym('gp_in2', 2, 1) mean = self.gp_dth.predict(gp_in2)[0] f_mean_dth = ca.Function('f_mean_dth', [gp_in2], [mean]) x = ca.SX.sym('x') y = ca.SX.sym('y') th = ca.SX.sym('th') v = ca.SX.sym('v') delta = ca.SX.sym('delta') state = np.array([x, y, th]) control = np.array([v, delta]) rhs = np.array([ f_mean_dx(np.array([np.cos(th), np.sin(th), v, delta])), f_mean_dy(np.array([np.cos(th), np.sin(th), v, delta])), f_mean_dth(np.array([v, delta])) ]) + state self.f_dyn = ca.Function('f_dyn', [state, control], [rhs]) self.mpc_opti = ca.casadi.Opti() self.U = self.mpc_opti.variable(2, self.H) self.X = self.mpc_opti.variable(3, self.H + 1) self.TH = self.mpc_opti.variable(self.H + 1) self.NU = self.mpc_opti.variable(self.H) self.P_1 = self.mpc_opti.parameter(3) self.P_2 = self.mpc_opti.parameter(2) J = 0 for k in range(self.H + 1): J += self.qc * self.e_c(self.X[0, k], self.X[ 1, k], self.TH[k])**2 + self.ql * self.e_l( self.X[0, k], self.X[1, k], self.TH[k])**2 for k in range(self.H - 1): J += self.Ru[0] * (self.U[0, k + 1] - self.U[0, k])**2 + self.Ru[ 1] * (self.U[1, k + 1] - self.U[1, k])**2 + self.Rv * ( self.NU[k + 1] - self.NU[k])**2 J += -self.gamma * self.TH[-1] self.mpc_opti.minimize(J) for k in range(self.H): self.mpc_opti.subject_to( self.X[:, k + 1] == self.f_dyn(self.X[:, k], self.U[:, k])) self.mpc_opti.subject_to(self.TH[k + 1] == self.TH[k] + self.T * self.NU[k]) self.mpc_opti.subject_to(0 <= self.TH) self.mpc_opti.subject_to(self.TH <= self.L) self.mpc_opti.subject_to(self.nu_min <= self.NU) self.mpc_opti.subject_to(self.NU <= self.nu_max) self.mpc_opti.subject_to(self.v_min <= self.U[0, :]) self.mpc_opti.subject_to(self.U[0, :] <= self.v_max) self.mpc_opti.subject_to(self.delta_min <= self.U[1, :]) self.mpc_opti.subject_to(self.U[1, :] <= self.delta_max) self.mpc_opti.subject_to(self.ramp_v_min <= self.U[0, 0] - self.P_2[0]) self.mpc_opti.subject_to(self.U[0, 0] - self.P_2[0] <= self.ramp_v_max) self.mpc_opti.subject_to( self.ramp_delta_min <= self.U[1, 0] - self.P_2[1]) self.mpc_opti.subject_to( self.U[1, 0] - self.P_2[1] <= self.ramp_delta_max) self.mpc_opti.subject_to( self.ramp_v_min <= self.U[0, 1:] - self.U[0, 0:-1]) self.mpc_opti.subject_to( self.U[0, 1:] - self.U[0, 0:-1] <= self.ramp_v_max) self.mpc_opti.subject_to( self.ramp_delta_min <= self.U[1, 1:] - self.U[1, 0:-1]) self.mpc_opti.subject_to( self.U[1, 1:] - self.U[1, 0:-1] <= self.ramp_delta_max) self.mpc_opti.subject_to(self.X[:, 0] == self.P_1[0:3]) p_opts = {'verbose_init': False} s_opts = {'tol': 0.01, 'print_level': 0, 'max_iter': 100} self.mpc_opti.solver('ipopt', p_opts, s_opts) # Warm up self.mpc_opti.set_value(self.P_1, self.state) self.mpc_opti.set_value(self.P_2, self.input) sol = self.mpc_opti.solve() self.mpc_opti.set_initial(self.U, sol.value(self.U)) self.mpc_opti.set_initial(self.X, sol.value(self.X)) self.mpc_opti.set_initial(self.NU, sol.value(self.NU)) self.mpc_opti.set_initial(self.TH, sol.value(self.TH))
def _prepare_function_for_casadi(self): Qsym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.CoMs = casadi.Function( "CoMbySegment", [Qsym], [self.m.CoMbySegmentInMatrix(Qsym).to_mx()])
def _prepare_function_for_casadi(self): q_sym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.markers = casadi.Function("Markers", [q_sym], [self.m.markers(q_sym)])
def direct_collocation(opt): # 对控制量和主变量都做离散化,不再需要对离散时间动态的构造 # 用了多项式插值的方法参数化整个状态轨迹,从而实现匹配 d = 3 # degree of interpolating polynomial tau_root = np.append(0,ca.collocation_points(d,'legendre')) #匹配方程的参数矩阵 C = np.zeros((d+1,d+1)) #连续方程的参数矩阵 D = np.zeros(d+1) #二次方程的参数矩阵 B = np.zeros(d+1) for j in range(d+1): p = np.poly1d([1]) for r in range(d+1): if r != j: p*= np.poly1d([1,-tau_root[r]]) / (tau_root[j] - tau_root[r]) D[j] = p(1.0) pder = np.polyder(p) for r in range(d+1): C[j,r] = pder(tau_root[r]) pint = np.polyint(p) B[j] = pint(1.0) T = 10 x1 = ca.MX.sym('x1') x2 = ca.MX.sym('x2') x = ca.vertcat(x1,x2) u = ca.MX.sym('u') xdot = ca.vertcat((1 - x2 ** 2) * x1 - x2 + u, x1) L = x1 ** 2 + x2 ** 2 + u ** 2 # 连续动态的建模 f = ca.Function('f',[x,u],[xdot,L],['x','u'],['xdot','L']) # 离散化控制量 N = 20 h = T/N w = [] w0 = [] lbw = [] ubw = [] J = 0 g = [] lbg = [] ubg = [] x_plot = [] u_plot = [] #这一步是与multi方法一致的 Xk = ca.MX.sym('X0',2) w.append(Xk) # 醉了,前面用append不好吗?明明优雅很多 lbw.append([0,1]) ubw.append([0,1]) w0.append([0,1]) x_plot.append(Xk) for k in range(N): Uk = ca.MX.sym('U_'+str(k)) w.append(Uk) lbw.append([-1]) ubw.append([1]) w0.append([0]) u_plot.append(Uk) Xc = [] for j in range(d): Xkj = ca.MX.sym('X_'+str(k)+'_'+str(j),2) Xc.append(Xkj) w.append(Xkj) lbw.append([-0.25,-np.inf]) ubw.append([np.inf,np.inf]) w0.append([0,0]) Xk_end = D[0]*Xk for j in range(1,d+1): xp = C[0,j]*Xk for r in range(d): xp = xp+C[r+1,j]*Xc[r] fj,qj = f(Xc[j-1],Uk) g.append(h*fj-xp) lbg.append([0,0]) ubg.append([0,0]) Xk_end = Xk_end + D[j]*Xc[j-1] J = J + B[j]*qj*h Xk = ca.MX.sym('X_'+str(k+1),2) w.append(Xk) lbw.append([-0.25,-np.inf]) ubw.append([np.inf,np.inf]) w0.append([0,0]) x_plot.append(Xk) g.append(Xk_end-Xk) lbg.append([0,0]) ubg.append([0,0]) w = ca.vertcat(*w) g = ca.vertcat(*g) x_plot = ca.horzcat(*x_plot) u_plot = ca.horzcat(*u_plot) w0 = np.concatenate(w0) lbw = np.concatenate(lbw) ubw = np.concatenate(ubw) lbg = np.concatenate(lbg) ubg = np.concatenate(ubg) prob = {'f':J,'x':w,'g':g} solver = ca.nlpsol('solver','ipopt',prob) trajectories = ca.Function('trajectories',[w],[x_plot,u_plot],['w'],['x','u']) sol = solver(x0 = w0, lbx = lbw, ubx = ubw, lbg = lbg, ubg = ubg) x_opt,u_opt = trajectories(sol['x']) x_opt = x_opt.full() # to numpy array u_opt = u_opt.full() tgrid = [T / N * k for k in range(N + 1)] import matplotlib.pyplot as plt plt.figure(1) plt.clf() plt.plot(tgrid, x_opt[0], '--') plt.plot(tgrid, x_opt[1], '-') plt.step(tgrid, np.append(np.nan,u_opt[0]), '-.') plt.xlabel('t') plt.legend(['x1', 'x2', 'u']) plt.grid() plt.show()
def direct_single_shooting(opt): T = 10 N = 20 x1 = ca.MX.sym('x1') x2 = ca.MX.sym('x2') x = ca.vertcat(x1,x2) u = ca.MX.sym('u') xdot = ca.vertcat((1-x2**2)*x1-x2+u,x1) L = x1**2+x2**2+u**2 if opt==False: # using CVodes instead of RK4 dae = {'x':x,'p':u,'ode':xdot,'quad':L} opts = {'tf':T/N} #这里是做了离散化,N是控制区间 F = ca.integrator('F','cvodes',dae,opts) else: #using fixed step Runge-kutta 4 integrator # ref: https: // blog.csdn.net / xiaokun19870825 / article / details / 78763739 M = 4 DT = T/M/N f = ca.Function('f',[x,u],[xdot,L]) X0 = ca.MX.sym('X0',2) U = ca.MX.sym('U') X = X0 Q = 0 for j in range(M): k1,k1_q = f(X,U) k2,k2_q = f(X+DT/2*k1,U) k3,k3_q = f(X+DT/2*k2,U) k4,k4_q = f(X+DT*k3,U) X = X+DT/6*(k1 + 2*k2+ 2*k3 + k4) Q = Q + DT / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q) F = ca.Function('F', [X0, U], [X, Q], ['x0', 'p'], ['xf', 'qf']) # Evaluate at a test point # Fk = F(x0=[0.2,0.3],p=0.4) # print(Fk['xf']) # print(Fk['qf']) # Start with an empty NLP w=[] w0 = [] lbw = [] ubw = [] J = 0 g=[] lbg = [] ubg = [] # Formulate the NLP # 这时u是离散控制量 Xk = ca.MX([0, 1]) for k in range(N): # New NLP variable for the control Uk = ca.MX.sym('U_' + str(k)) w += [Uk] # w是局部变量,这里就是u # 至于下列的约束为什么都是List类型: 因为是离散控制量,目标函数是L在区间上的积分,所以对每个分割内进行计算并求和 lbw += [-1] ubw += [1] w0 += [0] # Integrate till the end of the interval Fk = F(x0=Xk, p=Uk) Xk = Fk['xf'] J=J+Fk['qf'] # 通过累加操作来逼近积分,又因为用了RK4,所以积分的精度得到了保证。 # Add inequality constraint g += [Xk[0]] lbg += [-.25] ubg += [ca.inf] # Create an NLP solver # 这里x指向的是控制量!!! # 因为目标函数所求的是最小的u,x # 而x是由u给出的,得到了u就可以根据动态系统得到x prob = {'f': J, 'x': ca.vertcat(*w), 'g': ca.vertcat(*g)} solver = ca.nlpsol('solver', 'ipopt', prob) # Solve the NLP sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) w_opt = sol['x'] # Plot the solution u_opt = w_opt #这里的操作验证了上述注释的内容 x_opt = [[0, 1]] for k in range(N): Fk = F(x0=x_opt[-1], p=u_opt[k]) x_opt += [Fk['xf'].full()] #.full()转化为np.array x1_opt = [r[0] for r in x_opt] x2_opt = [r[1] for r in x_opt] tgrid = [T/N*k for k in range(N+1)] import matplotlib.pyplot as plt plt.figure(1) plt.clf() plt.plot(tgrid, x1_opt, '--') plt.plot(tgrid, x2_opt, '-') plt.step(tgrid, ca.vertcat(ca.DM.nan(1), u_opt), '-.') plt.xlabel('t') plt.legend(['x1','x2','u']) plt.grid() plt.show()
def direct_multi_shooting(opt): # control and state nodes start value in NLP # 与single_shooting相似,但是把状态点放在了特定的shooting nodes上,并作为NLP的决策变量: # 注意到在single_shooting方法中,NLP的决策变量唯一且为控制量。 # multi方法往往是优于single方法的,因为将问题的维度提高了,从而提高了问题的收敛性 T = 10 N = 20 x1 = ca.MX.sym("x1") x2 = ca.MX.sym("x2") x = ca.vertcat(x1,x2) u = ca.MX.sym('u') xdot = ca.vertcat((1-x2**2)*x1-x2+u,x1) L = x1**2+x2**2+u**2 if opt == False: dea = {'x':x,'p':u,'ode':xdot,'quad':L} opts = {'tf': T/N} F = ca.integrator('F','cvodes',dea,opts) else: M = 4 DT = T/N/M f = ca.Function('f',[x,u],[xdot,L]) X0 = ca.MX.sym('x0',2) # 这里的X0是initial U = ca.MX.sym('U') X = X0 Q = 0 # Q 是 L 的逼近斜率 for i in range(M): k1,k1_q = f(X,U) k2,k2_q = f(X+DT/2*k1,U) k3,k3_q = f(X+DT/2*k2,U) k4,k4_q = f(X+DT*k3,U) X = X+DT/6*(k1+2*k2+2*k3+k4) Q = Q+DT/6*(k1_q+2*k2_q+2*k3_q+k4_q) F = ca.Function('F',[X0,U],[X,Q],['x0','p'],['xf','qf']) Fk = F(x0 = [0.2,0.3],p=0.4) print(Fk['xf']) print(Fk['qf']) w = [] w0= [] lbw=[] ubw=[] J = 0 g = [] lbg = [] ubg = [] # 初始变量是[0,1] Xk = ca.MX.sym('X0',2) w += [Xk] #这里开始和single有区别:(single中为 w += [Uk]) lbw += [0,1] ubw += [0,1] w0 += [0,1] for k in range(N): Uk = ca.MX.sym('U_' + str(k)) w += [Uk] lbw += [-1] ubw += [1] w0 += [0] Fk = F(x0=Xk,p=Uk) Xk_end = Fk['xf'] J = J + Fk['qf'] Xk = ca.MX.sym('X_' + str(k+1),2) w += [Xk] lbw += [-0.25,-ca.inf] ubw += [ca.inf,ca.inf] w0 += [0,0] #这里添加了等式约束,在single中添加的是等式约束 g += [Xk_end-Xk] lbg += [0,0] ubg += [0,0] prob = {'f':J,'x':ca.vertcat(*w),'g':ca.vertcat(*g)} solver= ca.nlpsol('solver','ipopt',prob) sol = solver(x0 = w0,lbx = lbw, ubx = ubw, lbg = lbg, ubg= ubg) w_opt = sol['x'].full().flatten() x1_opt = w_opt[0::3] x2_opt = w_opt[1::3] u_opt = w_opt[2::3] tgrid = [T/N*k for k in range(N+1)] import matplotlib.pyplot as plt plt.figure(1) plt.clf() plt.plot(tgrid, x1_opt, '--') plt.plot(tgrid, x2_opt, '-') plt.step(tgrid, ca.vertcat(ca.DM.nan(1), u_opt), '-.') plt.xlabel('t') plt.legend(['x1','x2','u']) plt.grid() plt.show()
def set_up(self, model, inputs=None): """Unpack model, perform checks, simplify and calculate jacobian. Parameters ---------- model : :class:`pybamm.BaseModel` The model whose solution to calculate. Must have attributes rhs and initial_conditions inputs : dict, optional Any input parameters to pass to the model when solving """ # Check model.algebraic for ode solvers if self.ode_solver is True and len(model.algebraic) > 0: raise pybamm.SolverError( "Cannot use ODE solver '{}' to solve DAE model".format( self.name)) # Check model.rhs for algebraic solvers if self.algebraic_solver is True and len(model.rhs) > 0: raise pybamm.SolverError( """Cannot use algebraic solver to solve model with time derivatives""" ) inputs = inputs or {} y0 = model.concatenated_initial_conditions.evaluate(0, None, inputs=inputs) # Set model timescale model.timescale_eval = model.timescale.evaluate(inputs=inputs) if self.ode_solver is True: self.root_method = None if (isinstance(self, (pybamm.CasadiSolver, pybamm.CasadiAlgebraicSolver)) ) and model.convert_to_format != "casadi": pybamm.logger.warning( "Converting {} to CasADi for solving with CasADi solver". format(model.name)) model.convert_to_format = "casadi" if self.root_method == "casadi" and model.convert_to_format != "casadi": pybamm.logger.warning( "Converting {} to CasADi for calculating ICs with CasADi". format(model.name)) model.convert_to_format = "casadi" if model.convert_to_format != "casadi": simp = pybamm.Simplification() # Create Jacobian from concatenated rhs and algebraic y = pybamm.StateVector(slice(0, np.size(y0))) # set up Jacobian object, for re-use of dict jacobian = pybamm.Jacobian() else: # Convert model attributes to casadi t_casadi = casadi.MX.sym("t") y_diff = casadi.MX.sym( "y_diff", len(model.concatenated_rhs.evaluate(0, y0, inputs=inputs))) y_alg = casadi.MX.sym( "y_alg", len(model.concatenated_algebraic.evaluate(0, y0, inputs=inputs)), ) y_casadi = casadi.vertcat(y_diff, y_alg) p_casadi = {} for name, value in inputs.items(): if isinstance(value, numbers.Number): p_casadi[name] = casadi.MX.sym(name) else: p_casadi[name] = casadi.MX.sym(name, value.shape[0]) p_casadi_stacked = casadi.vertcat(*[p for p in p_casadi.values()]) def process(func, name, use_jacobian=None): def report(string): # don't log event conversion if "event" not in string: pybamm.logger.info(string) if use_jacobian is None: use_jacobian = model.use_jacobian if model.convert_to_format != "casadi": # Process with pybamm functions if model.use_simplify: report(f"Simplifying {name}") func = simp.simplify(func) if use_jacobian: report(f"Calculating jacobian for {name}") jac = jacobian.jac(func, y) if model.use_simplify: report(f"Simplifying jacobian for {name}") jac = simp.simplify(jac) if model.convert_to_format == "python": report(f"Converting jacobian for {name} to python") jac = pybamm.EvaluatorPython(jac) jac = jac.evaluate else: jac = None if model.convert_to_format == "python": report(f"Converting {name} to python") func = pybamm.EvaluatorPython(func) func = func.evaluate else: # Process with CasADi report(f"Converting {name} to CasADi") func = func.to_casadi(t_casadi, y_casadi, inputs=p_casadi) if use_jacobian: report(f"Calculating jacobian for {name} using CasADi") jac_casadi = casadi.jacobian(func, y_casadi) jac = casadi.Function( name, [t_casadi, y_casadi, p_casadi_stacked], [jac_casadi]) else: jac = None func = casadi.Function(name, [t_casadi, y_casadi, p_casadi_stacked], [func]) if name == "residuals": func_call = Residuals(func, name, model) else: func_call = SolverCallable(func, name, model) if jac is not None: jac_call = SolverCallable(jac, name + "_jac", model) else: jac_call = None return func, func_call, jac_call # Check for heaviside functions in rhs and algebraic and add discontinuity # events if these exist. # Note: only checks for the case of t < X, t <= X, X < t, or X <= t, but also # accounts for the fact that t might be dimensional # Only do this for DAE models as ODE models can deal with discontinuities fine if len(model.algebraic) > 0: for symbol in itertools.chain( model.concatenated_rhs.pre_order(), model.concatenated_algebraic.pre_order(), ): if isinstance(symbol, pybamm.Heaviside): found_t = False # Dimensionless if symbol.right.id == pybamm.t.id: expr = symbol.left found_t = True elif symbol.left.id == pybamm.t.id: expr = symbol.right found_t = True # Dimensional elif symbol.right.id == (pybamm.t * model.timescale).id: expr = symbol.left.new_copy( ) / symbol.right.right.new_copy() found_t = True elif symbol.left.id == (pybamm.t * model.timescale).id: expr = symbol.right.new_copy( ) / symbol.left.right.new_copy() found_t = True # Update the events if the heaviside function depended on t if found_t: model.events.append( pybamm.Event( str(symbol), expr.new_copy(), pybamm.EventType.DISCONTINUITY, )) # Process rhs, algebraic and event expressions rhs, rhs_eval, jac_rhs = process(model.concatenated_rhs, "RHS") algebraic, algebraic_eval, jac_algebraic = process( model.concatenated_algebraic, "algebraic") terminate_events_eval = [ process(event.expression, "event", use_jacobian=False)[1] for event in model.events if event.event_type == pybamm.EventType.TERMINATION ] # discontinuity events are evaluated before the solver is called, so don't need # to process them discontinuity_events_eval = [ event for event in model.events if event.event_type == pybamm.EventType.DISCONTINUITY ] # Add the solver attributes model.rhs_eval = rhs_eval model.algebraic_eval = algebraic_eval model.jac_algebraic_eval = jac_algebraic model.terminate_events_eval = terminate_events_eval model.discontinuity_events_eval = discontinuity_events_eval # Save CasADi functions for the CasADi solver # Note: when we pass to casadi the ode part of the problem must be in explicit # form so we pre-multiply by the inverse of the mass matrix if self.root_method == "casadi" or isinstance(self, pybamm.CasadiSolver): # can use DAE solver to solve model with algebraic equations only if len(model.rhs) > 0: mass_matrix_inv = casadi.MX(model.mass_matrix_inv.entries) explicit_rhs = mass_matrix_inv @ rhs(t_casadi, y_casadi, p_casadi_stacked) model.casadi_rhs = casadi.Function( "rhs", [t_casadi, y_casadi, p_casadi_stacked], [explicit_rhs]) model.casadi_algebraic = algebraic if self.algebraic_solver is True: # we don't calculate consistent initial conditions # for an algebraic solver as this will be the job of the algebraic solver model.residuals_eval = Residuals(algebraic, "residuals", model) model.jacobian_eval = jac_algebraic model.y0 = y0.flatten() elif len(model.algebraic) == 0: # can use DAE solver to solve ODE model # - no initial condition initialization needed model.residuals_eval = Residuals(rhs, "residuals", model) model.jacobian_eval = jac_rhs model.y0 = y0.flatten() # Calculate consistent initial conditions for the algebraic equations else: if len(model.rhs) > 0: all_states = pybamm.NumpyConcatenation( model.concatenated_rhs, model.concatenated_algebraic) # Process again, uses caching so should be quick residuals_eval, jacobian_eval = process( all_states, "residuals")[1:] model.residuals_eval = residuals_eval model.jacobian_eval = jacobian_eval else: model.residuals_eval = Residuals(algebraic, "residuals", model) model.jacobian_eval = jac_algebraic y0_guess = y0.flatten() model.y0 = self.calculate_consistent_state(model, 0, y0_guess, inputs) pybamm.logger.info("Finish solver set-up")
## Listing 1 ############################################## import nloed as nl, casadi as cs import numpy as np, pandas as pd #CasADi input and parameter symbols x = cs.SX.sym('x', 1) theta = cs.SX.sym('theta', 4) #Transform; ensures parameter positivity q = cs.exp(theta) #Expressions for the sampling statistics mean = (q[0] + q[1] * x[0]**q[2] / (q[3]**q[2] + x[0]**q[2])) s = 0.059 var = (s * mean)**2 #Create a sampling statistics vector stats = cs.vertcat(mean, var) #Create a CasADi function for f(.) func = cs.Function('GFP', [x, theta], [stats]) ## Listing 2 ############################################## #Input and parameter name lists xname = ['Light'] tname = ['ln_A0', 'ln_A', 'ln_N', 'ln_K'] #Assign a normal dist. to the output obs_var = [(func, 'Normal')] #Call the NLoed Model constructor model = nl.Model(obs_var, xname, tname) ## Listing 3 ############################################## #Initial green light levels (%) levels0 = [0.01, 0.01, 0.01, 1.5, 1.5, 1.5, 6, 6, 6, 25, 25, 25, 100, 100, 100] #Initial GFP observations (a.u.) obs0 = [
def Max_velocity(pts, config, show=False): mu = config['car']['mu'] m = config['car']['m'] g = config['car']['g'] l_f = config['car']['l_f'] l_r = config['car']['l_r'] safety_f = config['pp']['force_f'] f_max = mu * m * g * safety_f f_long_max = l_f / (l_r + l_f) * f_max max_v = config['lims'][ 'max_v'] # parameter to be adapted so that optimiser isnt too fast max_a = config['lims']['max_a'] s_i, th_i = convert_pts_s_th(pts) th_i_1 = th_i[:-1] s_i_1 = s_i[:-1] N = len(s_i) N1 = len(s_i) - 1 # setup possible casadi functions d_x = ca.MX.sym('d_x', N - 1) d_y = ca.MX.sym('d_y', N - 1) vel = ca.Function('vel', [d_x, d_y], [ca.sqrt(ca.power(d_x, 2) + ca.power(d_y, 2))]) dx = ca.MX.sym('dx', N) dy = ca.MX.sym('dy', N) dt = ca.MX.sym('t', N - 1) f_long = ca.MX.sym('f_long', N - 1) f_lat = ca.MX.sym('f_lat', N - 1) nlp = {\ 'x': ca.vertcat(dx, dy, dt, f_long, f_lat), 'f': ca.sum1(dt), 'g': ca.vertcat( # dynamic constraints dt - s_i_1 / ((vel(dx[:-1], dy[:-1]) + vel(dx[1:], dy[1:])) / 2 ), # ca.arctan2(dy, dx) - th_i, dx/dy - ca.tan(th_i), dx[1:] - (dx[:-1] + (ca.sin(th_i_1) * f_long + ca.cos(th_i_1) * f_lat) * dt / m), dy[1:] - (dy[:-1] + (ca.cos(th_i_1) * f_long - ca.sin(th_i_1) * f_lat) * dt / m), # path constraints ca.sqrt(ca.power(f_long, 2) + ca.power(f_lat, 2)), # boundary constraints # dx[0], dy[0] ) \ } S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}}) # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}}) # make init sol v0 = np.ones(N) * max_v / 2 dx0 = v0 * np.sin(th_i) dy0 = v0 * np.cos(th_i) dt0 = s_i_1 / ca.sqrt(ca.power(dx0[:-1], 2) + ca.power(dy0[:-1], 2)) f_long0 = np.zeros(N - 1) ddx0 = dx0[1:] - dx0[:-1] ddy0 = dy0[1:] - dy0[:-1] a0 = (ddx0**2 + ddy0**2)**0.5 f_lat0 = a0 * m x0 = ca.vertcat(dx0, dy0, dt0, f_long0, f_lat0) # make lbx, ubx # lbx = [-max_v] * N + [-max_v] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1 lbx = [-max_v] * N + [0] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max ] * N1 ubx = [max_v] * N + [max_v] * N + [10] * N1 + [f_long_max] * N1 + [f_max ] * N1 #make lbg, ubg lbg = [0] * N1 + [0] * N + [0] * 2 * N1 + [0] * N1 #+ [0] * 2 ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [f_max] * N1 #+ [0] * 2 r = S(x0=x0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) x_opt = r['x'] dx = np.array(x_opt[:N]) dy = np.array(x_opt[N:N * 2]) dt = np.array(x_opt[2 * N:N * 2 + N1]) f_long = np.array(x_opt[2 * N + N1:2 * N + N1 * 2]) f_lat = np.array(x_opt[-N1:]) f_t = (f_long**2 + f_lat**2)**0.5 # print(f"Dt: {dt.T}") # print(f"DT0: {dt[0]}") t = np.cumsum(dt) t = np.insert(t, 0, 0) # print(f"Dt: {dt.T}") # print(f"Dx: {dx.T}") # print(f"Dy: {dy.T}") vs = (dx**2 + dy**2)**0.5 if show: plt.figure(1) plt.title("Velocity vs dt") plt.plot(t, vs) plt.plot(t, th_i) plt.legend(['vs', 'ths']) # plt.plot(t, dx) # plt.plot(t, dy) # plt.legend(['v', 'dx', 'dy']) plt.plot(t, np.ones_like(t) * max_v, '--') plt.figure(2) plt.title("F_long, F_lat vs t") plt.plot(t[:-1], f_long) plt.plot(t[:-1], f_lat) plt.plot(t[:-1], f_t, linewidth=3) plt.plot(t, np.ones_like(t) * f_max, '--') plt.plot(t, np.ones_like(t) * -f_max, '--') plt.plot(t, np.ones_like(t) * f_long_max, '--') plt.plot(t, np.ones_like(t) * -f_long_max, '--') plt.legend(['Flong', "f_lat", "f_t"]) # plt.figure(3) # plt.title("Theta vs t") # plt.plot(t, th_i) # plt.plot(t, np.abs(th_i)) # plt.figure(5) # plt.title(f"t vs dt") # plt.plot(t[1:], dt) # plt.plot(t[1:], dt, '+') # plt.figure(9) # plt.clf() # plt.title("F_long, F_lat vs t") # plt.plot(t[:-1], f_long) # plt.plot(t[:-1], f_lat) # plt.plot(t[:-1], f_t, linewidth=3) # plt.plot(t, np.ones_like(t) * f_max, '--') # plt.plot(t, np.ones_like(t) * -f_max, '--') # plt.plot(t, np.ones_like(t) * f_long_max, '--') # plt.plot(t, np.ones_like(t) * -f_long_max, '--') # plt.legend(['Flong', "f_lat", "f_t"]) return vs
def wrapperRWSC(IC, args, optimal): # Converting the Optimal Control Problem into a Non-Linear Programming Problem numStates = 6 numInputs = 2 nodes = 30 # Keep this Number Small to Reduce Runtime dt = SX.sym("dt") states = SX.sym("state", nodes, numStates) controls = SX.sym("controls", nodes, numInputs) variables_list = [dt, states, controls] variables_name = ["dt", "states", "controls"] variables_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in variables_list]) pack_variables_fn = casadi.Function( "pack_variables_fn", variables_list, [variables_flat], variables_name, ["flat"] ) unpack_variables_fn = casadi.Function( "unpack_variables_fn", [variables_flat], variables_list, ["flat"], variables_name, ) # Bounds bds = [ [np.sqrt(np.finfo(float).eps), np.inf], [-100, 300], [0, np.inf], [-np.inf, np.inf], [-np.inf, np.inf], [np.sqrt(np.finfo(float).eps), np.inf], [-1, 1], [np.sqrt(np.finfo(float).eps), 1], ] lower_bounds = unpack_variables_fn(flat=-float("inf")) lower_bounds["dt"][:, :] = bds[0][0] lower_bounds["states"][:, 0] = bds[1][0] lower_bounds["states"][:, 1] = bds[2][0] lower_bounds["states"][:, 4] = bds[5][0] lower_bounds["controls"][:, 0] = bds[6][0] lower_bounds["controls"][:, 1] = bds[7][0] upper_bounds = unpack_variables_fn(flat=float("inf")) upper_bounds["dt"][:, :] = bds[0][1] upper_bounds["states"][:, 0] = bds[1][1] upper_bounds["controls"][:, 0] = bds[6][1] upper_bounds["controls"][:, 1] = bds[7][1] # Set Initial Conditions # Casadi does not accept equality constraints, so boundary constraints are # set as box constraints with 0 area. lower_bounds["states"][0, 0] = IC[0] lower_bounds["states"][0, 1] = IC[1] lower_bounds["states"][0, 2] = IC[2] lower_bounds["states"][0, 3] = IC[3] lower_bounds["states"][0, 4] = IC[4] lower_bounds["states"][0, 5] = IC[5] upper_bounds["states"][0, 0] = IC[0] upper_bounds["states"][0, 1] = IC[1] upper_bounds["states"][0, 2] = IC[2] upper_bounds["states"][0, 3] = IC[3] upper_bounds["states"][0, 4] = IC[4] upper_bounds["states"][0, 5] = IC[5] # Set Final Conditions # Currently set for a soft touchdown at the origin lower_bounds["states"][-1, 0] = 0 lower_bounds["states"][-1, 1] = 0 lower_bounds["states"][-1, 2] = 0 lower_bounds["states"][-1, 3] = 0 lower_bounds["states"][-1, 5] = 0 upper_bounds["states"][-1, 0] = 0 upper_bounds["states"][-1, 1] = 0 upper_bounds["states"][-1, 2] = 0 upper_bounds["states"][-1, 3] = 0 upper_bounds["states"][-1, 5] = 0 # Initial Guess Generation # Generate the initial guess as a line between initial and final conditions xIG = np.array( [ np.linspace(IC[0], 0, nodes), np.linspace(IC[1], 0, nodes), np.linspace(IC[2], 0, nodes), np.linspace(IC[3], 0, nodes), np.linspace(IC[4], IC[4] * 0.5, nodes), np.linspace(IC[5], 0, nodes), ] ).T uIG = np.array([np.linspace(0, 1, nodes), np.linspace(1, 1, nodes)]).T ig_list = [60 / nodes, xIG, uIG] ig_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in ig_list]) # Generating Defect Vector xLow = states[0 : (nodes - 1), :] xHigh = states[1:nodes, :] contLow = controls[0 : (nodes - 1), :] contHi = controls[1:nodes, :] contMid = (contLow + contHi) / 2 # Use a RK4 Method for Generating the Defects k1 = RWSC(xLow, contLow, args) k2 = RWSC(xLow + (0.5 * dt * k1), contMid, args) k3 = RWSC(xLow + (0.5 * dt * k2), contMid, args) k4 = RWSC(xLow + k3, contHi, args) xNew = xLow + ((dt / 6) * (k1 + (2 * k2) + (2 * k3) + k4)) defect = casadi.reshape(xNew - xHigh, -1, 1) # Choose the Cost Function if optimal == 1: J = -states[-1, 4] # Mass Optimal Cost Function elif optimal == 2: J = dt[0] # Time Optimal Cost Function # Put the OCP into a form that Casadi can solve nlp = {"x": variables_flat, "f": J, "g": defect} solver = casadi.nlpsol( "solver", "bonmin", nlp ) # Use bonmin instead of ipopt due to speed result = solver( x0=ig_flat, lbg=0.0, ubg=0.0, lbx=pack_variables_fn(**lower_bounds)["flat"], ubx=pack_variables_fn(**upper_bounds)["flat"], ) results = unpack_variables_fn(flat=result["x"]) return results
n_controls = 2 # len([controls]) rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega) # Obstacle states in each predictions MOx = ca.SX.sym('MOx') MOy = ca.SX.sym('MOy') MOth = ca.SX.sym('MOth') MOv = ca.SX.sym('MOv') MOr = ca.SX.sym('MOr') Ost = [MOx, MOy, MOth, MOv, MOr] n_MOst = len(Ost) # System setup for casadi mapping_func = ca.Function('f', [states, controls], [rhs]) # Declare empty system matrices U = ca.SX.sym('U', n_controls, N) # Parameters:initial state(x0), reference state (xref), obstacles (O) P = ca.SX.sym('P', n_states + n_states + n_MO * (N + 1) * n_MOst + n_SO*3) X = ca.SX.sym('X', n_states, (N + 1)) # Prediction matrix # Objective Function and Constraints # weighing matrices (states) Q = np.zeros((3, 3)) Q[0, 0] = 1 # x Q[1, 1] = 5 # y
( ca_tools.entry('v'), ca_tools.entry('omega') ) ]) v, omega = controls[...] n_controls = controls.size ## rhs rhs = ca_tools.struct_SX(states) rhs['x'] = v*ca.cos(theta) rhs['y'] = v*ca.sin(theta) rhs['theta'] = omega ## function f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs']) ## for MPC optimizing_target = ca_tools.struct_symSX([ ( ca_tools.entry('U', repeat=N, struct=controls), ca_tools.entry('X', repeat=N+1, struct=states) ) ]) U, X, = optimizing_target[...] # data are stored in list [], notice that ',' cannot be missed # X = ca.SX.sym('X', n_states, N+1) current_parameters = ca_tools.struct_symSX([ ( ca_tools.entry('P', shape=n_states+n_states),
def _convert(self, symbol, t, y, y_dot, inputs): """ See :meth:`CasadiConverter.convert()`. """ if isinstance( symbol, ( pybamm.Scalar, pybamm.Array, pybamm.Time, pybamm.InputParameter, pybamm.ExternalVariable, ), ): return casadi.MX(symbol.evaluate(t, y, y_dot, inputs)) elif isinstance(symbol, pybamm.StateVector): if y is None: raise ValueError( "Must provide a 'y' for converting state vectors") return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices]) elif isinstance(symbol, pybamm.StateVectorDot): if y_dot is None: raise ValueError( "Must provide a 'y_dot' for converting state vectors") return casadi.vertcat( *[y_dot[y_slice] for y_slice in symbol.y_slices]) elif isinstance(symbol, pybamm.BinaryOperator): left, right = symbol.children # process children converted_left = self.convert(left, t, y, y_dot, inputs) converted_right = self.convert(right, t, y, y_dot, inputs) if isinstance(symbol, pybamm.Modulo): return casadi.fmod(converted_left, converted_right) if isinstance(symbol, pybamm.Minimum): return casadi.fmin(converted_left, converted_right) if isinstance(symbol, pybamm.Maximum): return casadi.fmax(converted_left, converted_right) # _binary_evaluate defined in derived classes for specific rules return symbol._binary_evaluate(converted_left, converted_right) elif isinstance(symbol, pybamm.UnaryOperator): converted_child = self.convert(symbol.child, t, y, y_dot, inputs) if isinstance(symbol, pybamm.AbsoluteValue): return casadi.fabs(converted_child) if isinstance(symbol, pybamm.Floor): return casadi.floor(converted_child) if isinstance(symbol, pybamm.Ceiling): return casadi.ceil(converted_child) return symbol._unary_evaluate(converted_child) elif isinstance(symbol, pybamm.Function): converted_children = [ self.convert(child, t, y, y_dot, inputs) for child in symbol.children ] # Special functions if symbol.function == np.min: return casadi.mmin(*converted_children) elif symbol.function == np.max: return casadi.mmax(*converted_children) elif symbol.function == np.abs: return casadi.fabs(*converted_children) elif symbol.function == np.sqrt: return casadi.sqrt(*converted_children) elif symbol.function == np.sin: return casadi.sin(*converted_children) elif symbol.function == np.arcsinh: return casadi.arcsinh(*converted_children) elif symbol.function == np.arccosh: return casadi.arccosh(*converted_children) elif symbol.function == np.tanh: return casadi.tanh(*converted_children) elif symbol.function == np.cosh: return casadi.cosh(*converted_children) elif symbol.function == np.sinh: return casadi.sinh(*converted_children) elif symbol.function == np.cos: return casadi.cos(*converted_children) elif symbol.function == np.exp: return casadi.exp(*converted_children) elif symbol.function == np.log: return casadi.log(*converted_children) elif symbol.function == np.sign: return casadi.sign(*converted_children) elif symbol.function == special.erf: return casadi.erf(*converted_children) elif isinstance(symbol, pybamm.Interpolant): return casadi.interpolant( "LUT", "bspline", symbol.x, symbol.y.flatten())(*converted_children) elif symbol.function.__name__.startswith("elementwise_grad_of_"): differentiating_child_idx = int(symbol.function.__name__[-1]) # Create dummy symbolic variables in order to differentiate using CasADi dummy_vars = [ casadi.MX.sym("y_" + str(i)) for i in range(len(converted_children)) ] func_diff = casadi.gradient( symbol.differentiated_function(*dummy_vars), dummy_vars[differentiating_child_idx], ) # Create function and evaluate it using the children casadi_func_diff = casadi.Function("func_diff", dummy_vars, [func_diff]) return casadi_func_diff(*converted_children) # Other functions else: return symbol._function_evaluate(converted_children) elif isinstance(symbol, pybamm.Concatenation): converted_children = [ self.convert(child, t, y, y_dot, inputs) for child in symbol.children ] if isinstance(symbol, (pybamm.NumpyConcatenation, pybamm.SparseStack)): return casadi.vertcat(*converted_children) # DomainConcatenation specifies a particular ordering for the concatenation, # which we must follow elif isinstance(symbol, pybamm.DomainConcatenation): slice_starts = [] all_child_vectors = [] for i in range(symbol.secondary_dimensions_npts): child_vectors = [] for child_var, slices in zip(converted_children, symbol._children_slices): for child_dom, child_slice in slices.items(): slice_starts.append( symbol._slices[child_dom][i].start) child_vectors.append( child_var[child_slice[i].start:child_slice[i]. stop]) all_child_vectors.extend([ v for _, v in sorted(zip(slice_starts, child_vectors)) ]) return casadi.vertcat(*all_child_vectors) else: raise TypeError(""" Cannot convert symbol of type '{}' to CasADi. Symbols must all be 'linear algebra' at this stage. """.format(type(symbol)))
def MinCurvature(): track = run_map_gen() txs = track[:, 0] tys = track[:, 1] nvecs = track[:, 2:4] th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))] n_max = 3 N = len(track) n_f_a = ca.MX.sym('n_f', N) n_f = ca.MX.sym('n_f', N - 1) th_f = ca.MX.sym('n_f', N - 1) x0_f = ca.MX.sym('x0_f', N - 1) x1_f = ca.MX.sym('x1_f', N - 1) y0_f = ca.MX.sym('y0_f', N - 1) y1_f = ca.MX.sym('y1_f', N - 1) th1_f = ca.MX.sym('y1_f', N - 1) th2_f = ca.MX.sym('y1_f', N - 1) th1_f1 = ca.MX.sym('y1_f', N - 2) th2_f1 = ca.MX.sym('y1_f', N - 2) o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f]) o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f]) o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f]) o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f]) dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f], [ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)]) track_length = ca.Function('length', [n_f_a], [ dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]), o_y_e(n_f_a[1:])) ]) real = ca.Function( 'real', [th1_f, th2_f], [ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)]) im = ca.Function( 'im', [th1_f, th2_f], [-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)]) sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f], [ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))]) get_th_n = ca.Function( 'gth', [th_f], [sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))]) d_n = ca.Function('d_n', [n_f_a, th_f], [track_length(n_f_a) / ca.tan(get_th_n(th_f))]) # define symbols n = ca.MX.sym('n', N) th = ca.MX.sym('th', N) nlp = {\ 'x': ca.vertcat(n, th), 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])), 'g': ca.vertcat( # dynamic constraints n[1:] - (n[:-1] + d_n(n, th[:-1])), # boundary constraints n[0], th[0], n[-1], #th[-1], ) \ } S = ca.nlpsol('S', 'ipopt', nlp) ones = np.ones(N) n0 = ones * 0 th0 = [] for i in range(N - 1): th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2]) th0.append(th_00) th0.append(0) th0 = np.array(th0) x0 = ca.vertcat(n0, th0) lbx = [-n_max] * N + [-np.pi] * N ubx = [n_max] * N + [np.pi] * N r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) x_opt = r['x'] n_set = np.array(x_opt[:N]) thetas = np.array(x_opt[1 * N:2 * N]) plot_race_line(np.array(track), n_set, wait=True) return n_set
nx = sol['sys']['vars']['x'].shape[0] nu = sol['sys']['vars']['u'].shape[0] ns = sol['sys']['vars']['us'].shape[0] # set-up open-loop scenario Nmpc = 20 alpha_steps = 20 # tether length l_t = np.sqrt(sol['wsol']['x', 0][0]**2 + sol['wsol']['x', 0][1]**2 + sol['wsol']['x', 0][2]**2) opts = {} # add projection operator for terminal constraint opts['p_operator'] = ca.Function( 'p_operator', [sol['sys']['vars']['x']], [ct.vertcat(sol['sys']['vars']['x'][1:3], sol['sys']['vars']['x'][4:])]) # add MPC slacks to active constraints mpc_sys = preprocessing.add_mpc_slacks(sol['sys'], sol['lam_g'], sol['indeces_As'], slack_flag='active') # create controllers ctrls = {} # # economic MPC ctrls['EMPC'] = pmpc.Pmpc(N=Nmpc, sys=mpc_sys, cost=user_input['l'], wref=sol['wsol'],
def pre(self): # Call parent class first for default behaviour. super().pre() # Get curve fitting options from curvefit_options.ini file ini_path = os.path.join( self.__lookup_table_folder, 'curvefit_options.ini') try: ini_config = configparser.RawConfigParser() ini_config.readfp(open(ini_path)) no_curvefit_options = False except IOError: logger.info( "CSVLookupTableMixin: No curvefit_options.ini file found. Using default values.") no_curvefit_options = True def get_curvefit_options(curve_name, no_curvefit_options=no_curvefit_options): if no_curvefit_options: return 0, 0, 0 curvefit_options = [] def get_property(prop_name): try: prop = int(ini_config.get(curve_name, prop_name)) except configparser.NoSectionError: prop = 0 except configparser.NoOptionError: prop = 0 except ValueError: raise Exception( 'CSVLookupTableMixin: Invalid {0} constraint for {1}. {0} should ' 'be either -1, 0, or 1.'.format(prop_name, curve_name)) return prop for prop_name in ['monotonicity', 'monotonicity2', 'curvature']: curvefit_options.append(get_property(prop_name)) logger.debug("CSVLookupTableMixin: Curve fit option for {}:({},{},{})".format( curve_name, *curvefit_options)) return tuple(curvefit_options) # Read CSV files logger.info( "CSVLookupTableMixin: Generating Splines from lookup table data.") self.__lookup_tables = {} for filename in glob.glob(os.path.join(self.__lookup_table_folder, "*.csv")): logger.debug( "CSVLookupTableMixin: Reading lookup table from {}".format(filename)) csvinput = csv.load(filename, delimiter=self.csv_delimiter) output = csvinput.dtype.names[0] inputs = csvinput.dtype.names[1:] # Get monotonicity and curvature from ini file mono, mono2, curv = get_curvefit_options(output) logger.debug( "CSVLookupTableMixin: Output is {}, inputs are {}.".format(output, inputs)) tck = None # If tck file is newer than the csv file, first try to load the cached values from the tck file tck_filename = filename.replace('.csv', '.tck') valid_cache = False if os.path.exists(tck_filename): if no_curvefit_options: valid_cache = os.path.getmtime(filename) < os.path.getmtime(tck_filename) else: valid_cache = (os.path.getmtime(filename) < os.path.getmtime(tck_filename)) and \ (os.path.getmtime(ini_path) < os.path.getmtime(tck_filename)) if valid_cache: logger.debug( 'CSVLookupTableMixin: Attempting to use cached tck values for {}'.format(output)) with open(tck_filename, 'rb') as f: try: tck = pickle.load(f) except Exception: valid_cache = False if not valid_cache: logger.info( 'CSVLookupTableMixin: Recalculating tck values for {}'.format(output)) if len(csvinput.dtype.names) == 2: if not valid_cache: k = 3 # default value # 1D spline fitting needs k+1 data points if len(csvinput[output]) >= k + 1: tck = BSpline1D.fit(csvinput[inputs[0]], csvinput[ output], k=k, monotonicity=mono, curvature=curv) else: raise Exception( 'CSVLookupTableMixin: Too few data points in {} to do spline fitting. ' 'Need at least {} points.'.format(filename, k + 1)) if self.csv_lookup_table_debug: import pylab i = np.linspace(csvinput[inputs[0]][0], csvinput[ inputs[0]][-1], self.csv_lookup_table_debug_points) o = splev(i, tck) pylab.clf() # TODO: Figure out why cross-section B0607 in NZV does not # conform to constraints! pylab.plot(i, o) pylab.plot(csvinput[inputs[0]], csvinput[ output], linestyle='', marker='x', markersize=10) figure_filename = filename.replace('.csv', '.png') pylab.savefig(figure_filename) symbols = [ca.SX.sym(inputs[0])] function = ca.Function('f', symbols, [BSpline1D(*tck)(symbols[0])]) self.__lookup_tables[output] = LookupTable(symbols, function, tck) elif len(csvinput.dtype.names) == 3: if tck is None: kx = 3 # default value ky = 3 # default value # 2D spline fitting needs (kx+1)*(ky+1) data points if len(csvinput[output]) >= (kx + 1) * (ky + 1): # TODO: add curvature parameters from curvefit_options.ini # once 2d fit is implemented tck = bisplrep(csvinput[inputs[0]], csvinput[ inputs[1]], csvinput[output], kx=kx, ky=ky) else: raise Exception( 'CSVLookupTableMixin: Too few data points in {} to do spline fitting. ' 'Need at least {} points.'.format(filename, (kx + 1) * (ky + 1))) if self.csv_lookup_table_debug: import pylab i1 = np.linspace(csvinput[inputs[0]][0], csvinput[ inputs[0]][-1], self.csv_lookup_table_debug_points) i2 = np.linspace(csvinput[inputs[1]][0], csvinput[ inputs[1]][-1], self.csv_lookup_table_debug_points) i1, i2 = np.meshgrid(i1, i2) i1 = i1.flatten() i2 = i2.flatten() o = bisplev(i1, i2, tck) pylab.clf() pylab.plot_surface(i1, i2, o) figure_filename = filename.replace('.csv', '.png') pylab.savefig(figure_filename) symbols = [ca.SX.sym(inputs[0]), ca.SX.sym(inputs[1])] function = ca.Function('f', symbols, [BSpline2D(*tck)(symbols[0], symbols[1])]) self.__lookup_tables[output] = LookupTable(symbols, function, tck) else: raise Exception( 'CSVLookupTableMixin: {}-dimensional lookup tables not implemented yet.'.format( len(csvinput.dtype.names))) if not valid_cache: pickle.dump(tck, open(filename.replace('.csv', '.tck'), 'wb'), protocol=pickle.HIGHEST_PROTOCOL)
def MinCurvatureTrajectory(track, obs_map=None): w_min = -track[:, 4] * 0.9 w_max = track[:, 5] * 0.9 nvecs = track[:, 2:4] th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))] xgrid = np.arange(0, obs_map.shape[1]) ygrid = np.arange(0, obs_map.shape[0]) data_flat = np.array(obs_map).ravel(order='F') lut = ca.interpolant('lut', 'bspline', [xgrid, ygrid], data_flat) print(lut([10, 20])) n_max = 3 N = len(track) n_f_a = ca.MX.sym('n_f', N) n_f = ca.MX.sym('n_f', N - 1) th_f = ca.MX.sym('n_f', N - 1) x0_f = ca.MX.sym('x0_f', N - 1) x1_f = ca.MX.sym('x1_f', N - 1) y0_f = ca.MX.sym('y0_f', N - 1) y1_f = ca.MX.sym('y1_f', N - 1) th1_f = ca.MX.sym('y1_f', N - 1) th2_f = ca.MX.sym('y1_f', N - 1) th1_f1 = ca.MX.sym('y1_f', N - 2) th2_f1 = ca.MX.sym('y1_f', N - 2) o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f]) o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f]) o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f]) o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f]) dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f], [ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)]) track_length = ca.Function('length', [n_f_a], [ dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]), o_y_e(n_f_a[1:])) ]) real = ca.Function( 'real', [th1_f, th2_f], [ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)]) im = ca.Function( 'im', [th1_f, th2_f], [-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)]) sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f], [ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))]) get_th_n = ca.Function( 'gth', [th_f], [sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))]) d_n = ca.Function('d_n', [n_f_a, th_f], [track_length(n_f_a) / ca.tan(get_th_n(th_f))]) # objective real1 = ca.Function( 'real1', [th1_f1, th2_f1], [ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)]) im1 = ca.Function( 'im1', [th1_f1, th2_f1], [-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)]) sub_cmplx1 = ca.Function( 'a_cpx1', [th1_f1, th2_f1], [ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))]) # define symbols n = ca.MX.sym('n', N) th = ca.MX.sym('th', N - 1) nlp = {\ 'x': ca.vertcat(n, th), 'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])), # 'f': ca.sumsqr(track_length(n)), 'g': ca.vertcat( # dynamic constraints n[1:] - (n[:-1] + d_n(n, th)), # lut(ca.horzcat(o_x_s(n[:-1]), o_y_s(n[:-1])).T).T, # boundary constraints n[0], #th[0], n[-1], #th[-1], ) \ } S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}}) # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}}) ones = np.ones(N) n0 = ones * 0 th0 = [] for i in range(N - 1): th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2]) th0.append(th_00) th0 = np.array(th0) x0 = ca.vertcat(n0, th0) # lbx = [-n_max] * N + [-np.pi]*(N-1) # ubx = [n_max] * N + [np.pi]*(N-1) lbx = list(w_min) + [-np.pi] * (N - 1) ubx = list(w_max) + [np.pi] * (N - 1) r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) x_opt = r['x'] n_set = np.array(x_opt[:N]) thetas = np.array(x_opt[1 * N:2 * (N - 1)]) # lib.plot_race_line(np.array(track), n_set, wait=True) return n_set
# Define CasADi symbolic variables. x = casadi.SX.sym("x", Nx) u = casadi.SX.sym("u", Nu) # Make integrator object. ode_integrator = dict(x=x, p=u, ode=ode(x, u)) intoptions = { "abstol": 1e-8, "reltol": 1e-8, "tf": Delta, } vdp = casadi.integrator("int_ode", "cvodes", ode_integrator, intoptions) # Then get nonlinear casadi functions # and RK4 discretization. ode_casadi = casadi.Function("ode", [x, u], [ode(x, u)]) k1 = ode_casadi(x, u) k2 = ode_casadi(x + Delta / 2 * k1, u) k3 = ode_casadi(x + Delta / 2 * k2, u) k4 = ode_casadi(x + Delta * k3, u) xrk4 = x + Delta / 6 * (k1 + 2 * k2 + 2 * k3 + k4) ode_rk4_casadi = casadi.Function("ode_rk4", [x, u], [xrk4]) # Define stage cost and terminal weight. lfunc = (casadi.mtimes(x.T, x) + casadi.mtimes(u.T, u)) l = casadi.Function("l", [x, u], [lfunc]) Pffunc = casadi.mtimes(x.T, x) Pf = casadi.Function("Pf", [x], [Pffunc])
# x[4] ,\ # x[5] ,\ # -1/p[0]*(u[0] + u[1])*casadi.sin(x[2]) ,\ # 1/p[0]*(u[0] + u[1])*casadi.cos(x[2]) - p[3],\ # p[2]/p[1]*(u[0] - u[1]) # ) ode = casadi.vertcat( x[3] ,\ x[4] ,\ x[5] ,\ -1/m*(u[0] + u[1])*casadi.sin(x[2]) ,\ 1/m*(u[0] + u[1])*casadi.cos(x[2]) - g,\ l/I*(u[0] - u[1]) ) f = casadi.Function('f', [x,u], [ode], ['x','u'], ['ode']) # === create integrator expression === intg_options = dict(tf = dt) # define integration step dae = dict(x = x, p = u, ode = f(x,u)) # define the dae to integrate (ZOH) # define the integrator intg = casadi.integrator('intg', 'rk', dae, intg_options) res = intg(x0 = x, p = u) # define symbolic expression of integrator x_next = res['xf'] # define discretized ode F = casadi.Function('F', [x, u], [x_next], ['x', 'u'], ['x_next']) # === define OCP === '''
def _get_diff_eq(self, cost_func): '''Function that returns the RhS of the differential equations. See the papers for additional info''' RHS = [] # RHS that's in common for all cases rhs1 = self.q_dot rhs2 = self.M_inv @ (-self.Cq - self.G - self.Fd @ self.q_dot - self.Ff @ cs.sign(self.q_dot)) # Adjusting RHS for SEA with known inertia. Check the paper for more info. if self.sea and self.SEAdynamics: rhs2 -= self.M_inv @ self.tau_sea rhs3 = self.theta_dot rhs4 = cs.pinv(self.B) @ (-self.FDsea @ self.theta_dot + self.u + self.tau_sea - self.FMusea @ cs.sign(self.theta_dot)) RHS = [rhs1, rhs2, rhs3, rhs4] # Adjusting the lenght of the variables self.x = cs.vertcat(self.q, self.q_dot, self.theta, self.theta_dot) self.num_state_var = self.num_joints * 2 self.lower_q = self.lower_q + self.lower_theta self.lower_qd = self.lower_qd + self.lower_thetad self.upper_q = self.upper_q + self.upper_theta self.upper_qd = self.upper_qd + self.upper_thetad # Adjusting RHS for SEA modeling, with motor inertia unknown elif self.sea and not self.SEAdynamics: rhs2 += -self.M_inv @ self.K @ (self.q - self.u) # self.upper_u, self.lower_u = self.upper_q, self.lower_q RHS = [rhs1, rhs2] # State variable self.x = cs.vertcat(self.q, self.q_dot) self.num_state_var = self.num_joints # Adjusting RHS for classic robot modeling, without any SEA else: rhs2 += self.M_inv @ self.u RHS = [rhs1, rhs2] # State variable self.x = cs.vertcat(self.q, self.q_dot) self.num_state_var = self.num_joints # Evaluating q_ddot, in order to handle it when given as an input of cost function or constraints self.q_ddot_val = self._get_joints_accelerations(rhs2) # The differentiation of J will be the cost function given by the user, with our symbolic # variables as inputs if self.sea and self.SEAdynamics: q_ddot_J = self.q_ddot_val(self.q, self.q_dot, self.theta, self.theta_dot, self.u) else: q_ddot_J = self.q_ddot_val(self.q,self.q_dot, self.u) if self.sea and not self.SEAdynamics: u_J = self.u - self.q # the instantaneous spent energy is prop to |u-q| else: u_J = self.u J_dot = cost_func( self.q - self.traj, self.q_dot - self.traj_dot, q_ddot_J, self.ee_pos(self.q), u_J, self.t ) # Setting the relationship self.x_dot = cs.vertcat(*RHS) # Defining the differential equation f = cs.Function('f', [self.x, self.u, self.t], # inputs [self.x_dot, J_dot]) # outputs return f
def opt_mintime(reftrack: np.ndarray, coeffs_x: np.ndarray, coeffs_y: np.ndarray, normvectors: np.ndarray, pars: dict, tpamap_path: str, tpadata_path: str, export_path: str, print_debug: bool = False, plot_debug: bool = False) -> tuple: """ Created by: Fabian Christ Documentation: The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and nonlinear tire model. Please refer to our paper for further information: Christ, Wischnewski, Heilmeier, Lohmann Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients Inputs: reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m] coeffs_x: coefficient matrix of the x splines with size (no_splines x 4) coeffs_y: coefficient matrix of the y splines with size (no_splines x 4) normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component] pars: parameters dictionary tpamap_path: file path to tpa map (required for friction map loading) tpadata_path: file path to tpa data (required for friction map loading) export_path: path to output folder for warm start files and solution files print_debug: determines if debug messages are printed plot_debug: determines if debug plots are shown Outputs: alpha_opt: solution vector of the optimization problem containing the lateral shift in m for every point v_opt: velocity profile for the raceline """ # ------------------------------------------------------------------------------------------------------------------ # PREPARE TRACK INFORMATION ---------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # spline lengths spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(coeffs_x=coeffs_x, coeffs_y=coeffs_y) # calculate heading and curvature (numerically) kappa_refline = tph.calc_head_curv_num. \ calc_head_curv_num(path=reftrack[:, :2], el_lengths=spline_lengths_refline, is_closed=True, stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"], stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"], stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"], stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1] # close track kappa_refline_cl = np.append(kappa_refline, kappa_refline[0]) w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3]) w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2]) # step size along the reference line h = pars["stepsize_opts"]["stepsize_reg"] # optimization steps (0, 1, 2 ... end point/start point) steps = [i for i in range(kappa_refline_cl.size)] # number of control intervals N = steps[-1] # station along the reference line s_opt = np.linspace(0.0, N * h, N + 1) # interpolate curvature of reference line in terms of steps kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps], kappa_refline_cl) # interpolate track width (left and right to reference line) in terms of steps w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps], w_tr_left_cl) w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps], w_tr_right_cl) # describe friction coefficients from friction map with linear equations or gaussian basis functions if pars["optim_opts"]["var_friction"] is not None: w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \ approx_friction_map.approx_friction_map(reftrack=reftrack, normvectors=normvectors, tpamap_path=tpamap_path, tpadata_path=tpadata_path, pars=pars, dn=pars["optim_opts"]["dn"], n_gauss=pars["optim_opts"]["n_gauss"], print_debug=print_debug, plot_debug=plot_debug) # ------------------------------------------------------------------------------------------------------------------ # DIRECT GAUSS-LEGENDRE COLLOCATION -------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # degree of interpolating polynomial d = 3 # legendre collocation points tau = np.append(0, ca.collocation_points(d, 'legendre')) # coefficient matrix for formulating the collocation equation C = np.zeros((d + 1, d + 1)) # coefficient matrix for formulating the collocation equation D = np.zeros(d + 1) # coefficient matrix for formulating the collocation equation B = np.zeros(d + 1) # construct polynomial basis for j in range(d + 1): # construct Lagrange polynomials to get the polynomial basis at the collocation point p = np.poly1d([1]) for r in range(d + 1): if r != j: p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r]) # evaluate polynomial at the final time to get the coefficients of the continuity equation D[j] = p(1.0) # evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation p_der = np.polyder(p) for r in range(d + 1): C[j, r] = p_der(tau[r]) # evaluate integral of the polynomial to get the coefficients of the quadrature function pint = np.polyint(p) B[j] = pint(1.0) # ------------------------------------------------------------------------------------------------------------------ # STATE VARIABLES -------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # number of state variables nx = 5 # velocity [m/s] v_n = ca.SX.sym('v_n') v_s = 50 v = v_s * v_n # side slip angle [rad] beta_n = ca.SX.sym('beta_n') beta_s = 0.5 beta = beta_s * beta_n # yaw rate [rad/s] omega_z_n = ca.SX.sym('omega_z_n') omega_z_s = 1 omega_z = omega_z_s * omega_z_n # lateral distance to reference line (positive = left) [m] n_n = ca.SX.sym('n_n') n_s = 5.0 n = n_s * n_n # relative angle to tangent on reference line [rad] xi_n = ca.SX.sym('xi_n') xi_s = 1.0 xi = xi_s * xi_n # scaling factors for state variables x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s]) # put all states together x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n) # ------------------------------------------------------------------------------------------------------------------ # CONTROL VARIABLES ------------------------------------------------------------------------------------------------ # ------------------------------------------------------------------------------------------------------------------ # number of control variables nu = 4 # steer angle [rad] delta_n = ca.SX.sym('delta_n') delta_s = 0.5 delta = delta_s * delta_n # positive longitudinal force (drive) [N] f_drive_n = ca.SX.sym('f_drive_n') f_drive_s = 7500.0 f_drive = f_drive_s * f_drive_n # negative longitudinal force (brake) [N] f_brake_n = ca.SX.sym('f_brake_n') f_brake_s = 20000.0 f_brake = f_brake_s * f_brake_n # lateral wheel load transfer [N] gamma_y_n = ca.SX.sym('gamma_y_n') gamma_y_s = 5000.0 gamma_y = gamma_y_s * gamma_y_n # scaling factors for control variables u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s]) # put all controls together u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n) # ------------------------------------------------------------------------------------------------------------------ # MODEL EQUATIONS -------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # extract vehicle and tire parameters veh = pars["vehicle_params_mintime"] tire = pars["tire_params_mintime"] # general constants g = pars["veh_params"]["g"] mass = pars["veh_params"]["mass"] # curvature of reference line [rad/m] kappa = ca.SX.sym('kappa') # drag force [N] f_xdrag = pars["veh_params"]["dragcoeff"] * v ** 2 # rolling resistance forces [N] f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"] f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"] f_xroll = tire["c_roll"] * mass * g # static normal tire forces [N] f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"] f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"] f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"] # dynamic normal tire forces (aerodynamic downforces) [N] f_zlift_fl = 0.5 * veh["liftcoeff_front"] * v ** 2 f_zlift_fr = 0.5 * veh["liftcoeff_front"] * v ** 2 f_zlift_rl = 0.5 * veh["liftcoeff_rear"] * v ** 2 f_zlift_rr = 0.5 * veh["liftcoeff_rear"] * v ** 2 # dynamic normal tire forces (load transfers) [N] f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) - veh["k_roll"] * gamma_y) f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) + veh["k_roll"] * gamma_y) f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) - (1.0 - veh["k_roll"]) * gamma_y) f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll) + (1.0 - veh["k_roll"]) * gamma_y) # sum of all normal tire forces [N] f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr # slip angles [rad] alpha_fl = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) / (v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z)) alpha_fr = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) / (v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z)) alpha_rl = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) / (v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z)) alpha_rr = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) / (v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z)) # lateral tire forces [N] f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl * (1 + tire["eps_front"] * f_z_fl / tire["f_z0"]) * ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fl - tire["E_front"] * (tire["B_front"] * alpha_fl - ca.atan(tire["B_front"] * alpha_fl))))) f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr * (1 + tire["eps_front"] * f_z_fr / tire["f_z0"]) * ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fr - tire["E_front"] * (tire["B_front"] * alpha_fr - ca.atan(tire["B_front"] * alpha_fr))))) f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl * (1 + tire["eps_rear"] * f_z_rl / tire["f_z0"]) * ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"] * (tire["B_rear"] * alpha_rl - ca.atan(tire["B_rear"] * alpha_rl))))) f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr * (1 + tire["eps_rear"] * f_z_rr / tire["f_z0"]) * ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"] * (tire["B_rear"] * alpha_rr - ca.atan(tire["B_rear"] * alpha_rr))))) # longitudinal tire forces [N] f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fl f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fr f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rl f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rr # longitudinal acceleration [m/s²] ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) - (f_y_fl + f_y_fr) * ca.sin(delta) - pars["veh_params"]["dragcoeff"] * v ** 2) / mass # lateral acceleration [m/s²] ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr + (f_y_fl + f_y_fr) * ca.cos(delta)) / mass # time-distance scaling factor (dt/ds) sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta))) # model equations for two track model (ordinary differential equations) dv = (sf / mass) * ((f_x_rl + f_x_rr) * ca.cos(beta) + (f_x_fl + f_x_fr) * ca.cos(delta - beta) + (f_y_rl + f_y_rr) * ca.sin(beta) - (f_y_fl + f_y_fr) * ca.sin(delta - beta) - f_xdrag * ca.cos(beta)) dbeta = sf * (-omega_z + (-(f_x_rl + f_x_rr) * ca.sin(beta) + (f_x_fl + f_x_fr) * ca.sin(delta - beta) + (f_y_rl + f_y_rr) * ca.cos(beta) + (f_y_fl + f_y_fr) * ca.cos(delta - beta) + f_xdrag * ca.sin(beta)) / (mass * v)) domega_z = (sf / veh["I_z"]) * ((f_x_rr - f_x_rl) * veh["track_width_rear"] / 2 - (f_y_rl + f_y_rr) * veh["wheelbase_rear"] + ((f_x_fr - f_x_fl) * ca.cos(delta) + (f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2 + ((f_y_fl + f_y_fr) * ca.cos(delta) + (f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"]) dn = sf * v * ca.sin(xi + beta) dxi = sf * omega_z - kappa # put all ODEs together dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s # ------------------------------------------------------------------------------------------------------------------ # CONTROL BOUNDARIES ----------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ delta_min = -veh["delta_max"] / delta_s # min. steer angle [rad] delta_max = veh["delta_max"] / delta_s # max. steer angle [rad] f_drive_min = 0.0 # min. longitudinal drive force [N] f_drive_max = veh["f_drive_max"] / f_drive_s # max. longitudinal drive force [N] f_brake_min = -veh["f_brake_max"] / f_brake_s # min. longitudinal brake force [N] f_brake_max = 0.0 # max. longitudinal brake force [N] gamma_y_min = -np.inf # min. lateral wheel load transfer [N] gamma_y_max = np.inf # max. lateral wheel load transfer [N] # ------------------------------------------------------------------------------------------------------------------ # STATE BOUNDARIES ------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ v_min = 1.0 / v_s # min. velocity [m/s] v_max = pars["veh_params"]["v_max"] / v_s # max. velocity [m/s] beta_min = -0.5 * np.pi / beta_s # min. side slip angle [rad] beta_max = 0.5 * np.pi / beta_s # max. side slip angle [rad] omega_z_min = - 0.5 * np.pi / omega_z_s # min. yaw rate [rad/s] omega_z_max = 0.5 * np.pi / omega_z_s # max. yaw rate [rad/s] xi_min = - 0.5 * np.pi / xi_s # min. relative angle to tangent on reference line [rad] xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad] # ------------------------------------------------------------------------------------------------------------------ # INITIAL GUESS FOR DECISION VARIABLES ----------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ v_guess = 20.0 / v_s # ------------------------------------------------------------------------------------------------------------------ # HELPER FUNCTIONS ------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # continuous time dynamics f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'], ['dx', 'sf']) # longitudinal tire forces [N] f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr], ['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr']) # lateral tire forces [N] f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr], ['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr']) # vertical tire forces [N] f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr], ['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr']) # longitudinal and lateral acceleration [m/s²] f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay']) # ------------------------------------------------------------------------------------------------------------------ # FORMULATE NONLINEAR PROGRAM -------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # initialize NLP vectors w = [] w0 = [] lbw = [] ubw = [] J = 0 g = [] lbg = [] ubg = [] # initialize ouput vectors x_opt = [] u_opt = [] dt_opt = [] tf_opt = [] ax_opt = [] ay_opt = [] ec_opt = [] # initialize control vectors (for regularization) delta_p = [] F_p = [] # boundary constraint: lift initial conditions Xk = ca.MX.sym('X0', nx) w.append(Xk) n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min]) ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max]) w0.append([v_guess, 0.0, 0.0, 0.0, 0.0]) x_opt.append(Xk * x_s) # loop along the racetrack and formulate path constraints & system dynamic for k in range(N): # add decision variables for the control Uk = ca.MX.sym('U_' + str(k), nu) w.append(Uk) lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min]) ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max]) w0.append([0.0] * nu) # add decision variables for the state at collocation points Xc = [] for j in range(d): Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx) Xc.append(Xkj) w.append(Xkj) lbw.append([-np.inf] * nx) ubw.append([np.inf] * nx) w0.append([v_guess, 0.0, 0.0, 0.0, 0.0]) # loop over all collocation points Xk_end = D[0] * Xk sf_opt = [] for j in range(1, d + 1): # calculate the state derivative at the collocation point xp = C[0, j] * Xk for r in range(d): xp = xp + C[r + 1, j] * Xc[r] # interpolate kappa at the collocation point kappa_col = kappa_interp(k + tau[j]) # append collocation equations (system dynamic) fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col) g.append(h * fj - xp) lbg.append([0.0] * nx) ubg.append([0.0] * nx) # add contribution to the end state Xk_end = Xk_end + D[j] * Xc[j - 1] # add contribution to quadrature function J = J + B[j] * qj * h # add contribution to scaling factor (for calculating lap time) sf_opt.append(B[j] * qj * h) # calculate used energy dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2]) ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1]) # add new decision variables for state at end of the collocation interval Xk = ca.MX.sym('X_' + str(k + 1), nx) w.append(Xk) n_min = (-w_tr_right_interp(k + 1) + pars["optim_opts"]["width_opt"] / 2.0) / n_s n_max = (w_tr_left_interp(k + 1) - pars["optim_opts"]["width_opt"] / 2.0) / n_s lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min]) ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max]) w0.append([v_guess, 0.0, 0.0, 0.0, 0.0]) # add equality constraint g.append(Xk_end - Xk) lbg.append([0.0] * nx) ubg.append([0.0] * nx) # get tire forces f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk) f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk) f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk) # get accelerations (longitudinal + lateral) axk, ayk = f_a(Xk, Uk) # path constraint: limitied engine power g.append(Xk[0] * Uk[1]) lbg.append([-np.inf]) ubg.append([veh["power_max"] / (f_drive_s * v_s)]) # get constant friction coefficient if pars["optim_opts"]["var_friction"] is None: mue_fl = pars["optim_opts"]["mue"] mue_fr = pars["optim_opts"]["mue"] mue_rl = pars["optim_opts"]["mue"] mue_rr = pars["optim_opts"]["mue"] # calculate variable friction coefficients along the reference line (regression with linear equations) elif pars["optim_opts"]["var_friction"] == "linear": # friction coefficient for each tire mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1] mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1] mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1] mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1] # calculate variable friction coefficients along the reference line (regression with gaussian basis functions) elif pars["optim_opts"]["var_friction"] == "gauss": # gaussian basis functions sigma = 2.0 * center_dist[k + 1, 0] n_gauss = pars["optim_opts"]["n_gauss"] n_q = np.linspace(-n_gauss, n_gauss, 2 * n_gauss + 1) * center_dist[k + 1, 0] gauss_basis = [] for i in range(2 * n_gauss + 1): gauss_basis.append(ca.exp(-(Xk[3] * n_s - n_q[i]) ** 2 / (2 * (sigma ** 2)))) gauss_basis = ca.vertcat(*gauss_basis) mue_fl = ca.dot(w_mue_fl[k + 1, :-1], gauss_basis) + w_mue_fl[k + 1, -1] mue_fr = ca.dot(w_mue_fr[k + 1, :-1], gauss_basis) + w_mue_fr[k + 1, -1] mue_rl = ca.dot(w_mue_rl[k + 1, :-1], gauss_basis) + w_mue_rl[k + 1, -1] mue_rr = ca.dot(w_mue_rr[k + 1, :-1], gauss_basis) + w_mue_rr[k + 1, -1] else: raise ValueError("No friction coefficients are available!") # path constraint: Kamm's Circle for each wheel g.append(((f_x_flk / (mue_fl * f_z_flk)) ** 2 + (f_y_flk / (mue_fl * f_z_flk)) ** 2)) g.append(((f_x_frk / (mue_fr * f_z_frk)) ** 2 + (f_y_frk / (mue_fr * f_z_frk)) ** 2)) g.append(((f_x_rlk / (mue_rl * f_z_rlk)) ** 2 + (f_y_rlk / (mue_rl * f_z_rlk)) ** 2)) g.append(((f_x_rrk / (mue_rr * f_z_rrk)) ** 2 + (f_y_rrk / (mue_rr * f_z_rrk)) ** 2)) lbg.append([0.0] * 4) ubg.append([1.0] * 4) # path constraint: lateral wheel load transfer g.append(((f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk + (f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s)) * veh["cog_z"] / ((veh["track_width_front"] + veh["track_width_rear"]) / 2) - Uk[3] * gamma_y_s) lbg.append([0.0]) ubg.append([0.0]) # path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal) g.append(Uk[1] * Uk[2]) lbg.append([-20000.0 / (f_drive_s * f_brake_s)]) ubg.append([0.0]) # path constraint: actor dynamic if k > 0: sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s) g.append((Uk - w[1 + (k - 1) * nx]) / (pars["stepsize_opts"]["stepsize_reg"] * sigma)) lbg.append([delta_min / (veh["t_delta"]), -np.inf, f_brake_min / (veh["t_brake"]), -np.inf]) ubg.append([delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]), np.inf, np.inf]) # path constraint: safe trajectories with acceleration ellipse if pars["optim_opts"]["safe_traj"]: g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"]) ** 2 + (ayk / pars["optim_opts"]["ay_safe"]) ** 2) g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"]) ** 2 + (ayk / pars["optim_opts"]["ay_safe"]) ** 2) lbg.append([0.0] * 2) ubg.append([1.0] * 2) # append controls (for regularization) delta_p.append(Uk[0] * delta_s) F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0) # append outputs x_opt.append(Xk * x_s) u_opt.append(Uk * u_s) tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk]) tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk]) ax_opt.append(axk) ay_opt.append(ayk) # boundary constraint: start states = final states g.append(w[0] - Xk) lbg.append([0.0] * nx) ubg.append([0.0] * nx) # path constraint: limited energy consumption if pars["optim_opts"]["limit_energy"]: g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0) lbg.append([0]) ubg.append([pars["optim_opts"]["energy_limit"]]) # formulate differentiation matrix (for regularization) diff_matrix = np.eye(N) for i in range(N - 1): diff_matrix[i, i + 1] = -1.0 diff_matrix[N - 1, 0] = -1.0 # regularization (delta) delta_p = ca.vertcat(*delta_p) Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p) Jp_delta = ca.dot(Jp_delta, Jp_delta) # regularization (f_drive + f_brake) F_p = ca.vertcat(*F_p) Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p) Jp_f = ca.dot(Jp_f, Jp_f) # formulate objective J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"]["penalty_delta"] * Jp_delta # concatenate NLP vectors w = ca.vertcat(*w) g = ca.vertcat(*g) w0 = np.concatenate(w0) lbw = np.concatenate(lbw) ubw = np.concatenate(ubw) lbg = np.concatenate(lbg) ubg = np.concatenate(ubg) # concatenate output vectors x_opt = ca.vertcat(*x_opt) u_opt = ca.vertcat(*u_opt) tf_opt = ca.vertcat(*tf_opt) dt_opt = ca.vertcat(*dt_opt) ax_opt = ca.vertcat(*ax_opt) ay_opt = ca.vertcat(*ay_opt) ec_opt = ca.vertcat(*ec_opt) # ------------------------------------------------------------------------------------------------------------------ # CREATE NLP SOLVER ------------------------------------------------------------------------------------------------ # ------------------------------------------------------------------------------------------------------------------ # fill nlp structure nlp = {'f': J, 'x': w, 'g': g} # solver options opts = {"expand": True, "verbose": print_debug, "ipopt.max_iter": 2000, "ipopt.tol": 1e-7} # solver options for warm start if pars["optim_opts"]["warm_start"]: opts_warm_start = {"ipopt.warm_start_init_point": "yes", "ipopt.warm_start_bound_push": 1e-9, "ipopt.warm_start_mult_bound_push": 1e-9, "ipopt.warm_start_slack_bound_push": 1e-9, "ipopt.mu_init": 1e-9} opts.update(opts_warm_start) # load warm start files if pars["optim_opts"]["warm_start"]: try: w0 = np.loadtxt(os.path.join(export_path, 'w0.csv')) lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv')) lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv')) except IOError: print('\033[91m' + 'WARNING: Failed to load warm start files!' + '\033[0m') sys.exit(1) # check warm start files if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw): print('\033[91m' + 'WARNING: Warm start files do not fit to the dimension of the NLP!' + '\033[0m') sys.exit(1) # create solver instance solver = ca.nlpsol("solver", "ipopt", nlp, opts) # ------------------------------------------------------------------------------------------------------------------ # SOLVE NLP -------------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # start time measure t0 = time.perf_counter() # solve NLP if pars["optim_opts"]["warm_start"]: sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0) else: sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) # end time measure tend = time.perf_counter() # ------------------------------------------------------------------------------------------------------------------ # EXTRACT SOLUTION ------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # helper function to extract solution for state variables, control variables, tire forces, time f_sol = ca.Function('f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt], ['w'], ['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt']) # extract solution x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x']) # solution for state variables x_opt = np.reshape(x_opt, (N + 1, nx)) # solution for control variables u_opt = np.reshape(u_opt, (N, nu)) # solution for tire forces tf_opt = np.append(tf_opt[-12:], tf_opt[:]) tf_opt = np.reshape(tf_opt, (N + 1, 12)) # solution for time t_opt = np.hstack((0.0, np.cumsum(dt_opt))) # solution for acceleration ax_opt = np.append(ax_opt[-1], ax_opt) ay_opt = np.append(ay_opt[-1], ay_opt) atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2)) # solution for energy consumption ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0 # ------------------------------------------------------------------------------------------------------------------ # EXPORT SOLUTION -------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # export data to CSVs opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(file_path=export_path, s=s_opt, t=t_opt, x=x_opt, u=u_opt, tf=tf_opt, ax=ax_opt, ay=ay_opt, atot=atot_opt, w0=sol["x"], lam_x0=sol["lam_x"], lam_g0=sol["lam_g"]) # ------------------------------------------------------------------------------------------------------------------ # PLOT & PRINT RESULTS --------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ if plot_debug: opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(pars=pars, reftrack=reftrack, s=s_opt, t=t_opt, x=x_opt, u=u_opt, ax=ax_opt, ay=ay_opt, atot=atot_opt, tf=tf_opt, ec=ec_opt_cum) if print_debug: print("INFO: Laptime: %.3fs" % t_opt[-1]) print("INFO: NLP solving time: %.3fs" % (tend - t0)) print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt)) print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt)) print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt)) print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt)) print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1]) return -x_opt[:-1, 3], x_opt[:-1, 0]
K_dr = cs.exp(parameters[0]) K_T = cs.exp(parameters[1]) B_o_prime = cs.exp(parameters[2]) B_prime = cs.exp(parameters[3]) K_m = cs.exp(parameters[4]) ccar = 1 / (cs.sqrt(K_dr * (K_T / inputs + 1)**2 + 1) + cs.sqrt(K_dr) * (K_T / inputs + 1))**2 gfp_mean = (B_o_prime + B_prime * ccar / (K_m + ccar)) #assume some hetroskedasticity, std_dev 5% of mean expression level gfp_var = (0.05**2) * gfp_mean**2 #link the deterministic model to the sampling statistics (here normal mean and variance) gfp_stats = cs.vertcat(gfp_mean, gfp_var) #create a casadi function mapping input and parameters to sampling statistics (mean and var) gfp_model = cs.Function('GFP', [inputs, parameters], [gfp_stats]) # K_g_val = 68.61725640178 # Sigma_m_val = 0.01180105629 # Sigma_g_val = 0.01381497437 # K_dr_val = 0.00166160831 # K_T_val = 11979.25516474195 # K_m_val = 0.28169444728 # B_o_val = 0.00137146821 # B_val = 0.03525605965 K_g_val = 1.6260543 Sigma_m_val = 0.00490364 Sigma_g_val = 0.046474403 K_dr_val = 4.04461938 K_T_val = 11306.84788