GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/control-base.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 12 0.0%
Branches: 0 22 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, University of Trento,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_CORE_CONTROL_BASE_HPP_
11 #define CROCODDYL_CORE_CONTROL_BASE_HPP_
12
13 #include "crocoddyl/core/fwd.hpp"
14
15 namespace crocoddyl {
16
17 class ControlParametrizationModelBase {
18 public:
19 ✗ virtual ~ControlParametrizationModelBase() = default;
20
21 ✗ CROCODDYL_BASE_CAST(ControlParametrizationModelBase,
22 ControlParametrizationModelAbstractTpl)
23 };
24
25 /**
26 * @brief Abstract class for the control trajectory parametrization
27 *
28 * The control trajectory is a function of the (normalized) time.
29 * Normalized time is between 0 and 1, where 0 represents the beginning of the
30 * time step, and 1 represents its end. The trajectory depends on the control
31 * parameters u, whose size may be larger than the size of the control inputs w.
32 *
33 * The main computations are carried out in `calc`, `multiplyByJacobian` and
34 * `multiplyJacobianTransposeBy`, where the former computes control input
35 * \f$\mathbf{w}\in\mathbb{R}^{nw}\f$ from a set of control parameters
36 * \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ where `nw` and `nu` represent the
37 * dimension of the control inputs and parameters, respectively, and the latter
38 * defines useful operations across the Jacobian of the control-parametrization
39 * model. Finally, `params` allows us to obtain the control parameters from a
40 * the control input, i.e., it is the dual of `calc`. Note that
41 * `multiplyByJacobian` and `multiplyJacobianTransposeBy` requires to run `calc`
42 * first.
43 *
44 * \sa `calc()`, `calcDiff()`, `createData()`, `params`, `multiplyByJacobian`,
45 * `multiplyJacobianTransposeBy`
46 */
47 template <typename _Scalar>
48 class ControlParametrizationModelAbstractTpl
49 : public ControlParametrizationModelBase {
50 public:
51 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52
53 typedef _Scalar Scalar;
54 typedef MathBaseTpl<Scalar> MathBase;
55 typedef ControlParametrizationDataAbstractTpl<Scalar>
56 ControlParametrizationDataAbstract;
57 typedef typename MathBase::VectorXs VectorXs;
58 typedef typename MathBase::MatrixXs MatrixXs;
59
60 /**
61 * @brief Initialize the control dimensions
62 *
63 * @param[in] nw Dimension of control inputs
64 * @param[in] nu Dimension of control parameters
65 */
66 ControlParametrizationModelAbstractTpl(const std::size_t nw,
67 const std::size_t nu);
68 ✗ virtual ~ControlParametrizationModelAbstractTpl() = default;
69
70 /**
71 * @brief Get the value of the control at the specified time
72 *
73 * @param[in] data Data structure containing the control vector to write
74 * @param[in] t Time in [0,1]
75 * @param[in] u Control parameters
76 */
77 virtual void calc(
78 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
79 const Scalar t, const Eigen::Ref<const VectorXs>& u) const = 0;
80
81 /**
82 * @brief Get the value of the Jacobian of the control with respect to the
83 * parameters
84 *
85 * It assumes that `calc()` has been run first
86 *
87 * @param[in] data Control-parametrization data
88 * @param[in] t Time in [0,1]
89 * @param[in] u Control parameters
90 */
91 virtual void calcDiff(
92 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
93 const Scalar t, const Eigen::Ref<const VectorXs>& u) const = 0;
94
95 /**
96 * @brief Create the control-parametrization data
97 *
98 * @return the control-parametrization data
99 */
100 virtual std::shared_ptr<ControlParametrizationDataAbstract> createData();
101
102 /**
103 * @brief Update the control parameters u for a specified time t given the
104 * control input w
105 *
106 * @param[in] data Control-parametrization data
107 * @param[in] t Time in [0,1]
108 * @param[in] w Control inputs
109 */
110 virtual void params(
111 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
112 const Scalar t, const Eigen::Ref<const VectorXs>& w) const = 0;
113
114 /**
115 * @brief Convert the bounds on the control inputs w to bounds on the control
116 * parameters u
117 *
118 * @param[in] w_lb Control lower bound
119 * @param[in] w_ub Control lower bound
120 * @param[out] u_lb Control parameters lower bound
121 * @param[out] u_ub Control parameters upper bound
122 */
123 virtual void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
124 const Eigen::Ref<const VectorXs>& w_ub,
125 Eigen::Ref<VectorXs> u_lb,
126 Eigen::Ref<VectorXs> u_ub) const = 0;
127
128 /**
129 * @brief Compute the product between the given matrix A and the derivative of
130 * the control input with respect to the control parameters (i.e., A*dw_du).
131 *
132 * It assumes that `calc()` has been run first
133 *
134 * @param[in] data Control-parametrization data
135 * @param[in] A A matrix to multiply times the Jacobian
136 * @param[out] out Product between the matrix A and the Jacobian of the
137 * control with respect to the parameters
138 * @param[in] op Assignment operator which sets, adds, or removes the
139 * given results
140 */
141 virtual void multiplyByJacobian(
142 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
143 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
144 const AssignmentOp op = setto) const = 0;
145
146 virtual MatrixXs multiplyByJacobian_J(
147 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
148 const Eigen::Ref<const MatrixXs>& A, const AssignmentOp op = setto) const;
149
150 /**
151 * @brief Compute the product between the transpose of the derivative of the
152 * control input with respect to the control parameters and a given matrix A
153 * (i.e., dw_du^T*A)
154 *
155 * It assumes that `calc()` has been run first
156 *
157 * @param[in] data Control-parametrization data
158 * @param[in] A A matrix to multiply times the Jacobian
159 * @param[out] out Product between the transposed Jacobian of the control
160 * with respect to the parameters and the matrix A
161 * @param[in] op Assignment operator which sets, adds, or removes the
162 * given results
163 */
164 virtual void multiplyJacobianTransposeBy(
165 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
166 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
167 const AssignmentOp op = setto) const = 0;
168
169 virtual MatrixXs multiplyJacobianTransposeBy_J(
170 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
171 const Eigen::Ref<const MatrixXs>& A, const AssignmentOp op = setto) const;
172
173 /**
174 * @brief Checks that a specific data belongs to this model
175 */
176 virtual bool checkData(
177 const std::shared_ptr<ControlParametrizationDataAbstract>& data);
178
179 /**
180 * @brief Print information on the control model
181 */
182 template <class Scalar>
183 friend std::ostream& operator<<(
184 std::ostream& os,
185 const ControlParametrizationModelAbstractTpl<Scalar>& model);
186
187 /**
188 * @brief Print relevant information of the control model
189 *
190 * @param[out] os Output stream object
191 */
192 virtual void print(std::ostream& os) const;
193
194 /**
195 * @brief Return the dimension of the control inputs
196 */
197 std::size_t get_nw() const;
198
199 /**
200 * @brief Return the dimension of control parameters
201 */
202 std::size_t get_nu() const;
203
204 protected:
205 std::size_t nw_; //!< Control dimension
206 std::size_t nu_; //!< Control parameters dimension
207 ✗ ControlParametrizationModelAbstractTpl() : nw_(0), nu_(0) {};
208 };
209
210 template <typename _Scalar>
211 struct ControlParametrizationDataAbstractTpl {
212 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
213
214 typedef _Scalar Scalar;
215 typedef MathBaseTpl<Scalar> MathBase;
216 typedef typename MathBase::VectorXs VectorXs;
217 typedef typename MathBase::MatrixXs MatrixXs;
218
219 template <template <typename Scalar> class Model>
220 ✗ explicit ControlParametrizationDataAbstractTpl(Model<Scalar>* const model)
221 ✗ : w(model->get_nw()),
222 ✗ u(model->get_nu()),
223 ✗ dw_du(model->get_nw(), model->get_nu()) {
224 ✗ w.setZero();
225 ✗ u.setZero();
226 ✗ dw_du.setZero();
227 }
228 ✗ virtual ~ControlParametrizationDataAbstractTpl() = default;
229
230 VectorXs w; //!< value of the differential control
231 VectorXs u; //!< value of the control parameters
232 MatrixXs dw_du; //!< Jacobian of the differential control with respect to the
233 //!< parameters
234 };
235
236 } // namespace crocoddyl
237
238 /* --- Details -------------------------------------------------------------- */
239 /* --- Details -------------------------------------------------------------- */
240 /* --- Details -------------------------------------------------------------- */
241 #include "crocoddyl/core/control-base.hxx"
242
243 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
244 crocoddyl::ControlParametrizationModelAbstractTpl)
245 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
246 crocoddyl::ControlParametrizationDataAbstractTpl)
247
248 #endif // CROCODDYL_CORE_CONTROL_BASE_HPP_
249