spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

LongitudinalMpc: minor clean up (#23296) 

* correct order

* formatting

* Revert "formatting"

This reverts commit 481c390f40.

* use np.zeros

* typos and formatting

* typo

* typo
pull/23304/head
Shane Smiskol 2021-12-28 04:05:52 -07:00 committed by GitHub
parent 58a363e07d
commit 5387806400
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@ -40,7 +40,7 @@ CRASH_DISTANCE = .5
LIMIT_COST = 1e6
# Less timestamps doesn't hurt performance and leads to
# Fewer timestamps don't hurt performance and lead to
# much better convergence of the MPC with low iterations
N = 12
MAX_T = 10.0
@ -84,9 +84,9 @@ def gen_long_model():
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_obstacle = SX.sym('x_obstacle')
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
x_obstacle = SX.sym('x_obstacle')
prev_a = SX.sym('prev_a')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a)
@ -143,8 +143,8 @@ def gen_long_mpc_solver():
# Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it
# behaves like an assymetrical cost.
constraints = vertcat((v_ego),
# behaves like an asymmetrical cost.
constraints = vertcat(v_ego,
(a_ego - a_min),
(a_max - a_ego),
((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.))
@ -169,7 +169,7 @@ def gen_long_mpc_solver():
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
# The HPIPM solver can give decent solutions even when it is stopped early
# Which is critical for our purpose where the compute time is strictly bounded
# Which is critical for our purpose where compute time is strictly bounded
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
# does not cause issues since the problem is well bounded.
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
@ -190,7 +190,7 @@ def gen_long_mpc_solver():
return ocp
class LongitudinalMpc():
class LongitudinalMpc:
def __init__(self, e2e=False):
self.e2e = e2e
self.reset()
@ -201,10 +201,10 @@ class LongitudinalMpc():
def reset(self):
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
self.v_solution = [0.0 for i in range(N+1)]
self.a_solution = [0.0 for i in range(N+1)]
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.prev_a = np.array(self.a_solution)
self.j_solution = [0.0 for i in range(N)]
self.j_solution = np.zeros(N)
self.yref = np.zeros((N+1, COST_DIM))
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
@ -264,7 +264,8 @@ class LongitudinalMpc():
self.x0[1] = v
self.x0[2] = a
def extrapolate_lead(self, x_lead, v_lead, a_lead, a_lead_tau):
@staticmethod
def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
@ -351,8 +352,8 @@ class LongitudinalMpc():
self.accel_limit_arr[:,1] = 10.
x_obstacle = 1e5*np.ones(N+1)
self.params = np.concatenate([self.accel_limit_arr,
x_obstacle[:,None],
self.prev_a[:,None]], axis=1)
x_obstacle[:, None],
self.prev_a[:,None]], axis=1)
self.run()