crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
action.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, 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 
41 template <typename _Scalar>
43  public:
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 
46  typedef _Scalar Scalar;
51  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
52  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
53 
60  explicit ActionModelNumDiffTpl(boost::shared_ptr<Base> model, bool with_gauss_approx = false);
61  virtual ~ActionModelNumDiffTpl();
62 
66  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
67  const Eigen::Ref<const VectorXs>& u);
68 
72  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
73 
77  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
78  const Eigen::Ref<const VectorXs>& u);
79 
84  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
85 
89  virtual boost::shared_ptr<ActionDataAbstract> createData();
90 
94  const boost::shared_ptr<Base>& get_model() const;
95 
99  const Scalar get_disturbance() const;
100 
104  void set_disturbance(const Scalar disturbance);
105 
110 
111  protected:
113  using Base::nr_;
114  using Base::nu_;
115  using Base::state_;
116  using Base::u_lb_;
117  using Base::u_ub_;
118  using Base::unone_;
119 
120  private:
132  void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
133 
134  boost::shared_ptr<Base> model_;
135  Scalar disturbance_;
136  bool with_gauss_approx_;
137 };
138 
139 template <typename _Scalar>
140 struct ActionDataNumDiffTpl : public ActionDataAbstractTpl<_Scalar> {
141  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142 
143  typedef _Scalar Scalar;
146  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
147  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
148 
155  template <template <typename Scalar> class Model>
156  explicit ActionDataNumDiffTpl(Model<Scalar>* const model)
157  : Base(model),
158  Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
159  Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
160  dx(model->get_model()->get_state()->get_ndx()),
161  du(model->get_model()->get_nu()),
162  xp(model->get_model()->get_state()->get_nx()) {
163  Rx.setZero();
164  Ru.setZero();
165  dx.setZero();
166  du.setZero();
167  xp.setZero();
168 
169  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
170  const std::size_t nu = model->get_model()->get_nu();
171  data_0 = model->get_model()->createData();
172  for (std::size_t i = 0; i < ndx; ++i) {
173  data_x.push_back(model->get_model()->createData());
174  }
175  for (std::size_t i = 0; i < nu; ++i) {
176  data_u.push_back(model->get_model()->createData());
177  }
178  }
179 
180  using Base::cost;
181  using Base::Fu;
182  using Base::Fx;
183  using Base::Lu;
184  using Base::Luu;
185  using Base::Lx;
186  using Base::Lxu;
187  using Base::Lxx;
188  using Base::r;
189  using Base::xnext;
190 
191  MatrixXs Rx;
192  MatrixXs Ru;
193  VectorXs dx;
194  VectorXs du;
195  VectorXs xp;
196  boost::shared_ptr<Base> data_0;
197  std::vector<boost::shared_ptr<Base> > data_x;
198  std::vector<boost::shared_ptr<Base> > data_u;
199 };
200 
201 } // namespace crocoddyl
202 
203 /* --- Details -------------------------------------------------------------- */
204 /* --- Details -------------------------------------------------------------- */
205 /* --- Details -------------------------------------------------------------- */
206 #include "crocoddyl/core/numdiff/action.hxx"
207 
208 #endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
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 action model.
Definition: action.hpp:42
const Scalar get_disturbance() const
Return the disturbance used in the numerical differentiation routine.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
void set_disturbance(const Scalar disturbance)
Modify the disturbance used in the numerical differentiation routine.
ActionModelNumDiffTpl(boost::shared_ptr< Base > model, bool with_gauss_approx=false)
Initialize the numdiff action model.
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
Return the acton model that we use to numerical differentiate.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
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:198
MatrixXs Ru
Cost residual jacobian: .
Definition: action.hpp:192
MatrixXs Rx
Cost residual jacobian: .
Definition: action.hpp:191
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: action.hpp:196
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: action.hpp:195
VectorXs du
Control disturbance.
Definition: action.hpp:194
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: action.hpp:197
ActionDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff action data.
Definition: action.hpp:156
VectorXs dx
State disturbance.
Definition: action.hpp:193