crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
actuation.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, University of Edinburgh, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
10 #define CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
11 
12 #include <vector>
13 #include <iostream>
14 
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/actuation-base.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
21 class ActuationModelNumDiffTpl : public ActuationModelAbstractTpl<_Scalar> {
22  public:
23  typedef _Scalar Scalar;
24  typedef MathBaseTpl<Scalar> MathBase;
25  typedef ActuationModelAbstractTpl<Scalar> Base;
26  typedef ActuationDataNumDiffTpl<Scalar> Data;
27  typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
28  typedef typename MathBase::VectorXs VectorXs;
29  typedef typename MathBase::MatrixXs MatrixXs;
30 
36  explicit ActuationModelNumDiffTpl(boost::shared_ptr<Base> model);
37 
41  virtual ~ActuationModelNumDiffTpl();
42 
46  virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
47  const Eigen::Ref<const VectorXs>& u);
48 
52  virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
53  const Eigen::Ref<const VectorXs>& u);
54 
60  virtual boost::shared_ptr<ActuationDataAbstract> createData();
61 
67  const boost::shared_ptr<Base>& get_model() const;
68 
74  const Scalar get_disturbance() const;
75 
81  void set_disturbance(const Scalar disturbance);
82 
83  private:
87  boost::shared_ptr<Base> model_;
88 
93  Scalar disturbance_;
94 
95  protected:
96  using Base::nu_;
97 };
98 
99 template <typename _Scalar>
100 struct ActuationDataNumDiffTpl : public ActuationDataAbstractTpl<_Scalar> {
101  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102 
103  typedef _Scalar Scalar;
104  typedef MathBaseTpl<Scalar> MathBase;
105  typedef typename MathBase::VectorXs VectorXs;
106  typedef ActuationDataAbstractTpl<Scalar> Base;
107 
114  template <template <typename Scalar> class Model>
115  explicit ActuationDataNumDiffTpl(Model<Scalar>* const model)
116  : Base(model),
117  dx(model->get_model()->get_state()->get_ndx()),
118  du(model->get_model()->get_nu()),
119  xp(model->get_model()->get_state()->get_nx()) {
120  dx.setZero();
121  du.setZero();
122  xp.setZero();
123  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
124  const std::size_t nu = model->get_model()->get_nu();
125  data_0 = model->get_model()->createData();
126  for (std::size_t i = 0; i < ndx; ++i) {
127  data_x.push_back(model->get_model()->createData());
128  }
129  for (std::size_t i = 0; i < nu; ++i) {
130  data_u.push_back(model->get_model()->createData());
131  }
132  }
133 
134  VectorXs dx;
135  VectorXs du;
136  VectorXs xp;
137  boost::shared_ptr<Base> data_0;
138  std::vector<boost::shared_ptr<Base> > data_x;
139  std::vector<boost::shared_ptr<Base> > data_u;
140 
141  using Base::dtau_du;
142  using Base::dtau_dx;
143  using Base::tau;
144 };
145 
146 } // namespace crocoddyl
147 
148 /* --- Details -------------------------------------------------------------- */
149 /* --- Details -------------------------------------------------------------- */
150 /* --- Details -------------------------------------------------------------- */
151 #include "crocoddyl/core/numdiff/actuation.hxx"
152 
153 #endif // CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
VectorXs dx
State disturbance.
Definition: actuation.hpp:134
ActuationDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActuationDataNumDiff object.
Definition: actuation.hpp:115
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
virtual boost::shared_ptr< ActuationDataAbstract > createData()
Create a Data object from the given model.
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.
virtual void calcDiff(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: actuation.hpp:137
ActuationModelNumDiffTpl(boost::shared_ptr< Base > model)
Construct a new ActuationModelNumDiff object.
virtual ~ActuationModelNumDiffTpl()
Destroy the ActuationModelNumDiff object.
VectorXs du
Control disturbance.
Definition: actuation.hpp:135
const Scalar get_disturbance() const
Get the disturbance_ object.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: actuation.hpp:138
virtual void calc(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: actuation.hpp:139
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: actuation.hpp:136