Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/costs/cost-sum.hpp	Lines:	40	65	61.5 %
Date:	2024-02-13 11:12:33	Branches:	44	228	19.3 %


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

#ifndef CROCODDYL_CORE_COSTS_COST_SUM_HPP_
#define CROCODDYL_CORE_COSTS_COST_SUM_HPP_

#include <map>
#include <set>
#include <string>
#include <utility>

#include "crocoddyl/core/cost-base.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename _Scalar>
struct CostItemTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef CostModelAbstractTpl<Scalar> CostModelAbstract;

  CostItemTpl() {}
  CostItemTpl(const std::string& name,
              boost::shared_ptr<CostModelAbstract> cost, const Scalar weight,
              const bool active = true)
      : name(name), cost(cost), weight(weight), active(active) {}

  /**
   * @brief Print information on the cost item
   */
  friend std::ostream& operator<<(std::ostream& os,
                                  const CostItemTpl<Scalar>& model) {
    os << "{w=" << model.weight << ", " << *model.cost << "}";
    return os;
  }

  std::string name;
  boost::shared_ptr<CostModelAbstract> cost;
  Scalar weight;
  bool active;
};

/**
 * @brief Summation of individual cost models
 *
 * This class serves to manage a set of added cost models. The cost functions
 * might active or inactive, with this approach we avoid dynamic allocation of
 * memory. Each cost model is added through `addCost`, where the weight and its
 * status can be defined.
 *
 * The main computations are carring out in `calc()` and `calcDiff()` routines.
 * `calc()` computes the costs (and its residuals) and `calcDiff()` computes the
 * derivatives of the cost functions (and its residuals). Concretely speaking,
 * `calcDiff()` builds a linear-quadratic approximation of the total cost
 * function with the form: \f$\mathbf{\ell_x}\in\mathbb{R}^{ndx}\f$,
 * \f$\mathbf{\ell_u}\in\mathbb{R}^{nu}\f$,
 * \f$\mathbf{\ell_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$,
 * \f$\mathbf{\ell_{xu}}\in\mathbb{R}^{ndx\times nu}\f$,
 * \f$\mathbf{\ell_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Jacobians and
 * Hessians, respectively.
 *
 * \sa `CostModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class CostModelSumTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef CostDataSumTpl<Scalar> CostDataSum;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef CostModelAbstractTpl<Scalar> CostModelAbstract;
  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef CostItemTpl<Scalar> CostItem;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  typedef std::map<std::string, boost::shared_ptr<CostItem> >
      CostModelContainer;
  typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> >
      CostDataContainer;

  /**
   * @brief Initialize the cost-sum model
   *
   * @param[in] state  State description
   * @param[in] nu     Dimension of control vector
   */
  CostModelSumTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu);

  /**
   * @brief Initialize the cost-sum model
   *
   * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state  State description
   */
  explicit CostModelSumTpl(boost::shared_ptr<StateAbstract> state);
  ~CostModelSumTpl();

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

  /**
   * @brief Remove a cost item
   *
   * @param[in] name    Cost name
   */
  void removeCost(const std::string& name);

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

  /**
   * @brief Compute the total cost value
   *
   * @param[in] data  Cost 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<CostDataSum>& data,
            const Eigen::Ref<const VectorXs>& x,
            const Eigen::Ref<const VectorXs>& u);

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

  /**
   * @brief Compute the Jacobian and Hessian of the total cost
   *
   * @param[in] data  Cost 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<CostDataSum>& data,
                const Eigen::Ref<const VectorXs>& x,
                const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief Compute the Jacobian and Hessian of the total cost for nodes that
   * depends on the state only
   *
   * It updates the Jacobian and Hessian of the total cost based on the state
   * only. This function is used in the terminal nodes of an optimal control
   * problem.
   *
   * @param[in] data  Cost 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<CostDataSum>& data,
                const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Create the cost data
   *
   * The default data contains objects to store the values of the cost, residual
   * vector and their derivatives (first and second order derivatives). 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 cost data
   */
  boost::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data);

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

  /**
   * @brief Return the stack of cost models
   */
  const CostModelContainer& get_costs() const;

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

  /**
   * @brief Return the dimension of the active residual vector
   */
  std::size_t get_nr() const;

  /**
   * @brief Return the dimension of the total residual vector
   */
  std::size_t get_nr_total() const;

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

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

  DEPRECATED(
      "get_active() is deprecated and will be replaced with get_active_set()",
      const std::vector<std::string>& get_active() {
        active_.clear();
        active_.reserve(active_set_.size());
        for (const auto& contact : active_set_) {
          active_.push_back(contact);
        }
        return active_;
      };)

  DEPRECATED(
      "get_inactive() is deprecated and will be replaced with "
      "get_inactive_set()",
      const std::vector<std::string>& get_inactive() {
        inactive_.clear();
        inactive_.reserve(inactive_set_.size());
        for (const auto& contact : inactive_set_) {
          inactive_.push_back(contact);
        }
        return inactive_;
      };)

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

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

 private:
  boost::shared_ptr<StateAbstract> state_;  //!< State description
  CostModelContainer costs_;                //!< Stack of cost items
  std::size_t nu_;                          //!< Dimension of the control input
  std::size_t nr_;        //!< Dimension of the active residual vector
  std::size_t nr_total_;  //!< Dimension of the total residual vector
  std::set<std::string> active_set_;  //!< Names of the active set of cost items
  std::set<std::string>
      inactive_set_;  //!< Names of the inactive set of cost items

  // Vector variants. These are to maintain the API compatibility for the
  // deprecated syntax. These will be removed in future versions along with
  // get_active() / get_inactive()
  std::vector<std::string> active_;
  std::vector<std::string> inactive_;
};

template <typename _Scalar>
struct CostDataSumTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef CostItemTpl<Scalar> CostItem;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
      : Lx_internal(model->get_state()->get_ndx()),
        Lu_internal(model->get_nu()),
        Lxx_internal(model->get_state()->get_ndx(),
                     model->get_state()->get_ndx()),
        Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
        Luu_internal(model->get_nu(), model->get_nu()),
        shared(data),
        cost(Scalar(0.)),
        Lx(Lx_internal.data(), model->get_state()->get_ndx()),
        Lu(Lu_internal.data(), model->get_nu()),
        Lxx(Lxx_internal.data(), model->get_state()->get_ndx(),
            model->get_state()->get_ndx()),
        Lxu(Lxu_internal.data(), model->get_state()->get_ndx(),
            model->get_nu()),
        Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
    for (typename CostModelSumTpl<Scalar>::CostModelContainer::const_iterator
             it = model->get_costs().begin();
         it != model->get_costs().end(); ++it) {
      const boost::shared_ptr<CostItem>& item = it->second;
      costs.insert(std::make_pair(item->name, item->cost->createData(data)));
    }
  }

  template <class ActionData>
  void shareMemory(ActionData* const data) {
    // Save memory by setting the internal variables with null dimension
    Lx_internal.resize(0);
    Lu_internal.resize(0);
    Lxx_internal.resize(0, 0);
    Lxu_internal.resize(0, 0);
    Luu_internal.resize(0, 0);
    // Share memory with the differential action data
    new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
    new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
    new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(),
                                    data->Lxx.cols());
    new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(),
                                    data->Lxu.cols());
    new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(),
                                    data->Luu.cols());
  }

  VectorXs get_Lx() const { return Lx; }
  VectorXs get_Lu() const { return Lu; }
  MatrixXs get_Lxx() const { return Lxx; }
  MatrixXs get_Lxu() const { return Lxu; }
  MatrixXs get_Luu() const { return Luu; }

  void set_Lx(const VectorXs& _Lx) {
    if (Lx.size() != _Lx.size()) {
      throw_pretty("Invalid argument: "
                   << "Lx has wrong dimension (it should be " +
                          std::to_string(Lx.size()) + ")");
    }
    Lx = _Lx;
  }
  void set_Lu(const VectorXs& _Lu) {
    if (Lu.size() != _Lu.size()) {
      throw_pretty("Invalid argument: "
                   << "Lu has wrong dimension (it should be " +
                          std::to_string(Lu.size()) + ")");
    }
    Lu = _Lu;
  }
  void set_Lxx(const MatrixXs& _Lxx) {
    if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
      throw_pretty("Invalid argument: "
                   << "Lxx has wrong dimension (it should be " +
                          std::to_string(Lxx.rows()) + ", " +
                          std::to_string(Lxx.cols()) + ")");
    }
    Lxx = _Lxx;
  }
  void set_Lxu(const MatrixXs& _Lxu) {
    if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
      throw_pretty("Invalid argument: "
                   << "Lxu has wrong dimension (it should be " +
                          std::to_string(Lxu.rows()) + ", " +
                          std::to_string(Lxu.cols()) + ")");
    }
    Lxu = _Lxu;
  }
  void set_Luu(const MatrixXs& _Luu) {
    if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
      throw_pretty("Invalid argument: "
                   << "Luu has wrong dimension (it should be " +
                          std::to_string(Luu.rows()) + ", " +
                          std::to_string(Luu.cols()) + ")");
    }
    Luu = _Luu;
  }

  // Creates internal data in case we don't share it externally
  VectorXs Lx_internal;
  VectorXs Lu_internal;
  MatrixXs Lxx_internal;
  MatrixXs Lxu_internal;
  MatrixXs Luu_internal;

  typename CostModelSumTpl<Scalar>::CostDataContainer costs;
  DataCollectorAbstract* shared;
  Scalar cost;
  Eigen::Map<VectorXs> Lx;
  Eigen::Map<VectorXs> Lu;
  Eigen::Map<MatrixXs> Lxx;
  Eigen::Map<MatrixXs> Lxu;
  Eigen::Map<MatrixXs> Luu;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/costs/cost-sum.hxx"

#endif  // CROCODDYL_CORE_COSTS_COST_SUM_HPP_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_CORE_COSTS_COST_SUM_HPP_
11			#define CROCODDYL_CORE_COSTS_COST_SUM_HPP_
12
13			#include <map>
14			#include <set>
15			#include <string>
16			#include <utility>
17
18			#include "crocoddyl/core/cost-base.hpp"
19			#include "crocoddyl/core/fwd.hpp"
20			#include "crocoddyl/core/utils/exception.hpp"
21
22			namespace crocoddyl {
23
24			template <typename _Scalar>
25			struct CostItemTpl {
26			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27
28			typedef _Scalar Scalar;
29			typedef CostModelAbstractTpl<Scalar> CostModelAbstract;
30
31			CostItemTpl() {}
32		6097	CostItemTpl(const std::string& name,
33			boost::shared_ptr<CostModelAbstract> cost, const Scalar weight,
34			const bool active = true)
35		6097	: name(name), cost(cost), weight(weight), active(active) {}
36
37			/**
38			* @brief Print information on the cost item
39			*/
40			friend std::ostream& operator<<(std::ostream& os,
41			const CostItemTpl<Scalar>& model) {
42			os << "{w=" << model.weight << ", " << *model.cost << "}";
43			return os;
44			}
45
46			std::string name;
47			boost::shared_ptr<CostModelAbstract> cost;
48			Scalar weight;
49			bool active;
50			};
51
52			/**
53			* @brief Summation of individual cost models
54			*
55			* This class serves to manage a set of added cost models. The cost functions
56			* might active or inactive, with this approach we avoid dynamic allocation of
57			* memory. Each cost model is added through `addCost`, where the weight and its
58			* status can be defined.
59			*
60			* The main computations are carring out in `calc()` and `calcDiff()` routines.
61			* `calc()` computes the costs (and its residuals) and `calcDiff()` computes the
62			* derivatives of the cost functions (and its residuals). Concretely speaking,
63			* `calcDiff()` builds a linear-quadratic approximation of the total cost
64			* function with the form: \f$\mathbf{\ell_x}\in\mathbb{R}^{ndx}\f$,
65			* \f$\mathbf{\ell_u}\in\mathbb{R}^{nu}\f$,
66			* \f$\mathbf{\ell_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$,
67			* \f$\mathbf{\ell_{xu}}\in\mathbb{R}^{ndx\times nu}\f$,
68			* \f$\mathbf{\ell_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Jacobians and
69			* Hessians, respectively.
70			*
71			* \sa `CostModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
72			*/
73			template <typename _Scalar>
74			class CostModelSumTpl {
75			public:
76			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77
78			typedef _Scalar Scalar;
79			typedef MathBaseTpl<Scalar> MathBase;
80			typedef CostDataSumTpl<Scalar> CostDataSum;
81			typedef StateAbstractTpl<Scalar> StateAbstract;
82			typedef CostModelAbstractTpl<Scalar> CostModelAbstract;
83			typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
84			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
85			typedef CostItemTpl<Scalar> CostItem;
86			typedef typename MathBase::VectorXs VectorXs;
87			typedef typename MathBase::MatrixXs MatrixXs;
88
89			typedef std::map<std::string, boost::shared_ptr<CostItem> >
90			CostModelContainer;
91			typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> >
92			CostDataContainer;
93
94			/**
95			* @brief Initialize the cost-sum model
96			*
97			* @param[in] state State description
98			* @param[in] nu Dimension of control vector
99			*/
100			CostModelSumTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu);
101
102			/**
103			* @brief Initialize the cost-sum model
104			*
105			* The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
106			*
107			* @param[in] state State description
108			*/
109			explicit CostModelSumTpl(boost::shared_ptr<StateAbstract> state);
110			~CostModelSumTpl();
111
112			/**
113			* @brief Add a cost item
114			*
115			* @param[in] name Cost name
116			* @param[in] cost Cost model
117			* @param[in] weight Cost weight
118			* @param[in] active True if the cost is activated (default true)
119			*/
120			void addCost(const std::string& name,
121			boost::shared_ptr<CostModelAbstract> cost, const Scalar weight,
122			const bool active = true);
123
124			/**
125			* @brief Remove a cost item
126			*
127			* @param[in] name Cost name
128			*/
129			void removeCost(const std::string& name);
130
131			/**
132			* @brief Change the cost status
133			*
134			* @param[in] name Cost name
135			* @param[in] active Cost status (true for active and false for inactive)
136			*/
137			void changeCostStatus(const std::string& name, const bool active);
138
139			/**
140			* @brief Compute the total cost value
141			*
142			* @param[in] data Cost data
143			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
144			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
145			*/
146			void calc(const boost::shared_ptr<CostDataSum>& data,
147			const Eigen::Ref<const VectorXs>& x,
148			const Eigen::Ref<const VectorXs>& u);
149
150			/**
151			* @brief Compute the total cost value for nodes that depends only on the
152			* state
153			*
154			* It updates the total cost based on the state only. This function is used in
155			* the terminal nodes of an optimal control problem.
156			*
157			* @param[in] data Cost data
158			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
159			*/
160			void calc(const boost::shared_ptr<CostDataSum>& data,
161			const Eigen::Ref<const VectorXs>& x);
162
163			/**
164			* @brief Compute the Jacobian and Hessian of the total cost
165			*
166			* @param[in] data Cost data
167			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
168			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
169			*/
170			void calcDiff(const boost::shared_ptr<CostDataSum>& data,
171			const Eigen::Ref<const VectorXs>& x,
172			const Eigen::Ref<const VectorXs>& u);
173
174			/**
175			* @brief Compute the Jacobian and Hessian of the total cost for nodes that
176			* depends on the state only
177			*
178			* It updates the Jacobian and Hessian of the total cost based on the state
179			* only. This function is used in the terminal nodes of an optimal control
180			* problem.
181			*
182			* @param[in] data Cost data
183			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
184			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
185			*/
186			void calcDiff(const boost::shared_ptr<CostDataSum>& data,
187			const Eigen::Ref<const VectorXs>& x);
188
189			/**
190			* @brief Create the cost data
191			*
192			* The default data contains objects to store the values of the cost, residual
193			* vector and their derivatives (first and second order derivatives). However,
194			* it is possible to specialize this function is we need to create additional
195			* data, for instance, to avoid dynamic memory allocation.
196			*
197			* @param data Data collector
198			* @return the cost data
199			*/
200			boost::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data);
201
202			/**
203			* @brief Return the state
204			*/
205			const boost::shared_ptr<StateAbstract>& get_state() const;
206
207			/**
208			* @brief Return the stack of cost models
209			*/
210			const CostModelContainer& get_costs() const;
211
212			/**
213			* @brief Return the dimension of the control input
214			*/
215			std::size_t get_nu() const;
216
217			/**
218			* @brief Return the dimension of the active residual vector
219			*/
220			std::size_t get_nr() const;
221
222			/**
223			* @brief Return the dimension of the total residual vector
224			*/
225			std::size_t get_nr_total() const;
226
227			/**
228			* @brief Return the names of the set of active costs
229			*/
230			const std::set<std::string>& get_active_set() const;
231
232			/**
233			* @brief Return the names of the set of inactive costs
234			*/
235			const std::set<std::string>& get_inactive_set() const;
236
237			DEPRECATED(
238			"get_active() is deprecated and will be replaced with get_active_set()",
239			const std::vector<std::string>& get_active() {
240			active_.clear();
241			active_.reserve(active_set_.size());
242			for (const auto& contact : active_set_) {
243			active_.push_back(contact);
244			}
245			return active_;
246			};)
247
248			DEPRECATED(
249			"get_inactive() is deprecated and will be replaced with "
250			"get_inactive_set()",
251			const std::vector<std::string>& get_inactive() {
252			inactive_.clear();
253			inactive_.reserve(inactive_set_.size());
254			for (const auto& contact : inactive_set_) {
255			inactive_.push_back(contact);
256			}
257			return inactive_;
258			};)
259
260			/**
261			* @brief Return the status of a given cost name
262			*
263			* @param[in] name Cost name
264			*/
265			bool getCostStatus(const std::string& name) const;
266
267			/**
268			* @brief Print information on the stack of costs
269			*/
270			template <class Scalar>
271			friend std::ostream& operator<<(std::ostream& os,
272			const CostModelSumTpl<Scalar>& model);
273
274			private:
275			boost::shared_ptr<StateAbstract> state_; //!< State description
276			CostModelContainer costs_; //!< Stack of cost items
277			std::size_t nu_; //!< Dimension of the control input
278			std::size_t nr_; //!< Dimension of the active residual vector
279			std::size_t nr_total_; //!< Dimension of the total residual vector
280			std::set<std::string> active_set_; //!< Names of the active set of cost items
281			std::set<std::string>
282			inactive_set_; //!< Names of the inactive set of cost items
283
284			// Vector variants. These are to maintain the API compatibility for the
285			// deprecated syntax. These will be removed in future versions along with
286			// get_active() / get_inactive()
287			std::vector<std::string> active_;
288			std::vector<std::string> inactive_;
289			};
290
291			template <typename _Scalar>
292			struct CostDataSumTpl {
293			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
294
295			typedef _Scalar Scalar;
296			typedef MathBaseTpl<Scalar> MathBase;
297			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
298			typedef CostItemTpl<Scalar> CostItem;
299			typedef typename MathBase::VectorXs VectorXs;
300			typedef typename MathBase::MatrixXs MatrixXs;
301
302			template <template <typename Scalar> class Model>
303		95984	CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
304		95984	: Lx_internal(model->get_state()->get_ndx()),
305			Lu_internal(model->get_nu()),
306		95984	Lxx_internal(model->get_state()->get_ndx(),
307		95984	model->get_state()->get_ndx()),
308		95984	Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
309			Luu_internal(model->get_nu(), model->get_nu()),
310			shared(data),
311			cost(Scalar(0.)),
312		95984	Lx(Lx_internal.data(), model->get_state()->get_ndx()),
313			Lu(Lu_internal.data(), model->get_nu()),
314		95984	Lxx(Lxx_internal.data(), model->get_state()->get_ndx(),
315		95984	model->get_state()->get_ndx()),
316		95984	Lxu(Lxu_internal.data(), model->get_state()->get_ndx(),
317			model->get_nu()),
318	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✓ ✗✓✗✓ ✗✓✓✗ ✓✗✓✗ ✓✗✗✓ ✗✓✗✓ ✗✓✓✗ ✓✗✗✗ ✓✓✗✓ ✓✗✓✗ ✓✗✓✗	287952	Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
319	✓✗	95984	Lx.setZero();
320	✓✗	95984	Lu.setZero();
321	✓✗	95984	Lxx.setZero();
322	✓✗	95984	Lxu.setZero();
323	✓✗	95984	Luu.setZero();
324		434985	for (typename CostModelSumTpl<Scalar>::CostModelContainer::const_iterator
325		95984	it = model->get_costs().begin();
326	✓✓	530969	it != model->get_costs().end(); ++it) {
327		434985	const boost::shared_ptr<CostItem>& item = it->second;
328	✓✗✓✗ ✓✗	434985	costs.insert(std::make_pair(item->name, item->cost->createData(data)));
329			}
330		95984	}
331
332			template <class ActionData>
333		146959	void shareMemory(ActionData* const data) {
334			// Save memory by setting the internal variables with null dimension
335		146959	Lx_internal.resize(0);
336		146959	Lu_internal.resize(0);
337		146959	Lxx_internal.resize(0, 0);
338		146959	Lxu_internal.resize(0, 0);
339		146959	Luu_internal.resize(0, 0);
340			// Share memory with the differential action data
341	✓✗	146959	new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
342	✓✗	146959	new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
343	✓✗	146959	new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(),
344			data->Lxx.cols());
345	✓✗	146959	new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(),
346			data->Lxu.cols());
347	✓✗	146959	new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(),
348			data->Luu.cols());
349		146959	}
350
351		7	VectorXs get_Lx() const { return Lx; }
352		7	VectorXs get_Lu() const { return Lu; }
353		7	MatrixXs get_Lxx() const { return Lxx; }
354		7	MatrixXs get_Lxu() const { return Lxu; }
355		7	MatrixXs get_Luu() const { return Luu; }
356
357			void set_Lx(const VectorXs& _Lx) {
358			if (Lx.size() != _Lx.size()) {
359			throw_pretty("Invalid argument: "
360			<< "Lx has wrong dimension (it should be " +
361			std::to_string(Lx.size()) + ")");
362			}
363			Lx = _Lx;
364			}
365			void set_Lu(const VectorXs& _Lu) {
366			if (Lu.size() != _Lu.size()) {
367			throw_pretty("Invalid argument: "
368			<< "Lu has wrong dimension (it should be " +
369			std::to_string(Lu.size()) + ")");
370			}
371			Lu = _Lu;
372			}
373			void set_Lxx(const MatrixXs& _Lxx) {
374			if (Lxx.rows() != _Lxx.rows() \|\| Lxx.cols() != _Lxx.cols()) {
375			throw_pretty("Invalid argument: "
376			<< "Lxx has wrong dimension (it should be " +
377			std::to_string(Lxx.rows()) + ", " +
378			std::to_string(Lxx.cols()) + ")");
379			}
380			Lxx = _Lxx;
381			}
382			void set_Lxu(const MatrixXs& _Lxu) {
383			if (Lxu.rows() != _Lxu.rows() \|\| Lxu.cols() != _Lxu.cols()) {
384			throw_pretty("Invalid argument: "
385			<< "Lxu has wrong dimension (it should be " +
386			std::to_string(Lxu.rows()) + ", " +
387			std::to_string(Lxu.cols()) + ")");
388			}
389			Lxu = _Lxu;
390			}
391			void set_Luu(const MatrixXs& _Luu) {
392			if (Luu.rows() != _Luu.rows() \|\| Luu.cols() != _Luu.cols()) {
393			throw_pretty("Invalid argument: "
394			<< "Luu has wrong dimension (it should be " +
395			std::to_string(Luu.rows()) + ", " +
396			std::to_string(Luu.cols()) + ")");
397			}
398			Luu = _Luu;
399			}
400
401			// Creates internal data in case we don't share it externally
402			VectorXs Lx_internal;
403			VectorXs Lu_internal;
404			MatrixXs Lxx_internal;
405			MatrixXs Lxu_internal;
406			MatrixXs Luu_internal;
407
408			typename CostModelSumTpl<Scalar>::CostDataContainer costs;
409			DataCollectorAbstract* shared;
410			Scalar cost;
411			Eigen::Map<VectorXs> Lx;
412			Eigen::Map<VectorXs> Lu;
413			Eigen::Map<MatrixXs> Lxx;
414			Eigen::Map<MatrixXs> Lxu;
415			Eigen::Map<MatrixXs> Luu;
416			};
417
418			} // namespace crocoddyl
419
420			/* --- Details -------------------------------------------------------------- */
421			/* --- Details -------------------------------------------------------------- */
422			/* --- Details -------------------------------------------------------------- */
423			#include "crocoddyl/core/costs/cost-sum.hxx"
424
425			#endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_