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>
42 class DifferentialActionModelNumDiffTpl : public DifferentialActionModelAbstractTpl<_Scalar> {
43  public:
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 
46  typedef _Scalar Scalar;
47  typedef MathBaseTpl<Scalar> MathBase;
48  typedef DifferentialActionModelAbstractTpl<Scalar> Base;
49  typedef DifferentialActionDataNumDiffTpl<Scalar> Data;
50  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
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 
112  bool get_with_gauss_approx();
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>
131 struct DifferentialActionDataNumDiffTpl : public DifferentialActionDataAbstractTpl<_Scalar> {
132  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 
134  typedef _Scalar Scalar;
135  typedef MathBaseTpl<Scalar> MathBase;
136  typedef DifferentialActionDataAbstractTpl<Scalar> Base;
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_
VectorXs Lx
Jacobian of the cost function.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Lxx
Hessian of the cost function.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
const Scalar get_disturbance() const
Return the disturbance used in the numerical differentiation routine.
MatrixXs Luu
Hessian of the cost function.
std::size_t nr_
Dimension of the cost residual.
bool has_control_limits_
Indicates whether any of the control limits is finite.
MatrixXs Lxu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
const boost::shared_ptr< Base > & get_model() const
Return the differential acton model that we use to numerical differentiate.
void set_disturbance(const Scalar disturbance)
Modify the disturbance used in the numerical differentiation routine.
MatrixXs Fu
Jacobian of the dynamics.
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
VectorXs Lu
Jacobian of the cost function.
DifferentialActionModelNumDiffTpl(boost::shared_ptr< Base > model, const bool with_gauss_approx=false)
Initialize the numdiff differential action model.
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.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
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.