crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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_CORE_NUMDIFF_COST_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_COST_HPP_
12 
13 #include <boost/function.hpp>
14 #include "crocoddyl/multibody/fwd.hpp"
15 #include "crocoddyl/core/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;
30  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
31  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
32  typedef boost::function<void(const VectorXs&, const 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 
69  const boost::shared_ptr<Base>& get_model() const;
70 
74  const Scalar get_disturbance() const;
75 
79  void set_disturbance(const Scalar disturbance);
80 
87  bool get_with_gauss_approx();
88 
95  void set_reevals(const std::vector<ReevaluationFunction>& reevals);
96 
97  protected:
98  using Base::activation_;
99  using Base::nu_;
100  using Base::state_;
101  using Base::unone_;
102 
104  boost::shared_ptr<Base> model_;
105 
107  Scalar disturbance_;
108 
110  std::vector<ReevaluationFunction> reevals_;
111 
112  private:
124  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
125 };
126 
127 template <typename _Scalar>
128 struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> {
129  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
130 
131  typedef _Scalar Scalar;
136  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
137 
138  template <template <typename Scalar> class Model>
139  explicit CostDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
140  : Base(model, shared_data),
141  dx(model->get_state()->get_ndx()),
142  xp(model->get_state()->get_nx()),
143  du(model->get_nu()),
144  up(model->get_nu()) {
145  dx.setZero();
146  xp.setZero();
147  du.setZero();
148  up.setZero();
149 
150  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
151  const std::size_t nu = model->get_model()->get_nu();
152  data_0 = model->get_model()->createData(shared_data);
153  for (std::size_t i = 0; i < ndx; ++i) {
154  data_x.push_back(model->get_model()->createData(shared_data));
155  }
156  for (std::size_t i = 0; i < nu; ++i) {
157  data_u.push_back(model->get_model()->createData(shared_data));
158  }
159  }
160 
161  virtual ~CostDataNumDiffTpl() {}
162 
163  using Base::activation;
164  using Base::cost;
165  using Base::Lu;
166  using Base::Luu;
167  using Base::Lx;
168  using Base::Lxu;
169  using Base::Lxx;
170  using Base::residual;
171  using Base::shared;
172 
173  VectorXs dx;
174  VectorXs xp;
175  VectorXs du;
176  VectorXs up;
177  boost::shared_ptr<Base> data_0;
178  std::vector<boost::shared_ptr<Base> > data_x;
179  std::vector<boost::shared_ptr<Base> > data_u;
180 };
181 
182 } // namespace crocoddyl
183 
184 /* --- Details -------------------------------------------------------------- */
185 /* --- Details -------------------------------------------------------------- */
186 /* --- Details -------------------------------------------------------------- */
187 #include "crocoddyl/core/numdiff/cost.hxx"
188 
189 #endif // CROCODDYL_CORE_NUMDIFF_COST_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:49
virtual ~CostModelNumDiffTpl()
Initialize the numdiff cost model.
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)
Initialize the numdiff cost model.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: cost.hpp:178
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:235
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: cost.hpp:179
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:176
VectorXs du
Control disturbance.
Definition: cost.hpp:175
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: cost.hpp:174
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the cost value and its residual vector.
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian and Hessian of cost and its residual vector.
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: cost.hpp:107
const boost::shared_ptr< Base > & get_model() const
Return the original cost model.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:238
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: cost.hpp:110
VectorXs dx
State disturbance.
Definition: cost.hpp:173
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: cost.hpp:177
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:237
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:234
boost::shared_ptr< Base > model_
Model of the cost.
Definition: cost.hpp:104