crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
actuation.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, 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 
30 template <typename _Scalar>
31 class ActuationModelNumDiffTpl : public ActuationModelAbstractTpl<_Scalar> {
32  public:
33  typedef _Scalar Scalar;
34  typedef MathBaseTpl<Scalar> MathBase;
35  typedef ActuationModelAbstractTpl<Scalar> Base;
36  typedef ActuationDataNumDiffTpl<Scalar> Data;
37  typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
38  typedef typename MathBase::VectorXs VectorXs;
39  typedef typename MathBase::MatrixXs MatrixXs;
40 
46  explicit ActuationModelNumDiffTpl(boost::shared_ptr<Base> model);
47 
51  virtual ~ActuationModelNumDiffTpl();
52 
56  virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
57  const Eigen::Ref<const VectorXs>& u);
58 
63  virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
64 
68  virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
69  const Eigen::Ref<const VectorXs>& u);
70 
75  virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
76 
80  virtual boost::shared_ptr<ActuationDataAbstract> createData();
81 
85  const boost::shared_ptr<Base>& get_model() const;
86 
90  const Scalar get_disturbance() const;
91 
95  void set_disturbance(const Scalar disturbance);
96 
97  private:
98  boost::shared_ptr<Base> model_;
99  Scalar disturbance_;
100 
101  protected:
102  using Base::nu_;
103 };
104 
105 template <typename _Scalar>
106 struct ActuationDataNumDiffTpl : public ActuationDataAbstractTpl<_Scalar> {
107  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108 
109  typedef _Scalar Scalar;
110  typedef MathBaseTpl<Scalar> MathBase;
111  typedef typename MathBase::VectorXs VectorXs;
112  typedef ActuationDataAbstractTpl<Scalar> Base;
113 
120  template <template <typename Scalar> class Model>
121  explicit ActuationDataNumDiffTpl(Model<Scalar>* const model)
122  : Base(model),
123  dx(model->get_model()->get_state()->get_ndx()),
124  du(model->get_model()->get_nu()),
125  xp(model->get_model()->get_state()->get_nx()) {
126  dx.setZero();
127  du.setZero();
128  xp.setZero();
129  const std::size_t ndx = model->get_model()->get_state()->get_ndx();
130  const std::size_t nu = model->get_model()->get_nu();
131  data_0 = model->get_model()->createData();
132  for (std::size_t i = 0; i < ndx; ++i) {
133  data_x.push_back(model->get_model()->createData());
134  }
135  for (std::size_t i = 0; i < nu; ++i) {
136  data_u.push_back(model->get_model()->createData());
137  }
138  }
139 
140  VectorXs dx;
141  VectorXs du;
142  VectorXs xp;
143  boost::shared_ptr<Base> data_0;
144  std::vector<boost::shared_ptr<Base> > data_x;
145  std::vector<boost::shared_ptr<Base> > data_u;
146 
147  using Base::dtau_du;
148  using Base::dtau_dx;
149  using Base::tau;
150 };
151 
152 } // namespace crocoddyl
153 
154 /* --- Details -------------------------------------------------------------- */
155 /* --- Details -------------------------------------------------------------- */
156 /* --- Details -------------------------------------------------------------- */
157 #include "crocoddyl/core/numdiff/actuation.hxx"
158 
159 #endif // CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
std::size_t nu_
Control dimension.
VectorXs dx
State disturbance.
Definition: actuation.hpp:140
ActuationDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff actuation data.
Definition: actuation.hpp:121
const boost::shared_ptr< Base > & get_model() const
Return the original actuation model.
virtual boost::shared_ptr< ActuationDataAbstract > createData()
Create the actuation data.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
std::size_t get_nu() const
Return the dimension of the control input.
virtual void calcDiff(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the actuation function.
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: actuation.hpp:143
ActuationModelNumDiffTpl(boost::shared_ptr< Base > model)
Initialize the numdiff residual model.
virtual ~ActuationModelNumDiffTpl()
Destroy the numdiff actuation model.
VectorXs du
Control disturbance.
Definition: actuation.hpp:141
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: actuation.hpp:144
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
virtual void calc(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the actuation signal from the state point and control input .
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: actuation.hpp:145
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: actuation.hpp:142