<commit_before>
#include "drake/solvers/IpoptSolver.h"

#include <cstring>
#include <memory>
#include <vector>

// TODO(sam.creasey) figure out how to get the real pkgconfig cflags
#define HAVE_CSTDDEF
#include <IpIpoptApplication.hpp>
#include <IpTNLP.hpp>
#undef HAVE_CSTDDEF

#include "drake/core/Gradient.h"
#include "drake/solvers/Optimization.h"

using Ipopt::Index;
using Ipopt::IpoptCalculatedQuantities;
using Ipopt::IpoptData;
using Ipopt::Number;
using Ipopt::SolverReturn;

using Drake::DecisionVariableView;
using Drake::OptimizationProblem;
using Drake::TaylorVecXd;

namespace drake {
namespace solvers {
namespace {

size_t GetConstraintBounds(
    const Drake::Constraint& c, Number* lb, Number* ub) {
  const Eigen::VectorXd& lower_bound = c.lower_bound();
  const Eigen::VectorXd& upper_bound = c.upper_bound();
  for (size_t i = 0; i < c.num_constraints(); i++) {
    lb[i] = lower_bound(i);
    ub[i] = upper_bound(i);
  }

  return c.num_constraints();
}

/// @param[out] num_grad number of gradients
/// @return number of constraints
size_t GetNumGradients(
    const Drake::Constraint& c, const Drake::VariableList& variable_list,
    Index* num_grad) {
  const size_t m = c.num_constraints();

  size_t var_count = 0;
  for (const DecisionVariableView& v : variable_list) {
    var_count += v.size();
  }
  *num_grad = m * var_count;
  return m;
}

size_t GetGradientMatrix(
    const Drake::Constraint& c, const Drake::VariableList& variable_list,
    Index constraint_idx, Index* iRow, Index* jCol) {
  const size_t m = c.num_constraints();
  size_t grad_index = 0;

  for (const DecisionVariableView& v : variable_list) {
    for (size_t i = 0; i < m; i++) {
      for (size_t j = 0; j < v.size(); j++) {
        iRow[grad_index] = constraint_idx + i;
        jCol[grad_index] = v.index() + j;
        grad_index++;
      }
    }
  }
  return grad_index;
}

Eigen::VectorXd MakeEigenVector(Index n, const Number* x) {
  Eigen::VectorXd xvec(n);
  for (size_t i = 0; i < n; i++) {
    xvec[i] = x[i];
  }
  return xvec;
}

/// @return number of gradient entries populated
size_t EvaluateConstraint(
    const Eigen::VectorXd& xvec,
    const Drake::Constraint& c, const Drake::VariableList& variable_list,
    Number* result, Number* grad) {

  size_t var_count = 0;
  for (const DecisionVariableView& v : variable_list) {
    var_count += v.size();
  }

  auto tx = Drake::initializeAutoDiff(xvec);
  TaylorVecXd this_x(var_count);
  size_t index = 0;
  for (const DecisionVariableView& v : variable_list) {
    this_x.segment(index, v.size()) = tx.segment(v.index(), v.size());
    index += v.size();
  }

  TaylorVecXd ty(c.num_constraints());
  c.eval(this_x, ty);
  for (size_t i = 0; i < c.num_constraints(); i++) {
    result[i] = ty(i).value();
  }

  size_t grad_idx = 0;
  for (const DecisionVariableView& v : variable_list) {
    for (size_t i = 0; i < c.num_constraints(); i++) {
      for (size_t j = v.index(); j < v.index() + v.size(); j++) {
        grad[grad_idx++] = ty(i).derivatives()(j);
      }
    }
  }
  return grad_idx;
}

struct ResultCache {
  ResultCache(size_t x_size, size_t result_size, size_t grad_size) {
    // The choice of infinity as the default value below is arbitrary.
    x.resize(x_size, std::numeric_limits<double>::infinity());
    result.resize(result_size, std::numeric_limits<double>::infinity());
    grad.resize(grad_size, std::numeric_limits<double>::infinity());
  }

  bool is_x_equal(Index n, const Number* x_in) {
    assert(n == x.size());
    return !std::memcmp(x.data(), x_in, x.size() * sizeof(Number));
  }

  std::vector<Number> x;
  std::vector<Number> result;
  std::vector<Number> grad;
};

class IpoptSolver_NLP : public Ipopt::TNLP {
 public:
  explicit IpoptSolver_NLP(OptimizationProblem* problem)
      : problem_(problem),
        result_(SolutionResult::kUnknownError) {}

  virtual ~IpoptSolver_NLP() {}

  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style) {
    n = problem_->num_vars();

    cost_cache_.reset(new ResultCache(n, 1, n));

    m = 0;<commit_msg>Add some comments linking into IPOPT documentation<commit_after>
#include "drake/solvers/IpoptSolver.h"

#include <cstring>
#include <memory>
#include <vector>

// TODO(sam.creasey) figure out how to get the real pkgconfig cflags
#define HAVE_CSTDDEF
#include <IpIpoptApplication.hpp>
#include <IpTNLP.hpp>
#undef HAVE_CSTDDEF

#include "drake/core/Gradient.h"
#include "drake/solvers/Optimization.h"

using Ipopt::Index;
using Ipopt::IpoptCalculatedQuantities;
using Ipopt::IpoptData;
using Ipopt::Number;
using Ipopt::SolverReturn;

using Drake::DecisionVariableView;
using Drake::OptimizationProblem;
using Drake::TaylorVecXd;

namespace drake {
namespace solvers {
namespace {

size_t GetConstraintBounds(
    const Drake::Constraint& c, Number* lb, Number* ub) {
  const Eigen::VectorXd& lower_bound = c.lower_bound();
  const Eigen::VectorXd& upper_bound = c.upper_bound();
  for (size_t i = 0; i < c.num_constraints(); i++) {
    lb[i] = lower_bound(i);
    ub[i] = upper_bound(i);
  }

  return c.num_constraints();
}

/// @param[out] num_grad number of gradients
/// @return number of constraints
size_t GetNumGradients(
    const Drake::Constraint& c, const Drake::VariableList& variable_list,
    Index* num_grad) {
  const size_t m = c.num_constraints();

  size_t var_count = 0;
  for (const DecisionVariableView& v : variable_list) {
    var_count += v.size();
  }
  *num_grad = m * var_count;
  return m;
}

/// Parameters @p iRow and @p jCol are used in the same manner as
/// described in
/// http://www.coin-or.org/Ipopt/documentation/node23.html for the
/// eval_jac_g() function (in the mode where it's requesting the
/// sparsity structure of the Jacobian).  The triplet format is also
/// described in
/// http://www.coin-or.org/Ipopt/documentation/node38.html#app.triplet
///
/// @return the number of row/column pairs filled in.
size_t GetGradientMatrix(
    const Drake::Constraint& c, const Drake::VariableList& variable_list,
    Index constraint_idx, Index* iRow, Index* jCol) {
  const size_t m = c.num_constraints();
  size_t grad_index = 0;

  for (const DecisionVariableView& v : variable_list) {
    for (size_t i = 0; i < m; i++) {
      for (size_t j = 0; j < v.size(); j++) {
        iRow[grad_index] = constraint_idx + i;
        jCol[grad_index] = v.index() + j;
        grad_index++;
      }
    }
  }
  return grad_index;
}

Eigen::VectorXd MakeEigenVector(Index n, const Number* x) {
  Eigen::VectorXd xvec(n);
  for (size_t i = 0; i < n; i++) {
    xvec[i] = x[i];
  }
  return xvec;
}

/// @return number of gradient entries populated
size_t EvaluateConstraint(
    const Eigen::VectorXd& xvec,
    const Drake::Constraint& c, const Drake::VariableList& variable_list,
    Number* result, Number* grad) {

  size_t var_count = 0;
  for (const DecisionVariableView& v : variable_list) {
    var_count += v.size();
  }

  auto tx = Drake::initializeAutoDiff(xvec);
  TaylorVecXd this_x(var_count);
  size_t index = 0;
  for (const DecisionVariableView& v : variable_list) {
    this_x.segment(index, v.size()) = tx.segment(v.index(), v.size());
    index += v.size();
  }

  TaylorVecXd ty(c.num_constraints());
  c.eval(this_x, ty);
  for (size_t i = 0; i < c.num_constraints(); i++) {
    result[i] = ty(i).value();
  }

  size_t grad_idx = 0;
  for (const DecisionVariableView& v : variable_list) {
    for (size_t i = 0; i < c.num_constraints(); i++) {
      for (size_t j = v.index(); j < v.index() + v.size(); j++) {
        grad[grad_idx++] = ty(i).derivatives()(j);
      }
    }
  }
  return grad_idx;
}

// IPOPT uses separate callbacks to get the result and the gradients.
// Since Drake's eval() functions emit both of these at once, cache
// the result for IPOPT.
struct ResultCache {
  ResultCache(size_t x_size, size_t result_size, size_t grad_size) {
    // The choice of infinity as the default value below is arbitrary.
    x.resize(x_size, std::numeric_limits<double>::infinity());
    result.resize(result_size, std::numeric_limits<double>::infinity());
    grad.resize(grad_size, std::numeric_limits<double>::infinity());
  }

  bool is_x_equal(Index n, const Number* x_in) {
    assert(n == x.size());
    return !std::memcmp(x.data(), x_in, x.size() * sizeof(Number));
  }

  std::vector<Number> x;
  std::vector<Number> result;
  std::vector<Number> grad;
};

// The C++ interface for IPOPT is described here:
// http://www.coin-or.org/Ipopt/documentation/node23.html
//
// IPOPT provides a pure(-ish) virtual base class which you have to
// implement a concrete version of as the solver interface.
// IpoptSolver creates an instance of IpoptSolver_NLP which lives for
// the duration of the Solve() call.
class IpoptSolver_NLP : public Ipopt::TNLP {
 public:
  explicit IpoptSolver_NLP(OptimizationProblem* problem)
      : problem_(problem),
        result_(SolutionResult::kUnknownError) {}

  virtual ~IpoptSolver_NLP() {}

  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style) {
    n = problem_->num_vars();

    cost_cache_.reset(new ResultCache(n, 1, n));

    m = 0;
{'fraction_non_alphanumeric': 0.13839189480491484, 'fraction_numerical': 0.0034490191851692174, 'mean_word_length': 3.8227650727650726, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 27, 'https://': 0, 'lorem ipsum': 0, 'www.': 3, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '899744', 'n_tokens_mistral': 3376, 'n_tokens_neox': 3219, 'n_words': 831}