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
parent
ca8d4e417e
commit
d09dffb7cd
|
@ -44,6 +44,8 @@ cimport acados_solver
|
||||||
cimport numpy as cnp
|
cimport numpy as cnp
|
||||||
|
|
||||||
import os
|
import os
|
||||||
|
import json
|
||||||
|
from datetime import datetime
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
@ -51,9 +53,8 @@ cdef class AcadosOcpSolverFast:
|
||||||
"""
|
"""
|
||||||
Class to interact with the acados ocp solver C object.
|
Class to interact with the acados ocp solver C object.
|
||||||
|
|
||||||
:param acados_ocp: type AcadosOcp - description of the OCP for acados
|
:param model_name:
|
||||||
:param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
|
:param N:
|
||||||
:param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
cdef acados_solver.nlp_solver_capsule *capsule
|
cdef acados_solver.nlp_solver_capsule *capsule
|
||||||
|
@ -68,7 +69,7 @@ cdef class AcadosOcpSolverFast:
|
||||||
cdef int N
|
cdef int N
|
||||||
cdef bint solver_created
|
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.model_name = model_name
|
||||||
self.N = N
|
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_ineq: residual wrt inequality constraints (constraints) of the last QP solution
|
||||||
- qp_res_comp: residual wrt complementarity conditions 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):
|
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 filename: if not set, use model_name + timestamp + '.json'
|
||||||
:param overwrite: if false and filename exists add timestamp to filename
|
: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):
|
def load_iterate(self, filename):
|
||||||
"""
|
"""
|
||||||
Loads the iterate stored in json file with filename into the ocp solver.
|
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_):
|
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']
|
: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):
|
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].
|
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
|
# 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')
|
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
|
# treat parameters separately
|
||||||
if field_ == 'p':
|
if field_ == 'p':
|
||||||
value = np.ascontiguousarray(value_, dtype=np.double)
|
assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
|
||||||
assert acados_solver.acados_update_params(self.capsule, stage, <double *> &value[0], value.shape[0]) == 0
|
|
||||||
else:
|
else:
|
||||||
if field_ not in constraints_fields + cost_fields + out_fields:
|
if field_ not in constraints_fields + cost_fields + out_fields:
|
||||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
|
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])
|
msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
|
||||||
raise Exception(msg)
|
raise Exception(msg)
|
||||||
|
|
||||||
value = np.ascontiguousarray(value_, dtype=np.double)
|
|
||||||
if field_ in constraints_fields:
|
if field_ in constraints_fields:
|
||||||
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
|
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:
|
elif field_ in cost_fields:
|
||||||
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
|
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:
|
elif field_ in out_fields:
|
||||||
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
|
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_):
|
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])
|
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
|
# 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
|
# call getter
|
||||||
acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
|
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():
|
class LateralMpc():
|
||||||
def __init__(self, x0=np.zeros(X_DIM)):
|
def __init__(self, x0=np.zeros(X_DIM)):
|
||||||
self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
|
self.solver = AcadosOcpSolverFast('lat', N)
|
||||||
self.reset(x0)
|
self.reset(x0)
|
||||||
|
|
||||||
def reset(self, x0=np.zeros(X_DIM)):
|
def reset(self, x0=np.zeros(X_DIM)):
|
||||||
|
|
|
@ -24,7 +24,7 @@ SOURCES = ['lead0', 'lead1', 'cruise']
|
||||||
|
|
||||||
X_DIM = 3
|
X_DIM = 3
|
||||||
U_DIM = 1
|
U_DIM = 1
|
||||||
PARAM_DIM= 4
|
PARAM_DIM = 4
|
||||||
COST_E_DIM = 5
|
COST_E_DIM = 5
|
||||||
COST_DIM = COST_E_DIM + 1
|
COST_DIM = COST_E_DIM + 1
|
||||||
CONSTR_DIM = 4
|
CONSTR_DIM = 4
|
||||||
|
@ -34,7 +34,7 @@ X_EGO_COST = 0.
|
||||||
V_EGO_COST = 0.
|
V_EGO_COST = 0.
|
||||||
A_EGO_COST = 0.
|
A_EGO_COST = 0.
|
||||||
J_EGO_COST = 5.0
|
J_EGO_COST = 5.0
|
||||||
A_CHANGE_COST = .5
|
A_CHANGE_COST = 200.
|
||||||
DANGER_ZONE_COST = 100.
|
DANGER_ZONE_COST = 100.
|
||||||
CRASH_DISTANCE = .5
|
CRASH_DISTANCE = .5
|
||||||
LIMIT_COST = 1e6
|
LIMIT_COST = 1e6
|
||||||
|
@ -136,7 +136,7 @@ def gen_long_mpc_solver():
|
||||||
x_ego,
|
x_ego,
|
||||||
v_ego,
|
v_ego,
|
||||||
a_ego,
|
a_ego,
|
||||||
20*(a_ego - prev_a),
|
a_ego - prev_a,
|
||||||
j_ego]
|
j_ego]
|
||||||
ocp.model.cost_y_expr = vertcat(*costs)
|
ocp.model.cost_y_expr = vertcat(*costs)
|
||||||
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
|
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.hessian_approx = 'GAUSS_NEWTON'
|
||||||
ocp.solver_options.integrator_type = 'ERK'
|
ocp.solver_options.integrator_type = 'ERK'
|
||||||
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
|
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
|
# 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.
|
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
|
||||||
|
@ -197,7 +197,7 @@ class LongitudinalMpc:
|
||||||
self.source = SOURCES[2]
|
self.source = SOURCES[2]
|
||||||
|
|
||||||
def reset(self):
|
def reset(self):
|
||||||
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
|
self.solver = AcadosOcpSolverFast('long', N)
|
||||||
self.v_solution = np.zeros(N+1)
|
self.v_solution = np.zeros(N+1)
|
||||||
self.a_solution = np.zeros(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)
|
||||||
|
@ -215,7 +215,11 @@ class LongitudinalMpc:
|
||||||
self.status = False
|
self.status = False
|
||||||
self.crash_cnt = 0.0
|
self.crash_cnt = 0.0
|
||||||
self.solution_status = 0
|
self.solution_status = 0
|
||||||
|
# timers
|
||||||
self.solve_time = 0.0
|
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.x0 = np.zeros(X_DIM)
|
||||||
self.set_weights()
|
self.set_weights()
|
||||||
|
|
||||||
|
@ -232,6 +236,7 @@ class LongitudinalMpc:
|
||||||
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
|
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]))
|
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):
|
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])
|
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)
|
self.solver.cost_set(i, 'W', W)
|
||||||
# Setting the slice without the copy make the array not contiguous,
|
# Setting the slice without the copy make the array not contiguous,
|
||||||
|
@ -257,14 +262,12 @@ class LongitudinalMpc:
|
||||||
self.solver.cost_set(i, 'Zl', Zl)
|
self.solver.cost_set(i, 'Zl', Zl)
|
||||||
|
|
||||||
def set_cur_state(self, v, a):
|
def set_cur_state(self, v, a):
|
||||||
if abs(self.x0[1] - v) > 2.:
|
v_prev = self.x0[1]
|
||||||
self.x0[1] = v
|
self.x0[1] = v
|
||||||
self.x0[2] = a
|
self.x0[2] = a
|
||||||
|
if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
|
||||||
for i in range(0, N+1):
|
for i in range(0, N+1):
|
||||||
self.solver.set(i, 'x', self.x0)
|
self.solver.set(i, 'x', self.x0)
|
||||||
else:
|
|
||||||
self.x0[1] = v
|
|
||||||
self.x0[2] = a
|
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
|
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, "lbx", self.x0)
|
||||||
self.solver.constraints_set(0, "ubx", self.x0)
|
self.solver.constraints_set(0, "ubx", self.x0)
|
||||||
|
|
||||||
t = sec_since_boot()
|
|
||||||
self.solution_status = self.solver.solve()
|
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):
|
for i in range(N+1):
|
||||||
self.x_sol[i] = self.solver.get(i, 'x')
|
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)
|
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
|
||||||
|
|
||||||
|
t = sec_since_boot()
|
||||||
if self.solution_status != 0:
|
if self.solution_status != 0:
|
||||||
if t > self.last_cloudlog_t + 5.0:
|
if t > self.last_cloudlog_t + 5.0:
|
||||||
self.last_cloudlog_t = t
|
self.last_cloudlog_t = t
|
||||||
|
|
|
@ -21,7 +21,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0):
|
||||||
|
|
||||||
|
|
||||||
class TestFollowingDistance(unittest.TestCase):
|
class TestFollowingDistance(unittest.TestCase):
|
||||||
def test_following_distanc(self):
|
def test_following_distance(self):
|
||||||
for speed in np.arange(0, 40, 5):
|
for speed in np.arange(0, 40, 5):
|
||||||
print(f'Testing {speed} m/s')
|
print(f'Testing {speed} m/s')
|
||||||
v_lead = float(speed)
|
v_lead = float(speed)
|
||||||
|
|
|
@ -9,7 +9,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||||
# TODO: make new FCW tests
|
# TODO: make new FCW tests
|
||||||
maneuvers = [
|
maneuvers = [
|
||||||
Maneuver(
|
Maneuver(
|
||||||
'approach stopped car at 20m/s',
|
'approach stopped car at 20m/s, initial distance: 120m',
|
||||||
duration=20.,
|
duration=20.,
|
||||||
initial_speed=25.,
|
initial_speed=25.,
|
||||||
lead_relevancy=True,
|
lead_relevancy=True,
|
||||||
|
@ -18,7 +18,7 @@ maneuvers = [
|
||||||
breakpoints=[0., 1.],
|
breakpoints=[0., 1.],
|
||||||
),
|
),
|
||||||
Maneuver(
|
Maneuver(
|
||||||
'approach stopped car at 20m/s',
|
'approach stopped car at 20m/s, initial distance 90m',
|
||||||
duration=20.,
|
duration=20.,
|
||||||
initial_speed=20.,
|
initial_speed=20.,
|
||||||
lead_relevancy=True,
|
lead_relevancy=True,
|
||||||
|
@ -65,7 +65,7 @@ maneuvers = [
|
||||||
breakpoints=[2., 2.01, 8.8],
|
breakpoints=[2., 2.01, 8.8],
|
||||||
),
|
),
|
||||||
Maneuver(
|
Maneuver(
|
||||||
"approach stopped car at 20m/s",
|
"approach stopped car at 20m/s, with prob_lead_values",
|
||||||
duration=30.,
|
duration=30.,
|
||||||
initial_speed=20.,
|
initial_speed=20.,
|
||||||
lead_relevancy=True,
|
lead_relevancy=True,
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
67c8f283858998b75ac28879e1350a589a968e5d
|
7e6072a254791e4106a15ecbf94c16f40d54b459
|
|
@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then
|
||||||
fi
|
fi
|
||||||
cd acados_repo
|
cd acados_repo
|
||||||
git fetch
|
git fetch
|
||||||
git checkout 79e9e3e76f2751198858adf382c97837833ad31f
|
git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8
|
||||||
git submodule update --recursive --init
|
git submodule update --recursive --init
|
||||||
|
|
||||||
# build
|
# build
|
||||||
|
|
Loading…
Reference in New Issue