def _add_constraints(self): # State Bound Constraints self.opti.subject_to( self.opti.bounded(self.V_MIN, self.v_dv, self.V_MAX)) # Initial State Constraint self.opti.subject_to(self.x_dv[0] == self.z_curr[0]) self.opti.subject_to(self.y_dv[0] == self.z_curr[1]) self.opti.subject_to(self.psi_dv[0] == self.z_curr[2]) self.opti.subject_to(self.v_dv[0] == self.z_curr[3]) # State Dynamics Constraints for i in range(self.N): beta = casadi.atan(self.L_R / (self.L_F + self.L_R) * casadi.tan(self.df_dv[i])) self.opti.subject_to( self.x_dv[i + 1] == self.x_dv[i] + self.DT * (self.v_dv[i] * casadi.cos(self.psi_dv[i] + beta))) self.opti.subject_to( self.y_dv[i + 1] == self.y_dv[i] + self.DT * (self.v_dv[i] * casadi.sin(self.psi_dv[i] + beta))) self.opti.subject_to(self.psi_dv[i + 1] == self.psi_dv[i] + self.DT * (self.v_dv[i] / self.L_R * casadi.sin(beta))) self.opti.subject_to(self.v_dv[i + 1] == self.v_dv[i] + self.DT * (self.acc_dv[i])) # Input Bound Constraints self.opti.subject_to( self.opti.bounded(self.A_MIN, self.acc_dv, self.A_MAX)) self.opti.subject_to( self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX)) # Input Rate Bound Constraints self.opti.subject_to( self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[0], self.acc_dv[0] - self.u_prev[0], self.A_DOT_MAX + self.sl_acc_dv[0])) self.opti.subject_to( self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[0], self.df_dv[0] - self.u_prev[1], self.DF_DOT_MAX + self.sl_df_dv[0])) for i in range(self.N - 1): self.opti.subject_to( self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[i + 1], self.acc_dv[i + 1] - self.acc_dv[i], self.A_DOT_MAX + self.sl_acc_dv[i + 1])) self.opti.subject_to( self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[i + 1], self.df_dv[i + 1] - self.df_dv[i], self.DF_DOT_MAX + self.sl_df_dv[i + 1])) # Other Constraints self.opti.subject_to(0 <= self.sl_df_dv) self.opti.subject_to(0 <= self.sl_acc_dv)
def continuous_dynamics(x, u, p): #extract states and inputs posx = x[zvars.index('posx')-ninputs] posy = x[zvars.index('posy')-ninputs] phi = x[zvars.index('phi')-ninputs] vx = x[zvars.index('vx')-ninputs] vy = x[zvars.index('vy')-ninputs] omega = x[zvars.index('omega')-ninputs] d = x[zvars.index('d')-ninputs] delta = x[zvars.index('delta')-ninputs] theta = x[zvars.index('theta')-ninputs] ddot = u[zvars.index('ddot')] deltadot = u[zvars.index('deltadot')] thetadot = u[zvars.index('thetadot')] #build CasADi expressions for dynamic model #front lateral tireforce alphaf = -casadi.atan2((omega*lf + vy), vx) + delta Ffy = Df*casadi.sin(Cf*casadi.atan(Bf*alphaf)) #rear lateral tireforce alphar = casadi.atan2((omega*lr - vy),vx) Fry = Dr*casadi.sin(Cr*casadi.atan(Br*alphar)) #rear longitudinal forces Frx = (Cm1-Cm2*vx) * d - Croll -Cd*vx*vx #let z = [u, x] = [ddot, deltadot, thetadot, posx, posy, phi, vx, vy, omega, d, delta, theta] statedot = np.array([ vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot omega, #phidot 1/m * (Frx - Ffy*casadi.sin(delta) + m*vy*omega), #vxdot 1/m * (Fry + Ffy*casadi.cos(delta) - m*vx*omega), #vydot 1/Iz * (Ffy*lf*casadi.cos(delta) - Fry*lr), #omegadot ddot, deltadot, thetadot ]) return statedot
def smooth_track(track): N = len(track) xs = track[:, 0] ys = track[:, 1] th1_f = ca.MX.sym('y1_f', N-2) th2_f = ca.MX.sym('y2_f', N-2) x_f = ca.MX.sym('x_f', N) y_f = ca.MX.sym('y_f', N) 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.atan(im(th1_f, th2_f)/real(th1_f, th2_f))]) d_th = ca.Function('d_th', [x_f, y_f], [ca.if_else(ca.fabs(x_f[1:] - x_f[:-1]) < 0.01 ,ca.atan((y_f[1:] - y_f[:-1])/(x_f[1:] - x_f[:-1])), 10000)]) x = ca.MX.sym('x', N) y = ca.MX.sym('y', N) th = ca.MX.sym('th', N-1) B = 5 nlp = {\ 'x': ca.vertcat(x, y, th), 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])) + B* (ca.sumsqr(x-xs) + ca.sumsqr(y-ys)), # 'f': B* (ca.sumsqr(x-xs) + ca.sumsqr(y-ys)), 'g': ca.vertcat(\ th - d_th(x, y), x[0] - xs[0], y[0]- ys[0], x[-1] - xs[-1], y[-1]- ys[-1], )\ } S = ca.nlpsol('S', 'ipopt', nlp) # th0 = [lib.get_bearing(track[i, 0:2], track[i+1, 0:2]) for i in range(N-1)] th0 = d_th(xs, ys) x0 = ca.vertcat(xs, ys, th0) lbx = [0] *2* N + [-np.pi]*(N -1) ubx = [100] *2 * N + [np.pi]*(N-1) r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) print(f"Solution found") x_opt = r['x'] xs_new = np.array(x_opt[:N]) ys_new = np.array(x_opt[N:2*N]) track[:, 0] = xs_new[:, 0] track[:, 1] = ys_new[:, 0] return track
def _add_constraints(self): ## State Bound Constraints self.opti.subject_to( self.opti.bounded(self.EY_MIN, self.ey_dv, self.EY_MAX) ) self.opti.subject_to( self.opti.bounded(self.EPSI_MIN, self.epsi_dv, self.EPSI_MAX) ) ## Initial State Constraint self.opti.subject_to( self.s_dv[0] == self.z_curr[0] ) self.opti.subject_to( self.ey_dv[0] == self.z_curr[1] ) self.opti.subject_to( self.epsi_dv[0] == self.z_curr[2] ) self.opti.subject_to( self.v_dv[0] == self.z_curr[3] ) ## State Dynamics Constraints for i in range(self.N): beta = casadi.atan( self.L_R / (self.L_F + self.L_R) * casadi.tan(self.df_dv[i]) ) dyawdt = self.v_dv[i] / self.L_R * casadi.sin(beta) dsdt = self.v_dv[i] * casadi.cos(self.epsi_dv[i]+beta) / (1 - self.ey_dv[i] * self.curv_ref[i] ) ay = self.v_dv[i] * dyawdt self.opti.subject_to( self.s_dv[i+1] == self.s_dv[i] + self.DT * (dsdt) ) self.opti.subject_to( self.ey_dv[i+1] == self.ey_dv[i] + self.DT * (self.v_dv[i] * casadi.sin(self.epsi_dv[i] + beta)) ) self.opti.subject_to( self.epsi_dv[i+1] == self.epsi_dv[i] + self.DT * (dyawdt - dsdt * self.curv_ref[i]) ) self.opti.subject_to( self.v_dv[i+1] == self.v_dv[i] + self.DT * (self.acc_dv[i]) ) self.opti.subject_to( self.opti.bounded(self.AY_MIN - self.sl_ay_dv[i], ay, self.AY_MAX + self.sl_ay_dv[i]) ) ## Input Bound Constraints self.opti.subject_to( self.opti.bounded(self.AX_MIN, self.acc_dv, self.AX_MAX) ) self.opti.subject_to( self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX) ) # Input Rate Bound Constraints self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT, self.acc_dv[0] - self.u_prev[0], self.AX_DOT_MAX*self.DT) ) self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT, self.df_dv[0] - self.u_prev[1], self.DF_DOT_MAX*self.DT) ) for i in range(self.N - 1): self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT, self.acc_dv[i+1] - self.acc_dv[i], self.AX_DOT_MAX*self.DT) ) self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT, self.df_dv[i+1] - self.df_dv[i], self.DF_DOT_MAX*self.DT) ) # Other Constraints self.opti.subject_to( 0 <= self.sl_ay_dv ) # nonnegative lateral acceleration slack variable
def symbolicalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps, sym_p_I_P, sym_t_corr_G0, sym_T_corr_L_twist): errvec = casadi.MX.sym('', 0, 0) T_corr_L = casadi_pose.fromTwist(sym_T_corr_L_twist) for t_G0_P, p_L_P in zip(t_G0_Ps, p_L_Ps): T_L_I = T_L_I_of_t_G0.timeDerivativeSample(t_G0_P + sym_t_corr_G0, t_G0_P) if T_L_I is None: continue p_corr_P_guess = (T_corr_L * T_L_I * sym_p_I_P) sqn = casadi_pose.squareNorm(p_L_P - p_corr_P_guess) # Robust loss: Flatten the loss of errors > 5cm, most likely outliers. # TODO completely discard a portion of worst measurements instead? rloss = casadi.atan(sqn / 0.0025) errvec = casadi.vertcat(errvec, rloss) return errvec
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 import control s = control.tf([1, 0], [0, 1]) H = 140 * (s + 50) * (s + 50) / (s * (2138 * s + 208.8)) Hd = control.tf2ss(control.c2d(H, 0.01)) theta_c = (100 - p_n[2]) * (0.01) / (v_b[2] * ca.cos(p_n[2])) x_elev = ca.SX.sym('x_elev', 2) u_elev = ca.SX.sym('u_elev', 1) x_1 = ca.mtimes(Hd.A, x_elev) + ca.mtimes(Hd.B, u_elev) y_elev = ca.mtimes(Hd.C, x_elev) + ca.mtimes(Hd.D, u_elev) elev_c = (theta_c - p_n[2]) * y_elev / (0.5 * rho * v_b[2]**2) u_control[0] = 0.1 # mass flow rate u_control[1] = 0 u_control[2] = elev_c 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 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"] rho = pars["veh_params"]["rho_air"] 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 * 0.5 * veh["clA_front"] * rho * v**2 f_zlift_fr = 0.5 * 0.5 * veh["clA_front"] * rho * v**2 f_zlift_rl = 0.5 * 0.5 * veh["clA_rear"] * rho * v**2 f_zlift_rr = 0.5 * 0.5 * veh["clA_rear"] * rho * 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["optim_opts"]["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]
S = ca.nlpsol('S', 'ipopt', nlp) n0 = ones * 0 # n0 = np.random.uniform(-1, 1, N) 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) # calculate this afterwards d0 = ca.atan(l * (th0[1:] - th0[:-1]) / (ones * 2)[:-1]) d0 = np.insert(np.array(d0), -1, 0) v0, a0 = [1], [] for i in range(N - 1): if v0[-1] < 3: a0.append(0.2) else: a0.append(0) new_v = np.sqrt(v0[-1]**2 + 2 * a0[-1] * sls[i]) v0.append(new_v) a0.append(0) # last a t0 = [0] for i in range(N - 1): new_t = sls[i] / v0[i] # t0[-1]
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))] # sls = np.sqrt(np.sum(np.power(np.diff(track[:, :2], axis=0), 2), axis=1)) 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_a = ca.MX.sym('n_f', N) 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.atan(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]))]) real1 = ca.Function( 'real', [th1_f1, th2_f1], [ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)]) im1 = ca.Function( 'im', [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_cpx', [th1_f1, th2_f1], [ca.atan(im1(th1_f1, th2_f1) / real1(th1_f1, th2_f1))]) d_n = ca.Function('d_n', [n_f_a, th_f], [track_length(n_f_a) / ca.tan(get_th_n(th_f))]) # curvature = ca.Function('curv', [th_f_a], [th_f_a[1:] - th_f_a[:-1]]) grad = ca.Function( 'grad', [n_f_a], [(o_y_e(n_f_a[1:]) - o_y_s(n_f_a[:-1])) / ca.fmax(o_x_e(n_f_a[1:]) - o_x_s(n_f_a[:-1]), 0.1 * np.ones(N - 1))]) curvature = ca.Function( 'curv', [n_f_a], [grad(n_f_a)[1:] - grad(n_f_a)[:-1]]) # changes in grad # define symbols n = ca.MX.sym('n', N) th = ca.MX.sym('th', N) nlp = {\ 'x': ca.vertcat(n, th), # 'f': ca.sumsqr(curvature(n)), # 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])), 'f': ca.sumsqr(sub_cmplx1(sub_cmplx1(th[2:], th[1:-1]), sub_cmplx1(th[1:-1], th[:-2]))), # 'f': ca.sumsqr(track_length(n)), '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 print(curvature(n0)) r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) print(f"Solution found") x_opt = r['x'] n_set = np.array(x_opt[:N]) thetas = np.array(x_opt[1 * N:2 * N]) # print(sub_cmplx(thetas[1:], thetas[:-1])) plt.figure(2) th_data = sub_cmplx(thetas[1:], thetas[:-1]) plt.plot(th_data) plt.plot(ca.fabs(th_data), '--') plt.pause(0.001) plt.figure(3) plt.plot(abs(thetas), '--') plt.plot(thetas) plt.pause(0.001) plt.figure(4) plt.plot(sub_cmplx(th0[1:], th0[:-1])) plt.pause(0.001) plot_race_line(np.array(track), n_set, width=3, wait=True)
def MinCurvatureSteer(): 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))] sls = np.sqrt(np.sum(np.power(np.diff(track[:, :2], axis=0), 2), axis=1)) l = 0.33 a_max = 7.5 d_max = 0.2 v_max = 6 n_max = 3 N = len(track) n_f_a = ca.MX.sym('n_f', N) n_f = ca.MX.sym('n_f', N - 1) d_f = ca.MX.sym('d_f', N - 1) th_f = ca.MX.sym('th_f', N - 1) v_f = ca.MX.sym('v_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('th_f', N - 1) th2_f = ca.MX.sym('th_f', N - 1) 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:])) ]) # d_n = ca.Function('d_n', [n_f, th_f], [sls/ca.tan(th_ns[:-1] - th_f)]) # sls_f = ca.Function('sls_f', [n_f_a], ) # get_th = ca.Function('gth', [th_f], [th_ns[:-1] - th_f]) # get_th_n = ca.Function('gth', [th_f], [th_f - th_ns[:-1] + (np.pi/2)*np.ones(N-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.atan(im(th1_f, th2_f) / real(th1_f, th2_f))]) get_th_n = ca.Function('gth', [th_f], [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))]) d_t = ca.Function('d_t', [n_f_a, v_f], [ dis(o_x_e(n_f_a[1:]), o_x_s(n_f_a[:-1]), o_y_e(n_f_a[1:]), o_y_s(n_f_a[:-1])) / v_f ]) d_th = ca.Function('d_th', [v_f, d_f], [v_f / l * ca.tan(d_f)]) # define symbols n = ca.MX.sym('n', N) dt = ca.MX.sym('t', N) v = ca.MX.sym('v', N) th = ca.MX.sym('th', N) a = ca.MX.sym('a', N) d = ca.MX.sym('d', N) nlp = {\ 'x': ca.vertcat(n, dt, v, th, a, d), # 'f': ca.sumsqr(curvature(n)), # 'f': ca.sumsqr(track_length(n)), # 'f': ca.sumsqr(d) - ca.sumsqr(v), # 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])), 'f': ca.sum1(dt), # 'f': - ca.sumsqr(v), 'g': ca.vertcat( # dynamic constraints n[1:] - (n[:-1] + d_n(n, th[:-1])), th[1:] - (th[:-1] + d_th(v[:-1], d[:-1]) * (dt[:-1])), v[1:] - (v[:-1] + a[:-1] * dt[:-1]), dt[:-1] - (d_t(n, v[:-1])), # boundary constraints n[0], v[0] - 1, n[-1], th[-1], d[-1], dt[-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) d0 = [] for i in range(N - 1): angle = lib.sub_angles_complex(th0[i + 1], th0[i]) d00 = ca.atan(l * (angle) / 3) d0.append(d00) d0.append(0) d0 = np.array(d0) v0, a0 = [1], [] for i in range(N - 1): if v0[-1] < 3: a0.append(0.2) else: a0.append(0) new_v = np.sqrt(v0[-1]**2 + 2 * a0[-1] * sls[i]) v0.append(new_v) a0.append(0) # last a t0 = [0] for i in range(N - 1): new_t = sls[i] / v0[i] # t0[-1] t0.append(new_t) x0 = ca.vertcat(n0, t0, v0, th0, a0, d0) lbx = [-n_max] * N + [0] * N + [0] * N + [-np.pi] * N + [-a_max] * N + [ -d_max ] * N ubx = [n_max] * N + [np.inf] * N + [v_max] * N + [np.pi] * N + [ a_max ] * N + [d_max] * N # plot_x_opt(x0, track) r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) print(f"Solution found") x_opt = r['x'] plt.figure(7) plt.plot(track_length(x_opt[:N])) plot_x_opt(x_opt, track)
def log(cls, r): assert r.shape == (4, 1) or r.shape == (4, ) n = ca.norm_2(r[:3]) return ca.if_else(n > eps, 4 * ca.atan(n) * r[:3] / n, ca.SX([0, 0, 0]))
def __init__(self, modelparams, Ts, x0, nodes): with open(modelparams) as file: params = yaml.load(file, Loader=yaml.FullLoader) self.xvars = [ 'posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta' ] self.uvars = ['ddot', 'deltadot', 'thetadot'] m = params['m'] #[kg] lf = params['lf'] #[m] lr = params['lr'] #[m] Iz = params['Iz'] #[kg*m^3] lencar = lf + lr widthcar = lencar / 2 #pajecka and motor coefficients Bf = params['Bf'] Br = params['Br'] Cf = params['Cf'] Cr = params['Cr'] Cm1 = params['Cm1'] Cm2 = params['Cm2'] Croll = params['Croll'] Cd = params['Cd'] Df = params['Df'] Dr = params['Dr'] x = casadi.SX.sym("x", len(self.xvars)) u = casadi.SX.sym("u", len(self.uvars)) #extract states and inputs posx = x[self.xvars.index('posx')] posy = x[self.xvars.index('posy')] phi = x[self.xvars.index('phi')] vx = x[self.xvars.index('vx')] vy = x[self.xvars.index('vy')] omega = x[self.xvars.index('omega')] d = x[self.xvars.index('d')] delta = x[self.xvars.index('delta')] theta = x[self.xvars.index('theta')] ddot = u[self.uvars.index('ddot')] deltadot = u[self.uvars.index('deltadot')] thetadot = u[self.uvars.index('thetadot')] #build CasADi expressions for dynamic model #front lateral tireforce alphaf = -casadi.atan2((omega * lf + vy), vx) + delta Ffy = Df * casadi.sin(Cf * casadi.atan(Bf * alphaf)) #rear lateral tireforce alphar = casadi.atan2((omega * lr - vy), vx) Fry = Dr * casadi.sin(Cr * casadi.atan(Br * alphar)) #rear longitudinal forces Frx = (Cm1 - Cm2 * vx) * d - Croll - Cd * vx * vx #let z = [u, x] = [ddot, deltadot, thetadot, posx, posy, phi, vx, vy, omega, d, delta, theta] statedot = np.array([ vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot omega, #phidot 1 / m * (Frx - Ffy * casadi.sin(delta) + m * vy * omega), #vxdot 1 / m * (Fry + Ffy * casadi.cos(delta) - m * vx * omega), #vydot 1 / Iz * (Ffy * lf * casadi.cos(delta) - Fry * lr), #omegadot ddot, deltadot, thetadot ]) #xdot = f(x,u) self.f = casadi.Function('f', [x, u], [statedot]) #state of system self.x = x0 #sampling timestep self.Ts = Ts #integration nodes self.nodes = nodes
def dynamic_model(modelparams): # define casadi struct model = casadi.types.SimpleNamespace() constraints = casadi.types.SimpleNamespace() model_name = "f110_dynamic_model" model.name = model_name #loadparameters with open(modelparams) as file: params = yaml.load(file, Loader=yaml.FullLoader) m = params['m'] #[kg] lf = params['lf'] #[m] lr = params['lr'] #[m] Iz = params['Iz'] #[kg*m^3] #pajecka and motor coefficients Bf = params['Bf'] Br = params['Br'] Cf = params['Cf'] Cr = params['Cr'] Cm1 = params['Cm1'] Cm2 = params['Cm2'] Croll = params['Croll'] Cd = params['Cd'] Df = params['Df'] Dr = params['Dr'] print("CasADi model created with the following parameters: filename: ", modelparams, "\n values:", params) xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta'] uvars = ['ddot', 'deltadot', 'thetadot'] pvars = [ 'xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql', 'Q_theta', 'R_d', 'R_delta', 'r' ] #parameter vector, contains linearization poitns xt = casadi.SX.sym("xt") yt = casadi.SX.sym("yt") phit = casadi.SX.sym("phit") sin_phit = casadi.SX.sym("sin_phit") cos_phit = casadi.SX.sym("cos_phit") #stores linearization point theta_hat = casadi.SX.sym("theta_hat") Qc = casadi.SX.sym("Qc") Ql = casadi.SX.sym("Ql") Q_theta = casadi.SX.sym("Q_theta") #cost on smoothnes of motorinput R_d = casadi.SX.sym("R_d") #cost on smoothness of steering_angle R_delta = casadi.SX.sym("R_delta") #trackwidth r = casadi.SX.sym("r") #pvars = ['xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql', 'Q_theta', 'R_d', 'R_delta', 'r'] p = casadi.vertcat(xt, yt, phit, sin_phit, cos_phit, theta_hat, Qc, Ql, Q_theta, R_d, R_delta, r) #single track model with pajecka tireforces as in Optimization-Based Autonomous Racing of 1:43 Scale RC Cars Alexander Liniger, Alexander Domahidi and Manfred Morari #pose posx = casadi.SX.sym("posx") posy = casadi.SX.sym("posy") #vel (long and lateral) vx = casadi.SX.sym("vx") vy = casadi.SX.sym("vy") #body angular Rate omega = casadi.SX.sym("omega") #heading phi = casadi.SX.sym("phi") #steering_angle delta = casadi.SX.sym("delta") #motorinput d = casadi.SX.sym("d") #dynamic forces Frx = casadi.SX.sym("Frx") Fry = casadi.SX.sym("Fry") Ffx = casadi.SX.sym("Ffx") Ffy = casadi.SX.sym("Ffy") #arclength progress theta = casadi.SX.sym("theta") #temporal derivatives posxdot = casadi.SX.sym("xdot") posydot = casadi.SX.sym("ydot") vxdot = casadi.SX.sym("vxdot") vydot = casadi.SX.sym("vydot") phidot = casadi.SX.sym("phidot") omegadot = casadi.SX.sym("omegadot") deltadot = casadi.SX.sym("deltadot") thetadot = casadi.SX.sym("thetadot") ddot = casadi.SX.sym("ddot") #inputvector #uvars = ['ddot', 'deltadot', 'thetadot'] u = casadi.vertcat(ddot, deltadot, thetadot) #car state Dynamics #xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta'] x = casadi.vertcat(posx, posy, phi, vx, vy, omega, d, delta, theta) xdot = casadi.vertcat(posxdot, posydot, phidot, vxdot, vydot, omegadot, ddot, deltadot, thetadot) #build CasADi expressions for dynamic model #front lateral tireforce alphaf = -casadi.atan2((omega * lf + vy), vx) + delta Ffy = Df * casadi.sin(Cf * casadi.atan(Bf * alphaf)) #rear lateral tireforce alphar = casadi.atan2((omega * lr - vy), vx) Fry = Dr * casadi.sin(Cr * casadi.atan(Br * alphar)) #rear longitudinal forces Frx = (Cm1 - Cm2 * vx) * d - Croll - Cd * vx * vx f_expl = casadi.vertcat( vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot omega, #phidot 1 / m * (Frx - Ffy * casadi.sin(delta) + m * vy * omega), #vxdot 1 / m * (Fry + Ffy * casadi.cos(delta) - m * vx * omega), #vydot 1 / Iz * (Ffy * lf * casadi.cos(delta) - Fry * lr), #omegadot ddot, deltadot, thetadot) # algebraic variables z = casadi.vertcat([]) model.f_expl_expr = f_expl model.f_impl_expr = xdot - f_expl model.x = x model.xdot = xdot model.u = u model.p = p model.z = z #boxconstraints model.ddot_min = -10.0 #min change in d [-] model.ddot_max = 10.0 #max change in d [-] model.d_min = -0.1 #min d [-] model.d_max = 1 #max d [-] model.delta_min = -0.40 # minimum steering angle [rad] model.delta_max = 0.40 # maximum steering angle [rad] model.deltadot_min = -2 # minimum steering angle cahgne[rad/s] model.deltadot_max = 2 # maximum steering angle cahgne[rad/s] model.omega_min = -100 # minimum yawrate [rad/sec] model.omega_max = 100 # maximum yawrate [rad/sec] model.thetadot_min = 0.05 # minimum adv param speed [m/s] model.thetadot_max = 5 # maximum adv param speed [m/s] model.theta_min = 0.00 # minimum adv param [m] model.theta_max = 1000 # maximum adv param [m] model.vx_max = 3.5 # max long vel [m/s] model.vx_min = -0.5 #0.05 # min long vel [m/s] model.vy_max = 3 # max lat vel [m/s] model.vy_min = -3 # min lat vel [m/s] model.x0 = np.array([0, 0, 0, 1, 0.01, 0, 0, 0, 0]) #compute approximate linearized contouring and lag error xt_hat = xt + cos_phit * (theta - theta_hat) yt_hat = yt + sin_phit * (theta - theta_hat) e_cont = sin_phit * (xt_hat - posx) - cos_phit * (yt_hat - posy) e_lag = cos_phit * (xt_hat - posx) + sin_phit * (yt_hat - posy) cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot #error = casadi.vertcat(e_cont, e_lag) #set up stage cost #Q = diag(vertcat(Qc, Ql)) model.con_h_expr = (xt_hat - posx)**2 + (yt_hat - posy)**2 - r**2 model.stage_cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot return model, constraints