crocoddyl  1.9.0
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 
27 template <typename _Scalar>
28 class ResidualModelNumDiffTpl : public ResidualModelAbstractTpl<_Scalar> {
29  public:
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 
32  typedef _Scalar Scalar;
33  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
34  typedef ResidualModelAbstractTpl<Scalar> Base;
35  typedef ResidualDataNumDiffTpl<Scalar> Data;
36  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
37  typedef MathBaseTpl<Scalar> MathBase;
38  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
39  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
40  typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
41 
47  explicit ResidualModelNumDiffTpl(const boost::shared_ptr<Base>& model);
48 
52  virtual ~ResidualModelNumDiffTpl();
53 
57  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
58  const Eigen::Ref<const VectorXs>& u);
59 
64  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
65 
69  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
70  const Eigen::Ref<const VectorXs>& u);
71 
76  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
77 
81  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
82 
86  const boost::shared_ptr<Base>& get_model() const;
87 
91  const Scalar get_disturbance() const;
92 
96  void set_disturbance(const Scalar disturbance);
97 
104  void set_reevals(const std::vector<ReevaluationFunction>& reevals);
105 
106  protected:
107  using Base::nu_;
108  using Base::state_;
109  using Base::unone_;
110 
111  private:
123  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
124 
125  boost::shared_ptr<Base> model_;
126  Scalar disturbance_;
127  std::vector<ReevaluationFunction> reevals_;
128 };
129 
130 template <typename _Scalar>
131 struct ResidualDataNumDiffTpl : public ResidualDataAbstractTpl<_Scalar> {
132  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 
134  typedef _Scalar Scalar;
135  typedef MathBaseTpl<Scalar> MathBase;
136  typedef ResidualDataAbstractTpl<Scalar> Base;
137  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
138  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
139  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
140 
147  template <template <typename Scalar> class Model>
148  explicit ResidualDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
149  : Base(model, shared_data),
150  dx(model->get_state()->get_ndx()),
151  xp(model->get_state()->get_nx()),
152  du(model->get_nu()),
153  up(model->get_nu()) {
154  dx.setZero();
155  xp.setZero();
156  du.setZero();
157  up.setZero();
158 
159  const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
160  const std::size_t& nu = model->get_model()->get_nu();
161  data_0 = model->get_model()->createData(shared_data);
162  for (std::size_t i = 0; i < ndx; ++i) {
163  data_x.push_back(model->get_model()->createData(shared_data));
164  }
165  for (std::size_t i = 0; i < nu; ++i) {
166  data_u.push_back(model->get_model()->createData(shared_data));
167  }
168  }
169 
170  virtual ~ResidualDataNumDiffTpl() {}
171 
172  using Base::r;
173  using Base::Ru;
174  using Base::Rx;
175  using Base::shared;
176 
177  VectorXs dx;
178  VectorXs xp;
179  VectorXs du;
180  VectorXs up;
181  boost::shared_ptr<Base> data_0;
182  std::vector<boost::shared_ptr<Base> > data_x;
183  std::vector<boost::shared_ptr<Base> > data_u;
184 };
185 
186 } // namespace crocoddyl
187 
188 /* --- Details -------------------------------------------------------------- */
189 /* --- Details -------------------------------------------------------------- */
190 /* --- Details -------------------------------------------------------------- */
191 #include "crocoddyl/core/numdiff/residual.hxx"
192 
193 #endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
VectorXs du
Control disturbance.
Definition: residual.hpp:179
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
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.
ResidualDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff residual data.
Definition: residual.hpp:148
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: residual.hpp:178
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.
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: residual.hpp:181
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: residual.hpp:183
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.
VectorXs dx
State disturbance.
Definition: residual.hpp:177
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:180
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:182
const boost::shared_ptr< Base > & get_model() const
Return the original residual model.