crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
action.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, New York University,
5 // Max Planck Gesellschaft, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
12 
13 #include <vector>
14 
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/action-base.hpp"
17 
18 namespace crocoddyl {
19 
51 template <typename _Scalar>
53  public:
54  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55 
56  typedef _Scalar Scalar;
61  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
62  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
63 
69  explicit ActionModelNumDiffTpl(boost::shared_ptr<Base> model, bool with_gauss_approx = false);
70 
75 
79  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
80  const Eigen::Ref<const VectorXs>& u);
81 
85  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
86  const Eigen::Ref<const VectorXs>& u);
87 
93  virtual boost::shared_ptr<ActionDataAbstract> createData();
94 
100  const boost::shared_ptr<Base>& get_model() const;
101 
107  const Scalar get_disturbance() const;
108 
114  void set_disturbance(const Scalar disturbance);
115 
123 
124  protected:
126  using Base::nr_;
127  using Base::nu_;
128  using Base::state_;
129  using Base::u_lb_;
130  using Base::u_ub_;
131  using Base::unone_;
132 
133  private:
145  void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
146 
151  boost::shared_ptr<Base> model_;
152 
157  Scalar disturbance_;
158 
159  bool with_gauss_approx_;
160 };
161 
162 template <typename _Scalar>
163 struct ActionDataNumDiffTpl : public ActionDataAbstractTpl<_Scalar> {
164  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
165 
166  typedef _Scalar Scalar;
169  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
170  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
171 
178  template <template <typename Scalar> class Model>
179  explicit ActionDataNumDiffTpl(Model<Scalar>* const model)
180  : Base(model),
181  Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
182  Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
183  dx(model->get_model()->get_state()->get_ndx()),
184  du(model->get_model()->get_nu()),
185  xp(model->get_model()->get_state()->get_nx()) {
186  Rx.setZero();
187  Ru.setZero();
188  dx.setZero();
189  du.setZero();
190  xp.setZero();
191 
192  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
193  const std::size_t nu = model->get_model()->get_nu();
194  data_0 = model->get_model()->createData();
195  for (std::size_t i = 0; i < ndx; ++i) {
196  data_x.push_back(model->get_model()->createData());
197  }
198  for (std::size_t i = 0; i < nu; ++i) {
199  data_u.push_back(model->get_model()->createData());
200  }
201  }
202 
203  using Base::cost;
204  using Base::Fu;
205  using Base::Fx;
206  using Base::Lu;
207  using Base::Luu;
208  using Base::Lx;
209  using Base::Lxu;
210  using Base::Lxx;
211  using Base::r;
212  using Base::xnext;
213 
214  MatrixXs Rx;
215  MatrixXs Ru;
216  VectorXs dx;
217  VectorXs du;
218  VectorXs xp;
219  boost::shared_ptr<Base> data_0;
220  std::vector<boost::shared_ptr<Base> > data_x;
221  std::vector<boost::shared_ptr<Base> > data_u;
222 };
223 
224 } // namespace crocoddyl
225 
226 /* --- Details -------------------------------------------------------------- */
227 /* --- Details -------------------------------------------------------------- */
228 /* --- Details -------------------------------------------------------------- */
229 #include "crocoddyl/core/numdiff/action.hxx"
230 
231 #endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
Abstract class for action model.
Definition: action-base.hpp:60
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
This class computes the numerical differentiation of an ActionModel.
Definition: action.hpp:52
const Scalar get_disturbance() const
Get the disturbance_ object.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.
ActionModelNumDiffTpl(boost::shared_ptr< Base > model, bool with_gauss_approx=false)
Construct a new ActionModelNumDiff object.
virtual ~ActionModelNumDiffTpl()
Destroy the ActionModelNumDiff object.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create a Data object from the given model.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
VectorXs r
Cost residual.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: action.hpp:221
MatrixXs Ru
Cost residual jacobian: .
Definition: action.hpp:215
MatrixXs Rx
Cost residual jacobian: .
Definition: action.hpp:214
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: action.hpp:219
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: action.hpp:218
VectorXs du
Control disturbance.
Definition: action.hpp:217
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: action.hpp:220
ActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
Definition: action.hpp:179
VectorXs dx
State disturbance.
Definition: action.hpp:216