GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/controls/poly-two-rk.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 6 0.0%
Branches: 0 4 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_CONTROLS_POLY_TWO_RK_HPP_
11 #define CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
12
13 #include "crocoddyl/core/control-base.hpp"
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/integrator/rk.hpp"
16
17 namespace crocoddyl {
18
19 /**
20 * @brief A polynomial function of time of degree two, that is a quadratic
21 * function
22 *
23 * The size of the parameters \f$\mathbf{u}\f$ is 3 times the size of the
24 * control input \f$\mathbf{w}\f$. It defines a polynomial of degree two,
25 * customized for the RK4 and RK4 integrators (even though it can be used with
26 * whatever integration scheme). The first third of \f$\mathbf{u}\f$ represents
27 * the value of \f$\mathbf{w}\f$ at time 0. The second third of \f$\mathbf{u}\f$
28 * represents the value of \f$\mathbf{w}\f$ at time 0.5 or 1/3 for RK4 and RK3
29 * parametrization, respectively. The last third of \f$\mathbf{u}\f$ represents
30 * the value of \f$\mathbf{w}\f$ at time 1 or 2/3 for the RK4 and RK3
31 * parametrization, respectively. This parametrization is suitable to be used
32 * with the RK-4 or RK-3 integration schemes, because they require the value of
33 * \f$\mathbf{w}\f$ exactly at 0, 0.5, 1 (for RK4) or 0, 1/3, 2/3 (for RK3).
34 *
35 * The main computations are carried out in `calc`, `multiplyByJacobian` and
36 * `multiplyJacobianTransposeBy`, where the former computes control input
37 * \f$\mathbf{w}\in\mathbb{R}^{nw}\f$ from a set of control parameters
38 * \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ where `nw` and `nu` represent the
39 * dimension of the control inputs and parameters, respectively, and the latter
40 * defines useful operations across the Jacobian of the control-parametrization
41 * model. Finally, `params` allows us to obtain the control parameters from a
42 * the control input, i.e., it is the inverse of `calc`. Note that
43 * `multiplyByJacobian` and `multiplyJacobianTransposeBy` requires to run `calc`
44 * first.
45 *
46 * \sa `ControlParametrizationAbstractTpl`, `calc()`, `calcDiff()`,
47 * `createData()`, `params`, `multiplyByJacobian`, `multiplyJacobianTransposeBy`
48 */
49 template <typename _Scalar>
50 class ControlParametrizationModelPolyTwoRKTpl
51 : public ControlParametrizationModelAbstractTpl<_Scalar> {
52 public:
53 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 ✗ CROCODDYL_DERIVED_CAST(ControlParametrizationModelBase,
55 ControlParametrizationModelPolyTwoRKTpl)
56
57 typedef _Scalar Scalar;
58 typedef MathBaseTpl<Scalar> MathBase;
59 typedef ControlParametrizationDataAbstractTpl<Scalar>
60 ControlParametrizationDataAbstract;
61 typedef ControlParametrizationModelAbstractTpl<Scalar> Base;
62 typedef ControlParametrizationDataPolyTwoRKTpl<Scalar> Data;
63 typedef typename MathBase::VectorXs VectorXs;
64 typedef typename MathBase::MatrixXs MatrixXs;
65
66 /**
67 * @brief Initialize the poly-two RK control parametrization
68 *
69 * @param[in] nw Dimension of control vector
70 * @param[in] rktype Type of RK parametrization
71 */
72 explicit ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw,
73 const RKType rktype);
74 ✗ virtual ~ControlParametrizationModelPolyTwoRKTpl() = default;
75
76 /**
77 * @brief Get the value of the control at the specified time
78 *
79 * @param[in] data Poly-two-RK data
80 * @param[in] t Time in [0,1]
81 * @param[in] u Control parameters
82 */
83 virtual void calc(
84 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
85 const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
86
87 /**
88 * @brief Get the value of the Jacobian of the control with respect to the
89 * parameters
90 *
91 * It assumes that `calc()` has been run first
92 *
93 * @param[in] data Poly-two-RK data
94 * @param[in] t Time in [0,1]
95 * @param[in] u Control parameters
96 */
97 virtual void calcDiff(
98 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
99 const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
100
101 /**
102 * @brief Create the control-parametrization data
103 *
104 * @return the control-parametrization data
105 */
106 virtual std::shared_ptr<ControlParametrizationDataAbstract> createData()
107 override;
108
109 /**
110 * @brief Get a value of the control parameters u such that the control at the
111 * specified time t is equal to the specified value w
112 *
113 * @param[in] data Poly-two-RK data
114 * @param[in] t Time in [0,1]
115 * @param[in] w Control values
116 */
117 virtual void params(
118 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
119 const Scalar t, const Eigen::Ref<const VectorXs>& w) const override;
120
121 /**
122 * @brief Map the specified bounds from the control space to the parameter
123 * space
124 *
125 * @param[in] w_lb Control lower bound
126 * @param[in] w_ub Control lower bound
127 * @param[out] u_lb Control parameters lower bound
128 * @param[out] u_ub Control parameters upper bound
129 */
130 virtual void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
131 const Eigen::Ref<const VectorXs>& w_ub,
132 Eigen::Ref<VectorXs> u_lb,
133 Eigen::Ref<VectorXs> u_ub) const override;
134
135 /**
136 * @brief Compute the product between a specified matrix and the Jacobian of
137 * the control (with respect to the parameters)
138 *
139 * It assumes that `calc()` has been run first
140 *
141 * @param[in] data Poly-two-RK data
142 * @param[in] A A matrix to multiply times the Jacobian
143 * @param[out] out Product between the matrix A and the Jacobian of the
144 * control with respect to the parameters
145 * @param[in] op Assignment operator which sets, adds, or removes the
146 * given results
147 */
148 virtual void multiplyByJacobian(
149 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
150 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
151 const AssignmentOp = setto) const override;
152
153 /**
154 * @brief Compute the product between the transposed Jacobian of the control
155 * (with respect to the parameters) and a specified matrix
156 *
157 * It assumes that `calc()` has been run first
158 *
159 * @param[in] data Poly-two-RK data
160 * @param[in] A A matrix to multiply times the Jacobian
161 * @param[out] out Product between the transposed Jacobian of the control
162 * with respect to the parameters and the matrix A
163 * @param[in] op Assignment operator which sets, adds, or removes the
164 * given results
165 */
166 virtual void multiplyJacobianTransposeBy(
167 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
168 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
169 const AssignmentOp = setto) const override;
170
171 template <typename NewScalar>
172 ControlParametrizationModelPolyTwoRKTpl<NewScalar> cast() const;
173
174 virtual void print(std::ostream& os) const override;
175
176 protected:
177 using Base::nu_;
178 using Base::nw_;
179
180 private:
181 RKType rktype_;
182 };
183
184 template <typename _Scalar>
185 struct ControlParametrizationDataPolyTwoRKTpl
186 : public ControlParametrizationDataAbstractTpl<_Scalar> {
187 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188
189 typedef _Scalar Scalar;
190 typedef MathBaseTpl<Scalar> MathBase;
191 typedef ControlParametrizationDataAbstractTpl<Scalar> Base;
192 typedef typename MathBase::Vector3s Vector3s;
193
194 template <template <typename Scalar> class Model>
195 ✗ explicit ControlParametrizationDataPolyTwoRKTpl(Model<Scalar>* const model)
196 ✗ : Base(model), tmp_t2(0.) {
197 ✗ c.setZero();
198 }
199 ✗ virtual ~ControlParametrizationDataPolyTwoRKTpl() = default;
200
201 Vector3s c; //!< Polynomial coefficients of the second-order control model
202 //!< that depends on time
203 Scalar tmp_t2; //!< Temporary variable to store the square of the time
204 };
205
206 } // namespace crocoddyl
207
208 /* --- Details -------------------------------------------------------------- */
209 /* --- Details -------------------------------------------------------------- */
210 /* --- Details -------------------------------------------------------------- */
211 #include "crocoddyl/core/controls/poly-two-rk.hxx"
212
213 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
214 crocoddyl::ControlParametrizationModelPolyTwoRKTpl)
215 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
216 crocoddyl::ControlParametrizationDataPolyTwoRKTpl)
217
218 #endif // CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
219