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
Split 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
cd acados_repo
git fetch
git checkout 05bcbfe42818738c74572f27d06ad75a28d3b380
git checkout d6fb3868f85239ba2de014d7234dbfea65f238e6
git submodule update --recursive --init
# 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
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):
"""
Solve the ocp with current input.
@ -1179,61 +1196,61 @@ class AcadosOcpSolver:
stage = c_int(stage_)
# treat parameters separately
if field_ == 'p':
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
if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
\nPossible values are {}. Exiting.".format(field, \
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
else:
if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
\nPossible values are {}. Exiting.".format(field, \
constraints_fields + cost_fields + out_fields + ['p']))
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
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage_, field)
if value_.shape[0] != dims:
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
raise Exception(msg)
value_data = cast(value_.ctypes.data, POINTER(c_double))
value_data_p = cast((value_data), c_void_p)
if field_ in constraints_fields:
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]
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
elif field_ in cost_fields:
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]
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
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)
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage_, field)
if value_.shape[0] != dims:
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
raise Exception(msg)
value_data = cast(value_.ctypes.data, POINTER(c_double))
value_data_p = cast((value_data), c_void_p)
if field_ in constraints_fields:
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]
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
elif field_ in cost_fields:
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]
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
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
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'):
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'):
"""
@ -1244,71 +1261,23 @@ class AcadosOcpSolver:
:param value: of appropriate size
"""
# cast value_ to avoid conversion issues
if isinstance(value_, (float, int)):
value_ = np.array([value_])
value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
field = field_
field = field.encode('utf-8')
field = field_.encode('utf-8')
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_data = cast(dims.ctypes.data, POINTER(c_int))
self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
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.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'):
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'):
"""
@ -1321,64 +1290,19 @@ class AcadosOcpSolver:
# cast value_ to avoid conversion issues
if isinstance(value_, (float, int)):
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:])
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_data = cast(dims.ctypes.data, POINTER(c_int))
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
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.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
return
self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
value_.ctypes.data_as(POINTER(c_void_p)), dim)
def dynamics_get(self, stage_, field_):

Write Preview
Loading…
Cancel
Save