crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_COST_BASE_HPP_
10 #define CROCODDYL_CORE_COST_BASE_HPP_
11 
12 #include <boost/shared_ptr.hpp>
13 #include <boost/make_shared.hpp>
14 
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/state-base.hpp"
17 #include "crocoddyl/core/data-collector-base.hpp"
18 #include "crocoddyl/core/activation-base.hpp"
19 #include "crocoddyl/core/residual-base.hpp"
20 #include "crocoddyl/core/activations/quadratic.hpp"
21 #include "crocoddyl/core/utils/deprecate.hpp"
22 
23 namespace crocoddyl {
24 
48 template <typename _Scalar>
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  typedef _Scalar Scalar;
61  typedef typename MathBase::VectorXs VectorXs;
62  typedef typename MathBase::MatrixXs MatrixXs;
63 
71  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
72  boost::shared_ptr<ResidualModelAbstract> residual);
73 
81  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
82  const std::size_t nu);
83 
92  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation);
93 
102  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ResidualModelAbstract> residual);
103 
113  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu);
114 
125  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr);
126  virtual ~CostModelAbstractTpl();
127 
135  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
136  const Eigen::Ref<const VectorXs>& u) = 0;
137 
147  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
148  const Eigen::Ref<const VectorXs>& u) = 0;
149 
160  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
161 
168  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
169 
176  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
177 
181  const boost::shared_ptr<StateAbstract>& get_state() const;
182 
186  const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;
187 
191  const boost::shared_ptr<ResidualModelAbstract>& get_residual() const;
192 
196  std::size_t get_nu() const;
197 
201  template <class Scalar>
202  friend std::ostream& operator<<(std::ostream& os, const CostModelAbstractTpl<Scalar>& model);
203 
207  template <class ReferenceType>
208  void set_reference(ReferenceType ref);
209 
213  template <class ReferenceType>
214  ReferenceType get_reference();
215 
221  virtual void print(std::ostream& os) const;
222 
223  protected:
227  virtual void set_referenceImpl(const std::type_info&, const void*);
228 
232  virtual void get_referenceImpl(const std::type_info&, void*);
233 
234  boost::shared_ptr<StateAbstract> state_;
235  boost::shared_ptr<ActivationModelAbstract> activation_;
236  boost::shared_ptr<ResidualModelAbstract> residual_;
237  std::size_t nu_;
238  VectorXs unone_;
239 };
240 
241 template <typename _Scalar>
243  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
244 
245  typedef _Scalar Scalar;
250  typedef typename MathBase::VectorXs VectorXs;
251  typedef typename MathBase::MatrixXs MatrixXs;
252 
253  template <template <typename Scalar> class Model>
254  CostDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
255  : shared(data),
256  activation(model->get_activation()->createData()),
257  residual(model->get_residual()->createData(data)),
258  cost(Scalar(0.)),
259  Lx(model->get_state()->get_ndx()),
260  Lu(model->get_nu()),
261  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
262  Lxu(model->get_state()->get_ndx(), model->get_nu()),
263  Luu(model->get_nu(), model->get_nu()) {
264  Lx.setZero();
265  Lu.setZero();
266  Lxx.setZero();
267  Lxu.setZero();
268  Luu.setZero();
269  }
270  virtual ~CostDataAbstractTpl() {}
271 
272  DEPRECATED("Use residual.r", const VectorXs& get_r() const { return residual->r; };)
273  DEPRECATED("Use residual.Rx", const MatrixXs& get_Rx() const { return residual->Rx; };)
274  DEPRECATED("Use residual.Ru", const MatrixXs& get_Ru() const { return residual->Ru; };)
275  DEPRECATED("Use residual.r", void set_r(const VectorXs& r) { residual->r = r; };)
276  DEPRECATED("Use residual.Rx", void set_Rx(const MatrixXs& Rx) { residual->Rx = Rx; };)
277  DEPRECATED("Use residual.Ru", void set_Ru(const MatrixXs& Ru) { residual->Ru = Ru; };)
278 
279  DataCollectorAbstract* shared;
280  boost::shared_ptr<ActivationDataAbstract> activation;
281  boost::shared_ptr<ResidualDataAbstract> residual;
282  Scalar cost;
283  VectorXs Lx;
284  VectorXs Lu;
285  MatrixXs Lxx;
286  MatrixXs Lxu;
287  MatrixXs Luu;
288 };
289 
290 } // namespace crocoddyl
291 
292 /* --- Details -------------------------------------------------------------- */
293 /* --- Details -------------------------------------------------------------- */
294 /* --- Details -------------------------------------------------------------- */
295 #include "crocoddyl/core/cost-base.hxx"
296 
297 #endif // CROCODDYL_CORE_COST_BASE_HPP_
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the Jacobian and Hessian of cost and its residual vector.
void set_reference(ReferenceType ref)
Modify the cost reference.
boost::shared_ptr< ResidualModelAbstract > residual_
Residual model.
Definition: cost-base.hpp:236
Abstract class for cost models.
Definition: cost-base.hpp:49
const boost::shared_ptr< ActivationModelAbstract > & get_activation() const
Return the activation model.
virtual void get_referenceImpl(const std::type_info &, void *)
Abstract class for the state representation.
Definition: fwd.hpp:112
const boost::shared_ptr< ResidualModelAbstract > & get_residual() const
Return the residual model.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:235
virtual void set_referenceImpl(const std::type_info &, const void *)
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
Abstract class for residual models.
Definition: fwd.hpp:57
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, boost::shared_ptr< ResidualModelAbstract > residual)
Initialize the cost model.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the cost value and its residual vector.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:238
virtual void print(std::ostream &os) const
Print relevant information of the cost model.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:237
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:234
ReferenceType get_reference()
Return the cost reference.
std::size_t get_nu() const
Return the dimension of the control input.