crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
residual.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
10 #define CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
11 
12 #include <boost/function.hpp>
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/core/residual-base.hpp"
15 
16 namespace crocoddyl {
17 
18 template <typename _Scalar>
19 class ResidualModelNumDiffTpl : public ResidualModelAbstractTpl<_Scalar> {
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
24  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
25  typedef ResidualModelAbstractTpl<Scalar> Base;
26  typedef ResidualDataNumDiffTpl<Scalar> Data;
27  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
28  typedef MathBaseTpl<Scalar> MathBase;
29  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
30  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
31  typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
32 
38  explicit ResidualModelNumDiffTpl(const boost::shared_ptr<Base>& model);
39 
43  virtual ~ResidualModelNumDiffTpl();
44 
48  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49  const Eigen::Ref<const VectorXs>& u);
50 
54  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
55  const Eigen::Ref<const VectorXs>& u);
56 
63  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
64 
68  const boost::shared_ptr<Base>& get_model() const;
69 
73  const Scalar get_disturbance() const;
74 
78  void set_disturbance(const Scalar disturbance);
79 
86  void set_reevals(const std::vector<ReevaluationFunction>& reevals);
87 
88  protected:
89  using Base::nu_;
90  using Base::state_;
91  using Base::unone_;
92 
94  boost::shared_ptr<Base> model_;
95 
97  Scalar disturbance_;
98 
100  std::vector<ReevaluationFunction> reevals_;
101 
102  private:
114  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
115 };
116 
117 template <typename _Scalar>
118 struct ResidualDataNumDiffTpl : public ResidualDataAbstractTpl<_Scalar> {
119  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
120 
121  typedef _Scalar Scalar;
126  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
127 
128  template <template <typename Scalar> class Model>
129  explicit ResidualDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
130  : Base(model, shared_data),
131  dx(model->get_state()->get_ndx()),
132  xp(model->get_state()->get_nx()),
133  du(model->get_nu()),
134  up(model->get_nu()) {
135  dx.setZero();
136  xp.setZero();
137  du.setZero();
138  up.setZero();
139 
140  const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
141  const std::size_t& nu = model->get_model()->get_nu();
142  data_0 = model->get_model()->createData(shared_data);
143  for (std::size_t i = 0; i < ndx; ++i) {
144  data_x.push_back(model->get_model()->createData(shared_data));
145  }
146  for (std::size_t i = 0; i < nu; ++i) {
147  data_u.push_back(model->get_model()->createData(shared_data));
148  }
149  }
150 
151  virtual ~ResidualDataNumDiffTpl() {}
152 
153  using Base::r;
154  using Base::Ru;
155  using Base::Rx;
156  using Base::shared;
157 
158  VectorXs dx;
159  VectorXs xp;
160  VectorXs du;
161  VectorXs up;
162  boost::shared_ptr<Base> data_0;
163  std::vector<boost::shared_ptr<Base> > data_x;
164  std::vector<boost::shared_ptr<Base> > data_u;
165 };
166 
167 } // namespace crocoddyl
168 
169 /* --- Details -------------------------------------------------------------- */
170 /* --- Details -------------------------------------------------------------- */
171 /* --- Details -------------------------------------------------------------- */
172 #include "crocoddyl/core/numdiff/residual.hxx"
173 
174 #endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
VectorXs du
Control disturbance.
Definition: residual.hpp:160
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create a data object.
virtual ~ResidualModelNumDiffTpl()
Initialize the numdiff residual model.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual vector.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: residual.hpp:159
boost::shared_ptr< Base > model_
Model of the residual.
Definition: residual.hpp:94
Abstract class for residual models.
Definition: fwd.hpp:57
std::size_t nu_
Control dimension.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian of the residual vector.
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: residual.hpp:100
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: residual.hpp:162
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: residual.hpp:164
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
ResidualModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff residual model.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t get_nu() const
Return the dimension of the control input.
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: residual.hpp:97
VectorXs dx
State disturbance.
Definition: residual.hpp:158
VectorXs unone_
No control vector.
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
Definition: residual.hpp:161
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...
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: residual.hpp:163
const boost::shared_ptr< Base > & get_model() const
Return the original residual model.