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
