Skip to content

Commit

Permalink
Fix osqp eigen solver hessian and linear constraint matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Aug 1, 2023
1 parent 70d64d3 commit c2d372d
Show file tree
Hide file tree
Showing 3 changed files with 189 additions and 42 deletions.
140 changes: 133 additions & 7 deletions trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,11 @@
#include <trajopt_sqp/osqp_eigen_solver.h>

#include <OsqpEigen/OsqpEigen.h>
#include <osqp/cs.h>
#include <osqp/types.h>
#include <trajopt_common/macros.h>

const bool OSQP_COMPARE_DEBUG_MODE = true;

namespace trajopt_sqp
{
Expand Down Expand Up @@ -71,8 +76,57 @@ bool OSQPEigenSolver::solve()
if (!solver_.isInitialized()) // NOLINT
solver_.initSolver();

if (solver_.solve())
if (OSQP_COMPARE_DEBUG_MODE)
{
Eigen::IOFormat format(5);
const auto* osqp_data = solver_.data()->getData();
std::cout << "OSQP Number of Variables:" << osqp_data->n << std::endl;
std::cout << "OSQP Number of Constraints:" << osqp_data->m << std::endl;

Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> P_p_vec(osqp_data->P->p, osqp_data->P->n + 1);
Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> P_i_vec(osqp_data->P->i, osqp_data->P->nzmax);
Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> P_x_vec(osqp_data->P->x, osqp_data->P->nzmax);
std::cout << "OSQP Hessian:" << std::endl;
std::cout << " nzmax:" << osqp_data->P->nzmax << std::endl;
std::cout << " nz:" << osqp_data->P->nz << std::endl;
std::cout << " m:" << osqp_data->P->m << std::endl;
std::cout << " n:" << osqp_data->P->n << std::endl;
std::cout << " p:" << P_p_vec.transpose().format(format) << std::endl;
std::cout << " i:" << P_i_vec.transpose().format(format) << std::endl;
std::cout << " x:" << P_x_vec.transpose().format(format) << std::endl;

Eigen::Map<Eigen::VectorXd> q_vec(osqp_data->q, osqp_data->n);
std::cout << "OSQP Gradient: " << q_vec.transpose().format(format) << std::endl;

Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> A_p_vec(osqp_data->A->p, osqp_data->A->n + 1);
Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> A_i_vec(osqp_data->A->i, osqp_data->A->nzmax);
Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> A_x_vec(osqp_data->A->x, osqp_data->A->nzmax);
std::cout << "OSQP Constraint Matrix:" << std::endl;
std::cout << " nzmax:" << osqp_data->A->nzmax << std::endl;
std::cout << " m:" << osqp_data->A->m << std::endl;
std::cout << " n:" << osqp_data->A->n << std::endl;
std::cout << " p:" << A_p_vec.transpose().format(format) << std::endl;
std::cout << " i:" << A_i_vec.transpose().format(format) << std::endl;
std::cout << " x:" << A_x_vec.transpose().format(format) << std::endl;

Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> l_vec(osqp_data->l, osqp_data->m);
Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> u_vec(osqp_data->u, osqp_data->m);
std::cout << "OSQP Lower Bounds: " << l_vec.transpose().format(format) << std::endl;
std::cout << "OSQP Upper Bounds: " << u_vec.transpose().format(format) << std::endl;

std::cout << "OSQP Variable Names: " << std::endl;
}

/** @todo Need to check if this is what we want in the new version */
if (solver_.solve() || solver_.workspace()->info->status_val == OSQP_SOLVED_INACCURATE)
{
if (OSQP_COMPARE_DEBUG_MODE)
{
Eigen::IOFormat format(5);
std::cout << "OSQP Solution: " << solver_.getSolution().transpose().format(format) << std::endl;
}
return true;
}

if (verbosity > 0) // NOLINT
{
Expand Down Expand Up @@ -123,17 +177,73 @@ Eigen::VectorXd OSQPEigenSolver::getSolution()
return solution;
}

// This code is required because there is a bug in OSQP 0.6.X scs_spalloc method
/** @todo Remove when upgrading to OSQP 1.0.0 */
TRAJOPT_IGNORE_WARNINGS_PUSH
static void* csc_malloc(c_int n, c_int size)
{
return c_malloc(n * size); // NOLINT
}

static void* csc_calloc(c_int n, c_int size)
{
return c_calloc(n, size); // NOLINT
}

csc* csc_spalloc_fix(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet)
{
csc* A = (csc*)(csc_calloc(1, sizeof(csc))); /* allocate the csc struct */ // NOLINT

if (!A) // NOLINT
return OSQP_NULL; /* out of memory */

A->m = m; /* define dimensions and nzmax */
A->n = n;
A->nzmax = nzmax = c_max(nzmax, 0);
A->nz = triplet ? 0 : -1; /* allocate triplet or comp.col */ // NOLINT
A->p = (c_int*)(csc_malloc(triplet ? nzmax : n + 1, sizeof(c_int))); // NOLINT
for (int i = 0; i < n + 1; ++i)
A->p[i] = 0;
A->i = values ? (c_int*)(csc_malloc(nzmax, sizeof(c_int))) : OSQP_NULL; // NOLINT
A->x = values ? (c_float*)(csc_malloc(nzmax, sizeof(c_float))) : OSQP_NULL; // NOLINT
if (!A->p || (values && !A->i) || (values && !A->x)) // NOLINT
{
csc_spfree(A);
return OSQP_NULL;
}

return A; // NOLINT
}
TRAJOPT_IGNORE_WARNINGS_POP

bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian)
{
// Clean up values close to 0
// Also multiply by 2 because OSQP is multiplying by (1/2) for the objective fuction
SparseMatrix cleaned = 2.0 * hessian.pruned(1e-7);
SparseMatrix cleaned = 2.0 * hessian.pruned(1e-7, 1); // Any value < 1e-7 will be removed

if (solver_.isInitialized())
return solver_.updateHessianMatrix(cleaned);
{
bool success = solver_.updateHessianMatrix(cleaned);
if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */
{
csc_spfree(solver_.data()->getData()->P);
solver_.data()->getData()->P = nullptr;
solver_.data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0);
}
return success;
}

solver_.data()->clearHessianMatrix();
return solver_.data()->setHessianMatrix(cleaned);
bool success = solver_.data()->setHessianMatrix(cleaned);
if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */
{
csc_spfree(solver_.data()->getData()->P);
solver_.data()->getData()->P = nullptr;
solver_.data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0);
}

return success;
}

bool OSQPEigenSolver::updateGradient(const Eigen::Ref<const Eigen::VectorXd>& gradient)
Expand Down Expand Up @@ -178,12 +288,28 @@ bool OSQPEigenSolver::updateLinearConstraintsMatrix(const SparseMatrix& linearCo
assert(num_vars_ == linearConstraintsMatrix.cols());

solver_.data()->clearLinearConstraintsMatrix();
SparseMatrix cleaned = linearConstraintsMatrix.pruned(1e-7);
SparseMatrix cleaned = linearConstraintsMatrix.pruned(1e-7, 1); // Any value < 1e-7 will be removed

if (solver_.isInitialized())
return solver_.updateLinearConstraintsMatrix(linearConstraintsMatrix);
{
bool success = solver_.updateLinearConstraintsMatrix(cleaned);
if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */
{
csc_spfree(solver_.data()->getData()->A);
solver_.data()->getData()->A = nullptr;
solver_.data()->getData()->A = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0);
}
return success;
}

return solver_.data()->setLinearConstraintsMatrix(cleaned);
bool success = solver_.data()->setLinearConstraintsMatrix(cleaned);
if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */
{
csc_spfree(solver_.data()->getData()->A);
solver_.data()->getData()->A = nullptr;
solver_.data()->getData()->A = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0);
}
return success;
}

} // namespace trajopt_sqp
4 changes: 2 additions & 2 deletions trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,10 +117,10 @@ void runNumericalIKTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env
// 5) Setup solver
auto qp_solver = std::make_shared<trajopt_sqp::OSQPEigenSolver>();
trajopt_sqp::TrustRegionSQPSolver solver(qp_solver);
qp_solver->solver_.settings()->setVerbosity(true);
qp_solver->solver_.settings()->setVerbosity(false);
qp_solver->solver_.settings()->setWarmStart(true);
qp_solver->solver_.settings()->setPolish(true);
qp_solver->solver_.settings()->setAdaptiveRho(false);
qp_solver->solver_.settings()->setAdaptiveRho(true);
qp_solver->solver_.settings()->setMaxIteration(8192);
qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4);
qp_solver->solver_.settings()->setRelativeTolerance(1e-6);
Expand Down
87 changes: 54 additions & 33 deletions trajopt_sco/src/osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
namespace sco
{
const double OSQP_INFINITY = std::numeric_limits<double>::infinity();
const bool SUPER_DEBUG_MODE = false;
const bool OSQP_COMPARE_DEBUG_MODE = true;

OSQPModelConfig::OSQPModelConfig()
{
Expand All @@ -28,7 +28,7 @@ OSQPModelConfig::OSQPModelConfig()
settings.max_iter = 8192;
settings.polish = 1;
settings.adaptive_rho = 1;
settings.verbose = static_cast<c_int>(SUPER_DEBUG_MODE);
settings.verbose = 0;
}

Model::Ptr createOSQPModel(const ModelConfig::ConstPtr& config = nullptr)
Expand Down Expand Up @@ -118,8 +118,7 @@ void OSQPModel::updateObjective()
// Copy triangular upper into empty matrix
Eigen::SparseMatrix<double> triangular_sm;
triangular_sm = sm.triangularView<Eigen::Upper>();
if (SUPER_DEBUG_MODE)
std::cout << std::fixed << std::setprecision(3) << "OSQP Hessian:\n" << triangular_sm.toDense() << std::endl;

eigenToCSC(triangular_sm, P_row_indices_, P_column_pointers_, P_csc_data_);

P_.reset(csc_matrix(osqp_data_.n,
Expand All @@ -131,12 +130,6 @@ void OSQPModel::updateObjective()

osqp_data_.P = P_.get();
osqp_data_.q = q_.data();

if (SUPER_DEBUG_MODE)
{
Eigen::Map<Eigen::VectorXd> q_vec(q_.data(), q_.size());
std::cout << std::fixed << std::setprecision(3) << "OSQP Gradient: " << q_vec.transpose() << std::endl;
}
}

void OSQPModel::updateConstraints()
Expand Down Expand Up @@ -172,8 +165,7 @@ void OSQPModel::updateConstraints()
u_[i_bnd + m] = fmin(ubs_[i_bnd], OSQP_INFINITY);
sm.insert(static_cast<Eigen::Index>(i_bnd + m), static_cast<Eigen::Index>(i_bnd)) = 1.;
}
if (SUPER_DEBUG_MODE)
std::cout << std::fixed << std::setprecision(3) << "OSQP Constraint Matrix:\n" << sm.toDense() << std::endl;

eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_);

A_.reset(csc_matrix(osqp_data_.m,
Expand All @@ -185,25 +177,6 @@ void OSQPModel::updateConstraints()

osqp_data_.A = A_.get();

if (SUPER_DEBUG_MODE)
{
Eigen::Map<Eigen::VectorXd> l_vec(l_.data(), static_cast<Eigen::Index>(l_.size()));
Eigen::Map<Eigen::VectorXd> u_vec(u_.data(), static_cast<Eigen::Index>(u_.size()));
std::cout << "OSQP Constraint Lower Bounds: " << l_vec.head(static_cast<Eigen::Index>(m)).transpose() << std::endl;
std::cout << "OSQP Constraint Upper Bounds: " << u_vec.head(static_cast<Eigen::Index>(m)).transpose() << std::endl;

std::vector<std::string> vars_names(vars_.size());
for (const auto& var : vars_)
vars_names[var.var_rep->index] = var.var_rep->name;

std::cout << "OSQP Variable Names: ";
for (const auto& var_name : vars_names)
std::cout << var_name << ",";
std::cout << std::endl;

std::cout << "OSQP Variable Lower Bounds: " << l_vec.tail(static_cast<Eigen::Index>(n)).transpose() << std::endl;
std::cout << "OSQP Variable Upper Bounds: " << u_vec.tail(static_cast<Eigen::Index>(n)).transpose() << std::endl;
}
osqp_data_.l = l_.data();
osqp_data_.u = u_.data();
}
Expand Down Expand Up @@ -310,6 +283,53 @@ CvxOptStatus OSQPModel::optimize()
return CVX_FAILED;
}

if (OSQP_COMPARE_DEBUG_MODE)
{
Eigen::IOFormat format(5);
std::cout << "OSQP Number of Variables:" << osqp_data_.n << std::endl;
std::cout << "OSQP Number of Constraints:" << osqp_data_.m << std::endl;

Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> P_p_vec(osqp_data_.P->p, osqp_data_.P->n + 1);
Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> P_i_vec(osqp_data_.P->i, osqp_data_.P->nzmax);
Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> P_x_vec(osqp_data_.P->x, osqp_data_.P->nzmax);
std::cout << "OSQP Hessian:" << std::endl;
std::cout << " nzmax:" << osqp_data_.P->nzmax << std::endl;
std::cout << " nz:" << osqp_data_.P->nz << std::endl;
std::cout << " m:" << osqp_data_.P->m << std::endl;
std::cout << " n:" << osqp_data_.P->n << std::endl;
std::cout << " p:" << P_p_vec.transpose().format(format) << std::endl;
std::cout << " i:" << P_i_vec.transpose().format(format) << std::endl;
std::cout << " x:" << P_x_vec.transpose().format(format) << std::endl;

Eigen::Map<Eigen::VectorXd> q_vec(osqp_data_.q, osqp_data_.n);
std::cout << "OSQP Gradient: " << q_vec.transpose().format(format) << std::endl;

Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> A_p_vec(osqp_data_.A->p, osqp_data_.A->n + 1);
Eigen::Map<Eigen::Matrix<c_int, Eigen::Dynamic, 1>> A_i_vec(osqp_data_.A->i, osqp_data_.A->nzmax);
Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> A_x_vec(osqp_data_.A->x, osqp_data_.A->nzmax);
std::cout << "OSQP Constraint Matrix:" << std::endl;
std::cout << " nzmax:" << osqp_data_.A->nzmax << std::endl;
std::cout << " m:" << osqp_data_.A->m << std::endl;
std::cout << " n:" << osqp_data_.A->n << std::endl;
std::cout << " p:" << A_p_vec.transpose().format(format) << std::endl;
std::cout << " i:" << A_i_vec.transpose().format(format) << std::endl;
std::cout << " x:" << A_x_vec.transpose().format(format) << std::endl;

Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> l_vec(osqp_data_.l, osqp_data_.m);
Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> u_vec(osqp_data_.u, osqp_data_.m);
std::cout << "OSQP Lower Bounds: " << l_vec.transpose().format(format) << std::endl;
std::cout << "OSQP Upper Bounds: " << u_vec.transpose().format(format) << std::endl;

std::vector<std::string> vars_names(vars_.size());
for (const auto& var : vars_)
vars_names[var.var_rep->index] = var.var_rep->name;

std::cout << "OSQP Variable Names: ";
for (const auto& var_name : vars_names)
std::cout << var_name << ",";
std::cout << std::endl;
}

// Solve Problem
const c_int retcode = osqp_solve(osqp_workspace_);

Expand All @@ -318,10 +338,11 @@ CvxOptStatus OSQPModel::optimize()
// opt += m_objective.affexpr.constant;
solution_ = DblVec(osqp_workspace_->solution->x, osqp_workspace_->solution->x + vars_.size());

if (SUPER_DEBUG_MODE)
if (OSQP_COMPARE_DEBUG_MODE)
{
Eigen::IOFormat format(5);
Eigen::Map<Eigen::VectorXd> solution_vec(solution_.data(), static_cast<Eigen::Index>(solution_.size()));
std::cout << "Solution: " << solution_vec.transpose() << std::endl;
std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl;
}

auto status = static_cast<int>(osqp_workspace_->info->status_val);
Expand Down

0 comments on commit c2d372d

Please sign in to comment.