make faster

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, <void *> &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':
+            # Get elements in column major order
-                pass
+            value = np.ravel(value_, order='F')
-            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))
 
         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, <void *> &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':
+            # Get elements in column major order
-                pass
+            value = np.ravel(value_, order='F')
-            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))
 
         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, <void *> &value[0])
 

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
+    W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight]))
-    self.Ws[:,1,1] = heading_weight
-    self.Ws[:,2,2] = 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)

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.

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:
+  with cProfile.Profile(builtins=False) as pr:
-    sm.update()
+    i = 0
-
+    while True:
-    if sm.updated['modelV2']:
+      sm.update()
-      lateral_planner.update(sm, CP)
+
-      lateral_planner.publish(sm, pm)
+      if sm.updated['modelV2']:
-      longitudinal_planner.update(sm, CP)
+        lateral_planner.update(sm, CP)
-      longitudinal_planner.publish(sm, pm)
+        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):

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