constraint-manager.hpp

// BSD 3-Clause License
//
// Copyright (C) 2020-2024, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
#define CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
 
#include <map>
#include <set>
#include <string>
#include <utility>
 
#include "crocoddyl/core/constraint-base.hpp"
#include "crocoddyl/core/utils/exception.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
struct ConstraintItemTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract;
 
  ConstraintItemTpl() {}
  ConstraintItemTpl(const std::string& name,
                    boost::shared_ptr<ConstraintModelAbstract> constraint,
                    bool active = true)
      : name(name), constraint(constraint), active(active) {}
 
  friend std::ostream& operator<<(std::ostream& os,
                                  const ConstraintItemTpl<Scalar>& model) {
    os << "{" << *model.constraint << "}";
    return os;
  }
 
  std::string name;
  boost::shared_ptr<ConstraintModelAbstract> constraint;
  bool active;
};
 
template <typename _Scalar>
class ConstraintModelManagerTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract;
  typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef ConstraintItemTpl<Scalar> ConstraintItem;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  typedef std::map<std::string, boost::shared_ptr<ConstraintItem> >
      ConstraintModelContainer;
  typedef std::map<std::string, boost::shared_ptr<ConstraintDataAbstract> >
      ConstraintDataContainer;
 
  ConstraintModelManagerTpl(boost::shared_ptr<StateAbstract> state,
                            const std::size_t nu);
 
  explicit ConstraintModelManagerTpl(boost::shared_ptr<StateAbstract> state);
  ~ConstraintModelManagerTpl();
 
  void addConstraint(const std::string& name,
                     boost::shared_ptr<ConstraintModelAbstract> constraint,
                     const bool active = true);
 
  void removeConstraint(const std::string& name);
 
  void changeConstraintStatus(const std::string& name, bool active);
 
  void calc(const boost::shared_ptr<ConstraintDataManager>& data,
            const Eigen::Ref<const VectorXs>& x,
            const Eigen::Ref<const VectorXs>& u);
 
  void calc(const boost::shared_ptr<ConstraintDataManager>& data,
            const Eigen::Ref<const VectorXs>& x);
 
  void calcDiff(const boost::shared_ptr<ConstraintDataManager>& data,
                const Eigen::Ref<const VectorXs>& x,
                const Eigen::Ref<const VectorXs>& u);
 
  void calcDiff(const boost::shared_ptr<ConstraintDataManager>& data,
                const Eigen::Ref<const VectorXs>& x);
 
  boost::shared_ptr<ConstraintDataManager> createData(
      DataCollectorAbstract* const data);
 
  const boost::shared_ptr<StateAbstract>& get_state() const;
 
  const ConstraintModelContainer& get_constraints() const;
 
  std::size_t get_nu() const;
 
  std::size_t get_ng() const;
 
  std::size_t get_nh() const;
 
  std::size_t get_ng_T() const;
 
  std::size_t get_nh_T() const;
 
  const std::set<std::string>& get_active_set() const;
 
  const std::set<std::string>& get_inactive_set() const;
 
  const VectorXs& get_lb() const;
 
  const VectorXs& get_ub() const;
 
  bool getConstraintStatus(const std::string& name) const;
 
  template <class Scalar>
  friend std::ostream& operator<<(
      std::ostream& os, const ConstraintModelManagerTpl<Scalar>& model);
 
 private:
  boost::shared_ptr<StateAbstract> state_;  
  ConstraintModelContainer constraints_;    
  VectorXs lb_;                             
  VectorXs ub_;                             
  std::size_t nu_;                          
  std::size_t ng_;    
  std::size_t nh_;    
  std::size_t ng_T_;  
  std::size_t nh_T_;  
  std::set<std::string> active_set_;  
  std::set<std::string>
      inactive_set_;  
};
 
template <typename _Scalar>
struct ConstraintDataManagerTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef ConstraintItemTpl<Scalar> ConstraintItem;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  template <template <typename Scalar> class Model>
  ConstraintDataManagerTpl(Model<Scalar>* const model,
                           DataCollectorAbstract* const data)
      : g_internal(model->get_ng()),
        Gx_internal(model->get_ng(), model->get_state()->get_ndx()),
        Gu_internal(model->get_ng(), model->get_nu()),
        h_internal(model->get_nh()),
        Hx_internal(model->get_nh(), model->get_state()->get_ndx()),
        Hu_internal(model->get_nh(), model->get_nu()),
        shared(data),
        g(g_internal.data(), model->get_ng()),
        Gx(Gx_internal.data(), model->get_ng(), model->get_state()->get_ndx()),
        Gu(Gu_internal.data(), model->get_ng(), model->get_nu()),
        h(h_internal.data(), model->get_nh()),
        Hx(Hx_internal.data(), model->get_nh(), model->get_state()->get_ndx()),
        Hu(Hu_internal.data(), model->get_nh(), model->get_nu()) {
    g.setZero();
    Gx.setZero();
    Gu.setZero();
    h.setZero();
    Hx.setZero();
    Hu.setZero();
    for (typename ConstraintModelManagerTpl<
             Scalar>::ConstraintModelContainer::const_iterator it =
             model->get_constraints().begin();
         it != model->get_constraints().end(); ++it) {
      const boost::shared_ptr<ConstraintItem>& item = it->second;
      constraints.insert(
          std::make_pair(item->name, item->constraint->createData(data)));
    }
  }
 
  template <class ActionData>
  void shareMemory(ActionData* const data) {
    // Save memory by setting the internal variables with null dimension
    g_internal.resize(0);
    Gx_internal.resize(0, 0);
    Gu_internal.resize(0, 0);
    h_internal.resize(0);
    Hx_internal.resize(0, 0);
    Hu_internal.resize(0, 0);
    // Share memory with the differential action data
    new (&g) Eigen::Map<VectorXs>(data->g.data(), data->g.size());
    new (&Gx)
        Eigen::Map<MatrixXs>(data->Gx.data(), data->Gx.rows(), data->Gx.cols());
    new (&Gu)
        Eigen::Map<MatrixXs>(data->Gu.data(), data->Gu.rows(), data->Gu.cols());
    new (&h) Eigen::Map<VectorXs>(data->h.data(), data->h.size());
    new (&Hx)
        Eigen::Map<MatrixXs>(data->Hx.data(), data->Hx.rows(), data->Hx.cols());
    new (&Hu)
        Eigen::Map<MatrixXs>(data->Hu.data(), data->Hu.rows(), data->Hu.cols());
  }
 
  template <class Model>
  void resize(Model* const model, const bool running_node = true) {
    const std::size_t ndx = model->get_state()->get_ndx();
    const std::size_t nu = model->get_nu();
    const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
    const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
    new (&g) Eigen::Map<VectorXs>(g_internal.data(), ng);
    new (&Gx) Eigen::Map<MatrixXs>(Gx_internal.data(), ng, ndx);
    new (&Gu) Eigen::Map<MatrixXs>(Gu_internal.data(), ng, nu);
    new (&h) Eigen::Map<VectorXs>(h_internal.data(), nh);
    new (&Hx) Eigen::Map<MatrixXs>(Hx_internal.data(), nh, ndx);
    new (&Hu) Eigen::Map<MatrixXs>(Hu_internal.data(), nh, nu);
  }
 
  template <class ActionModel, class ActionData>
  void resize(ActionModel* const model, ActionData* const data,
              const bool running_node = true) {
    const std::size_t ndx = model->get_state()->get_ndx();
    const std::size_t nu = model->get_nu();
    const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
    const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
    data->g.conservativeResize(ng);
    data->Gx.conservativeResize(ng, ndx);
    data->Gu.conservativeResize(ng, nu);
    data->h.conservativeResize(nh);
    data->Hx.conservativeResize(nh, ndx);
    data->Hu.conservativeResize(nh, nu);
    new (&g) Eigen::Map<VectorXs>(data->g.data(), ng);
    new (&Gx) Eigen::Map<MatrixXs>(data->Gx.data(), ng, ndx);
    new (&Gu) Eigen::Map<MatrixXs>(data->Gu.data(), ng, nu);
    new (&h) Eigen::Map<VectorXs>(data->h.data(), nh);
    new (&Hx) Eigen::Map<MatrixXs>(data->Hx.data(), nh, ndx);
    new (&Hu) Eigen::Map<MatrixXs>(data->Hu.data(), nh, nu);
  }
 
  VectorXs get_g() const { return g; }
  MatrixXs get_Gx() const { return Gx; }
  MatrixXs get_Gu() const { return Gu; }
  VectorXs get_h() const { return h; }
  MatrixXs get_Hx() const { return Hx; }
  MatrixXs get_Hu() const { return Hu; }
 
  void set_g(const VectorXs& _g) {
    if (g.size() != _g.size()) {
      throw_pretty(
          "Invalid argument: " << "g has wrong dimension (it should be " +
                                      std::to_string(g.size()) + ")");
    }
    g = _g;
  }
  void set_Gx(const MatrixXs& _Gx) {
    if (Gx.rows() != _Gx.rows() || Gx.cols() != _Gx.cols()) {
      throw_pretty(
          "Invalid argument: " << "Gx has wrong dimension (it should be " +
                                      std::to_string(Gx.rows()) + ", " +
                                      std::to_string(Gx.cols()) + ")");
    }
    Gx = _Gx;
  }
  void set_Gu(const MatrixXs& _Gu) {
    if (Gu.rows() != _Gu.rows() || Gu.cols() != _Gu.cols()) {
      throw_pretty(
          "Invalid argument: " << "Gu has wrong dimension (it should be " +
                                      std::to_string(Gu.rows()) + ", " +
                                      std::to_string(Gu.cols()) + ")");
    }
    Gu = _Gu;
  }
  void set_h(const VectorXs& _h) {
    if (h.size() != _h.size()) {
      throw_pretty(
          "Invalid argument: " << "h has wrong dimension (it should be " +
                                      std::to_string(h.size()) + ")");
    }
    h = _h;
  }
  void set_Hx(const MatrixXs& _Hx) {
    if (Hx.rows() != _Hx.rows() || Hx.cols() != _Hx.cols()) {
      throw_pretty(
          "Invalid argument: " << "Hx has wrong dimension (it should be " +
                                      std::to_string(Hx.rows()) + ", " +
                                      std::to_string(Hx.cols()) + ")");
    }
    Hx = _Hx;
  }
  void set_Hu(const MatrixXs& _Hu) {
    if (Hu.rows() != _Hu.rows() || Hu.cols() != _Hu.cols()) {
      throw_pretty(
          "Invalid argument: " << "Hu has wrong dimension (it should be " +
                                      std::to_string(Hu.rows()) + ", " +
                                      std::to_string(Hu.cols()) + ")");
    }
    Hu = _Hu;
  }
 
  // Creates internal data in case we don't share it externally
  VectorXs g_internal;
  MatrixXs Gx_internal;
  MatrixXs Gu_internal;
  VectorXs h_internal;
  MatrixXs Hx_internal;
  MatrixXs Hu_internal;
 
  typename ConstraintModelManagerTpl<Scalar>::ConstraintDataContainer
      constraints;
  DataCollectorAbstract* shared;
  Eigen::Map<VectorXs> g;
  Eigen::Map<MatrixXs> Gx;
  Eigen::Map<MatrixXs> Gu;
  Eigen::Map<VectorXs> h;
  Eigen::Map<MatrixXs> Hx;
  Eigen::Map<MatrixXs> Hu;
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/constraints/constraint-manager.hxx"
 
#endif  // CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_