GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/control.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 10 0.0%
Branches: 0 16 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_CONTROL_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
12
13 #include "crocoddyl/core/control-base.hpp"
14 #include "crocoddyl/core/fwd.hpp"
15
16 namespace crocoddyl {
17
18 template <typename _Scalar>
19 class ControlParametrizationModelNumDiffTpl
20 : public ControlParametrizationModelAbstractTpl<_Scalar> {
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 ✗ CROCODDYL_DERIVED_CAST(ControlParametrizationModelBase,
24 ControlParametrizationModelNumDiffTpl)
25
26 typedef _Scalar Scalar;
27 typedef MathBaseTpl<Scalar> MathBase;
28 typedef ControlParametrizationModelAbstractTpl<_Scalar> Base;
29 typedef ControlParametrizationDataNumDiffTpl<_Scalar> Data;
30 typedef ControlParametrizationDataAbstractTpl<_Scalar>
31 ControlParametrizationDataAbstract;
32 typedef typename MathBase::VectorXs VectorXs;
33 typedef typename MathBase::MatrixXs MatrixXs;
34
35 /**
36 * @brief Construct a new ControlParametrizationModelNumDiff object
37 *
38 * @param model
39 */
40 explicit ControlParametrizationModelNumDiffTpl(std::shared_ptr<Base> model);
41 ✗ virtual ~ControlParametrizationModelNumDiffTpl() = default;
42
43 /**
44 * @brief Get the value of the control at the specified time
45 *
46 * @param[in] data Control-parametrization numdiff data
47 * @param[in] t Time in [0,1]
48 * @param[in] u Control parameters
49 */
50 void calc(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
51 const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
52
53 /**
54 * @brief Get the value of the Jacobian of the control with respect to the
55 * parameters
56 *
57 * @param[in] data Control-parametrization numdiff data
58 * @param[in] t Time in [0,1]
59 * @param[in] u Control parameters
60 */
61 void calcDiff(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
62 const Scalar t,
63 const Eigen::Ref<const VectorXs>& u) const override;
64
65 /**
66 * @brief Create the control-parametrization data
67 *
68 * @return the control-parametrization data
69 */
70 virtual std::shared_ptr<ControlParametrizationDataAbstract> createData()
71 override;
72
73 /**
74 * @brief Get a value of the control parameters such that the control at the
75 * specified time t is equal to the specified value u
76 *
77 * @param[in] data Control-parametrization numdiff data
78 * @param[in] t Time in [0,1]
79 * @param[in] w Control values
80 */
81 void params(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
82 const Scalar t,
83 const Eigen::Ref<const VectorXs>& w) const override;
84
85 /**
86 * @brief Convert the bounds on the control to bounds on the control
87 * parameters
88 *
89 * @param[in] w_lb Control lower bound
90 * @param[in] w_ub Control upper bound
91 * @param[in] u_lb Control parameter lower bound
92 * @param[in] u_ub Control parameter upper bound
93 */
94 void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
95 const Eigen::Ref<const VectorXs>& w_ub,
96 Eigen::Ref<VectorXs> u_lb,
97 Eigen::Ref<VectorXs> u_ub) const override;
98
99 /**
100 * @brief Compute the product between a specified matrix and the Jacobian of
101 * the control (with respect to the parameters)
102 *
103 * @param[in] data Control-parametrization numdiff data
104 * @param[in] A A matrix to multiply times the Jacobian
105 * @param[out] out Product between the matrix A and the Jacobian of the
106 * control with respect to the parameters
107 * @param[in] op Assignment operator which sets, adds, or removes the
108 * given results
109 */
110 void multiplyByJacobian(
111 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
112 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
113 const AssignmentOp = setto) const override;
114
115 /**
116 * @brief Compute the product between the transposed Jacobian of the control
117 * (with respect to the parameters) and a specified matrix
118 *
119 * @param[in] data Control-parametrization numdiff data
120 * @param[in] A A matrix to multiply times the Jacobian
121 * @param[out] out Product between the transposed Jacobian of the control
122 * with respect to the parameters and the matrix A
123 * @param[in] op Assignment operator which sets, adds, or removes the
124 * given results
125 */
126 void multiplyJacobianTransposeBy(
127 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
128 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
129 const AssignmentOp = setto) const override;
130
131 template <typename NewScalar>
132 ControlParametrizationModelNumDiffTpl<NewScalar> cast() const;
133 /**
134 * @brief Get the model_ object
135 *
136 * @return Base&
137 */
138 const std::shared_ptr<Base>& get_model() const;
139
140 /**
141 * @brief Return the disturbance constant used in the numerical
142 * differentiation routine
143 */
144 const Scalar get_disturbance() const;
145
146 /**
147 * @brief Modify the disturbance constant used in the numerical
148 * differentiation routine
149 */
150 void set_disturbance(const Scalar disturbance);
151
152 private:
153 std::shared_ptr<Base>
154 model_; //!< model we need to compute the numerical differentiation
155 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
156 //!< calculation
157
158 protected:
159 using Base::nu_;
160 using Base::nw_;
161 };
162
163 template <typename _Scalar>
164 struct ControlParametrizationDataNumDiffTpl
165 : public ControlParametrizationDataAbstractTpl<_Scalar> {
166 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167
168 typedef _Scalar Scalar;
169 typedef MathBaseTpl<Scalar> MathBase;
170 typedef typename MathBase::VectorXs VectorXs;
171 typedef typename MathBase::MatrixXs MatrixXs;
172 typedef ControlParametrizationDataAbstractTpl<Scalar> Base;
173
174 template <template <typename Scalar> class Model>
175 ✗ explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model)
176 ✗ : Base(model), du(model->get_nu()) {
177 ✗ du.setZero();
178
179 ✗ const std::size_t nu = model->get_model()->get_nu();
180 ✗ data_0 = model->get_model()->createData();
181 ✗ for (std::size_t i = 0; i < nu; ++i) {
182 ✗ data_u.push_back(model->get_model()->createData());
183 }
184 }
185
186 ✗ virtual ~ControlParametrizationDataNumDiffTpl() {}
187
188 VectorXs du; //!< temporary variable used for finite differencing
189 std::shared_ptr<Base> data_0; //!< The data that contains the final results
190 std::vector<std::shared_ptr<Base> >
191 data_u; //!< The temporary data associated with the control variation
192 };
193
194 } // namespace crocoddyl
195
196 /* --- Details -------------------------------------------------------------- */
197 /* --- Details -------------------------------------------------------------- */
198 /* --- Details -------------------------------------------------------------- */
199 #include "crocoddyl/core/numdiff/control.hxx"
200
201 #endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
202