GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/integrator/euler.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 12 0.0%
Branches: 0 44 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
11 #define CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
12
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/integ-action-base.hpp"
15
16 namespace crocoddyl {
17
18 /**
19 * @brief Symplectic Euler integrator
20 *
21 * It applies a symplectic Euler integration scheme to a differential (i.e.,
22 * continuous time) action model.
23 *
24 * This symplectic Euler scheme introduces also the possibility to parametrize
25 * the control trajectory inside an integration step, for instance using
26 * polynomials. This requires introducing some notation to clarify the
27 * difference between the control inputs of the differential model and the
28 * control inputs to the integrated model. We have decided to use
29 * \f$\mathbf{w}\f$ to refer to the control inputs of the differential model and
30 * \f$\mathbf{u}\f$ for the control inputs of the integrated action model. Note
31 * that the zero-order (e.g., `ControlParametrizationModelPolyZeroTpl`) are the
32 * only ones that make sense to use within this integrator.
33 *
34 * \sa `calc()`, `calcDiff()`, `createData()`
35 */
36 template <typename _Scalar>
37 class IntegratedActionModelEulerTpl
38 : public IntegratedActionModelAbstractTpl<_Scalar> {
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 ✗ CROCODDYL_DERIVED_CAST(ActionModelBase, IntegratedActionModelEulerTpl)
42
43 typedef _Scalar Scalar;
44 typedef MathBaseTpl<Scalar> MathBase;
45 typedef IntegratedActionModelAbstractTpl<Scalar> Base;
46 typedef IntegratedActionDataEulerTpl<Scalar> Data;
47 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
48 typedef DifferentialActionModelAbstractTpl<Scalar>
49 DifferentialActionModelAbstract;
50 typedef ControlParametrizationModelAbstractTpl<Scalar>
51 ControlParametrizationModelAbstract;
52 typedef ControlParametrizationDataAbstractTpl<Scalar>
53 ControlParametrizationDataAbstract;
54 typedef typename MathBase::VectorXs VectorXs;
55 typedef typename MathBase::MatrixXs MatrixXs;
56
57 /**
58 * @brief Initialize the symplectic Euler integrator
59 *
60 * @param[in] model Differential action model
61 * @param[in] control Control parametrization
62 * @param[in] time_step Step time (default 1e-3)
63 * @param[in] with_cost_residual Compute cost residual (default true)
64 */
65 IntegratedActionModelEulerTpl(
66 std::shared_ptr<DifferentialActionModelAbstract> model,
67 std::shared_ptr<ControlParametrizationModelAbstract> control,
68 const Scalar time_step = Scalar(1e-3),
69 const bool with_cost_residual = true);
70
71 /**
72 * @brief Initialize the symplectic Euler integrator
73 *
74 * This initialization uses `ControlParametrizationPolyZeroTpl` for the
75 * control parametrization.
76 *
77 * @param[in] model Differential action model
78 * @param[in] time_step Step time (default 1e-3)
79 * @param[in] with_cost_residual Compute cost residual (default true)
80 */
81 IntegratedActionModelEulerTpl(
82 std::shared_ptr<DifferentialActionModelAbstract> model,
83 const Scalar time_step = Scalar(1e-3),
84 const bool with_cost_residual = true);
85 ✗ virtual ~IntegratedActionModelEulerTpl() = default;
86
87 /**
88 * @brief Integrate the differential action model using symplectic Euler
89 * scheme
90 *
91 * @param[in] data Symplectic Euler data
92 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
93 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
94 */
95 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>& x,
97 const Eigen::Ref<const VectorXs>& u) override;
98
99 /**
100 * @brief Integrate the total cost value for nodes that depends only on the
101 * state using symplectic Euler scheme
102 *
103 * It computes the total cost and defines the next state as the current one.
104 * This function is used in the terminal nodes of an optimal control problem.
105 *
106 * @param[in] data Symplectic Euler data
107 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
108 */
109 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>& x) override;
111
112 /**
113 * @brief Compute the partial derivatives of the symplectic Euler integrator
114 *
115 * @param[in] data Symplectic Euler data
116 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
117 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
118 */
119 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
120 const Eigen::Ref<const VectorXs>& x,
121 const Eigen::Ref<const VectorXs>& u) override;
122
123 /**
124 * @brief Compute the partial derivatives of the cost
125 *
126 * It updates the derivatives of the cost function with respect to the state
127 * only. This function is used in the terminal nodes of an optimal control
128 * problem.
129 *
130 * @param[in] data Symplectic Euler data
131 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
132 */
133 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
134 const Eigen::Ref<const VectorXs>& x) override;
135
136 /**
137 * @brief Create the symplectic Euler data
138 *
139 * @return the symplectic Euler data
140 */
141 virtual std::shared_ptr<ActionDataAbstract> createData() override;
142
143 /**
144 * @brief Cast the Euler integrated-action model to a different scalar type.
145 *
146 * It is useful for operations requiring different precision or scalar types.
147 *
148 * @tparam NewScalar The new scalar type to cast to.
149 * @return IntegratedActionModelEulerTpl<NewScalar> An action model with the
150 * new scalar type.
151 */
152 template <typename NewScalar>
153 IntegratedActionModelEulerTpl<NewScalar> cast() const;
154
155 /**
156 * @brief Checks that a specific data belongs to this model
157 */
158 virtual bool checkData(
159 const std::shared_ptr<ActionDataAbstract>& data) override;
160
161 /**
162 * @brief Computes the quasic static commands
163 *
164 * The quasic static commands are the ones produced for a the reference
165 * posture as an equilibrium point, i.e. for
166 * \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
167 *
168 * @param[in] data Symplectic Euler data
169 * @param[out] u Quasic static commands
170 * @param[in] x State point (velocity has to be zero)
171 * @param[in] maxiter Maximum allowed number of iterations
172 * @param[in] tol Tolerance
173 */
174 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
175 Eigen::Ref<VectorXs> u,
176 const Eigen::Ref<const VectorXs>& x,
177 const std::size_t maxiter = 100,
178 const Scalar tol = Scalar(1e-9)) override;
179
180 /**
181 * @brief Print relevant information of the Euler integrator model
182 *
183 * @param[out] os Output stream object
184 */
185 virtual void print(std::ostream& os) const override;
186
187 protected:
188 using Base::control_; //!< Control parametrization
189 using Base::differential_; //!< Differential action model
190 using Base::ng_; //!< Number of inequality constraints
191 using Base::nh_; //!< Number of equality constraints
192 using Base::nu_; //!< Dimension of the control
193 using Base::state_; //!< Model of the state
194 using Base::time_step2_; //!< Square of the time step used for integration
195 using Base::time_step_; //!< Time step used for integration
196 using Base::with_cost_residual_; //!< Flag indicating whether a cost residual
197 //!< is used
198 };
199
200 template <typename _Scalar>
201 struct IntegratedActionDataEulerTpl
202 : public IntegratedActionDataAbstractTpl<_Scalar> {
203 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204
205 typedef _Scalar Scalar;
206 typedef MathBaseTpl<Scalar> MathBase;
207 typedef IntegratedActionDataAbstractTpl<Scalar> Base;
208 typedef DifferentialActionDataAbstractTpl<Scalar>
209 DifferentialActionDataAbstract;
210 typedef ControlParametrizationDataAbstractTpl<Scalar>
211 ControlParametrizationDataAbstract;
212 typedef typename MathBase::VectorXs VectorXs;
213 typedef typename MathBase::MatrixXs MatrixXs;
214
215 template <template <typename Scalar> class Model>
216 ✗ explicit IntegratedActionDataEulerTpl(Model<Scalar>* const model)
217 ✗ : Base(model) {
218 ✗ differential = model->get_differential()->createData();
219 ✗ control = model->get_control()->createData();
220 ✗ const std::size_t ndx = model->get_state()->get_ndx();
221 ✗ const std::size_t nv = model->get_state()->get_nv();
222 ✗ dx = VectorXs::Zero(ndx);
223 ✗ da_du = MatrixXs::Zero(nv, model->get_nu());
224 ✗ Lwu = MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu());
225 }
226 ✗ virtual ~IntegratedActionDataEulerTpl() = default;
227
228 std::shared_ptr<DifferentialActionDataAbstract>
229 differential; //!< Differential model data
230 std::shared_ptr<ControlParametrizationDataAbstract>
231 control; //!< Control parametrization data
232 VectorXs dx;
233 MatrixXs da_du;
234 MatrixXs Lwu; //!< Hessian of the cost function with respect to the control
235 //!< input (w) and control parameters (u)
236
237 using Base::cost;
238 using Base::Fu;
239 using Base::Fx;
240 using Base::Lu;
241 using Base::Luu;
242 using Base::Lx;
243 using Base::Lxu;
244 using Base::Lxx;
245 using Base::r;
246 using Base::xnext;
247 };
248
249 } // namespace crocoddyl
250
251 /* --- Details -------------------------------------------------------------- */
252 /* --- Details -------------------------------------------------------------- */
253 /* --- Details -------------------------------------------------------------- */
254 #include "crocoddyl/core/integrator/euler.hxx"
255
256 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
257 crocoddyl::IntegratedActionModelEulerTpl)
258 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
259 crocoddyl::IntegratedActionDataEulerTpl)
260
261 #endif // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
262