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.
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.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
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.
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.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.