mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-10-01 03:43:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

update acados (#22202)

Browse Source 
* update acados

* cleanup
This commit is contained in:
Adeeb Shihadeh
2021-09-11 21:50:16 -07:00
committed by GitHub
parent a19738cba8
commit 63453c951e
2 changed files with 70 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files
phonelibs/acados/build.sh
+1 -1
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
pyextra/acados_template/acados_ocp_solver.py
+69 -145
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']))
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage_, field)
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
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)
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage_, field)
value_data = cast(value_.ctypes.data, POINTER(c_double))
value_data_p = cast((value_data), c_void_p)
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)
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_):