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_
crocoddyl::DifferentialActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: diff-action-base.hpp:271
crocoddyl::DifferentialActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: diff-action-base.hpp:220
crocoddyl::DifferentialActionModelNumDiffTpl::set_disturbance
void set_disturbance(const Scalar disturbance)
Modify the disturbance used in the numerical differentiation routine.
crocoddyl::DifferentialActionModelNumDiffTpl::get_model
const boost::shared_ptr< Base > & get_model() const
Return the differential acton model that we use to numerical differentiate.
crocoddyl::DifferentialActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: diff-action-base.hpp:268
crocoddyl::DifferentialActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: diff-action-base.hpp:225
crocoddyl::DifferentialActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: diff-action-base.hpp:269
crocoddyl::DifferentialActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: diff-action-base.hpp:222
crocoddyl::DifferentialActionModelNumDiffTpl::calc
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.
crocoddyl::DifferentialActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: diff-action-base.hpp:265
crocoddyl::DifferentialActionDataAbstractTpl::xout
VectorXs xout
evolution state
Definition: diff-action-base.hpp:264
crocoddyl::DifferentialActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: diff-action-base.hpp:219
crocoddyl::DifferentialActionModelNumDiffTpl::get_with_gauss_approx
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
crocoddyl::DifferentialActionModelNumDiffTpl::DifferentialActionModelNumDiffTpl
DifferentialActionModelNumDiffTpl(boost::shared_ptr< Base > model, const bool with_gauss_approx=false)
Initialize the numdiff differential action model.
crocoddyl::DifferentialActionModelNumDiffTpl::calcDiff
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.
crocoddyl::DifferentialActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: diff-action-base.hpp:267
crocoddyl::DifferentialActionDataAbstractTpl
Definition: diff-action-base.hpp:231
crocoddyl::DifferentialActionModelNumDiffTpl::get_disturbance
const Scalar get_disturbance() const
Return the disturbance used in the numerical differentiation routine.
crocoddyl::DifferentialActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: diff-action-base.hpp:221
crocoddyl::DifferentialActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: diff-action-base.hpp:272
crocoddyl::DifferentialActionModelNumDiffTpl::createData
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
crocoddyl::DifferentialActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: diff-action-base.hpp:270
crocoddyl::DifferentialActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: diff-action-base.hpp:263
crocoddyl::DifferentialActionDataNumDiffTpl::DifferentialActionDataNumDiffTpl
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
Definition: diff-action.hpp:147
crocoddyl::DifferentialActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: diff-action-base.hpp:266
crocoddyl::DifferentialActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: diff-action-base.hpp:223
crocoddyl::DifferentialActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: diff-action-base.hpp:224