constraint-set.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/fwd.hpp"
#include "aligator/context.hpp"
 
namespace aligator {

template <typename _Scalar> struct ConstraintSetTpl {
public:
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using ActiveType = Eigen::Matrix<bool, Eigen::Dynamic, 1>;
 
  ConstraintSetTpl() = default;

  virtual bool disableGaussNewton() const { return true; }

  virtual Scalar evaluate(const ConstVectorRef & /*zproj*/) const { return 0.; }

  virtual void projection(const ConstVectorRef &z, VectorRef zout) const = 0;

  virtual void normalConeProjection(const ConstVectorRef &z,
                                    VectorRef zout) const = 0;

  virtual void applyProjectionJacobian(const ConstVectorRef &z,
                                       MatrixRef Jout) const;

  virtual void applyNormalConeProjectionJacobian(const ConstVectorRef &z,
                                                 MatrixRef Jout) const;

  void setProxParameter(const Scalar mu) const {
    mu_ = mu;
    mu_inv_ = 1. / mu;
  };

  virtual void computeActiveSet(const ConstVectorRef &z,
                                Eigen::Ref<ActiveType> out) const = 0;
 
  virtual ~ConstraintSetTpl() = default;
 
  bool operator==(const ConstraintSetTpl<Scalar> &rhs) { return this == &rhs; }

  Scalar evaluateMoreauEnvelope(const ConstVectorRef &zin,
                                const ConstVectorRef &zproj) const {
    Scalar res = evaluate(zin - zproj);
    res += static_cast<Scalar>(0.5) * mu_inv_ * zproj.squaredNorm();
    return res;
  }

  Scalar computeMoreauEnvelope(const ConstVectorRef &zin,
                               VectorRef zprojout) const {
    normalConeProjection(zin, zprojout);
    return evaluateMoreauEnvelope(zin, zprojout);
  }
 
  Scalar mu() const { return mu_; }
  Scalar mu_inv() const { return mu_inv_; }
 
protected:
  mutable Scalar mu_ = 0.;
  mutable Scalar mu_inv_;
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct ALIGATOR_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
    ConstraintSetTpl<context::Scalar>;
#endif
 
} // namespace aligator
 
#include "aligator/core/constraint-set.hxx"