def casadi(self, x, u, dxdt): """ write dynamics as first order ODE: dxdt = f(x(t)) x is a 6x1 vector: [x, y, psi, vx, vy, omega]^T u is a 2x1 vector: [acc/pwm, steer]^T dxdt is a casadi.SX variable """ pwm = u[0] steer = u[1] psi = x[2] vx = x[3] vy = x[4] omega = x[5] vmin = 0.05 vy = cs.if_else(vx < vmin, 0, vy) omega = cs.if_else(vx < vmin, 0, omega) steer = cs.if_else(vx < vmin, 0, steer) vx = cs.if_else(vx < vmin, vmin, vx) Frx = (self.Cm1 - self.Cm2 * vx) * pwm - self.Cr0 - self.Cr2 * (vx**2) alphaf = steer - cs.atan2((self.lf * omega + vy), vx) alphar = cs.atan2((self.lr * omega - vy), vx) Ffy = self.Df * cs.sin(self.Cf * cs.arctan(self.Bf * alphaf)) Fry = self.Dr * cs.sin(self.Cr * cs.arctan(self.Br * alphar)) dxdt[0] = vx * cs.cos(psi) - vy * cs.sin(psi) dxdt[1] = vx * cs.sin(psi) + vy * cs.cos(psi) dxdt[2] = omega dxdt[3] = 1 / self.mass * (Frx - Ffy * cs.sin(steer)) + vy * omega dxdt[4] = 1 / self.mass * (Fry + Ffy * cs.cos(steer)) - vx * omega dxdt[5] = 1 / self.Iz * (Ffy * self.lf * cs.cos(steer) - Fry * self.lr) return dxdt
def q_err(q_t, q_r): # """ # Compute angular error between two unit quaternions. # :param q_t: New quaternion # :type q_t: ca.DM, list, np.array # :param q_r: Reference quaternion # :type q_r: ca.DM, list, np.array # :return: vector corresponding to SK matrix # :rtype: ca.DM # """ q_upper_t = [q_r[3], -q_r[0], -q_r[1], -q_r[2]] q_lower_t = [q_t[3], q_t[0], q_t[1], q_t[2]] qd_t = [ q_upper_t[0] * q_lower_t[0] - q_upper_t[1] * q_lower_t[1] - q_upper_t[2] * q_lower_t[2] - q_upper_t[3] * q_lower_t[3], q_upper_t[1] * q_lower_t[0] + q_upper_t[0] * q_lower_t[1] + q_upper_t[2] * q_lower_t[3] - q_upper_t[3] * q_lower_t[2], q_upper_t[0] * q_lower_t[2] - q_upper_t[1] * q_lower_t[3] + q_upper_t[2] * q_lower_t[0] + q_upper_t[3] * q_lower_t[1], q_upper_t[0] * q_lower_t[3] + q_upper_t[1] * q_lower_t[2] - q_upper_t[2] * q_lower_t[1] + q_upper_t[3] * q_lower_t[0] ] phi_t = ca.atan2(2 * (qd_t[0] * qd_t[1] + qd_t[2] * qd_t[3]), 1 - 2 * (qd_t[1]**2 + qd_t[2]**2)) theta_t = ca.asin(2 * (qd_t[0] * qd_t[2] - qd_t[3] * qd_t[1])) psi_t = ca.atan2(2 * (qd_t[0] * qd_t[3] + qd_t[1] * qd_t[2]), 1 - 2 * (qd_t[2]**2 + qd_t[3]**2)) return ca.vertcat(phi_t, theta_t, psi_t)
def kinematics(self, state, U, epsilon=0): f,_,_ = self.proc_model() w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0])) w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1])) w = c.DM([[w0],[w1]]) #state_n = state + self.dt*c.blockcat([[vx],[vy],[vtheta]]) state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]])) # state_n = c.MX(self.nx,1) # state_n[0] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[0] # state_n[1] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[1] # state_n[2] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[2] # state_n[3] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[3] state_n[2] = c.atan2(c.sin(state_n[2]),c.cos(state_n[2])) state_n[4] = c.atan2(c.sin(state_n[4]),c.cos(state_n[4])) state_n[5] = c.atan2(c.sin(state_n[5]),c.cos(state_n[5])) return state_n
def rotation_matrix_to_rxyz_casadi(R): r11, r12, r13 = R[0, 0], R[0, 1], R[0, 1] r23 = R[1, 2] r33 = R[2, 2] r_x = ca.atan2(-r23, r33) r_y = ca.atan2(r13, np.sqrt(r11 ** 2 + r12 ** 2)) r_z = ca.atan2(-r12, r11) return [r_x, r_y, r_z]
def from_quat(cls, q): assert q.shape == (4, 1) or q.shape == (4, ) e = ca.SX(3, 1) a = q[0] b = q[1] c = q[2] d = q[3] e[0] = ca.atan2(2 * (a * b + c * d), 1 - 2 * (b**2 + c**2)) e[1] = ca.asin(2 * (a * c - d * b)) e[2] = ca.atan2(2 * (a * d + b * c), 1 - 2 * (c**2 + d**2)) return e
def JK(chi, chi_des, t_r, v_ini): T_s_est = Xu * chi[0] ud = ca.sqrt(chi[0]**2 + v_ini**2) d = ca.sqrt((chi_des[0] - chi[3])**2 + (chi_des[1] - chi[4])**2) Dpsi = wraptopi( ca.atan2(chi_des[1] - chi[4], chi_des[0] - chi[3]) - ca.atan2(chi[1], chi[0]) - chi[5]) t_rem = 2 * t_r + ca.fabs(d / ud - 2 * t_r * ca.sin(Dpsi) / (Dpsi + 1e-16)) J_horizontal = cal_yaw_moment(Dpsi, t_r, chi[2], T_s_est) J_K = ex_f * t_rem + 2 * cal_p( T_s_est / 2) * (t_rem - 2 * t_r) + J_horizontal return J_K
def kinematics(self, state, U, epsilon=0): f,_,_ = self.proc_model() w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0])) w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1])) w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2])) w3 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[3,3])) w = c.DM([[w0],[w1],[w2],[w3]]) state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]],[U[2] + w[2]],[U[3] + w[3]]])) state_n[6] = c.atan2(c.sin(state_n[6]),c.cos(state_n[6])) state_n[7] = c.atan2(c.sin(state_n[7]),c.cos(state_n[7])) state_n[8] = c.atan2(c.sin(state_n[8]),c.cos(state_n[8])) return state_n
def dynamics_rear(X_prev, S, v): dt = 0.1 L = 2.33 X_new = X_prev + dt*casadi.blockcat([[casadi.mtimes(v/2.0,casadi.cos(X_prev[2] - S - 8*m.pi/180)+casadi.cos(X_prev[2] + (S + 8*m.pi/180)))], [casadi.mtimes(v/2.0,casadi.sin(X_prev[2] - S - 8*m.pi/180) + casadi.sin(X_prev[2] + (S + 8*m.pi/180)))], [casadi.mtimes(v,casadi.sin(-S - 8*m.pi/180)*1/L)]]) X_new[2] = casadi.atan2(casadi.sin(X_new[2]), casadi.cos(X_new[2])) return X_new
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 rpy_from_matrix(rotation_matrix): """ !takes time to compile! :param rotation_matrix: 4x4 Matrix :type rotation_matrix: Matrix :return: roll, pitch, yaw :rtype: (Union[float, Symbol], Union[float, Symbol], Union[float, Symbol]) """ i = 0 j = 1 k = 2 cy = sqrt(rotation_matrix[i, i] * rotation_matrix[i, i] + rotation_matrix[j, i] * rotation_matrix[j, i]) if0 = cy - _EPS ax = diffable_if_greater_zero( if0, atan2(rotation_matrix[k, j], rotation_matrix[k, k]), atan2(-rotation_matrix[j, k], rotation_matrix[j, j])) ay = diffable_if_greater_zero(if0, atan2(-rotation_matrix[k, i], cy), atan2(-rotation_matrix[k, i], cy)) az = diffable_if_greater_zero( if0, atan2(rotation_matrix[j, i], rotation_matrix[i, i]), 0) return ax, ay, az
def multi_receiver_heading_2d(x, params=None): """ Computes the heading of (Receiver B/Object) with respect to Receiver A """ if "y" in params: if "idx" not in params: idx = [0, 1] else: idx = params["idx"] r_y = params["y"][1] - x[idx[1]] r_x = params["y"][0] - x[idx[0]] elif "idxA" in params and "idxB" in params: idxA = params["idxA"] idxB = params["idxB"] r_y = x[idxB[1]] - x[idxA[1]] r_x = x[idxB[0]] - x[idxA[0]] + .00001 return atan2(r_x, r_y)
def kinematics(self, state, vx, vy, vtheta, epsilon=0): f,_,_ = self.proc_model() #vx = vx + epsilon*self.vel_max*np.random.normal(0,1) #vy = vy + epsilon*self.vel_max*np.random.normal(0,1) #vtheta = vtheta + epsilon*self.ang_vel_max*np.random.normal(0,1) w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0])) w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1])) w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2])) w = c.DM([[w0],[w1],[w2]]) #state_n = state + self.dt*c.blockcat([[vx],[vy],[vtheta]]) state_n = f(state,c.blockcat([[vx],[vy],[vtheta]])) + c.mtimes(self.G,w) state_n[2] = c.atan2(c.sin(state_n[2]),c.cos(state_n[2])) return state_n
def f_ct(self, x, u, type='casadi'): if type == 'casadi': beta = lambda d: ca.atan2(self.L_r * ca.tan(d), self.L_r + self.L_f ) x_dot = ca.vcat([ x[3] * ca.cos(x[2] + beta(u[0])), x[3] * ca.sin(x[2] + beta(u[0])), x[3] * ca.sin(beta(u[0])) / self.L_r, u[1] ]) elif type == 'numpy': beta = lambda d: np.arctan2(self.L_r * np.tan(d), self.L_r + self. L_f) x_dot = np.array([ x[3] * np.cos(x[2] + beta(u[0])), x[3] * np.sin(x[2] + beta(u[0])), x[3] * np.sin(beta(u[0])) / self.L_r, u[1] ]) else: raise RuntimeError('Dynamics type %s not recognized' % type) return x_dot
def solve(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False): x = ca.SX.sym('x', self.n_x*(N+1)) u = ca.SX.sym('y', self.n_u*N) slack = ca.SX.sym('slack', self.n_x) z = ca.vertcat(x, u, slack) # Flatten candidate solutions into 1-D array (- x_1 -, - x_2 -, ..., - x_N+1 -) if x_guess is not None: x_guess_flat = x_guess.flatten(order='F') else: x_guess_flat = np.zeros(self.n_x*(N+1)) if u_guess is not None: u_guess_flat = u_guess.flatten(order='F') else: u_guess_flat = np.zeros(self.n_u*N) slack_guess = np.zeros(self.n_x) z_guess = np.concatenate((x_guess_flat, u_guess_flat, slack_guess)) lb_slack = [-1000.0]*self.n_x ub_slack = [1000.0]*self.n_x # Box constraints on decision variables lb_x = x_0.tolist() + self.state_lb*(N) + self.input_lb*(N) + lb_slack ub_x = x_0.tolist() + self.state_ub*(N) + self.input_ub*(N) + ub_slack # Constraints on functions of decision variables lb_g = [] ub_g = [] stage_cost = 0 constraint = [] for i in range(N): stage_cost += 1 # Formulate dynamics equality constraints as inequalities beta = ca.atan2(self.l_r*ca.tan(u[self.n_u*i+0]), self.l_f+self.l_r) constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+0] - (x[self.n_x*i+0] + self.dt*x[self.n_x*i+3]*ca.cos(x[self.n_x*i+2] + beta))) constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+1] - (x[self.n_x*i+1] + self.dt*x[self.n_x*i+3]*ca.sin(x[self.n_x*i+2] + beta))) constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+2] - (x[self.n_x*i+2] + self.dt*x[self.n_x*i+3]*ca.sin(beta))) constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+3] - (x[self.n_x*i+3] + self.dt*u[self.n_u*i+1])) lb_g += [0.0]*self.n_x ub_g += [0.0]*self.n_x # Steering rate constraints if self.ddf_lim is not None: if i == 0: # Constrain steering rate with respect to previously applied input constraint = ca.vertcat(constraint, u[self.n_u*(i)+0]-last_u[0]) if i < N-1: # Constrain steering rate along horizon constraint = ca.vertcat(constraint, u[self.n_u*(i+1)+0]-u[self.n_u*(i)+0]) lb_g += [self.ddf_lim[0]*self.dt] ub_g += [self.ddf_lim[1]*self.dt] # Throttle rate constraints if self.da_lim is not None: if i == 0: # Constrain throttle rate constraint = ca.vertcat(constraint, u[self.n_u*i+1]-last_u[1]) if i < N-1: constraint = ca.vertcat(constraint, u[self.n_u*(i+1)+1]-u[self.n_u*(i)+1]) lb_g += [self.da_lim[0]*self.dt] ub_g += [self.da_lim[1]*self.dt] # Exploration constraints on predicted positions of agent if expl_constraints is not None: V = expl_constraints[i][0] w = expl_constraints[i][1] for j in range(V.shape[0]): constraint = ca.vertcat(constraint, V[j,0]*x[self.n_x*i+0] + V[j,1]*x[self.n_x*i+1] + w[j]) lb_g += [-1e9] ub_g += [0] # Formulate terminal soft equality constraint as inequalities constraint = ca.vertcat(constraint, x[self.n_x*N:] - x_ss + slack) lb_g += [0.0]*self.n_x ub_g += [0.0]*self.n_x slack_cost = 1e6*ca.sumsqr(slack) total_cost = stage_cost + slack_cost opts = {'verbose' : False, 'print_time' : 0, 'ipopt.print_level' : 5, 'ipopt.mu_strategy' : 'adaptive', 'ipopt.mu_init' : 1e-5, 'ipopt.mu_min' : 1e-15, 'ipopt.barrier_tol_factor' : 1, 'ipopt.linear_solver': 'ma27'} nlp = {'x' : z, 'f' : total_cost, 'g' : constraint} solver = ca.nlpsol('solver', 'ipopt', nlp, opts) sol = solver(lbx=lb_x, ubx=ub_x, lbg=lb_g, ubg=ub_g, x0=z_guess) pdb.set_trace() if solver.stats()['success']: if la.norm(slack_val) <= 1e-8: print('Solve success for safe set point', x_ss) feasible = True z_sol = np.array(sol['x']) x_pred = z_sol[:self.n_x*(N+1)].reshape((N+1,self.n_x)).T u_pred = z_sol[self.n_x*(N+1):self.n_x*(N+1)+self.n_u*N].reshape((N,self.n_u)).T slack_val = z_sol[self.n_x*(N+1)+self.n_u*N:] cost_val = sol['f'] else: print('Warning! Solved, but with slack norm of %g is greater than 1e-8!' % la.norm(slack_val)) feasible = False x_pred = None u_pred = None cost_val = None else: print(solver.stats()['return_status']) feasible = False x_pred = None u_pred = None cost_val = None # pdb.set_trace() # pdb.set_trace() return x_pred, u_pred, cost_val
def solve_opti(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False): opti = ca.Opti() x = opti.variable(self.n_x, N+1) u = opti.variable(self.n_u, N) slack = opti.variable(self.n_x) if x_guess is not None: opti.set_initial(x, x_guess) if u_guess is not None: opti.set_initial(u, u_guess) opti.set_initial(slack, np.zeros(self.n_x)) da_lim = self.agent.da_lim ddf_lim = self.agent.ddf_lim opti.subject_to(x[:,0] == np.squeeze(x_0)) opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[0,0]-last_u[0], ddf_lim[1]*self.dt)) opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[1,0]-last_u[1], da_lim[1]*self.dt)) stage_cost = 0 for i in range(N): stage_cost = stage_cost + 1 beta = ca.atan2(self.l_r*ca.tan(u[0,i]), self.l_f+self.l_r) opti.subject_to(x[0,i+1] == x[0,i] + self.dt*x[3,i]*ca.cos(x[2,i] + beta)) opti.subject_to(x[1,i+1] == x[1,i] + self.dt*x[3,i]*ca.sin(x[2,i] + beta)) opti.subject_to(x[2,i+1] == x[2,i] + self.dt*x[3,i]*ca.sin(beta)) opti.subject_to(x[3,i+1] == x[3,i] + self.dt*u[1,i]) if self.F is not None: opti.subject_to(ca.mtimes(self.F, x[:,i]) <= self.b) if self.H is not None: opti.subject_to(ca.mtimes(self.H, u[:,i]) <= self.g) if expl_constraints is not None: V = expl_constraints[i][0] w = expl_constraints[i][1] opti.subject_to(ca.mtimes(V, x[:2,i]) + w <= np.zeros(len(w))) if i < N-1: opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[0,i+1]-u[0,i], ddf_lim[1]*self.dt)) opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[1,i+1]-u[1,i], da_lim[1]*self.dt)) opti.subject_to(x[:,N] - x_ss == slack) slack_cost = 1e6*ca.sumsqr(slack) total_cost = stage_cost + slack_cost opti.minimize(total_cost) solver_opts = { "mu_strategy" : "adaptive", "mu_init" : 1e-5, "mu_min" : 1e-15, "barrier_tol_factor" : 1, "print_level" : 0, "linear_solver" : "ma27" } # solver_opts = {} plugin_opts = {"verbose" : False, "print_time" : False, "print_out" : False} opti.solver('ipopt', plugin_opts, solver_opts) sol = opti.solve() slack_val = sol.value(slack) if la.norm(slack_val) > 1e-6: print('Warning! Solved, but with slack norm of %g is greater than 1e-8!' % la.norm(slack_val)) if sol.stats()['success'] and la.norm(slack_val) <= 1e-6: print('Solve success, with slack norm of %g!' % la.norm(slack_val)) feasible = True x_pred = sol.value(x) u_pred = sol.value(u) sol_cost = sol.value(stage_cost) else: # print(sol.stats()['return_status']) # print(opti.debug.show_infeasibilities()) feasible = False x_pred = None u_pred = None sol_cost = None # pdb.set_trace() return x_pred, u_pred, sol_cost
# Load the dual if load_dual: with open(dual_location, 'r') as infile: dual_vals = json.load(infile) if len(dual_vals) != opti.ng: raise Exception( "Couldn't load the dual, since your problem has %i cons and the cached problem has %i cons." % (opti.ng, len(dual_vals))) for i in tqdm(range(opti.ng), desc="Loading dual variables:"): opti.set_initial(opti.lam_g[i], dual_vals[i]) sind = lambda theta: cas.sin(theta * cas.pi / 180) cosd = lambda theta: cas.cos(theta * cas.pi / 180) tand = lambda theta: cas.tan(theta * cas.pi / 180) atan2d = lambda y_val, x_val: cas.atan2(y_val, x_val) * 180 / np.pi clip = lambda x, min, max: cas.fmin(cas.fmax(min, x), max) def smoothmax(value1, value2, hardness): """ A smooth maximum between two functions. Useful because it's differentiable and convex! Great writeup by John D Cook here: https://www.johndcook.com/soft_maximum.pdf :param value1: Value of function 1. :param value2: Value of function 2. :param hardness: Hardness parameter. Higher values make this closer to max(x1, x2). :return: Soft maximum of the two supplied values. """ value1 = value1 * hardness
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 MPC_cost(v,K,S,S_i,x_i,goal,true_goal,signal_positive,signal_negative): #velocity, Horizon, Steering variables, Steering angle initial, start, goal, true_goal cost = 0 S = casadi.reshape(S,1,K) R = casadi.DM([[0.5,0],[0,0.5]]) # s, s_dot Q = casadi.DM([35]) # heading_error Qf = casadi.DM([100]) X = casadi.MX(3,K) #x,y,theta,heading_error S_vec = casadi.MX(2,1) # s and s_dot for i in range(K): if i==0: X[:,i] = dynamics_rear(x_i[:],S[i],casadi.DM([v])) #calculate new state S_vec[0] = S[i] S_vec[1] = S_i-S[i] else: X[:,i] = dynamics_rear(X[:,i-1],S[i],casadi.DM([v])) #calculate new state S_vec[0] = S[i] S_vec[1] = S[i]-S[i-1] """ Longitudinal control """ x = X[0,i] y = X[1,i] theta = X[2,i] """ heading error """ beta = casadi.atan2((goal[1]-y),(goal[0]-x)) heading_error = casadi.atan2(casadi.sin(beta - theta), casadi.cos(beta - theta)) e = heading_error #may be need MX here if signal_positive: e = (casadi.pi - e) if signal_negative: e = -(casadi.pi - casadi.fabs(e)) cost = cost + (casadi.mtimes(casadi.mtimes(S_vec.T,R),S_vec) + casadi.mtimes(casadi.mtimes(e.T,Q),e)) x = X[0,i] y = X[1,i] theta = X[2,i] beta = casadi.atan2((goal[1]-y),(goal[0]-x)) heading_error = casadi.atan2(casadi.sin(beta - theta), casadi.cos(beta - theta)) e = heading_error #may be need MX here if signal_positive: e = (casadi.pi - e) if signal_negative: e = -(casadi.pi - casadi.fabs(e)) cost = cost + casadi.mtimes(casadi.mtimes(e.T,Q),e) #terminal cost return cost
def build_opti0_solver(self, agt_idx): self.opti0 = ca.Opti() self.x0 = self.opti0.variable(self.n_x, self.N + 1) self.u0 = self.opti0.variable(self.n_u, self.N) self.x_s0 = self.opti0.parameter(self.n_x) self.x_f0 = self.opti0.parameter(self.n_x) self.agt_x0 = [] for i in range(agt_idx): self.agt_x0.append(self.opti0.parameter(self.n_x, self.N + 1)) self.opti0.set_value(self.x_f0, self.x_refs[self.x_refs_idx]) da_lim = self.agent.da_lim ddf_lim = self.agent.ddf_lim r = self.agent.r self.opti0.subject_to(self.x0[0, 0] == self.x_s0[0]) self.opti0.subject_to(self.x0[1, 0] == self.x_s0[1]) # self.opti0.subject_to(self.opti0.bounded(-2*np.pi, self.x0[2,0], 2*np.pi)) self.opti0.subject_to(self.x0[2, 0] == self.x_s0[2]) self.opti0.subject_to(self.x0[3, 0] == self.x_s0[3]) stage_cost = 0 for i in range(self.N): stage_cost += ca.bilin(self.Q, self.x0[:, i + 1] - self.x_f0, self.x0[:, i + 1] - self.x_f0) + ca.bilin( self.R, self.u0[:, i], self.u0[:, i]) beta = ca.atan2(self.l_r * ca.tan(self.u0[0, i]), self.l_f + self.l_r) self.opti0.subject_to( self.x0[0, i + 1] == self.x0[0, i] + self.dt * self.x0[3, i] * ca.cos(self.x0[2, i] + beta)) self.opti0.subject_to( self.x0[1, i + 1] == self.x0[1, i] + self.dt * self.x0[3, i] * ca.sin(self.x0[2, i] + beta)) self.opti0.subject_to(self.x0[2, i + 1] == self.x0[2, i] + self.dt * self.x0[3, i] * ca.sin(beta)) self.opti0.subject_to(self.x0[3, i + 1] == self.x0[3, i] + self.dt * self.u0[1, i]) if self.F is not None: self.opti0.subject_to( ca.mtimes(self.F, self.x0[:, i + 1]) <= self.b) if self.H is not None: self.opti0.subject_to( ca.mtimes(self.H, self.u0[:, i]) <= self.g) # Treat init and final states of agents before as obstacles for j in range(agt_idx): self.opti0.subject_to( ca.bilin(np.eye(2), self.x0[:2, i + 1] - self.agt_x0[j][:2, i + 1], self.x0[:2, i + 1] - self.agt_x0[j][:2, i + 1]) >= (1.5 * 2 * r)**2) if i < self.N - 1: stage_cost += ca.bilin(self.Rd, self.u0[:, i + 1] - self.u0[:, i], self.u0[:, i + 1] - self.u0[:, i]) self.opti0.subject_to( self.opti0.bounded(ddf_lim[0] * self.dt, self.u0[0, i + 1] - self.u0[0, i], ddf_lim[1] * self.dt)) self.opti0.subject_to( self.opti0.bounded(da_lim[0] * self.dt, self.u0[1, i + 1] - self.u0[1, i], da_lim[1] * self.dt)) self.cost0 = stage_cost self.opti0.minimize(self.cost0) solver_opts = { "mu_strategy": "adaptive", "mu_init": 1e-5, "mu_min": 1e-15, "barrier_tol_factor": 1, "print_level": 0, "linear_solver": "ma27" } # solver_opts = {} plugin_opts = { "verbose": False, "print_time": False, "print_out": False } self.opti0.solver('ipopt', plugin_opts, solver_opts)
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
def solve_opti(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False): opti = ca.Opti() x = opti.variable(self.n_x*self.n_a, N+1) u = opti.variable(self.n_u*self.n_a, N) slack = opti.variable(self.n_x*self.n_a) if x_guess is not None: opti.set_initial(x, x_guess) if u_guess is not None: opti.set_initial(u, u_guess) opti.set_initial(slack, np.zeros(self.n_x*self.n_a)) da_lim = self.da_lim ddf_lim = self.ddf_lim pairs = list(itertools.combinations(range(self.n_a), 2)) opti.subject_to(x[:,0] == np.squeeze(x_0)) for j in range(self.n_a): opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[j*self.n_u+0,0]-last_u[j*self.n_u+0], ddf_lim[1]*self.dt)) opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[j*self.n_u+1,0]-last_u[j*self.n_u+1], da_lim[1]*self.dt)) stage_cost = 0 for i in range(N): stage_cost = stage_cost + 1 for j in range(self.n_a): opti.subject_to(x[j*self.n_x+0,i+1] == x[j*self.n_x+0,i] + self.dt*x[j*self.n_x+3,i]*ca.cos(x[j*self.n_x+2,i] + ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r))) opti.subject_to(x[j*self.n_x+1,i+1] == x[j*self.n_x+1,i] + self.dt*x[j*self.n_x+3,i]*ca.sin(x[j*self.n_x+2,i] + ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r))) opti.subject_to(x[j*self.n_x+2,i+1] == x[j*self.n_x+2,i] + self.dt*x[j*self.n_x+3,i]*ca.sin(ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r))) opti.subject_to(x[j*self.n_x+3,i+1] == x[j*self.n_x+3,i] + self.dt*u[j*self.n_u+1,i]) if i < N-1: opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[j*self.n_u+0,i+1]-u[j*self.n_u+0,i], ddf_lim[1]*self.dt)) opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[j*self.n_u+1,i+1]-u[j*self.n_u+1,i], da_lim[1]*self.dt)) if self.F is not None: opti.subject_to(ca.mtimes(self.F, x[:,i]) <= self.b) if self.H is not None: opti.subject_to(ca.mtimes(self.H, u[:,i]) <= self.g) if expl_constraints is not None: V = expl_constraints[i][0] w = expl_constraints[i][1] opti.subject_to(ca.mtimes(V, x[:2,i]) + w <= np.zeros(len(w))) # Collision avoidance constraint for p in pairs: opti.subject_to(ca.norm_2(x[p[0]*self.n_x:p[0]*self.n_x+2,i] - x[p[1]*self.n_x:p[1]*self.n_x+2,i]) >= 2*self.r) opti.subject_to(x[:,N] - x_ss == slack) slack_cost = 1e6*ca.sumsqr(slack) total_cost = stage_cost + slack_cost opti.minimize(total_cost) solver_opts = { "mu_strategy" : "adaptive", "mu_init" : 1e-5, "mu_min" : 1e-15, "barrier_tol_factor" : 1, "print_level" : 0, "linear_solver" : "ma27" } # solver_opts = {} plugin_opts = {"verbose" : False, "print_time" : False, "print_out" : False} opti.solver('ipopt', plugin_opts, solver_opts) sol = opti.solve() slack_val = sol.value(slack) slack_valid = True for j in range(self.n_a): if la.norm(slack_val[j*self.n_x:(j+1)*self.n_x]) > 2e-8: slack_valid = False print('Warning! Solved, but with agent %i slack norm of %g is greater than 2e-8!' % (j+1, la.norm(slack_val[j*self.n_x:(j+1)*self.n_x]))) break if sol.stats()['success'] and slack_valid: feasible = True x_pred = sol.value(x) u_pred = sol.value(u) sol_cost = sol.value(stage_cost) else: # print(sol.stats()['return_status']) # print(opti.debug.show_infeasibilities()) feasible = False x_pred = None u_pred = None sol_cost = None # pdb.set_trace() return x_pred, u_pred, sol_cost
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
def atan2d(y, x): return cas.atan2(y, x) * 180 / np.pi
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
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