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

update acados (#22202)

Browse Source 
* update acados

* cleanup
pull/22205/head
Adeeb Shihadeh 4 years ago committed by GitHub
parent a19738cba8
commit 63453c951e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 73 additions and 149 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      phonelibs/acados/build.sh
  2. 220
      pyextra/acados_template/acados_ocp_solver.py

2
phonelibs/acados/build.sh
Unescape View File

@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then
fi fi
cd acados_repo cd acados_repo
git fetch git fetch
git checkout 05bcbfe42818738c74572f27d06ad75a28d3b380 git checkout d6fb3868f85239ba2de014d7234dbfea65f238e6
git submodule update --recursive --init git submodule update --recursive --init
# build # build

220
pyextra/acados_template/acados_ocp_solver.py
Unescape View File

@ -855,6 +855,23 @@ class AcadosOcpSolver:
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule) self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
# treat parameters separately
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
def solve(self): def solve(self):
""" """
Solve the ocp with current input. Solve the ocp with current input.
@ -1179,61 +1196,61 @@ class AcadosOcpSolver:
stage = c_int(stage_) stage = c_int(stage_)
# treat parameters separately if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
if field_ == 'p': raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] \nPossible values are {}. Exiting.".format(field, \
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int constraints_fields + cost_fields + out_fields + ['p']))
value_data = cast(value_.ctypes.data, POINTER(c_double)) self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
else: self.nlp_dims, self.nlp_out, stage_, field)
if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ if value_.shape[0] != dims:
\nPossible values are {}. Exiting.".format(field, \ msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
constraints_fields + cost_fields + out_fields + ['p'])) msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
raise Exception(msg)
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p] value_data = cast(value_.ctypes.data, POINTER(c_double))
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int value_data_p = cast((value_data), c_void_p)
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ if field_ in constraints_fields:
self.nlp_dims, self.nlp_out, stage_, field) self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
if value_.shape[0] != dims: self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) self.nlp_dims, self.nlp_in, stage, field, value_data_p)
msg += 'with dimension {} (you have {})'.format(dims, value_.shape) elif field_ in cost_fields:
raise Exception(msg) self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
value_data = cast(value_.ctypes.data, POINTER(c_double)) self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
value_data_p = cast((value_data), c_void_p) self.nlp_dims, self.nlp_in, stage, field, value_data_p)
elif field_ in out_fields:
if field_ in constraints_fields: self.shared_lib.ocp_nlp_out_set.argtypes = \
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ self.nlp_dims, self.nlp_out, stage, field, value_data_p)
self.nlp_dims, self.nlp_in, stage, field, value_data_p) elif field_ in mem_fields:
elif field_ in cost_fields: self.shared_lib.ocp_nlp_set.argtypes = \
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_set(self.nlp_config, \
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ self.nlp_solver, stage, field, value_data_p)
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
elif field_ in out_fields:
self.shared_lib.ocp_nlp_out_set.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, value_data_p)
elif field_ in mem_fields:
self.shared_lib.ocp_nlp_set.argtypes = \
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
self.shared_lib.ocp_nlp_set(self.nlp_config, \
self.nlp_solver, stage, field, value_data_p)
return return
def set_param(self, stage_, value_):
# cast value_ to avoid conversion issues
#if isinstance(value_, (float, int)):
# value_ = np.array([value_])
#value_ = value_.astype(float)
#stage = c_int(stage_)
#value_data = cast(value_.ctypes.data, POINTER(c_double))
self._set_param(self.capsule, stage_, value_.ctypes.data_as(POINTER(c_double)), value_.shape[0])
def cost_set(self, start_stage_, field_, value_, api='warn'): def cost_set(self, start_stage_, field_, value_, api='warn'):
self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
return
def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
""" """
@ -1244,71 +1261,23 @@ class AcadosOcpSolver:
:param value: of appropriate size :param value: of appropriate size
""" """
# cast value_ to avoid conversion issues # cast value_ to avoid conversion issues
if isinstance(value_, (float, int)): field = field_.encode('utf-8')
value_ = np.array([value_])
value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
field = field_
field = field.encode('utf-8')
dim = np.product(value_.shape[1:]) dim = np.product(value_.shape[1:])
start_stage = c_int(start_stage_)
end_stage = c_int(end_stage_)
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
dims_data = cast(dims.ctypes.data, POINTER(c_int)) dims_data = cast(dims.ctypes.data, POINTER(c_int))
self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
value_shape = value_.shape
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
if len(value_shape) == 2:
value_shape = (value_shape[0], value_shape[1], 0)
elif len(value_shape) == 3:
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))
if value_shape != expected_shape:
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
' for field "{}" with dimension {} (you have {})'.format( \
field_, expected_shape, value_shape))
value_data = cast(value_.ctypes.data, POINTER(c_double))
value_data_p = cast((value_data), c_void_p)
self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \ self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
cast(value_.ctypes.data, c_void_p), dim)
return
def constraints_set(self, start_stage_, field_, value_, api='warn'): def constraints_set(self, start_stage_, field_, value_, api='warn'):
self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
return
def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
""" """
@ -1321,64 +1290,19 @@ class AcadosOcpSolver:
# cast value_ to avoid conversion issues # cast value_ to avoid conversion issues
if isinstance(value_, (float, int)): if isinstance(value_, (float, int)):
value_ = np.array([value_]) value_ = np.array([value_])
value_ = value_.astype(float)
field = field_ field = field_.encode('utf-8')
field = field.encode('utf-8')
dim = np.product(value_.shape[1:]) dim = np.product(value_.shape[1:])
start_stage = c_int(start_stage_)
end_stage = c_int(end_stage_)
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
dims_data = cast(dims.ctypes.data, POINTER(c_int)) dims_data = cast(dims.ctypes.data, POINTER(c_int))
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
value_shape = value_.shape
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
if len(value_shape) == 2:
value_shape = (value_shape[0], value_shape[1], 0)
elif len(value_shape) == 3:
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))
if value_shape != expected_shape:
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
value_data = cast(value_.ctypes.data, POINTER(c_double))
value_data_p = cast((value_data), c_void_p)
self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \ self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
value_.ctypes.data_as(POINTER(c_void_p)), dim)
return
def dynamics_get(self, stage_, field_): def dynamics_get(self, stage_, field_):

Write Preview
Loading…
Cancel
Save