Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

work on acados lateral MPC #23558

Merged
merged 7 commits into from
Jan 18, 2022
Merged

work on acados lateral MPC #23558

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7e24b2c
lat_mpc: make v_ego, rotation_radius parameters instead of states
FreyJo Jan 17, 2022
1a1b288
lat_mpc: remove rotation_radius argument, since it is part of the par…
FreyJo Jan 17, 2022
efe9727
lat_mpc: use qp_solver_cond_N = 1
FreyJo Jan 17, 2022
f498eb6
adapt test_lateral_mpc to formulation with parameters
FreyJo Jan 17, 2022
8c6cdcd
lat_mpc: set parameters in reset() and copy values
FreyJo Jan 17, 2022
ef8a69d
acados_ocp_solver_pyx: make options_set useable
FreyJo Jan 18, 2022
7a51785
update ref
pd0wm Jan 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions pyextra/acados_template/acados_ocp_solver_pyx.pyx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -417,8 +417,9 @@ cdef class AcadosOcpSolverFast:
string_value = value_.encode('utf-8')
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])

raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
else:
raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))


def __del__(self):
Expand Down
33 changes: 20 additions & 13 deletions selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@
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()
Expand All @@ -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')
Expand All @@ -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
Expand All @@ -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, ))
Expand All @@ -96,15 +97,15 @@ 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'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
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
Expand All @@ -130,6 +131,7 @@ def reset(self, x0=np.zeros(X_DIM)):
# 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()
Expand All @@ -144,14 +146,19 @@ def set_weights(self, path_weight, heading_weight, steer_rate_weight):
#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()
Expand Down
10 changes: 5 additions & 5 deletions selfdrive/controls/lib/lateral_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,9 +61,9 @@ def __init__(self, CP, use_lanelines=True, wide_camera=False):
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)

Expand Down Expand Up @@ -175,10 +175,10 @@ def update(self, sm):

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
Expand Down
6 changes: 3 additions & 3 deletions selfdrive/controls/tests/test_lateral_mpc.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion selfdrive/test/process_replay/ref_commit
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1 +1 @@
280a712ece99c231ea036c3b66d6aafa55548211
ef8a69dd1e52e2441d5c6155836a676ff98950a6