Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/constraints/constraint-manager.hpp	Lines:	54	87	62.1 %
Date:	2024-02-13 11:12:33	Branches:	59	278	21.2 %


///////////////////////////////////////////////////////////////////////////////
// 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) {}

  /**
   * @brief Print information on the constraint item
   */
  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;
};

/**
 * @brief Manage the individual constraint models
 *
 * This class serves to manage a set of added constraint models. The constraint
 * functions might active or inactive, with this approach we avoid dynamic
 * allocation of memory. Each constraint model is added through `addConstraint`,
 * where its status can be defined.
 *
 * The main computations are carring out in `calc` and `calcDiff` routines.
 * `calc` computes the constraint residuals and `calcDiff` computes the
 * Jacobians of the constraint functions. Concretely speaking, `calcDiff` builds
 * a linear approximation of the total constraint function (both inequality and
 * equality) with the form: \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$,
 * \f$\mathbf{h_x}\in\mathbb{R}^{nh\times ndx}\f$
 * \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$.
 *
 * \sa `StateAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
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;

  /**
   * @brief Initialize the constraint-manager model
   *
   * @param[in] state  State of the multibody system
   * @param[in] nu     Dimension of control vector
   */
  ConstraintModelManagerTpl(boost::shared_ptr<StateAbstract> state,
                            const std::size_t nu);

  /**
   * @brief Initialize the constraint-manager model
   *
   * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state  State of the multibody system
   */
  explicit ConstraintModelManagerTpl(boost::shared_ptr<StateAbstract> state);
  ~ConstraintModelManagerTpl();

  /**
   * @brief Add a constraint item
   *
   * @param[in] name        Constraint name
   * @param[in] constraint  Constraint model
   * @param[in] weight      Constraint weight
   * @param[in] active      True if the constraint is activated (default true)
   */
  void addConstraint(const std::string& name,
                     boost::shared_ptr<ConstraintModelAbstract> constraint,
                     const bool active = true);

  /**
   * @brief Remove a constraint item
   *
   * @param[in] name  Constraint name
   */
  void removeConstraint(const std::string& name);

  /**
   * @brief Change the constraint status
   *
   * @param[in] name    Constraint name
   * @param[in] active  Constraint status (true for active and false for
   * inactive)
   */
  void changeConstraintStatus(const std::string& name, bool active);

  /**
   * @brief Compute the total constraint value
   *
   * @param[in] data  Constraint data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  void calc(const boost::shared_ptr<ConstraintDataManager>& data,
            const Eigen::Ref<const VectorXs>& x,
            const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief Compute the total constraint value for nodes that depends only on
   * the state
   *
   * It updates the constraint based on the state only. This function is
   * commonly used in the terminal nodes of an optimal control problem.
   *
   * @param[in] data  Constraint data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   */
  void calc(const boost::shared_ptr<ConstraintDataManager>& data,
            const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Compute the Jacobian of the total constraint
   *
   * @param[in] data  Constraint data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  void calcDiff(const boost::shared_ptr<ConstraintDataManager>& data,
                const Eigen::Ref<const VectorXs>& x,
                const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief Compute the Jacobian of the total constraint with respect to the
   * state only
   *
   * It computes the Jacobian of the constraint function based on the state
   * only. This function is commonly used in the terminal nodes of an optimal
   * control problem.
   *
   * @param[in] data  Constraint data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   */
  void calcDiff(const boost::shared_ptr<ConstraintDataManager>& data,
                const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Create the constraint data
   *
   * The default data contains objects to store the values of the constraint and
   * their derivatives (i.e. Jacobians). However, it is possible to specialize
   * this function is we need to create additional data, for instance, to avoid
   * dynamic memory allocation.
   *
   * @param data  Data collector
   * @return the constraint data
   */
  boost::shared_ptr<ConstraintDataManager> createData(
      DataCollectorAbstract* const data);

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Return the stack of constraint models
   */
  const ConstraintModelContainer& get_constraints() const;

  /**
   * @brief Return the dimension of the control input
   */
  std::size_t get_nu() const;

  /**
   * @brief Return the number of active inequality constraints
   */
  std::size_t get_ng() const;

  /**
   * @brief Return the number of active equality constraints
   */
  std::size_t get_nh() const;

  /**
   * @brief Return the names of the set of active constraints
   */
  const std::set<std::string>& get_active_set() const;

  /**
   * @brief Return the names of the set of inactive constraints
   */
  const std::set<std::string>& get_inactive_set() const;

  /**
   * @brief Return the state lower bound
   */
  const VectorXs& get_lb() const;

  /**
   * @brief Return the state upper bound
   */
  const VectorXs& get_ub() const;

  /**
   * @brief Return the status of a given constraint name
   *
   * @param[in] name  Constraint name
   */
  bool getConstraintStatus(const std::string& name) const;

  /**
   * @brief Print information on the stack of constraints
   */
  template <class Scalar>
  friend std::ostream& operator<<(
      std::ostream& os, const ConstraintModelManagerTpl<Scalar>& model);

 private:
  boost::shared_ptr<StateAbstract> state_;  //!< State description
  ConstraintModelContainer constraints_;    //!< Stack of constraint items
  VectorXs lb_;                             //!< Lower bound of the constraint
  VectorXs ub_;                             //!< Upper bound of the constraint
  std::size_t nu_;                          //!< Dimension of the control input
  std::size_t ng_;  //!< Number of the active inequality constraints
  std::size_t nh_;  //!< Number of the active equality constraints
  std::set<std::string> active_set_;  //!< Names of the active constraint items
  std::set<std::string>
      inactive_set_;  //!< Names of the inactive constraint items
};

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 ActionModel, class ActionData>
  void resize(ActionModel* const model, ActionData* const data) {
    const std::size_t ndx = model->get_state()->get_ndx();
    const std::size_t nu = model->get_nu();
    const std::size_t ng = model->get_ng();
    const std::size_t nh = model->get_nh();
    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_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2020-2024, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
10			#define CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
11
12			#include <map>
13			#include <set>
14			#include <string>
15			#include <utility>
16
17			#include "crocoddyl/core/constraint-base.hpp"
18			#include "crocoddyl/core/utils/exception.hpp"
19
20			namespace crocoddyl {
21
22			template <typename _Scalar>
23			struct ConstraintItemTpl {
24			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25
26			typedef _Scalar Scalar;
27			typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract;
28
29			ConstraintItemTpl() {}
30		2453	ConstraintItemTpl(const std::string& name,
31			boost::shared_ptr<ConstraintModelAbstract> constraint,
32			bool active = true)
33		2453	: name(name), constraint(constraint), active(active) {}
34
35			/**
36			* @brief Print information on the constraint item
37			*/
38			friend std::ostream& operator<<(std::ostream& os,
39			const ConstraintItemTpl<Scalar>& model) {
40			os << "{" << *model.constraint << "}";
41			return os;
42			}
43
44			std::string name;
45			boost::shared_ptr<ConstraintModelAbstract> constraint;
46			bool active;
47			};
48
49			/**
50			* @brief Manage the individual constraint models
51			*
52			* This class serves to manage a set of added constraint models. The constraint
53			* functions might active or inactive, with this approach we avoid dynamic
54			* allocation of memory. Each constraint model is added through `addConstraint`,
55			* where its status can be defined.
56			*
57			* The main computations are carring out in `calc` and `calcDiff` routines.
58			* `calc` computes the constraint residuals and `calcDiff` computes the
59			* Jacobians of the constraint functions. Concretely speaking, `calcDiff` builds
60			* a linear approximation of the total constraint function (both inequality and
61			* equality) with the form: \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$,
62			* \f$\mathbf{h_x}\in\mathbb{R}^{nh\times ndx}\f$
63			* \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$.
64			*
65			* \sa `StateAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
66			*/
67			template <typename _Scalar>
68			class ConstraintModelManagerTpl {
69			public:
70			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71
72			typedef _Scalar Scalar;
73			typedef MathBaseTpl<Scalar> MathBase;
74			typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
75			typedef StateAbstractTpl<Scalar> StateAbstract;
76			typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract;
77			typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
78			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
79			typedef ConstraintItemTpl<Scalar> ConstraintItem;
80			typedef typename MathBase::VectorXs VectorXs;
81			typedef typename MathBase::MatrixXs MatrixXs;
82
83			typedef std::map<std::string, boost::shared_ptr<ConstraintItem> >
84			ConstraintModelContainer;
85			typedef std::map<std::string, boost::shared_ptr<ConstraintDataAbstract> >
86			ConstraintDataContainer;
87
88			/**
89			* @brief Initialize the constraint-manager model
90			*
91			* @param[in] state State of the multibody system
92			* @param[in] nu Dimension of control vector
93			*/
94			ConstraintModelManagerTpl(boost::shared_ptr<StateAbstract> state,
95			const std::size_t nu);
96
97			/**
98			* @brief Initialize the constraint-manager model
99			*
100			* The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
101			*
102			* @param[in] state State of the multibody system
103			*/
104			explicit ConstraintModelManagerTpl(boost::shared_ptr<StateAbstract> state);
105			~ConstraintModelManagerTpl();
106
107			/**
108			* @brief Add a constraint item
109			*
110			* @param[in] name Constraint name
111			* @param[in] constraint Constraint model
112			* @param[in] weight Constraint weight
113			* @param[in] active True if the constraint is activated (default true)
114			*/
115			void addConstraint(const std::string& name,
116			boost::shared_ptr<ConstraintModelAbstract> constraint,
117			const bool active = true);
118
119			/**
120			* @brief Remove a constraint item
121			*
122			* @param[in] name Constraint name
123			*/
124			void removeConstraint(const std::string& name);
125
126			/**
127			* @brief Change the constraint status
128			*
129			* @param[in] name Constraint name
130			* @param[in] active Constraint status (true for active and false for
131			* inactive)
132			*/
133			void changeConstraintStatus(const std::string& name, bool active);
134
135			/**
136			* @brief Compute the total constraint value
137			*
138			* @param[in] data Constraint data
139			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
140			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
141			*/
142			void calc(const boost::shared_ptr<ConstraintDataManager>& data,
143			const Eigen::Ref<const VectorXs>& x,
144			const Eigen::Ref<const VectorXs>& u);
145
146			/**
147			* @brief Compute the total constraint value for nodes that depends only on
148			* the state
149			*
150			* It updates the constraint based on the state only. This function is
151			* commonly used in the terminal nodes of an optimal control problem.
152			*
153			* @param[in] data Constraint data
154			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
155			*/
156			void calc(const boost::shared_ptr<ConstraintDataManager>& data,
157			const Eigen::Ref<const VectorXs>& x);
158
159			/**
160			* @brief Compute the Jacobian of the total constraint
161			*
162			* @param[in] data Constraint data
163			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
164			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
165			*/
166			void calcDiff(const boost::shared_ptr<ConstraintDataManager>& data,
167			const Eigen::Ref<const VectorXs>& x,
168			const Eigen::Ref<const VectorXs>& u);
169
170			/**
171			* @brief Compute the Jacobian of the total constraint with respect to the
172			* state only
173			*
174			* It computes the Jacobian of the constraint function based on the state
175			* only. This function is commonly used in the terminal nodes of an optimal
176			* control problem.
177			*
178			* @param[in] data Constraint data
179			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
180			*/
181			void calcDiff(const boost::shared_ptr<ConstraintDataManager>& data,
182			const Eigen::Ref<const VectorXs>& x);
183
184			/**
185			* @brief Create the constraint data
186			*
187			* The default data contains objects to store the values of the constraint and
188			* their derivatives (i.e. Jacobians). However, it is possible to specialize
189			* this function is we need to create additional data, for instance, to avoid
190			* dynamic memory allocation.
191			*
192			* @param data Data collector
193			* @return the constraint data
194			*/
195			boost::shared_ptr<ConstraintDataManager> createData(
196			DataCollectorAbstract* const data);
197
198			/**
199			* @brief Return the state
200			*/
201			const boost::shared_ptr<StateAbstract>& get_state() const;
202
203			/**
204			* @brief Return the stack of constraint models
205			*/
206			const ConstraintModelContainer& get_constraints() const;
207
208			/**
209			* @brief Return the dimension of the control input
210			*/
211			std::size_t get_nu() const;
212
213			/**
214			* @brief Return the number of active inequality constraints
215			*/
216			std::size_t get_ng() const;
217
218			/**
219			* @brief Return the number of active equality constraints
220			*/
221			std::size_t get_nh() const;
222
223			/**
224			* @brief Return the names of the set of active constraints
225			*/
226			const std::set<std::string>& get_active_set() const;
227
228			/**
229			* @brief Return the names of the set of inactive constraints
230			*/
231			const std::set<std::string>& get_inactive_set() const;
232
233			/**
234			* @brief Return the state lower bound
235			*/
236			const VectorXs& get_lb() const;
237
238			/**
239			* @brief Return the state upper bound
240			*/
241			const VectorXs& get_ub() const;
242
243			/**
244			* @brief Return the status of a given constraint name
245			*
246			* @param[in] name Constraint name
247			*/
248			bool getConstraintStatus(const std::string& name) const;
249
250			/**
251			* @brief Print information on the stack of constraints
252			*/
253			template <class Scalar>
254			friend std::ostream& operator<<(
255			std::ostream& os, const ConstraintModelManagerTpl<Scalar>& model);
256
257			private:
258			boost::shared_ptr<StateAbstract> state_; //!< State description
259			ConstraintModelContainer constraints_; //!< Stack of constraint items
260			VectorXs lb_; //!< Lower bound of the constraint
261			VectorXs ub_; //!< Upper bound of the constraint
262			std::size_t nu_; //!< Dimension of the control input
263			std::size_t ng_; //!< Number of the active inequality constraints
264			std::size_t nh_; //!< Number of the active equality constraints
265			std::set<std::string> active_set_; //!< Names of the active constraint items
266			std::set<std::string>
267			inactive_set_; //!< Names of the inactive constraint items
268			};
269
270			template <typename _Scalar>
271			struct ConstraintDataManagerTpl {
272			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
273
274			typedef _Scalar Scalar;
275			typedef MathBaseTpl<Scalar> MathBase;
276			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
277			typedef ConstraintItemTpl<Scalar> ConstraintItem;
278			typedef typename MathBase::VectorXs VectorXs;
279			typedef typename MathBase::MatrixXs MatrixXs;
280
281			template <template <typename Scalar> class Model>
282		50024	ConstraintDataManagerTpl(Model<Scalar>* const model,
283			DataCollectorAbstract* const data)
284			: g_internal(model->get_ng()),
285		50024	Gx_internal(model->get_ng(), model->get_state()->get_ndx()),
286			Gu_internal(model->get_ng(), model->get_nu()),
287			h_internal(model->get_nh()),
288		50024	Hx_internal(model->get_nh(), model->get_state()->get_ndx()),
289			Hu_internal(model->get_nh(), model->get_nu()),
290			shared(data),
291			g(g_internal.data(), model->get_ng()),
292		50024	Gx(Gx_internal.data(), model->get_ng(), model->get_state()->get_ndx()),
293			Gu(Gu_internal.data(), model->get_ng(), model->get_nu()),
294			h(h_internal.data(), model->get_nh()),
295		50024	Hx(Hx_internal.data(), model->get_nh(), model->get_state()->get_ndx()),
296	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✗✓ ✗✓✗✓ ✗✗✓✓ ✗✓✗✓ ✓✗✓✗ ✗✓✗✓ ✓✗✓✓ ✗✗✓✗ ✓✓✗✓ ✓✗✓✓ ✗✓✓✗ ✓✗✓✗ ✓✗	150072	Hu(Hu_internal.data(), model->get_nh(), model->get_nu()) {
297	✓✗	50024	g.setZero();
298	✓✗	50024	Gx.setZero();
299	✓✗	50024	Gu.setZero();
300	✓✗	50024	h.setZero();
301	✓✗	50024	Hx.setZero();
302	✓✗	50024	Hu.setZero();
303		220648	for (typename ConstraintModelManagerTpl<
304		50024	Scalar>::ConstraintModelContainer::const_iterator it =
305		50024	model->get_constraints().begin();
306	✓✓	270672	it != model->get_constraints().end(); ++it) {
307		220648	const boost::shared_ptr<ConstraintItem>& item = it->second;
308	✓✗	220648	constraints.insert(
309	✓✗✓✗	220648	std::make_pair(item->name, item->constraint->createData(data)));
310			}
311		50024	}
312
313			template <class ActionData>
314		97929	void shareMemory(ActionData* const data) {
315			// Save memory by setting the internal variables with null dimension
316		97929	g_internal.resize(0);
317		97929	Gx_internal.resize(0, 0);
318		97929	Gu_internal.resize(0, 0);
319		97929	h_internal.resize(0);
320		97929	Hx_internal.resize(0, 0);
321		97929	Hu_internal.resize(0, 0);
322			// Share memory with the differential action data
323	✓✗	97929	new (&g) Eigen::Map<VectorXs>(data->g.data(), data->g.size());
324	✓✗	97929	new (&Gx)
325			Eigen::Map<MatrixXs>(data->Gx.data(), data->Gx.rows(), data->Gx.cols());
326	✓✗	97929	new (&Gu)
327			Eigen::Map<MatrixXs>(data->Gu.data(), data->Gu.rows(), data->Gu.cols());
328	✓✗	97929	new (&h) Eigen::Map<VectorXs>(data->h.data(), data->h.size());
329	✓✗	97929	new (&Hx)
330			Eigen::Map<MatrixXs>(data->Hx.data(), data->Hx.rows(), data->Hx.cols());
331	✓✗	97929	new (&Hu)
332			Eigen::Map<MatrixXs>(data->Hu.data(), data->Hu.rows(), data->Hu.cols());
333		97929	}
334
335			template <class ActionModel, class ActionData>
336		93771	void resize(ActionModel* const model, ActionData* const data) {
337		93771	const std::size_t ndx = model->get_state()->get_ndx();
338		93771	const std::size_t nu = model->get_nu();
339		93771	const std::size_t ng = model->get_ng();
340		93771	const std::size_t nh = model->get_nh();
341		93771	data->g.conservativeResize(ng);
342		93771	data->Gx.conservativeResize(ng, ndx);
343		93771	data->Gu.conservativeResize(ng, nu);
344		93771	data->h.conservativeResize(nh);
345		93771	data->Hx.conservativeResize(nh, ndx);
346		93771	data->Hu.conservativeResize(nh, nu);
347	✓✗	93771	new (&g) Eigen::Map<VectorXs>(data->g.data(), ng);
348	✓✗	93771	new (&Gx) Eigen::Map<MatrixXs>(data->Gx.data(), ng, ndx);
349	✓✗	93771	new (&Gu) Eigen::Map<MatrixXs>(data->Gu.data(), ng, nu);
350	✓✗	93771	new (&h) Eigen::Map<VectorXs>(data->h.data(), nh);
351	✓✗	93771	new (&Hx) Eigen::Map<MatrixXs>(data->Hx.data(), nh, ndx);
352	✓✗	93771	new (&Hu) Eigen::Map<MatrixXs>(data->Hu.data(), nh, nu);
353		93771	}
354
355			VectorXs get_g() const { return g; }
356			MatrixXs get_Gx() const { return Gx; }
357			MatrixXs get_Gu() const { return Gu; }
358			VectorXs get_h() const { return h; }
359			MatrixXs get_Hx() const { return Hx; }
360			MatrixXs get_Hu() const { return Hu; }
361
362			void set_g(const VectorXs& _g) {
363			if (g.size() != _g.size()) {
364			throw_pretty("Invalid argument: "
365			<< "g has wrong dimension (it should be " +
366			std::to_string(g.size()) + ")");
367			}
368			g = _g;
369			}
370			void set_Gx(const MatrixXs& _Gx) {
371			if (Gx.rows() != _Gx.rows() \|\| Gx.cols() != _Gx.cols()) {
372			throw_pretty("Invalid argument: "
373			<< "Gx has wrong dimension (it should be " +
374			std::to_string(Gx.rows()) + ", " +
375			std::to_string(Gx.cols()) + ")");
376			}
377			Gx = _Gx;
378			}
379			void set_Gu(const MatrixXs& _Gu) {
380			if (Gu.rows() != _Gu.rows() \|\| Gu.cols() != _Gu.cols()) {
381			throw_pretty("Invalid argument: "
382			<< "Gu has wrong dimension (it should be " +
383			std::to_string(Gu.rows()) + ", " +
384			std::to_string(Gu.cols()) + ")");
385			}
386			Gu = _Gu;
387			}
388			void set_h(const VectorXs& _h) {
389			if (h.size() != _h.size()) {
390			throw_pretty("Invalid argument: "
391			<< "h has wrong dimension (it should be " +
392			std::to_string(h.size()) + ")");
393			}
394			h = _h;
395			}
396			void set_Hx(const MatrixXs& _Hx) {
397			if (Hx.rows() != _Hx.rows() \|\| Hx.cols() != _Hx.cols()) {
398			throw_pretty("Invalid argument: "
399			<< "Hx has wrong dimension (it should be " +
400			std::to_string(Hx.rows()) + ", " +
401			std::to_string(Hx.cols()) + ")");
402			}
403			Hx = _Hx;
404			}
405			void set_Hu(const MatrixXs& _Hu) {
406			if (Hu.rows() != _Hu.rows() \|\| Hu.cols() != _Hu.cols()) {
407			throw_pretty("Invalid argument: "
408			<< "Hu has wrong dimension (it should be " +
409			std::to_string(Hu.rows()) + ", " +
410			std::to_string(Hu.cols()) + ")");
411			}
412			Hu = _Hu;
413			}
414
415			// Creates internal data in case we don't share it externally
416			VectorXs g_internal;
417			MatrixXs Gx_internal;
418			MatrixXs Gu_internal;
419			VectorXs h_internal;
420			MatrixXs Hx_internal;
421			MatrixXs Hu_internal;
422
423			typename ConstraintModelManagerTpl<Scalar>::ConstraintDataContainer
424			constraints;
425			DataCollectorAbstract* shared;
426			Eigen::Map<VectorXs> g;
427			Eigen::Map<MatrixXs> Gx;
428			Eigen::Map<MatrixXs> Gu;
429			Eigen::Map<VectorXs> h;
430			Eigen::Map<MatrixXs> Hx;
431			Eigen::Map<MatrixXs> Hu;
432			};
433
434			} // namespace crocoddyl
435
436			/* --- Details -------------------------------------------------------------- */
437			/* --- Details -------------------------------------------------------------- */
438			/* --- Details -------------------------------------------------------------- */
439			#include "crocoddyl/core/constraints/constraint-manager.hxx"
440
441			#endif // CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_