Cython acados and minor (#23835)
* acados_ocp_solver_pyx.pyx: implement get_stats for timings and ints * long_mpc: use acados timers * acados_ocp_solver_pyx.pyx: fix dynamics_get * acados_ocp_solver_pyx.pyx: get statistics * use acados_ocp_solver_pyx.pyx from commaai/cython2 branch * acados_ocp_solver_pyx.pyx: implement store_iterate * acados_ocp_solver_pyx.pyx: implement get_residuals * acados_ocp_solver_pyx.pyx: fix set() for empty fields * acados_ocp_solver_pyx.pyx: load_iterate * cython acados: add print_statistics * test_following_distance: fix typo * test_longitudinal: unique names for test maneuvers * longitudinal MPC: comments for evaluation * longitudinal MPC: add comments to eval acados residuals * long_mpc: use qp_solver_cond_N = 1 * long MPC: comments, simplify set_cur_state * update acados version in build script * longitudinal mpc: weigh a_change in 1 place only * update ref * Update ref Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>pull/23865/head
Jonathan Frey
GitHub
parent
ca8d4e417e
commit
d09dffb7cd
|
|
@ -44,6 +44,8 @@ cimport acados_solver
|
|||
cimport numpy as cnp
|
||||
|
||||
import os
|
||||
import json
|
||||
from datetime import datetime
|
||||
import numpy as np
|
||||
|
||||
|
||||
|
|
@ -51,9 +53,8 @@ cdef class AcadosOcpSolverFast:
|
|||
"""
|
||||
Class to interact with the acados ocp solver C object.
|
||||
|
||||
:param acados_ocp: type AcadosOcp - description of the OCP for acados
|
||||
:param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
|
||||
:param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
|
||||
:param model_name:
|
||||
:param N:
|
||||
"""
|
||||
|
||||
cdef acados_solver.nlp_solver_capsule *capsule
|
||||
|
|
@ -68,7 +69,7 @@ cdef class AcadosOcpSolverFast:
|
|||
cdef int N
|
||||
cdef bint solver_created
|
||||
|
||||
def __cinit__(self, str model_name, int N, str code_export_dir):
|
||||
def __cinit__(self, str model_name, int N):
|
||||
self.model_name = model_name
|
||||
self.N = N
|
||||
|
||||
|
|
@ -168,7 +169,7 @@ cdef class AcadosOcpSolverFast:
|
|||
- qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
|
||||
- qp_res_comp: residual wrt complementarity conditions of the last QP solution
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
acados_solver.acados_print_stats(self.capsule)
|
||||
|
||||
|
||||
def store_iterate(self, filename='', overwrite=False):
|
||||
|
|
@ -178,14 +179,48 @@ cdef class AcadosOcpSolverFast:
|
|||
:param filename: if not set, use model_name + timestamp + '.json'
|
||||
:param overwrite: if false and filename exists add timestamp to filename
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
if filename == '':
|
||||
filename += self.model_name + '_' + 'iterate' + '.json'
|
||||
|
||||
if not overwrite:
|
||||
# append timestamp
|
||||
if os.path.isfile(filename):
|
||||
filename = filename[:-5]
|
||||
filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
|
||||
|
||||
# get iterate:
|
||||
solution = dict()
|
||||
|
||||
for i in range(self.N+1):
|
||||
solution['x_'+str(i)] = self.get(i,'x')
|
||||
solution['u_'+str(i)] = self.get(i,'u')
|
||||
solution['z_'+str(i)] = self.get(i,'z')
|
||||
solution['lam_'+str(i)] = self.get(i,'lam')
|
||||
solution['t_'+str(i)] = self.get(i, 't')
|
||||
solution['sl_'+str(i)] = self.get(i, 'sl')
|
||||
solution['su_'+str(i)] = self.get(i, 'su')
|
||||
for i in range(self.N):
|
||||
solution['pi_'+str(i)] = self.get(i,'pi')
|
||||
|
||||
# save
|
||||
with open(filename, 'w') as f:
|
||||
json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True)
|
||||
print("stored current iterate in ", os.path.join(os.getcwd(), filename))
|
||||
|
||||
|
||||
def load_iterate(self, filename):
|
||||
"""
|
||||
Loads the iterate stored in json file with filename into the ocp solver.
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
if not os.path.isfile(filename):
|
||||
raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
|
||||
|
||||
with open(filename, 'r') as f:
|
||||
solution = json.load(f)
|
||||
|
||||
for key in solution.keys():
|
||||
(field, stage) = key.split('_')
|
||||
self.set(int(stage), field, np.array(solution[key]))
|
||||
|
||||
|
||||
def get_stats(self, field_):
|
||||
|
|
@ -194,7 +229,67 @@ cdef class AcadosOcpSolverFast:
|
|||
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
fields = ['time_tot', # total cpu time previous call
|
||||
'time_lin', # cpu time for linearization
|
||||
'time_sim', # cpu time for integrator
|
||||
'time_sim_ad', # cpu time for integrator contribution of external function calls
|
||||
'time_sim_la', # cpu time for integrator contribution of linear algebra
|
||||
'time_qp', # cpu time qp solution
|
||||
'time_qp_solver_call', # cpu time inside qp solver (without converting the QP)
|
||||
'time_qp_xcond',
|
||||
'time_glob', # cpu time globalization
|
||||
'time_reg', # cpu time regularization
|
||||
'sqp_iter', # number of SQP iterations
|
||||
'qp_iter', # vector of QP iterations for last SQP call
|
||||
'statistics', # table with info about last iteration
|
||||
'stat_m',
|
||||
'stat_n',]
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
if (field_ not in fields):
|
||||
raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(fields, fields))
|
||||
|
||||
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
|
||||
return self.__get_stat_int(field)
|
||||
|
||||
elif field_.startswith('time'):
|
||||
return self.__get_stat_double(field)
|
||||
|
||||
elif field_ == 'statistics':
|
||||
sqp_iter = self.get_stats("sqp_iter")
|
||||
stat_m = self.get_stats("stat_m")
|
||||
stat_n = self.get_stats("stat_n")
|
||||
min_size = min([stat_m, sqp_iter+1])
|
||||
return self.__get_stat_matrix(field, stat_n+1, min_size)
|
||||
|
||||
elif field_ == 'qp_iter':
|
||||
NotImplementedError("TODO! Cython not aware if SQP or SQP_RTI")
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
out = full_stats[6, :]
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
out = full_stats[2, :]
|
||||
else:
|
||||
NotImplementedError("TODO!")
|
||||
|
||||
|
||||
def __get_stat_int(self, field):
|
||||
cdef int out
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &out)
|
||||
return out
|
||||
|
||||
def __get_stat_double(self, field):
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,))
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
|
||||
return out
|
||||
|
||||
def __get_stat_matrix(self, field, n, m):
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64)
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out_mat.data)
|
||||
return out_mat
|
||||
|
||||
|
||||
def get_cost(self):
|
||||
|
|
@ -217,7 +312,32 @@ cdef class AcadosOcpSolverFast:
|
|||
"""
|
||||
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
# TODO: check if RTI, only eval then
|
||||
# if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
# compute residuals if RTI
|
||||
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output array
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
|
||||
cdef double double_value
|
||||
|
||||
field = "res_stat".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[0] = double_value
|
||||
|
||||
field = "res_eq".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[1] = double_value
|
||||
|
||||
field = "res_ineq".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[2] = double_value
|
||||
|
||||
field = "res_comp".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[3] = double_value
|
||||
|
||||
return out
|
||||
|
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set
|
||||
|
|
@ -246,12 +366,11 @@ cdef class AcadosOcpSolverFast:
|
|||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
cdef double[::1] value
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64)
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
value = np.ascontiguousarray(value_, dtype=np.double)
|
||||
assert acados_solver.acados_update_params(self.capsule, stage, <double *> &value[0], value.shape[0]) == 0
|
||||
assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
|
||||
else:
|
||||
if field_ not in constraints_fields + cost_fields + out_fields:
|
||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
|
||||
|
|
@ -266,16 +385,15 @@ cdef class AcadosOcpSolverFast:
|
|||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
|
||||
raise Exception(msg)
|
||||
|
||||
value = np.ascontiguousarray(value_, dtype=np.double)
|
||||
if field_ in constraints_fields:
|
||||
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
|
||||
elif field_ in cost_fields:
|
||||
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
|
||||
elif field_ in out_fields:
|
||||
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, stage, field, <void *> &value[0])
|
||||
self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
|
||||
|
||||
|
||||
def cost_set(self, int stage, str field_, value_):
|
||||
|
|
@ -361,7 +479,7 @@ cdef class AcadosOcpSolverFast:
|
|||
acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
|
||||
|
||||
# create output data
|
||||
out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F')
|
||||
|
||||
# call getter
|
||||
acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
|
||||
|
|
|
|||
|
|
@ -117,7 +117,7 @@ def gen_lat_mpc_solver():
|
|||
|
||||
class LateralMpc():
|
||||
def __init__(self, x0=np.zeros(X_DIM)):
|
||||
self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
|
||||
self.solver = AcadosOcpSolverFast('lat', N)
|
||||
self.reset(x0)
|
||||
|
||||
def reset(self, x0=np.zeros(X_DIM)):
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ SOURCES = ['lead0', 'lead1', 'cruise']
|
|||
|
||||
X_DIM = 3
|
||||
U_DIM = 1
|
||||
PARAM_DIM= 4
|
||||
PARAM_DIM = 4
|
||||
COST_E_DIM = 5
|
||||
COST_DIM = COST_E_DIM + 1
|
||||
CONSTR_DIM = 4
|
||||
|
|
@ -34,7 +34,7 @@ X_EGO_COST = 0.
|
|||
V_EGO_COST = 0.
|
||||
A_EGO_COST = 0.
|
||||
J_EGO_COST = 5.0
|
||||
A_CHANGE_COST = .5
|
||||
A_CHANGE_COST = 200.
|
||||
DANGER_ZONE_COST = 100.
|
||||
CRASH_DISTANCE = .5
|
||||
LIMIT_COST = 1e6
|
||||
|
|
@ -136,7 +136,7 @@ def gen_long_mpc_solver():
|
|||
x_ego,
|
||||
v_ego,
|
||||
a_ego,
|
||||
20*(a_ego - prev_a),
|
||||
a_ego - prev_a,
|
||||
j_ego]
|
||||
ocp.model.cost_y_expr = vertcat(*costs)
|
||||
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
|
||||
|
|
@ -176,7 +176,7 @@ def gen_long_mpc_solver():
|
|||
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_cond_N = N//4
|
||||
ocp.solver_options.qp_solver_cond_N = 1
|
||||
|
||||
# More iterations take too much time and less lead to inaccurate convergence in
|
||||
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
|
||||
|
|
@ -197,7 +197,7 @@ class LongitudinalMpc:
|
|||
self.source = SOURCES[2]
|
||||
|
||||
def reset(self):
|
||||
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
|
||||
self.solver = AcadosOcpSolverFast('long', N)
|
||||
self.v_solution = np.zeros(N+1)
|
||||
self.a_solution = np.zeros(N+1)
|
||||
self.prev_a = np.array(self.a_solution)
|
||||
|
|
@ -215,7 +215,11 @@ class LongitudinalMpc:
|
|||
self.status = False
|
||||
self.crash_cnt = 0.0
|
||||
self.solution_status = 0
|
||||
# timers
|
||||
self.solve_time = 0.0
|
||||
self.time_qp_solution = 0.0
|
||||
self.time_linearization = 0.0
|
||||
self.time_integrator = 0.0
|
||||
self.x0 = np.zeros(X_DIM)
|
||||
self.set_weights()
|
||||
|
||||
|
|
@ -232,6 +236,7 @@ class LongitudinalMpc:
|
|||
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
|
||||
W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]))
|
||||
for i in range(N):
|
||||
# reduce the cost on (a-a_prev) later in the horizon.
|
||||
W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
|
||||
self.solver.cost_set(i, 'W', W)
|
||||
# Setting the slice without the copy make the array not contiguous,
|
||||
|
|
@ -257,14 +262,12 @@ class LongitudinalMpc:
|
|||
self.solver.cost_set(i, 'Zl', Zl)
|
||||
|
||||
def set_cur_state(self, v, a):
|
||||
if abs(self.x0[1] - v) > 2.:
|
||||
self.x0[1] = v
|
||||
self.x0[2] = a
|
||||
v_prev = self.x0[1]
|
||||
self.x0[1] = v
|
||||
self.x0[2] = a
|
||||
if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
|
||||
for i in range(0, N+1):
|
||||
self.solver.set(i, 'x', self.x0)
|
||||
else:
|
||||
self.x0[1] = v
|
||||
self.x0[2] = a
|
||||
|
||||
@staticmethod
|
||||
def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
|
||||
|
|
@ -355,9 +358,17 @@ class LongitudinalMpc:
|
|||
self.solver.constraints_set(0, "lbx", self.x0)
|
||||
self.solver.constraints_set(0, "ubx", self.x0)
|
||||
|
||||
t = sec_since_boot()
|
||||
self.solution_status = self.solver.solve()
|
||||
self.solve_time = sec_since_boot() - t
|
||||
self.solve_time = float(self.solver.get_stats('time_tot')[0])
|
||||
self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
|
||||
self.time_linearization = float(self.solver.get_stats('time_lin')[0])
|
||||
self.time_integrator = float(self.solver.get_stats('time_sim')[0])
|
||||
|
||||
# qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
|
||||
# print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
|
||||
# res = self.solver.get_residuals()
|
||||
# print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
|
||||
# self.solver.print_statistics()
|
||||
|
||||
for i in range(N+1):
|
||||
self.x_sol[i] = self.solver.get(i, 'x')
|
||||
|
|
@ -370,6 +381,7 @@ class LongitudinalMpc:
|
|||
|
||||
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
|
||||
|
||||
t = sec_since_boot()
|
||||
if self.solution_status != 0:
|
||||
if t > self.last_cloudlog_t + 5.0:
|
||||
self.last_cloudlog_t = t
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0):
|
|||
|
||||
|
||||
class TestFollowingDistance(unittest.TestCase):
|
||||
def test_following_distanc(self):
|
||||
def test_following_distance(self):
|
||||
for speed in np.arange(0, 40, 5):
|
||||
print(f'Testing {speed} m/s')
|
||||
v_lead = float(speed)
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
|||
# TODO: make new FCW tests
|
||||
maneuvers = [
|
||||
Maneuver(
|
||||
'approach stopped car at 20m/s',
|
||||
'approach stopped car at 20m/s, initial distance: 120m',
|
||||
duration=20.,
|
||||
initial_speed=25.,
|
||||
lead_relevancy=True,
|
||||
|
|
@ -18,7 +18,7 @@ maneuvers = [
|
|||
breakpoints=[0., 1.],
|
||||
),
|
||||
Maneuver(
|
||||
'approach stopped car at 20m/s',
|
||||
'approach stopped car at 20m/s, initial distance 90m',
|
||||
duration=20.,
|
||||
initial_speed=20.,
|
||||
lead_relevancy=True,
|
||||
|
|
@ -65,7 +65,7 @@ maneuvers = [
|
|||
breakpoints=[2., 2.01, 8.8],
|
||||
),
|
||||
Maneuver(
|
||||
"approach stopped car at 20m/s",
|
||||
"approach stopped car at 20m/s, with prob_lead_values",
|
||||
duration=30.,
|
||||
initial_speed=20.,
|
||||
lead_relevancy=True,
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
67c8f283858998b75ac28879e1350a589a968e5d
|
||||
7e6072a254791e4106a15ecbf94c16f40d54b459
|
||||
|
|
@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then
|
|||
fi
|
||||
cd acados_repo
|
||||
git fetch
|
||||
git checkout 79e9e3e76f2751198858adf382c97837833ad31f
|
||||
git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8
|
||||
git submodule update --recursive --init
|
||||
|
||||
# build
|
||||
|
|
|
|||
Loading…
Reference in New Issue