crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
diff-action.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, 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 
20 template <typename _Scalar>
22  public:
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
30  typedef typename MathBase::VectorXs VectorXs;
31  typedef typename MathBase::MatrixXs MatrixXs;
32 
33  explicit DifferentialActionModelNumDiffTpl(boost::shared_ptr<Base> model, const bool with_gauss_approx = false);
35 
36  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
37  const Eigen::Ref<const VectorXs>& u);
38  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
39  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
40  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
41 
42  const boost::shared_ptr<Base>& get_model() const;
43  const Scalar get_disturbance() const;
44  void set_disturbance(const Scalar disturbance);
45  bool get_with_gauss_approx();
46 
47  protected:
49  using Base::nr_;
50  using Base::nu_;
51  using Base::state_;
52  using Base::u_lb_;
53  using Base::u_ub_;
54  using Base::unone_;
55 
56  private:
57  void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
58  boost::shared_ptr<Base> model_;
59  bool with_gauss_approx_;
60  Scalar disturbance_;
61 };
62 
63 template <typename _Scalar>
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  typedef _Scalar Scalar;
70  typedef typename MathBase::VectorXs VectorXs;
71  typedef typename MathBase::MatrixXs MatrixXs;
72 
79  template <template <typename Scalar> class Model>
80  explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
81  : Base(model),
82  Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
83  Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
84  dx(model->get_model()->get_state()->get_ndx()),
85  du(model->get_model()->get_nu()),
86  xp(model->get_model()->get_state()->get_nx()) {
87  Rx.setZero();
88  Ru.setZero();
89  dx.setZero();
90  du.setZero();
91  xp.setZero();
92 
93  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
94  const std::size_t nu = model->get_model()->get_nu();
95  data_0 = model->get_model()->createData();
96  for (std::size_t i = 0; i < ndx; ++i) {
97  data_x.push_back(model->get_model()->createData());
98  }
99  for (std::size_t i = 0; i < nu; ++i) {
100  data_u.push_back(model->get_model()->createData());
101  }
102  }
103 
104  MatrixXs Rx;
105  MatrixXs Ru;
106  VectorXs dx;
107  VectorXs du;
108  VectorXs xp;
109  boost::shared_ptr<Base> data_0;
110  std::vector<boost::shared_ptr<Base> > data_x;
111  std::vector<boost::shared_ptr<Base> > data_u;
112 
113  using Base::cost;
114  using Base::Fu;
115  using Base::Fx;
116  using Base::Lu;
117  using Base::Luu;
118  using Base::Lx;
119  using Base::Lxu;
120  using Base::Lxx;
121  using Base::r;
122  using Base::xout;
123 };
124 
125 } // namespace crocoddyl
126 
127 /* --- Details -------------------------------------------------------------- */
128 /* --- Details -------------------------------------------------------------- */
129 /* --- Details -------------------------------------------------------------- */
130 #include "crocoddyl/core/numdiff/diff-action.hxx"
131 
132 #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.
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.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
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.
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.
Definition: diff-action.hpp:80