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
parent
58a363e07d
commit
5387806400
|
@ -40,7 +40,7 @@ CRASH_DISTANCE = .5
|
||||||
LIMIT_COST = 1e6
|
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
|
# much better convergence of the MPC with low iterations
|
||||||
N = 12
|
N = 12
|
||||||
MAX_T = 10.0
|
MAX_T = 10.0
|
||||||
|
@ -84,9 +84,9 @@ def gen_long_model():
|
||||||
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
|
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
|
||||||
|
|
||||||
# live parameters
|
# live parameters
|
||||||
x_obstacle = SX.sym('x_obstacle')
|
|
||||||
a_min = SX.sym('a_min')
|
a_min = SX.sym('a_min')
|
||||||
a_max = SX.sym('a_max')
|
a_max = SX.sym('a_max')
|
||||||
|
x_obstacle = SX.sym('x_obstacle')
|
||||||
prev_a = SX.sym('prev_a')
|
prev_a = SX.sym('prev_a')
|
||||||
model.p = vertcat(a_min, a_max, x_obstacle, 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
|
# Constraints on speed, acceleration and desired distance to
|
||||||
# the obstacle, which is treated as a slack constraint so it
|
# the obstacle, which is treated as a slack constraint so it
|
||||||
# behaves like an assymetrical cost.
|
# behaves like an asymmetrical cost.
|
||||||
constraints = vertcat((v_ego),
|
constraints = vertcat(v_ego,
|
||||||
(a_ego - a_min),
|
(a_ego - a_min),
|
||||||
(a_max - a_ego),
|
(a_max - a_ego),
|
||||||
((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.))
|
((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)
|
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
|
||||||
|
|
||||||
# The HPIPM solver can give decent solutions even when it is stopped early
|
# 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
|
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
|
||||||
# does not cause issues since the problem is well bounded.
|
# does not cause issues since the problem is well bounded.
|
||||||
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
|
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
|
||||||
|
@ -190,7 +190,7 @@ def gen_long_mpc_solver():
|
||||||
return ocp
|
return ocp
|
||||||
|
|
||||||
|
|
||||||
class LongitudinalMpc():
|
class LongitudinalMpc:
|
||||||
def __init__(self, e2e=False):
|
def __init__(self, e2e=False):
|
||||||
self.e2e = e2e
|
self.e2e = e2e
|
||||||
self.reset()
|
self.reset()
|
||||||
|
@ -201,10 +201,10 @@ class LongitudinalMpc():
|
||||||
|
|
||||||
def reset(self):
|
def reset(self):
|
||||||
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
|
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
|
||||||
self.v_solution = [0.0 for i in range(N+1)]
|
self.v_solution = np.zeros(N+1)
|
||||||
self.a_solution = [0.0 for i in range(N+1)]
|
self.a_solution = np.zeros(N+1)
|
||||||
self.prev_a = np.array(self.a_solution)
|
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))
|
self.yref = np.zeros((N+1, COST_DIM))
|
||||||
for i in range(N):
|
for i in range(N):
|
||||||
self.solver.cost_set(i, "yref", self.yref[i])
|
self.solver.cost_set(i, "yref", self.yref[i])
|
||||||
|
@ -264,7 +264,8 @@ class LongitudinalMpc():
|
||||||
self.x0[1] = v
|
self.x0[1] = v
|
||||||
self.x0[2] = a
|
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.)
|
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)
|
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)
|
x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
|
||||||
|
@ -351,8 +352,8 @@ class LongitudinalMpc():
|
||||||
self.accel_limit_arr[:,1] = 10.
|
self.accel_limit_arr[:,1] = 10.
|
||||||
x_obstacle = 1e5*np.ones(N+1)
|
x_obstacle = 1e5*np.ones(N+1)
|
||||||
self.params = np.concatenate([self.accel_limit_arr,
|
self.params = np.concatenate([self.accel_limit_arr,
|
||||||
x_obstacle[:,None],
|
x_obstacle[:, None],
|
||||||
self.prev_a[:,None]], axis=1)
|
self.prev_a[:,None]], axis=1)
|
||||||
self.run()
|
self.run()
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue