diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 2d85532354..b14c37f409 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -32,7 +32,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # # cython: language_level=3 -# cython: profile=False +# cython: profile=True # distutils: language=c cimport cython @@ -277,10 +277,8 @@ cdef class AcadosOcpSolverFast: acados_solver_common.ocp_nlp_out_set(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &value[0]) - return - - def cost_set(self, int stage, str field_, value_, api='warn'): + def cost_set(self, int stage, str field_, value_): """ Set numerical data in the cost module of the solver. @@ -288,58 +286,33 @@ cdef class AcadosOcpSolverFast: :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' :param value: of appropriate size """ - # cast value_ to avoid conversion issues - if isinstance(value_, (float, int)): - value_ = np.array([value_]) - value_ = value_.astype(float) - field = field_.encode('utf-8') cdef int dims[2] acados_solver_common.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + cdef double[::1] value + value_shape = value_.shape if len(value_shape) == 1: value_shape = (value_shape[0], 0) - # value_ = np.ravel(value_, order='F') + data_ptr = np.asfortranarray(value_) elif len(value_shape) == 2: - if api=='old': - pass - elif api=='warn': - if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): - raise Exception("Ambiguity in API detected.\n" - "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" - "Are you seeing this error suddenly in previously running code? Read on.\n" - " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + - " acados_template now correctly passes on any matrices to acados in column major format.\n" + - " Two options to fix this error: \n" + - " * Add api='old' to cost_set to restore old incorrect behaviour\n" + - " * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " + - "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + - "If there is no such manipulation, then you have probably been getting an incorrect solution before.") - # Get elements in column major order - value_ = np.ravel(value_, order='F') - elif api=='new': - # Get elements in column major order - value_ = np.ravel(value_, order='F') - else: - raise Exception("Unknown api: '{}'".format(api)) + # Get elements in column major order + value = np.ravel(value_, order='F') if value_shape[0] != dims[0] or value_shape[1] != dims[1]: raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \ ' for field "{}" with dimension {} (you have {})'.format( \ field_, tuple(dims), value_shape)) - cdef double[::1] value = np.asfortranarray(value_, dtype=np.double) acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \ self.nlp_dims, self.nlp_in, stage, field, &value[0]) - return - - def constraints_set(self, int stage, str field_, value_, api='warn'): + def constraints_set(self, int stage, str field_, value_): """ Set numerical data in the constraint module of the solver. @@ -347,50 +320,27 @@ cdef class AcadosOcpSolverFast: :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] :param value: of appropriate size """ - # cast value_ to avoid conversion issues - if isinstance(value_, (float, int)): - value_ = np.array([value_]) - value_ = value_.astype(float) - field = field_.encode('utf-8') cdef int dims[2] acados_solver_common.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + cdef double[::1] value + value_shape = value_.shape if len(value_shape) == 1: value_shape = (value_shape[0], 0) - # value_ = np.ravel(value_, order='F') + value = np.asfortranarray(value_) elif len(value_shape) == 2: - if api=='old': - pass - elif api=='warn': - if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): - raise Exception("Ambiguity in API detected.\n" - "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" - "Are you seeing this error suddenly in previously running code? Read on.\n" - " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + - " acados_template now correctly passes on any matrices to acados in column major format.\n" + - " Two options to fix this error: \n" + - " * Add api='old' to constraints_set to restore old incorrect behaviour\n" + - " * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " + - "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + - "If there is no such manipulation, then you have probably been getting an incorrect solution before.") - # Get elements in column major order - value_ = np.ravel(value_, order='F') - elif api=='new': - # Get elements in column major order - value_ = np.ravel(value_, order='F') - else: - raise Exception("Unknown api: '{}'".format(api)) + # Get elements in column major order + value = np.ravel(value_, order='F') if value_shape[0] != dims[0] or value_shape[1] != dims[1]: raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \ ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) - cdef double[::1] value = np.asfortranarray(value_, dtype=np.double) acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \ self.nlp_dims, self.nlp_in, stage, field, &value[0]) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index f42c4c5ab9..6162cf68a5 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -124,8 +124,6 @@ class LateralMpc(): for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:2]) - W = np.eye(3) - self.Ws = np.tile(W[None], reps=(N,1,1)) # Somehow needed for stable init for i in range(N+1): @@ -137,13 +135,11 @@ class LateralMpc(): self.cost = 0 def set_weights(self, path_weight, heading_weight, steer_rate_weight): - self.Ws[:,0,0] = path_weight - self.Ws[:,1,1] = heading_weight - self.Ws[:,2,2] = steer_rate_weight + W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight])) for i in range(N): - self.solver.cost_set(i, 'W', self.Ws[i], api='new') + self.solver.cost_set(i, 'W', W) #TODO hacky weights to keep behavior the same - self.solver.cost_set(N, 'W', (3/20.)*self.Ws[0,:2,:2]) + self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts): x0_cp = np.copy(x0) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b73c38c4e6..df6d8e0fb6 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -222,9 +222,9 @@ class LongitudinalMpc(): self.set_weights_for_lead_policy() def set_weights_for_lead_policy(self): - W = np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST]) + W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST])) for i in range(N): - self.solver.cost_set(i, 'W', W, api='new') + self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) @@ -232,12 +232,12 @@ class LongitudinalMpc(): # Set L2 slack cost on lower bound constraints Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]) for i in range(N): - self.solver.cost_set(i, 'Zl', Zl, api='new') + self.solver.cost_set(i, 'Zl', Zl) def set_weights_for_xva_policy(self): - W = np.diag([0., 10., 1., 10., 1.]) + W = np.asfortranarray(np.diag([0., 10., 1., 10., 1.])) for i in range(N): - self.solver.cost_set(i, 'W', W, api='new') + self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) @@ -245,7 +245,7 @@ class LongitudinalMpc(): # Set L2 slack cost on lower bound constraints Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]) for i in range(N): - self.solver.cost_set(i, 'Zl', Zl, api='new') + self.solver.cost_set(i, 'Zl', Zl) def set_cur_state(self, v, a): if abs(self.x0[1] - v) > 1.: @@ -333,7 +333,7 @@ class LongitudinalMpc(): self.yref[:,2] = v self.yref[:,3] = a for i in range(N): - self.solver.cost_set(i, "yref", self.yref[i], api='new') + self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) self.accel_limit_arr[:,0] = -10. self.accel_limit_arr[:,1] = 10. diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 88734b3c13..3c2ea7b5a6 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +import pstats +import cProfile + from cereal import car from common.params import Params from common.realtime import Priority, config_realtime_process @@ -32,14 +35,22 @@ def plannerd_thread(sm=None, pm=None): if pm is None: pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan']) - while True: - sm.update() - - if sm.updated['modelV2']: - lateral_planner.update(sm, CP) - lateral_planner.publish(sm, pm) - longitudinal_planner.update(sm, CP) - longitudinal_planner.publish(sm, pm) + with cProfile.Profile(builtins=False) as pr: + i = 0 + while True: + sm.update() + + if sm.updated['modelV2']: + lateral_planner.update(sm, CP) + lateral_planner.publish(sm, pm) + longitudinal_planner.update(sm, CP) + longitudinal_planner.publish(sm, pm) + + i+= 1 + if i == 80: + stats = pstats.Stats(pr) + stats.sort_stats('time') + stats.dump_stats('/tmp/plannerd_stats') def main(sm=None, pm=None): diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 1068fec0e6..8dc5b77be0 100755 --- a/third_party/acados/build.sh +++ b/third_party/acados/build.sh @@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then fi cd acados_repo git fetch -git checkout fab10b3c504c48385fbbe3efb5c284984e792b1c +git checkout b13def633e77832c7075cc152075667c2be030ee git submodule update --recursive --init # build