diff --git a/phonelibs/acados/build.sh b/phonelibs/acados/build.sh index af071bf5fb..910ea61b1f 100755 --- a/phonelibs/acados/build.sh +++ b/phonelibs/acados/build.sh @@ -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 diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py index c27b0dbaf8..072ee0b6a9 100644 --- a/pyextra/acados_template/acados_ocp_solver.py +++ b/pyextra/acados_template/acados_ocp_solver.py @@ -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_):