crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
diff-action.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh,
5 // New York University, Max Planck Gesellschaft
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
12 
13 #include <vector>
14 #include <iostream>
15 
16 #include "crocoddyl/core/diff-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 MathBase::VectorXs VectorXs;
52  typedef typename MathBase::MatrixXs MatrixXs;
53 
60  explicit DifferentialActionModelNumDiffTpl(boost::shared_ptr<Base> model, const bool with_gauss_approx = false);
62 
66  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
67  const Eigen::Ref<const VectorXs>& u);
68 
73  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
74  const Eigen::Ref<const VectorXs>& x);
75 
79  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
80  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
81 
86  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87  const Eigen::Ref<const VectorXs>& x);
88 
92  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
93 
97  const boost::shared_ptr<Base>& get_model() const;
98 
102  const Scalar get_disturbance() const;
103 
107  void set_disturbance(const Scalar disturbance);
108 
113 
114  protected:
116  using Base::nr_;
117  using Base::nu_;
118  using Base::state_;
119  using Base::u_lb_;
120  using Base::u_ub_;
121  using Base::unone_;
122 
123  private:
124  void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
125  boost::shared_ptr<Base> model_;
126  bool with_gauss_approx_;
127  Scalar disturbance_;
128 };
129 
130 template <typename _Scalar>
132  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 
134  typedef _Scalar Scalar;
137  typedef typename MathBase::VectorXs VectorXs;
138  typedef typename MathBase::MatrixXs MatrixXs;
139 
146  template <template <typename Scalar> class Model>
147  explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
148  : Base(model),
149  Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
150  Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
151  dx(model->get_model()->get_state()->get_ndx()),
152  du(model->get_model()->get_nu()),
153  xp(model->get_model()->get_state()->get_nx()) {
154  Rx.setZero();
155  Ru.setZero();
156  dx.setZero();
157  du.setZero();
158  xp.setZero();
159 
160  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
161  const std::size_t nu = model->get_model()->get_nu();
162  data_0 = model->get_model()->createData();
163  for (std::size_t i = 0; i < ndx; ++i) {
164  data_x.push_back(model->get_model()->createData());
165  }
166  for (std::size_t i = 0; i < nu; ++i) {
167  data_u.push_back(model->get_model()->createData());
168  }
169  }
170 
171  MatrixXs Rx;
172  MatrixXs Ru;
173  VectorXs dx;
174  VectorXs du;
175  VectorXs xp;
176  boost::shared_ptr<Base> data_0;
177  std::vector<boost::shared_ptr<Base> > data_x;
178  std::vector<boost::shared_ptr<Base> > data_u;
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::xout;
190 };
191 
192 } // namespace crocoddyl
193 
194 /* --- Details -------------------------------------------------------------- */
195 /* --- Details -------------------------------------------------------------- */
196 /* --- Details -------------------------------------------------------------- */
197 #include "crocoddyl/core/numdiff/diff-action.hxx"
198 
199 #endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
Abstract class for differential action model.
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 nr_
Dimension of the cost residual.
This class computes the numerical differentiation of a differential action model.
Definition: diff-action.hpp:42
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
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.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
const boost::shared_ptr< Base > & get_model() const
Return the differential acton model that we use to numerical differentiate.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration and cost value.
DifferentialActionModelNumDiffTpl(boost::shared_ptr< Base > model, const bool with_gauss_approx=false)
Initialize the numdiff differential action model.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
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.
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.