1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, LAAS-CNRS, New York University, |
5 |
|
|
// Max Planck Gesellschaft, University of Edinburgh |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#ifndef CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_ |
11 |
|
|
#define CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_ |
12 |
|
|
|
13 |
|
|
#include <iostream> |
14 |
|
|
#include <vector> |
15 |
|
|
|
16 |
|
|
#include "crocoddyl/core/activation-base.hpp" |
17 |
|
|
#include "crocoddyl/core/fwd.hpp" |
18 |
|
|
|
19 |
|
|
namespace crocoddyl { |
20 |
|
|
|
21 |
|
|
template <typename _Scalar> |
22 |
|
|
class ActivationModelNumDiffTpl : public ActivationModelAbstractTpl<_Scalar> { |
23 |
|
|
public: |
24 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
25 |
|
|
|
26 |
|
|
typedef _Scalar Scalar; |
27 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
28 |
|
|
typedef ActivationModelAbstractTpl<Scalar> Base; |
29 |
|
|
typedef ActivationDataNumDiffTpl<Scalar> Data; |
30 |
|
|
typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract; |
31 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
32 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
33 |
|
|
|
34 |
|
|
/** |
35 |
|
|
* @brief Construct a new ActivationModelNumDiff object |
36 |
|
|
* |
37 |
|
|
* @param model |
38 |
|
|
*/ |
39 |
|
|
explicit ActivationModelNumDiffTpl(boost::shared_ptr<Base> model); |
40 |
|
|
|
41 |
|
|
/** |
42 |
|
|
* @brief Destroy the ActivationModelNumDiff object |
43 |
|
|
*/ |
44 |
|
|
virtual ~ActivationModelNumDiffTpl(); |
45 |
|
|
|
46 |
|
|
/** |
47 |
|
|
* @brief @copydoc Base::calc() |
48 |
|
|
*/ |
49 |
|
|
virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, |
50 |
|
|
const Eigen::Ref<const VectorXs>& r); |
51 |
|
|
|
52 |
|
|
/** |
53 |
|
|
* @brief @copydoc Base::calcDiff() |
54 |
|
|
*/ |
55 |
|
|
virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, |
56 |
|
|
const Eigen::Ref<const VectorXs>& r); |
57 |
|
|
|
58 |
|
|
/** |
59 |
|
|
* @brief Create a Data object from the given model. |
60 |
|
|
* |
61 |
|
|
* @return boost::shared_ptr<ActivationDataAbstract> |
62 |
|
|
*/ |
63 |
|
|
virtual boost::shared_ptr<ActivationDataAbstract> createData(); |
64 |
|
|
|
65 |
|
|
/** |
66 |
|
|
* @brief Get the model_ object |
67 |
|
|
* |
68 |
|
|
* @return Base& |
69 |
|
|
*/ |
70 |
|
|
const boost::shared_ptr<Base>& get_model() const; |
71 |
|
|
|
72 |
|
|
/** |
73 |
|
|
* @brief Return the disturbance constant used in the numerical |
74 |
|
|
* differentiation routine |
75 |
|
|
*/ |
76 |
|
|
const Scalar get_disturbance() const; |
77 |
|
|
|
78 |
|
|
/** |
79 |
|
|
* @brief Modify the disturbance constant used in the numerical |
80 |
|
|
* differentiation routine |
81 |
|
|
*/ |
82 |
|
|
void set_disturbance(const Scalar disturbance); |
83 |
|
|
|
84 |
|
|
private: |
85 |
|
|
boost::shared_ptr<Base> |
86 |
|
|
model_; //!< model to compute the finite differentiation from |
87 |
|
|
Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian |
88 |
|
|
//!< calculation |
89 |
|
|
|
90 |
|
|
protected: |
91 |
|
|
using Base::nr_; |
92 |
|
|
}; |
93 |
|
|
|
94 |
|
|
template <typename _Scalar> |
95 |
|
|
struct ActivationDataNumDiffTpl : public ActivationDataAbstractTpl<_Scalar> { |
96 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
97 |
|
|
|
98 |
|
|
typedef _Scalar Scalar; |
99 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
100 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
101 |
|
|
typedef ActivationDataAbstractTpl<Scalar> Base; |
102 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
103 |
|
|
|
104 |
|
|
/** |
105 |
|
|
* @brief Construct a new ActivationDataNumDiff object |
106 |
|
|
* |
107 |
|
|
* @tparam Model is the type of the ActivationModel. |
108 |
|
|
* @param model is the object to compute the numerical differentiation from. |
109 |
|
|
*/ |
110 |
|
|
template <template <typename Scalar> class Model> |
111 |
|
11 |
explicit ActivationDataNumDiffTpl(Model<Scalar>* const model) |
112 |
|
|
: Base(model), |
113 |
|
11 |
dr(model->get_model()->get_nr()), |
114 |
|
11 |
rp(model->get_model()->get_nr()), |
115 |
✓✗✓✗
✓✗✓✗
✓✗ |
11 |
Arr_(Arr.rows(), Arr.cols()) { |
116 |
✓✗ |
11 |
dr.setZero(); |
117 |
✓✗ |
11 |
rp.setZero(); |
118 |
✓✗ |
11 |
Arr_.setZero(); |
119 |
✓✗ |
11 |
data_0 = model->get_model()->createData(); |
120 |
|
11 |
const std::size_t nr = model->get_model()->get_nr(); |
121 |
|
11 |
data_rp.clear(); |
122 |
✓✓ |
60 |
for (std::size_t i = 0; i < nr; ++i) { |
123 |
✓✗✓✗
|
49 |
data_rp.push_back(model->get_model()->createData()); |
124 |
|
|
} |
125 |
|
|
|
126 |
|
11 |
data_r2p.clear(); |
127 |
✓✓ |
55 |
for (std::size_t i = 0; i < 4; ++i) { |
128 |
✓✗✓✗
|
44 |
data_r2p.push_back(model->get_model()->createData()); |
129 |
|
|
} |
130 |
|
11 |
} |
131 |
|
|
|
132 |
|
|
VectorXs dr; //!< disturbance: \f$ [\hdot \;\; disturbance \;\; \hdot] \f$ |
133 |
|
|
VectorXs rp; //!< The input + the disturbance on one DoF "\f$ r^+ = rp = \int |
134 |
|
|
//!< r + dr \f$" |
135 |
|
|
boost::shared_ptr<Base> data_0; //!< The data that contains the final results |
136 |
|
|
std::vector<boost::shared_ptr<Base> > |
137 |
|
|
data_rp; //!< The temporary data associated with the input variation |
138 |
|
|
std::vector<boost::shared_ptr<Base> > |
139 |
|
|
data_r2p; //!< The temporary data associated with the input variation |
140 |
|
|
|
141 |
|
|
MatrixXs Arr_; |
142 |
|
|
using Base::a_value; |
143 |
|
|
using Base::Ar; |
144 |
|
|
using Base::Arr; |
145 |
|
|
}; |
146 |
|
|
|
147 |
|
|
} // namespace crocoddyl |
148 |
|
|
|
149 |
|
|
/* --- Details -------------------------------------------------------------- */ |
150 |
|
|
/* --- Details -------------------------------------------------------------- */ |
151 |
|
|
/* --- Details -------------------------------------------------------------- */ |
152 |
|
|
#include "crocoddyl/core/numdiff/activation.hxx" |
153 |
|
|
|
154 |
|
|
#endif // CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_ |