GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/actuation.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 16 0.0%
Branches: 0 42 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
12
13 #include "crocoddyl/core/actuation-base.hpp"
14 #include "crocoddyl/core/fwd.hpp"
15
16 namespace crocoddyl {
17
18 /**
19 * @brief This class computes the numerical differentiation of an actuation
20 * model.
21 *
22 * It computes the Jacobian of the residual model via numerical differentiation,
23 * i.e., \f$\frac{\partial\boldsymbol{\tau}}{\partial\mathbf{x}}\f$ and
24 * \f$\frac{\partial\boldsymbol{\tau}}{\partial\mathbf{u}}\f$ which denote the
25 * Jacobians of the actuation function
26 * \f$\boldsymbol{\tau}(\mathbf{x},\mathbf{u})\f$.
27 *
28 * \sa `ActuationModelAbstractTpl()`, `calcDiff()`
29 */
30 template <typename _Scalar>
31 class ActuationModelNumDiffTpl : public ActuationModelAbstractTpl<_Scalar> {
32 public:
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 ✗ CROCODDYL_DERIVED_CAST(ActuationModelBase, ActuationModelNumDiffTpl)
35
36 typedef _Scalar Scalar;
37 typedef MathBaseTpl<Scalar> MathBase;
38 typedef ActuationModelAbstractTpl<Scalar> Base;
39 typedef ActuationDataNumDiffTpl<Scalar> Data;
40 typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
41 typedef typename MathBase::VectorXs VectorXs;
42 typedef typename MathBase::MatrixXs MatrixXs;
43
44 /**
45 * @brief Initialize the numdiff residual model
46 *
47 * @param model Actuation model that we want to apply the numerical
48 * differentiation
49 */
50 explicit ActuationModelNumDiffTpl(std::shared_ptr<Base> model);
51
52 /**
53 * @brief Destroy the numdiff actuation model
54 */
55 ✗ virtual ~ActuationModelNumDiffTpl() = default;
56
57 /**
58 * @brief @copydoc Base::calc()
59 */
60 virtual void calc(const std::shared_ptr<ActuationDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>& x,
62 const Eigen::Ref<const VectorXs>& u) override;
63
64 /**
65 * @brief @copydoc Base::calc(const std::shared_ptr<ActuationDataAbstract>&
66 * data, const Eigen::Ref<const VectorXs>& x)
67 */
68 virtual void calc(const std::shared_ptr<ActuationDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>& x);
70
71 /**
72 * @brief @copydoc Base::calcDiff()
73 */
74 virtual void calcDiff(const std::shared_ptr<ActuationDataAbstract>& data,
75 const Eigen::Ref<const VectorXs>& x,
76 const Eigen::Ref<const VectorXs>& u) override;
77
78 /**
79 * @brief @copydoc Base::calcDiff(const
80 * std::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const
81 * VectorXs>& x)
82 */
83 virtual void calcDiff(const std::shared_ptr<ActuationDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>& x);
85
86 /**
87 * @brief @copydoc Base::commands()
88 */
89 virtual void commands(const std::shared_ptr<ActuationDataAbstract>& data,
90 const Eigen::Ref<const VectorXs>& x,
91 const Eigen::Ref<const VectorXs>& tau) override;
92
93 /**
94 * @brief @copydoc Base::torqueTransform()
95 */
96 virtual void torqueTransform(
97 const std::shared_ptr<ActuationDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x,
99 const Eigen::Ref<const VectorXs>& u) override;
100
101 /**
102 * @brief @copydoc Base::createData()
103 */
104 virtual std::shared_ptr<ActuationDataAbstract> createData() override;
105
106 template <typename NewScalar>
107 ActuationModelNumDiffTpl<NewScalar> cast() const;
108
109 /**
110 * @brief Return the original actuation model
111 */
112 const std::shared_ptr<Base>& get_model() const;
113
114 /**
115 * @brief Return the disturbance constant used by the numerical
116 * differentiation routine
117 */
118 const Scalar get_disturbance() const;
119
120 /**
121 * @brief Modify the disturbance constant used by the numerical
122 * differentiation routine
123 */
124 void set_disturbance(const Scalar disturbance);
125
126 private:
127 std::shared_ptr<Base> model_; //!< Actuation model hat we want to apply the
128 //!< numerical differentiation
129 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
130 //!< calculation
131
132 protected:
133 using Base::nu_;
134 };
135
136 template <typename _Scalar>
137 struct ActuationDataNumDiffTpl : public ActuationDataAbstractTpl<_Scalar> {
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139
140 typedef _Scalar Scalar;
141 typedef MathBaseTpl<Scalar> MathBase;
142 typedef typename MathBase::VectorXs VectorXs;
143 typedef ActuationDataAbstractTpl<Scalar> Base;
144
145 /**
146 * @brief Initialize the numdiff actuation data
147 *
148 * @tparam Model is the type of the `ActuationModelAbstractTpl`.
149 * @param model is the object to compute the numerical differentiation from.
150 */
151 template <template <typename Scalar> class Model>
152 ✗ explicit ActuationDataNumDiffTpl(Model<Scalar>* const model)
153 : Base(model),
154 ✗ dx(model->get_model()->get_state()->get_ndx()),
155 ✗ du(model->get_model()->get_nu()),
156 ✗ xp(model->get_model()->get_state()->get_nx()) {
157 ✗ dx.setZero();
158 ✗ du.setZero();
159 ✗ xp.setZero();
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 Scalar x_norm; //!< Norm of the state vector
172 Scalar
173 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
174 Scalar
175 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
176 VectorXs dx; //!< State disturbance
177 VectorXs du; //!< Control disturbance
178 VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$
179 //!< \int x dx_i \f$"
180 std::shared_ptr<Base> data_0; //!< The data that contains the final results
181 std::vector<std::shared_ptr<Base> >
182 data_x; //!< The temporary data associated with the state variation
183 std::vector<std::shared_ptr<Base> >
184 data_u; //!< The temporary data associated with the control variation
185
186 using Base::dtau_du;
187 using Base::dtau_dx;
188 using Base::tau;
189 };
190
191 } // namespace crocoddyl
192
193 /* --- Details -------------------------------------------------------------- */
194 /* --- Details -------------------------------------------------------------- */
195 /* --- Details -------------------------------------------------------------- */
196 #include "crocoddyl/core/numdiff/actuation.hxx"
197
198 #endif // CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
199