diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 68365f8d21..4ef952a574 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4ec5899c033238f4815409cabdb109b5e7b833db9018a54345e927885bf12d1a -size 17627 +oid sha256:9df0412c0f77fbf72ceb0ba8186e683f6b467521b7707156d7e2baa1f5d88430 +size 17649 diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index eb56e20177..eeda25627a 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -17,7 +17,8 @@ else: LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = "acados_ocp_lat.json" -X_DIM = 6 +X_DIM = 4 +P_DIM = 2 def gen_lat_model(): model = AcadosModel() @@ -28,9 +29,12 @@ def gen_lat_model(): y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') curv_ego = SX.sym('curv_ego') + model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) + + # parameters v_ego = SX.sym('v_ego') rotation_radius = SX.sym('rotation_radius') - model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius) + model.p = vertcat(v_ego, rotation_radius) # controls curv_rate = SX.sym('curv_rate') @@ -41,18 +45,14 @@ def gen_lat_model(): y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') curv_ego_dot = SX.sym('curv_ego_dot') - v_ego_dot = SX.sym('v_ego_dot') - rotation_radius_dot = SX.sym('rotation_radius_dot') - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot, - v_ego_dot, rotation_radius_dot) + + model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) # dynamics model f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), v_ego * curv_ego, - curv_rate, - 0.0, - 0.0) + curv_rate) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model @@ -79,8 +79,9 @@ def gen_lat_mpc_solver(): y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] curv_rate = ocp.model.u[0] - v_ego = ocp.model.x[4] + v_ego = ocp.model.p[0] + ocp.parameter_values = np.zeros((P_DIM, )) ocp.cost.yref = np.zeros((3, )) ocp.cost.yref_e = np.zeros((2, )) @@ -96,7 +97,7 @@ def gen_lat_mpc_solver(): ocp.constraints.idxbx = np.array([2,3]) ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)]) ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)]) - x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + x0 = np.zeros((X_DIM,)) ocp.constraints.x0 = x0 ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' @@ -104,7 +105,7 @@ def gen_lat_mpc_solver(): ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP_RTI' ocp.solver_options.qp_solver_iter_max = 1 - ocp.solver_options.qp_solver_cond_N = N//4 + ocp.solver_options.qp_solver_cond_N = 1 # set prediction horizon ocp.solver_options.tf = Tf @@ -130,6 +131,7 @@ class LateralMpc(): # Somehow needed for stable init for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) + self.solver.set(i, 'p', np.zeros(P_DIM)) self.solver.constraints_set(0, "lbx", x0) self.solver.constraints_set(0, "ubx", x0) self.solver.solve() @@ -144,14 +146,19 @@ class LateralMpc(): #TODO hacky weights to keep behavior the same self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) - def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts): + def run(self, x0, p, y_pts, heading_pts): x0_cp = np.copy(x0) + p_cp = np.copy(p) self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts + v_ego = p_cp[0] + # rotation_radius = p_cp[1] self.yref[:,1] = heading_pts*(v_ego+5.0) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) + self.solver.set(i, "p", p_cp) + self.solver.set(N, "p", p_cp) self.solver.cost_set(N, "yref", self.yref[N][:2]) t = sec_since_boot() diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 7ff4fc954e..31bd42a84f 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -61,9 +61,9 @@ class LateralPlanner: self.y_pts = np.zeros(TRAJECTORY_SIZE) self.lat_mpc = LateralMpc() - self.reset_mpc(np.zeros(6)) + self.reset_mpc(np.zeros(4)) - def reset_mpc(self, x0=np.zeros(6)): + def reset_mpc(self, x0=np.zeros(4)): self.x0 = x0 self.lat_mpc.reset(x0=self.x0) @@ -175,10 +175,10 @@ class LateralPlanner: assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 - self.x0[4] = v_ego + # self.x0[4] = v_ego + p = np.array([v_ego, CAR_ROTATION_RADIUS]) self.lat_mpc.run(self.x0, - v_ego, - CAR_ROTATION_RADIUS, + p, y_pts, heading_pts) # init state for next diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 7c4fe7b6ad..4630e28a61 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -14,12 +14,12 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur y_pts = poly_shift * np.ones(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1) - x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS]) + x0 = np.array([x_init, y_init, psi_init, curvature_init]) + p = np.array([v_ref, CAR_ROTATION_RADIUS]) # converge in no more than 10 iterations for _ in range(10): - lat_mpc.run(x0, v_ref, - CAR_ROTATION_RADIUS, + lat_mpc.run(x0, p, y_pts, heading_pts) return lat_mpc.x_sol diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2d3c904da9..38f04c3d3e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -280a712ece99c231ea036c3b66d6aafa55548211 \ No newline at end of file +ef8a69dd1e52e2441d5c6155836a676ff98950a6 \ No newline at end of file