box-qp.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2025, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.

#ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename _Scalar>
struct BoxQPSolutionTpl {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 BoxQPSolutionTpl() {}

 BoxQPSolutionTpl(const MatrixXs& Hff_inv, const VectorXs& x,
 const std::vector<size_t>& free_idx,
 const std::vector<size_t>& clamped_idx)
 : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(clamped_idx) {}

 template <typename NewScalar>
 BoxQPSolutionTpl<NewScalar> cast() const {
 typedef BoxQPSolutionTpl<NewScalar> ReturnType;
 ReturnType ret(Hff_inv.template cast<NewScalar>(),
 x.template cast<NewScalar>(), free_idx, clamped_idx);
 return ret;
 }

 MatrixXs Hff_inv;
 VectorXs x;
 std::vector<size_t> free_idx;
 std::vector<size_t> clamped_idx;
};

template <typename _Scalar>
class BoxQPTpl {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef BoxQPSolutionTpl<Scalar> BoxQPSolution;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 BoxQPTpl(const std::size_t nx, const std::size_t maxiter = 100,
 const Scalar th_acceptstep = Scalar(0.1),
 const Scalar th_grad = Scalar(1e-9),
 const Scalar reg = Scalar(1e-9));
 ~BoxQPTpl() = default;

 const BoxQPSolution& solve(const MatrixXs& H, const VectorXs& q,
 const VectorXs& lb, const VectorXs& ub,
 const VectorXs& xinit);

 template <typename NewScalar>
 BoxQPTpl<NewScalar> cast() const;

 const BoxQPSolution& get_solution() const;

 std::size_t get_nx() const;

 std::size_t get_maxiter() const;

 Scalar get_th_acceptstep() const;

 Scalar get_th_grad() const;

 Scalar get_reg() const;

 const std::vector<Scalar>& get_alphas() const;

 void set_nx(const std::size_t nx);

 void set_maxiter(const std::size_t maxiter);

 void set_th_acceptstep(const Scalar th_acceptstep);

 void set_th_grad(const Scalar th_grad);

 void set_reg(const Scalar reg);

 void set_alphas(const std::vector<Scalar>& alphas);

 private:
 std::size_t nx_;
 BoxQPSolution solution_;
 std::size_t maxiter_;
 Scalar th_acceptstep_;
 Scalar
 th_grad_;
 Scalar reg_;

 Scalar fold_;
 Scalar fnew_;
 std::size_t nf_;
 std::size_t nc_;
 std::vector<Scalar>
 alphas_;
 VectorXs x_;
 VectorXs xnew_;
 VectorXs g_;
 VectorXs dx_;

 VectorXs xo_;
 VectorXs
 dxo_;
 VectorXs qo_;
 MatrixXs Ho_;

 Eigen::LLT<MatrixXs> Hff_inv_llt_;
};

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/solvers/box-qp.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::BoxQPTpl)

#endif // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_