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;
122  typedef MathBaseTpl<Scalar> MathBase;
123  typedef ResidualDataAbstractTpl<Scalar> Base;
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_
crocoddyl::DataCollectorAbstractTpl
Definition: data-collector-base.hpp:18
crocoddyl::ResidualModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
State description.
Definition: residual-base.hpp:173
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ResidualModelNumDiffTpl::model_
boost::shared_ptr< Base > model_
Model of the residual.
Definition: residual.hpp:94
crocoddyl::ResidualDataNumDiffTpl::dx
VectorXs dx
State disturbance.
Definition: residual.hpp:158
crocoddyl::ResidualModelNumDiffTpl::reevals_
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: residual.hpp:100
crocoddyl::ResidualModelNumDiffTpl::createData
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create a data object.
crocoddyl::ResidualModelNumDiffTpl::~ResidualModelNumDiffTpl
virtual ~ResidualModelNumDiffTpl()
Initialize the numdiff residual model.
crocoddyl::ResidualModelNumDiffTpl::get_disturbance
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
crocoddyl::ResidualModelAbstractTpl::unone_
VectorXs unone_
No control vector.
Definition: residual-base.hpp:176
crocoddyl::ResidualDataNumDiffTpl
Definition: fwd.hpp:194
crocoddyl::ResidualDataAbstractTpl::Ru
MatrixXs Ru
Jacobian of the residual vector with respect the control.
Definition: residual-base.hpp:207
crocoddyl::ResidualDataNumDiffTpl::up
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
Definition: residual.hpp:161
crocoddyl::ResidualDataAbstractTpl::Rx
MatrixXs Rx
Jacobian of the residual vector with respect the state.
Definition: residual-base.hpp:206
crocoddyl::ResidualModelNumDiffTpl::calc
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.
crocoddyl::ResidualDataAbstractTpl::shared
DataCollectorAbstract * shared
Shared data allocated by the action model.
Definition: residual-base.hpp:204
crocoddyl::ResidualModelNumDiffTpl::calcDiff
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.
crocoddyl::ResidualModelNumDiffTpl::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::ResidualDataAbstractTpl
Definition: fwd.hpp:59
crocoddyl::ResidualDataNumDiffTpl::data_0
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: residual.hpp:162
crocoddyl::ResidualModelNumDiffTpl::get_model
const boost::shared_ptr< Base > & get_model() const
Return the original residual model.
crocoddyl::ResidualDataNumDiffTpl::xp
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: residual.hpp:159
crocoddyl::ResidualModelNumDiffTpl::set_disturbance
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
crocoddyl::ResidualDataAbstractTpl::r
VectorXs r
Residual vector.
Definition: residual-base.hpp:205
crocoddyl::ResidualDataNumDiffTpl::data_x
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: residual.hpp:163
crocoddyl::ResidualDataNumDiffTpl::data_u
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: residual.hpp:164
crocoddyl::ResidualModelNumDiffTpl::disturbance_
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: residual.hpp:97
crocoddyl::ResidualModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: residual-base.hpp:175
crocoddyl::ResidualDataNumDiffTpl::du
VectorXs du
Control disturbance.
Definition: residual.hpp:160
crocoddyl::ActivationDataAbstractTpl< Scalar >
crocoddyl::ResidualModelNumDiffTpl::ResidualModelNumDiffTpl
ResidualModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff residual model.