Squashed 'third_party/osqp-cpp/' content from commit 8cd904e2b4
Change-Id: Ic67c163724b33720d49822debd467a344121342e
git-subtree-dir: third_party/osqp-cpp
git-subtree-split: 8cd904e2b49c24dd41d11f8c6e0adb113dd5e26c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/src/osqp++.cc b/src/osqp++.cc
new file mode 100644
index 0000000..5fa119e
--- /dev/null
+++ b/src/osqp++.cc
@@ -0,0 +1,992 @@
+// Copyright 2020 Google LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <algorithm>
+#include <cassert>
+#include <string>
+#include <type_traits>
+
+#include "absl/status/status.h"
+#include "absl/status/statusor.h"
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_format.h"
+#include "absl/strings/string_view.h"
+#include "Eigen/Core"
+#include "Eigen/SparseCore"
+#include "ctrlc.h"
+#include "osqp.h"
+#include "osqp++.h"
+
+// Fails to compile if OSQP's typedefs change. This lets us avoid including
+// osqp.h in osqp++.h.
+static_assert(
+ std::is_same_v<osqp::c_int, ::c_int>,
+ "OSQP's c_int typedef does not match the definition in osqp++.h.");
+static_assert(std::is_same_v<c_float, double>,
+ "OSQP's c_float typedef is unexpectedly not the same as double");
+
+static_assert(sizeof(OSQPSettings) == 176,
+ "The size of OSQPSettings has changed unexpectedly. Make sure "
+ "that the map between ::OSQPSettings and osqp::OsqpSettings "
+ "remains up to date.");
+
+namespace osqp {
+
+namespace {
+
+using ::Eigen::Map;
+using ::Eigen::Ref;
+using ::Eigen::VectorXd;
+
+// The mapping between ::OSQPSettings and osqp::OsqpSettings is maintained
+// manually and may need to be updated for new releases of OSQP.
+
+void CopyFromInternalSettings(const ::OSQPSettings& osqp_settings,
+ OsqpSettings* settings) {
+ settings->rho = osqp_settings.rho;
+ settings->sigma = osqp_settings.sigma;
+ settings->scaling = osqp_settings.scaling;
+ settings->adaptive_rho = osqp_settings.adaptive_rho;
+ settings->adaptive_rho_interval = osqp_settings.adaptive_rho_interval;
+ settings->adaptive_rho_tolerance = osqp_settings.adaptive_rho_tolerance;
+ settings->adaptive_rho_fraction = osqp_settings.adaptive_rho_fraction;
+ settings->max_iter = osqp_settings.max_iter;
+ settings->eps_abs = osqp_settings.eps_abs;
+ settings->eps_rel = osqp_settings.eps_rel;
+ settings->eps_prim_inf = osqp_settings.eps_prim_inf;
+ settings->eps_dual_inf = osqp_settings.eps_dual_inf;
+ settings->alpha = osqp_settings.alpha;
+ settings->delta = osqp_settings.delta;
+ settings->polish = osqp_settings.polish;
+ settings->polish_refine_iter = osqp_settings.polish_refine_iter;
+ settings->verbose = osqp_settings.verbose;
+ settings->scaled_termination = osqp_settings.scaled_termination;
+ settings->check_termination = osqp_settings.check_termination;
+ settings->warm_start = osqp_settings.warm_start;
+ settings->time_limit = osqp_settings.time_limit;
+}
+
+::OSQPSettings ToInternalSettings(const OsqpSettings& settings) {
+ OSQPSettings osqp_settings;
+ osqp_settings.rho = settings.rho;
+ osqp_settings.sigma = settings.sigma;
+ osqp_settings.scaling = settings.scaling;
+ osqp_settings.adaptive_rho = settings.adaptive_rho;
+ osqp_settings.adaptive_rho_interval = settings.adaptive_rho_interval;
+ osqp_settings.adaptive_rho_tolerance = settings.adaptive_rho_tolerance;
+ osqp_settings.adaptive_rho_fraction = settings.adaptive_rho_fraction;
+ osqp_settings.max_iter = settings.max_iter;
+ osqp_settings.eps_abs = settings.eps_abs;
+ osqp_settings.eps_rel = settings.eps_rel;
+ osqp_settings.eps_prim_inf = settings.eps_prim_inf;
+ osqp_settings.eps_dual_inf = settings.eps_dual_inf;
+ osqp_settings.alpha = settings.alpha;
+ osqp_settings.delta = settings.delta;
+ osqp_settings.polish = settings.polish;
+ osqp_settings.polish_refine_iter = settings.polish_refine_iter;
+ osqp_settings.verbose = settings.verbose;
+ osqp_settings.scaled_termination = settings.scaled_termination;
+ osqp_settings.check_termination = settings.check_termination;
+ osqp_settings.warm_start = settings.warm_start;
+ osqp_settings.time_limit = settings.time_limit;
+ osqp_settings.linsys_solver = ::QDLDL_SOLVER;
+ return osqp_settings;
+}
+
+} // namespace
+
+OsqpSettings::OsqpSettings() {
+ ::OSQPSettings osqp_settings;
+ osqp_set_default_settings(&osqp_settings);
+ CopyFromInternalSettings(osqp_settings, this);
+}
+
+struct OSQPWorkspaceHelper : public ::OSQPWorkspace {};
+
+void OsqpSolver::OsqpDeleter::operator()(OSQPWorkspaceHelper* workspace) const {
+ osqp_cleanup(workspace);
+}
+
+// OSQP_HANDLE_EXITCODE(x) expands to 'case x: return "x"'. Using this macro
+// prevents typos in the strings.
+#define OSQP_HANDLE_EXITCODE(x) \
+ case x: \
+ return #x
+
+std::string ToString(OsqpExitCode exitcode) {
+ switch (exitcode) {
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kOptimal);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kPrimalInfeasible);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kDualInfeasible);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kOptimalInaccurate);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kPrimalInfeasibleInaccurate);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kDualInfeasibleInaccurate);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kMaxIterations);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kInterrupted);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kTimeLimitReached);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kNonConvex);
+ OSQP_HANDLE_EXITCODE(OsqpExitCode::kUnknown);
+ }
+ return "Unknown exit code";
+}
+
+#undef OSQP_HANDLE_EXITCODE
+
+namespace {
+
+absl::Status CheckDimensions(const int left_value, const int right_value,
+ absl::string_view left_name,
+ absl::string_view right_name) {
+ if (left_value != right_value) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Dimension mismatch: ", left_name, " (= ", left_value,
+ ") must equal ", right_name, " (= ", right_value, ")."));
+ } else {
+ return absl::OkStatus();
+ }
+}
+
+} // namespace
+
+#define OSQP_CHECK_DIMENSIONS(left_value, right_value) \
+ CheckDimensions((left_value), (right_value), #left_value, #right_value)
+
+#define OSQP_RETURN_IF_ERROR(expr) \
+ { \
+ const absl::Status result = expr; \
+ if (!result.ok()) return result; \
+ }
+
+#define OSQP_CHECK(expr) assert(expr)
+
+absl::Status OsqpSolver::Init(const OsqpInstance& instance,
+ const OsqpSettings& settings) {
+ if (!instance.objective_matrix.isCompressed()) {
+ return absl::InvalidArgumentError(
+ "objective_matrix must be compressed (call makeCompressed()).");
+ }
+ if (!instance.constraint_matrix.isCompressed()) {
+ return absl::InvalidArgumentError(
+ "constraint_matrix must be compressed (call makeCompressed()).");
+ }
+ const c_int num_variables = instance.num_variables();
+ const c_int num_constraints = instance.num_constraints();
+
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(instance.objective_matrix.cols(), num_variables));
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(instance.objective_matrix.rows(), num_variables));
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(instance.objective_vector.size(), num_variables));
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(instance.lower_bounds.size(), num_constraints));
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(instance.upper_bounds.size(), num_constraints));
+
+ // Clip bounds using OSQP_INFTY. Failing to do this causes subtle convergence
+ // issues instead of producing explicit errors (e.g., the
+ // DetectsPrimalInfeasible test fails). The osqp-python interface also clips
+ // the bounds in the same way.
+ VectorXd clipped_lower_bounds = instance.lower_bounds.cwiseMax(-OSQP_INFTY);
+ VectorXd clipped_upper_bounds = instance.upper_bounds.cwiseMin(OSQP_INFTY);
+
+ // OSQP copies all the data, so it's okay to discard this struct after
+ // osqp_setup. It also does not modify the input data (note osqp_setup takes a
+ // const OSQPData*). const_cast is needed here to fill in the input data
+ // structures.
+ OSQPData data;
+ data.n = num_variables;
+ data.m = num_constraints;
+
+ // TODO(ml): This copy could be avoided if the matrix is already upper
+ // triangular.
+ Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>
+ objective_matrix_upper_triangle =
+ instance.objective_matrix.triangularView<Eigen::Upper>();
+
+ // OSQP's csc struct represents sparse matrices in compressed sparse column
+ // (CSC) format and (confusingly) triplet format. osqp_setup() assumes the
+ // input is provided in CSC format. The mapping from Eigen::SparseMatrix's
+ // outerIndexPtr(), innerIndexPtr(), and valuePtr() is direct because we
+ // require the sparse matrices to be compressed and follow the Eigen::ColMajor
+ // storage scheme. For further background, see
+ // https://eigen.tuxfamily.org/dox/group__TutorialSparse.html for the
+ // description of CSC format in Eigen and osqp/include/types.h for the
+ // definition of OSQP's csc struct.
+ ::csc objective_matrix = {
+ objective_matrix_upper_triangle.outerIndexPtr()[num_variables],
+ num_variables,
+ num_variables,
+ const_cast<c_int*>(objective_matrix_upper_triangle.outerIndexPtr()),
+ const_cast<c_int*>(objective_matrix_upper_triangle.innerIndexPtr()),
+ const_cast<double*>(objective_matrix_upper_triangle.valuePtr()),
+ -1};
+ data.P = &objective_matrix;
+
+ ::csc constraint_matrix = {
+ instance.constraint_matrix.outerIndexPtr()[num_variables],
+ num_constraints,
+ num_variables,
+ const_cast<c_int*>(instance.constraint_matrix.outerIndexPtr()),
+ const_cast<c_int*>(instance.constraint_matrix.innerIndexPtr()),
+ const_cast<double*>(instance.constraint_matrix.valuePtr()),
+ -1};
+ data.A = &constraint_matrix;
+
+ data.q = const_cast<double*>(instance.objective_vector.data());
+ data.l = clipped_lower_bounds.data();
+ data.u = clipped_upper_bounds.data();
+
+ ::OSQPSettings osqp_settings = ToInternalSettings(settings);
+
+ OSQPWorkspace* workspace = nullptr;
+ const int return_code = osqp_setup(&workspace, &data, &osqp_settings);
+ workspace_.reset(static_cast<OSQPWorkspaceHelper*>(workspace));
+ if (return_code == 0) {
+ return absl::OkStatus();
+ }
+ switch (static_cast<osqp_error_type>(return_code)) {
+ case OSQP_DATA_VALIDATION_ERROR:
+ return absl::InvalidArgumentError(
+ "Unable to initialize OSQP: data validation error.");
+ case OSQP_SETTINGS_VALIDATION_ERROR:
+ return absl::InvalidArgumentError(
+ "Unable to initialize OSQP: invalid settings.");
+ case OSQP_LINSYS_SOLVER_LOAD_ERROR:
+ // This should never happen because qdldl is statically linked in.
+ return absl::UnknownError(
+ "Unable to initialize OSQP: unable to load linear solver.");
+ case OSQP_LINSYS_SOLVER_INIT_ERROR:
+ return absl::UnknownError(
+ "Unable to initialize OSQP: unable to initialize linear solver.");
+ case OSQP_NONCVX_ERROR:
+ return absl::InvalidArgumentError(
+ "Unable to initialize OSQP: the problem appears non-convex.");
+ case OSQP_MEM_ALLOC_ERROR:
+ return absl::UnknownError(
+ "Unable to initialize OSQP: memory allocation error.");
+ case OSQP_WORKSPACE_NOT_INIT_ERROR:
+ return absl::UnknownError(
+ "Unable to initialize OSQP: workspace not initialized.");
+ }
+ return absl::UnknownError(
+ "Unable to initialize OSQP: unrecognized error code.");
+}
+
+namespace {
+
+absl::Status VerifySameSparsity(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>& new_matrix,
+ const csc* ref_matrix, size_t num_variables) {
+ if (new_matrix.nonZeros() != ref_matrix->p[num_variables]) {
+ return absl::InvalidArgumentError(
+ "The new new matrix should have the same number of non-zero "
+ "elements.");
+ }
+
+ for (size_t i = 0; i < num_variables; ++i) {
+ if (ref_matrix->p[i] != new_matrix.outerIndexPtr()[i]) {
+ return absl::InvalidArgumentError(
+ "Sparsity of the new matrix differs from the previously "
+ "defined matrix.");
+ }
+ }
+
+ for (size_t i = 0; i < new_matrix.nonZeros(); ++i) {
+ if (ref_matrix->i[i] != new_matrix.innerIndexPtr()[i]) {
+ return absl::InvalidArgumentError(
+ "Sparsity of the new matrix differs from the previously "
+ "defined matrix.");
+ }
+ }
+
+ return absl::OkStatus();
+}
+
+// Helper function for calling osqp_update_P with an upper triangular objective
+// matrix. Assumes objective_matrix_upper_triangle is always upper triangular.
+absl::Status UpdateUpperTriangularObjectiveMatrix(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>&
+ objective_matrix_upper_triangle,
+ OSQPWorkspaceHelper* workspace) {
+ const c_int num_variables = workspace->data->n;
+
+ if (objective_matrix_upper_triangle.rows() !=
+ objective_matrix_upper_triangle.cols() ||
+ objective_matrix_upper_triangle.rows() != num_variables) {
+ return absl::InvalidArgumentError(absl::StrFormat(
+ "The new objective matrix should be square with dimension equal to the "
+ "number of variables. Matrix dimensions: %d x %d, num_variables=%d",
+ objective_matrix_upper_triangle.rows(),
+ objective_matrix_upper_triangle.cols(), num_variables));
+ }
+
+ OSQP_RETURN_IF_ERROR(VerifySameSparsity(objective_matrix_upper_triangle,
+ workspace->data->P, num_variables));
+
+ c_int nnzP = objective_matrix_upper_triangle.nonZeros();
+
+ const int return_code = osqp_update_P(
+ workspace, objective_matrix_upper_triangle.valuePtr(), OSQP_NULL, nnzP);
+ if (return_code == 0) {
+ return absl::OkStatus();
+ }
+ return absl::UnknownError(
+ "Unable to update OSQP P matrix: unrecognized error code.");
+}
+
+// Helper function for calling osqp_update_P_A with an upper triangular
+// objective matrix. Assumes objective_matrix_upper_triangle is always upper
+// triangular.
+absl::Status UpdateUpperTriangularObjectiveMatrixAndConstraintMatrix(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>&
+ objective_matrix_upper_triangle,
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>&
+ constraint_matrix,
+ OSQPWorkspaceHelper* workspace) {
+ const c_int num_variables = workspace->data->n;
+
+ if (objective_matrix_upper_triangle.rows() !=
+ objective_matrix_upper_triangle.cols() ||
+ objective_matrix_upper_triangle.rows() != num_variables) {
+ return absl::InvalidArgumentError(absl::StrFormat(
+ "The new objective matrix should be square with dimension equal to the "
+ "number of variables. Matrix dimensions: %d x %d, num_variables=%d",
+ objective_matrix_upper_triangle.rows(),
+ objective_matrix_upper_triangle.cols(), num_variables));
+ }
+ if (constraint_matrix.cols() != num_variables) {
+ return absl::InvalidArgumentError(absl::StrFormat(
+ "The new constraint matrix should column size equal to the "
+ "number of variables. Matrix dimensions: %d x %d, num_variables=%d",
+ constraint_matrix.rows(), constraint_matrix.cols(), num_variables));
+ }
+
+ OSQP_RETURN_IF_ERROR(VerifySameSparsity(objective_matrix_upper_triangle,
+ workspace->data->P, num_variables));
+
+ c_int nnzP = objective_matrix_upper_triangle.nonZeros();
+
+ OSQP_RETURN_IF_ERROR(
+ VerifySameSparsity(constraint_matrix, workspace->data->A, num_variables));
+
+ c_int nnzA = constraint_matrix.nonZeros();
+
+ const int return_code = osqp_update_P_A(
+ workspace, objective_matrix_upper_triangle.valuePtr(), OSQP_NULL, nnzP,
+ constraint_matrix.valuePtr(), OSQP_NULL, nnzA);
+ if (return_code == 0) {
+ return absl::OkStatus();
+ }
+ return absl::UnknownError(
+ "Unable to update OSQP P and A matrix: unrecognized error code.");
+}
+
+// Returns true if the sparse matrix 'matrix' is upper triangular.
+bool IsUpperTriangular(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>& matrix) {
+ // Iterate through all non-zero elements, and ensure that their indices are
+ // only on the upper-right triangle, including the diagonal.
+ for (int i = 0; i < matrix.outerSize(); ++i) {
+ for (Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>::InnerIterator it(
+ matrix, i);
+ it; ++it) {
+ if (it.col() < it.row()) {
+ return false;
+ }
+ }
+ }
+ return true;
+}
+
+} // namespace
+
+absl::Status OsqpSolver::UpdateObjectiveMatrix(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>&
+ objective_matrix) {
+ // If the objective matrix is already upper triangular, we can skip the
+ // temporary.
+ if (IsUpperTriangular(objective_matrix)) {
+ return UpdateUpperTriangularObjectiveMatrix(objective_matrix,
+ workspace_.get());
+ }
+
+ // If not upper triangular, make a temporary.
+ Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>
+ objective_matrix_upper_triangle =
+ objective_matrix.triangularView<Eigen::Upper>();
+ return UpdateUpperTriangularObjectiveMatrix(objective_matrix_upper_triangle,
+ workspace_.get());
+}
+
+absl::Status OsqpSolver::UpdateConstraintMatrix(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>&
+ constraint_matrix) {
+ const c_int num_variables = workspace_->data->n;
+
+ if (constraint_matrix.cols() != num_variables) {
+ return absl::InvalidArgumentError(absl::StrFormat(
+ "The new constraint matrix should column size equal to the "
+ "number of variables. Matrix dimensions: %d x %d, num_variables=%d",
+ constraint_matrix.rows(), constraint_matrix.cols(), num_variables));
+ }
+
+ OSQP_RETURN_IF_ERROR(VerifySameSparsity(constraint_matrix,
+ workspace_->data->A, num_variables));
+
+ c_int nnzA = constraint_matrix.nonZeros();
+
+ const int return_code = osqp_update_A(
+ workspace_.get(), constraint_matrix.valuePtr(), OSQP_NULL, nnzA);
+ if (return_code == 0) {
+ return absl::OkStatus();
+ }
+ return absl::UnknownError(
+ "Unable to update OSQP A matrix: unrecognized error code.");
+}
+
+absl::Status OsqpSolver::UpdateObjectiveAndConstraintMatrices(
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>& objective_matrix,
+ const Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>&
+ constraint_matrix) {
+ // If the objective matrix is already upper triangular, we can skip the
+ // temporary.
+ if (IsUpperTriangular(objective_matrix)) {
+ return UpdateUpperTriangularObjectiveMatrixAndConstraintMatrix(
+ objective_matrix, constraint_matrix, workspace_.get());
+ }
+
+ // If not upper triangular, make a temporary.
+ Eigen::SparseMatrix<double, Eigen::ColMajor, c_int>
+ objective_matrix_upper_triangle =
+ objective_matrix.triangularView<Eigen::Upper>();
+ return UpdateUpperTriangularObjectiveMatrixAndConstraintMatrix(
+ objective_matrix_upper_triangle, constraint_matrix, workspace_.get());
+}
+
+namespace {
+OsqpExitCode StatusToExitCode(const c_int status_val) {
+ switch (status_val) {
+ case OSQP_SOLVED:
+ return OsqpExitCode::kOptimal;
+ case OSQP_SOLVED_INACCURATE:
+ return OsqpExitCode::kOptimalInaccurate;
+ case OSQP_PRIMAL_INFEASIBLE:
+ return OsqpExitCode::kPrimalInfeasible;
+ case OSQP_PRIMAL_INFEASIBLE_INACCURATE:
+ return OsqpExitCode::kPrimalInfeasibleInaccurate;
+ case OSQP_DUAL_INFEASIBLE:
+ return OsqpExitCode::kDualInfeasible;
+ case OSQP_DUAL_INFEASIBLE_INACCURATE:
+ return OsqpExitCode::kDualInfeasibleInaccurate;
+ case OSQP_MAX_ITER_REACHED:
+ return OsqpExitCode::kMaxIterations;
+ case OSQP_SIGINT:
+ return OsqpExitCode::kInterrupted;
+ case OSQP_TIME_LIMIT_REACHED:
+ return OsqpExitCode::kTimeLimitReached;
+ case OSQP_NON_CVX:
+ return OsqpExitCode::kNonConvex;
+ default:
+ return OsqpExitCode::kUnknown;
+ }
+}
+
+} // namespace
+
+OsqpExitCode OsqpSolver::Solve() {
+ OSQP_CHECK(IsInitialized());
+ if (osqp_solve(workspace_.get()) != 0) {
+ // From looking at the code, this can happen if the solve is interrupted
+ // with ctrl-c or if updating "rho" fails.
+ if (osqp_is_interrupted()) {
+ return OsqpExitCode::kInterrupted;
+ }
+ return OsqpExitCode::kUnknown;
+ }
+ return StatusToExitCode(workspace_->info->status_val);
+}
+
+c_int OsqpSolver::iterations() const {
+ OSQP_CHECK(IsInitialized());
+ return workspace_->info->iter;
+}
+
+double OsqpSolver::objective_value() const {
+ OSQP_CHECK(IsInitialized());
+ return workspace_->info->obj_val;
+}
+
+Map<const VectorXd> OsqpSolver::primal_solution() const {
+ OSQP_CHECK(IsInitialized());
+ return Map<const VectorXd>(workspace_->solution->x, workspace_->data->n);
+}
+
+Map<const VectorXd> OsqpSolver::dual_solution() const {
+ OSQP_CHECK(IsInitialized());
+ return Map<const VectorXd>(workspace_->solution->y, workspace_->data->m);
+}
+
+Map<const VectorXd> OsqpSolver::primal_infeasibility_certificate() const {
+ OSQP_CHECK(IsInitialized());
+ const OsqpExitCode exit_code = StatusToExitCode(workspace_->info->status_val);
+ OSQP_CHECK(exit_code == OsqpExitCode::kPrimalInfeasible ||
+ exit_code == OsqpExitCode::kPrimalInfeasibleInaccurate);
+ return Map<const VectorXd>(workspace_->delta_y, workspace_->data->m);
+}
+
+absl::Status OsqpSolver::SetWarmStart(const Ref<const VectorXd>& primal_vector,
+ const Ref<const VectorXd>& dual_vector) {
+ // This is identical to calling osqp_warm_start with both vectors at once.
+ OSQP_RETURN_IF_ERROR(SetPrimalWarmStart(primal_vector));
+ OSQP_RETURN_IF_ERROR(SetDualWarmStart(dual_vector));
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::SetPrimalWarmStart(
+ const Ref<const VectorXd>& primal_vector) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ const c_int num_variables = workspace_->data->n;
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(primal_vector.size(), num_variables));
+
+ const int return_code =
+ osqp_warm_start_x(workspace_.get(), primal_vector.data());
+ if (return_code != 0) {
+ return absl::UnknownError("osqp_warm_start_x unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::SetDualWarmStart(
+ const Ref<const VectorXd>& dual_vector) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ const c_int num_constraints = workspace_->data->m;
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(dual_vector.size(), num_constraints));
+
+ const int return_code =
+ osqp_warm_start_y(workspace_.get(), dual_vector.data());
+ if (return_code != 0) {
+ return absl::UnknownError("osqp_warm_start_y unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::SetObjectiveVector(
+ const Ref<const VectorXd>& objective_vector) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ const c_int num_variables = workspace_->data->n;
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(objective_vector.size(), num_variables));
+
+ const int return_code =
+ osqp_update_lin_cost(workspace_.get(), objective_vector.data());
+ if (return_code != 0) {
+ return absl::UnknownError("osqp_update_lin_cost unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+// NOTE(ml): osqp_update_lower_bound and osqp_update_upper_bound are not
+// exposed because they have confusing semantics. They immediately error if a
+// new set of bounds is inconsistent with the existing bounds on the other side.
+absl::Status OsqpSolver::SetBounds(const Ref<const VectorXd>& lower_bounds,
+ const Ref<const VectorXd>& upper_bounds) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ const c_int num_constraints = workspace_->data->m;
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(lower_bounds.size(), num_constraints));
+ OSQP_RETURN_IF_ERROR(
+ OSQP_CHECK_DIMENSIONS(upper_bounds.size(), num_constraints));
+ // OSQP does this check internally, but we can return a better error message.
+ for (int i = 0; i < num_constraints; i++) {
+ if (lower_bounds[i] > upper_bounds[i]) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Inconsistent bounds at index ", i, ", ",
+ lower_bounds[i], " must be <= ", upper_bounds[i], "."));
+ }
+ }
+
+ const int return_code = osqp_update_bounds(
+ workspace_.get(), lower_bounds.data(), upper_bounds.data());
+ if (return_code != 0) {
+ return absl::UnknownError("osqp_update_bounds unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::StatusOr<double> OsqpSolver::GetRho() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->rho;
+}
+
+absl::StatusOr<double> OsqpSolver::GetSigma() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->sigma;
+}
+
+absl::StatusOr<c_int> OsqpSolver::GetScaling() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->scaling;
+}
+
+absl::StatusOr<bool> OsqpSolver::GetAdaptiveRho() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->adaptive_rho;
+}
+
+absl::StatusOr<c_int> OsqpSolver::GetAdaptiveRhoInterval() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->adaptive_rho_interval;
+}
+
+absl::StatusOr<double> OsqpSolver::GetAdaptiveRhoTolerance() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->adaptive_rho_tolerance;
+}
+
+absl::StatusOr<double> OsqpSolver::GetAdaptiveRhoFraction() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->adaptive_rho_fraction;
+}
+
+absl::StatusOr<c_int> OsqpSolver::GetMaxIter() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->max_iter;
+}
+
+absl::StatusOr<double> OsqpSolver::GetEpsAbs() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->eps_abs;
+}
+
+absl::StatusOr<double> OsqpSolver::GetEpsRel() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->eps_rel;
+}
+
+absl::StatusOr<double> OsqpSolver::GetEpsPrimInf() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->eps_prim_inf;
+}
+
+absl::StatusOr<double> OsqpSolver::GetEpsDualInf() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->eps_dual_inf;
+}
+
+absl::StatusOr<double> OsqpSolver::GetAlpha() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->alpha;
+}
+
+absl::StatusOr<double> OsqpSolver::GetDelta() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->delta;
+}
+
+absl::StatusOr<bool> OsqpSolver::GetPolish() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->polish;
+}
+
+absl::StatusOr<c_int> OsqpSolver::GetPolishRefineIter() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->polish_refine_iter;
+}
+
+absl::StatusOr<bool> OsqpSolver::GetVerbose() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->verbose;
+}
+
+absl::StatusOr<bool> OsqpSolver::GetScaledTermination() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->scaled_termination;
+}
+
+absl::StatusOr<c_int> OsqpSolver::GetCheckTermination() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->check_termination;
+}
+
+absl::StatusOr<bool> OsqpSolver::GetWarmStart() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->warm_start;
+}
+
+absl::StatusOr<double> OsqpSolver::GetTimeLimit() const {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ return workspace_->settings->time_limit;
+}
+
+absl::Status OsqpSolver::UpdateRho(const double rho_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (rho_new <= 0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid rho value: ", rho_new));
+ }
+ if (osqp_update_rho(workspace_.get(), rho_new) != 0) {
+ return absl::UnknownError("osqp_update_rho unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateMaxIter(const int max_iter_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (max_iter_new <= 0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid max_iter value: ", max_iter_new));
+ }
+ if (osqp_update_max_iter(workspace_.get(), max_iter_new) != 0) {
+ return absl::UnknownError("osqp_update_max_iter unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateEpsAbs(const double eps_abs_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (eps_abs_new < 0.0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid eps_abs value: ", eps_abs_new));
+ }
+ if (osqp_update_eps_abs(workspace_.get(), eps_abs_new) != 0) {
+ return absl::UnknownError("osqp_update_eps_abs unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateEpsRel(const double eps_rel_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (eps_rel_new < 0.0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid eps_rel value: ", eps_rel_new));
+ }
+ if (osqp_update_eps_rel(workspace_.get(), eps_rel_new) != 0) {
+ return absl::UnknownError("osqp_update_eps_rel unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateEpsPrimInf(const double eps_prim_inf_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (eps_prim_inf_new < 0.0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid eps_prim_inf value: ", eps_prim_inf_new));
+ }
+ if (osqp_update_eps_prim_inf(workspace_.get(), eps_prim_inf_new) != 0) {
+ return absl::UnknownError("osqp_update_eps_prim_inf unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateEpsDualInf(const double eps_dual_inf_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (eps_dual_inf_new < 0.0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid eps_dual_inf value: ", eps_dual_inf_new));
+ }
+ if (osqp_update_eps_dual_inf(workspace_.get(), eps_dual_inf_new) != 0) {
+ return absl::UnknownError("osqp_update_eps_dual_inf unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateAlpha(const double alpha_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (alpha_new <= 0.0 || alpha_new >= 2) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid alpha value: ", alpha_new));
+ }
+ if (osqp_update_alpha(workspace_.get(), alpha_new) != 0) {
+ return absl::UnknownError("osqp_update_alpha unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateDelta(const double delta_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (delta_new <= 0.0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid delta value: ", delta_new));
+ }
+ if (osqp_update_delta(workspace_.get(), delta_new) != 0) {
+ return absl::UnknownError("osqp_update_delta unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdatePolish(const bool polish_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (osqp_update_polish(workspace_.get(), polish_new) != 0) {
+ return absl::UnknownError("osqp_update_polish unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdatePolishRefineIter(
+ const int polish_refine_iter_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (polish_refine_iter_new <= 0.0) {
+ return absl::InvalidArgumentError(absl::StrCat(
+ "Invalid polish_refine_iter value: ", polish_refine_iter_new));
+ }
+ if (osqp_update_polish_refine_iter(workspace_.get(),
+ polish_refine_iter_new) != 0) {
+ return absl::UnknownError(
+ "osqp_update_polish_refine_iter unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateVerbose(const bool verbose_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (osqp_update_verbose(workspace_.get(), verbose_new) != 0) {
+ return absl::UnknownError("osqp_update_verbose unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateScaledTermination(
+ const bool scaled_termination_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (osqp_update_scaled_termination(workspace_.get(),
+ scaled_termination_new) != 0) {
+ return absl::UnknownError(
+ "osqp_update_scaled_termination unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateCheckTermination(
+ const c_int check_termination_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (check_termination_new < 0.0) {
+ return absl::InvalidArgumentError(absl::StrCat(
+ "Invalid check_termination value: ", check_termination_new));
+ }
+ if (osqp_update_check_termination(workspace_.get(), check_termination_new) !=
+ 0) {
+ return absl::UnknownError(
+ "osqp_update_check_termination unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateWarmStart(const bool warm_start_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (osqp_update_warm_start(workspace_.get(), warm_start_new) != 0) {
+ return absl::UnknownError("osqp_update_warm_start unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+absl::Status OsqpSolver::UpdateTimeLimit(const double time_limit_new) {
+ if (!IsInitialized()) {
+ return absl::FailedPreconditionError("OsqpSolver is not initialized.");
+ }
+ if (time_limit_new < 0.0) {
+ return absl::InvalidArgumentError(
+ absl::StrCat("Invalid time_limit value: ", time_limit_new));
+ }
+ if (osqp_update_time_limit(workspace_.get(), time_limit_new) != 0) {
+ return absl::UnknownError("osqp_update_time_limit unexpectedly failed.");
+ }
+ return absl::OkStatus();
+}
+
+#undef OSQP_CHECK_DIMENSIONS
+#undef OSQP_RETURN_IF_ERROR
+#undef OSQP_CHECK
+
+} // namespace osqp