crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh, New York University,
5 // Max Planck Gesellschaft
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_NUMDIFF_COST_HPP_
11 #define CROCODDYL_MULTIBODY_NUMDIFF_COST_HPP_
12 
13 #include <boost/function.hpp>
14 #include "crocoddyl/multibody/fwd.hpp"
15 #include "crocoddyl/multibody/cost-base.hpp"
16 
17 namespace crocoddyl {
18 
19 template <typename _Scalar>
20 class CostModelNumDiffTpl : public CostModelAbstractTpl<_Scalar> {
21  public:
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
25  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
26  typedef CostModelAbstractTpl<Scalar> Base;
27  typedef CostDataNumDiffTpl<Scalar> Data;
28  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
29  typedef MathBaseTpl<Scalar> MathBase;
30  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
31  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
32  typedef boost::function<void(const typename MathBaseTpl<Scalar>::VectorXs&)> ReevaluationFunction;
33 
39  explicit CostModelNumDiffTpl(const boost::shared_ptr<Base>& model);
40 
44  virtual ~CostModelNumDiffTpl();
45 
49  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
50  const Eigen::Ref<const VectorXs>& u);
51 
55  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
56  const Eigen::Ref<const VectorXs>& u);
57 
64  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
65 
71  const boost::shared_ptr<Base>& get_model() const;
72 
78  const Scalar& get_disturbance() const;
79 
85  void set_disturbance(const Scalar& disturbance);
86 
93  bool get_with_gauss_approx();
94 
101  void set_reevals(const std::vector<ReevaluationFunction>& reevals);
102 
103  protected:
104  using Base::activation_;
105  using Base::nu_;
106  using Base::state_;
107  using Base::unone_;
108 
110  boost::shared_ptr<Base> model_;
111 
113  Scalar disturbance_;
114 
116  std::vector<ReevaluationFunction> reevals_;
117 
118  private:
130  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
131 };
132 
133 template <typename _Scalar>
134 struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> {
135  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
136 
137  typedef _Scalar Scalar;
142  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
143 
144  template <template <typename Scalar> class Model>
145  explicit CostDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
146  : Base(model, shared_data),
147  dx(model->get_state()->get_ndx()),
148  xp(model->get_state()->get_nx()),
149  du(model->get_nu()),
150  up(model->get_nu()) {
151  dx.setZero();
152  xp.setZero();
153  du.setZero();
154  up.setZero();
155 
156  const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
157  const std::size_t& nu = model->get_model()->get_nu();
158  data_0 = model->get_model()->createData(shared_data);
159  for (std::size_t i = 0; i < ndx; ++i) {
160  data_x.push_back(model->get_model()->createData(shared_data));
161  }
162  for (std::size_t i = 0; i < nu; ++i) {
163  data_u.push_back(model->get_model()->createData(shared_data));
164  }
165  }
166 
167  virtual ~CostDataNumDiffTpl() {}
168 
169  using Base::activation;
170  using Base::cost;
171  using Base::Lu;
172  using Base::Luu;
173  using Base::Lx;
174  using Base::Lxu;
175  using Base::Lxx;
176  using Base::r;
177  using Base::Ru;
178  using Base::Rx;
179  using Base::shared;
180 
181  VectorXs dx;
182  VectorXs xp;
183  VectorXs du;
184  VectorXs up;
185  boost::shared_ptr<Base> data_0;
186  std::vector<boost::shared_ptr<Base> > data_x;
187  std::vector<boost::shared_ptr<Base> > data_u;
188 };
189 
190 } // namespace crocoddyl
191 
192 /* --- Details -------------------------------------------------------------- */
193 /* --- Details -------------------------------------------------------------- */
194 /* --- Details -------------------------------------------------------------- */
195 #include "crocoddyl/multibody/numdiff/cost.hxx"
196 
197 #endif // CROCODDYL_MULTIBODY_NUMDIFF_COST_HPP_
virtual ~CostModelNumDiffTpl()
Default destructor of the CostModelNumDiff object.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create a Data object.
CostModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Construct a new CostModelNumDiff object from a CostModelAbstract.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: cost.hpp:186
const Scalar & get_disturbance() const
Get the disturbance_ object.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: cost.hpp:187
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that updates the shared data computed for a system rollout The updated data is use...
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
Definition: cost.hpp:184
VectorXs du
Control disturbance.
Definition: cost.hpp:183
void set_disturbance(const Scalar &disturbance)
Set the disturbance_ object.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: cost.hpp:182
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: cost.hpp:113
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: cost.hpp:116
VectorXs dx
State disturbance.
Definition: cost.hpp:181
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: cost.hpp:185
boost::shared_ptr< Base > model_
Model of the cost.
Definition: cost.hpp:110