sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

make faster

Browse Source
pull/22784/head
Joost Wooning 4 years ago
parent 15da7a7e98
commit ab3b2f7824
5 changed files with 43 additions and 86 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 72
      pyextra/acados_template/acados_ocp_solver_pyx.pyx
  2. 10
      selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
  3. 14
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
  4. 11
      selfdrive/controls/plannerd.py
  5. 2
      third_party/acados/build.sh

72
pyextra/acados_template/acados_ocp_solver_pyx.pyx
Unescape View File

@ -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':
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))
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, <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':
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))
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, <void *> &value[0])

10
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
Unescape View File

@ -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)

14
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Unescape View File

@ -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.

11
selfdrive/controls/plannerd.py
Unescape View File

@ -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,6 +35,8 @@ def plannerd_thread(sm=None, pm=None):
if pm is None:
pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan'])
with cProfile.Profile(builtins=False) as pr:
i = 0
while True:
sm.update()
@ -41,6 +46,12 @@ def plannerd_thread(sm=None, pm=None):
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):
plannerd_thread(sm, pm)

2
third_party/acados/build.sh vendored
Unescape View File

@ -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

Write Preview
Loading…
Cancel
Save