crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, 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 
28 template <typename _Scalar>
29 class CostModelNumDiffTpl : public CostModelAbstractTpl<_Scalar> {
30  public:
31  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 
33  typedef _Scalar Scalar;
39  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
40  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
41  typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
42 
48  explicit CostModelNumDiffTpl(const boost::shared_ptr<Base>& model);
49 
53  virtual ~CostModelNumDiffTpl();
54 
58  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
59  const Eigen::Ref<const VectorXs>& u);
60 
64  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
65 
69  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
70  const Eigen::Ref<const VectorXs>& u);
71 
76  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
77 
84  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
85 
89  const boost::shared_ptr<Base>& get_model() const;
90 
94  const Scalar get_disturbance() const;
95 
99  void set_disturbance(const Scalar disturbance);
100 
107  bool get_with_gauss_approx();
108 
115  void set_reevals(const std::vector<ReevaluationFunction>& reevals);
116 
117  protected:
118  using Base::activation_;
119  using Base::nu_;
120  using Base::state_;
121  using Base::unone_;
122 
123  private:
135  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
136 
137  boost::shared_ptr<Base> model_;
138  Scalar disturbance_;
139  std::vector<ReevaluationFunction> reevals_;
140 };
141 
142 template <typename _Scalar>
143 struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> {
144  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 
146  typedef _Scalar Scalar;
151  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
152 
159  template <template <typename Scalar> class Model>
160  explicit CostDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
161  : Base(model, shared_data),
162  dx(model->get_state()->get_ndx()),
163  xp(model->get_state()->get_nx()),
164  du(model->get_nu()),
165  up(model->get_nu()) {
166  dx.setZero();
167  xp.setZero();
168  du.setZero();
169  up.setZero();
170 
171  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
172  const std::size_t nu = model->get_model()->get_nu();
173  data_0 = model->get_model()->createData(shared_data);
174  for (std::size_t i = 0; i < ndx; ++i) {
175  data_x.push_back(model->get_model()->createData(shared_data));
176  }
177  for (std::size_t i = 0; i < nu; ++i) {
178  data_u.push_back(model->get_model()->createData(shared_data));
179  }
180  }
181 
182  virtual ~CostDataNumDiffTpl() {}
183 
184  using Base::activation;
185  using Base::cost;
186  using Base::Lu;
187  using Base::Luu;
188  using Base::Lx;
189  using Base::Lxu;
190  using Base::Lxx;
191  using Base::residual;
192  using Base::shared;
193 
194  VectorXs dx;
195  VectorXs xp;
196  VectorXs du;
197  VectorXs up;
198  boost::shared_ptr<Base> data_0;
199  std::vector<boost::shared_ptr<Base> > data_x;
200  std::vector<boost::shared_ptr<Base> > data_u;
201 };
202 
203 } // namespace crocoddyl
204 
205 /* --- Details -------------------------------------------------------------- */
206 /* --- Details -------------------------------------------------------------- */
207 /* --- Details -------------------------------------------------------------- */
208 #include "crocoddyl/core/numdiff/cost.hxx"
209 
210 #endif // CROCODDYL_CORE_NUMDIFF_COST_HPP_
crocoddyl::CostDataNumDiffTpl::data_0
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: cost.hpp:198
crocoddyl::CostDataNumDiffTpl::dx
VectorXs dx
State disturbance.
Definition: cost.hpp:194
crocoddyl::CostModelNumDiffTpl
This class computes the numerical differentiation of a cost model.
Definition: cost.hpp:29
crocoddyl::CostModelNumDiffTpl::calc
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.
crocoddyl::DataCollectorAbstractTpl
Definition: data-collector-base.hpp:18
crocoddyl::CostModelNumDiffTpl::calcDiff
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.
crocoddyl::CostDataNumDiffTpl::xp
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: cost.hpp:195
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::CostModelAbstractTpl::unone_
VectorXs unone_
No control vector.
Definition: cost-base.hpp:244
crocoddyl::CostModelAbstractTpl::activation_
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:241
crocoddyl::CostModelNumDiffTpl::CostModelNumDiffTpl
CostModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff cost model.
crocoddyl::CostModelAbstractTpl
Abstract class for cost models.
Definition: cost-base.hpp:49
crocoddyl::CostModelNumDiffTpl::~CostModelNumDiffTpl
virtual ~CostModelNumDiffTpl()
Initialize the numdiff cost model.
crocoddyl::CostDataNumDiffTpl::data_x
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: cost.hpp:199
crocoddyl::CostDataNumDiffTpl
Definition: cost.hpp:143
crocoddyl::CostModelNumDiffTpl::get_model
const boost::shared_ptr< Base > & get_model() const
Return the original cost model.
crocoddyl::CostDataNumDiffTpl::up
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
Definition: cost.hpp:197
crocoddyl::CostDataNumDiffTpl::du
VectorXs du
Control disturbance.
Definition: cost.hpp:196
crocoddyl::CostModelNumDiffTpl::get_with_gauss_approx
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
crocoddyl::CostModelNumDiffTpl::set_disturbance
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
crocoddyl::CostModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:240
crocoddyl::CostDataAbstractTpl
Definition: cost-base.hpp:248
crocoddyl::CostDataNumDiffTpl::CostDataNumDiffTpl
CostDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff cost data.
Definition: cost.hpp:160
crocoddyl::CostModelNumDiffTpl::get_disturbance
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
crocoddyl::CostDataNumDiffTpl::data_u
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: cost.hpp:200
crocoddyl::CostModelNumDiffTpl::set_reevals
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...
crocoddyl::CostModelNumDiffTpl::createData
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create a numdiff cost data.
crocoddyl::ActivationDataAbstractTpl< Scalar >
crocoddyl::CostModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:243