GCC Code Coverage Report

Directory:	./
File:	include/crocoddyl/core/integrator/euler.hpp
Date:	2025-06-03 08:14:12

	Total	Coverage
Lines:	13	0.0%
Functions:	12	0.0%
Branches:	46	0.0%

  
      Line
      Branch
      Exec
      Source
    
      ///////////////////////////////////////////////////////////////////////////////
    
      // BSD 3-Clause License
    
      //
    
      // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
    
      //                          University of Oxford, Heriot-Watt University
    
      // Copyright note valid unless otherwise stated in individual files.
    
      // All rights reserved.
    
      ///////////////////////////////////////////////////////////////////////////////
    
      #ifndef CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
    
      #define CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
    
      #include "crocoddyl/core/fwd.hpp"
    
      #include "crocoddyl/core/integ-action-base.hpp"
    
      namespace crocoddyl {
    
      /**
    
       * @brief Symplectic Euler integrator
    
       *
    
       * It applies a symplectic Euler integration scheme to a differential (i.e.,
    
       * continuous time) action model.
    
       *
    
       * This symplectic Euler scheme introduces also the possibility to parametrize
    
       * the control trajectory inside an integration step, for instance using
    
       * polynomials. This requires introducing some notation to clarify the
    
       * difference between the control inputs of the differential model and the
    
       * control inputs to the integrated model. We have decided to use
    
       * \f$\mathbf{w}\f$ to refer to the control inputs of the differential model and
    
       * \f$\mathbf{u}\f$ for the control inputs of the integrated action model. Note
    
       * that the zero-order (e.g., `ControlParametrizationModelPolyZeroTpl`) are the
    
       * only ones that make sense to use within this integrator.
    
       *
    
       * \sa `calc()`, `calcDiff()`, `createData()`
    
       */
    
      template <typename _Scalar>
    
      class IntegratedActionModelEulerTpl
    
          : public IntegratedActionModelAbstractTpl<_Scalar> {
    
       public:
    
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
      ✗
        CROCODDYL_DERIVED_CAST(ActionModelBase, IntegratedActionModelEulerTpl)
    
        typedef _Scalar Scalar;
    
        typedef MathBaseTpl<Scalar> MathBase;
    
        typedef IntegratedActionModelAbstractTpl<Scalar> Base;
    
        typedef IntegratedActionDataEulerTpl<Scalar> Data;
    
        typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
    
        typedef DifferentialActionModelAbstractTpl<Scalar>
    
            DifferentialActionModelAbstract;
    
        typedef ControlParametrizationModelAbstractTpl<Scalar>
    
            ControlParametrizationModelAbstract;
    
        typedef ControlParametrizationDataAbstractTpl<Scalar>
    
            ControlParametrizationDataAbstract;
    
        typedef typename MathBase::VectorXs VectorXs;
    
        typedef typename MathBase::MatrixXs MatrixXs;
    
        /**
    
         * @brief Initialize the symplectic Euler integrator
    
         *
    
         * @param[in] model               Differential action model
    
         * @param[in] control             Control parametrization
    
         * @param[in] time_step           Step time (default 1e-3)
    
         * @param[in] with_cost_residual  Compute cost residual (default true)
    
         */
    
        IntegratedActionModelEulerTpl(
    
            std::shared_ptr<DifferentialActionModelAbstract> model,
    
            std::shared_ptr<ControlParametrizationModelAbstract> control,
    
            const Scalar time_step = Scalar(1e-3),
    
            const bool with_cost_residual = true);
    
        /**
    
         * @brief Initialize the symplectic Euler integrator
    
         *
    
         * This initialization uses `ControlParametrizationPolyZeroTpl` for the
    
         * control parametrization.
    
         *
    
         * @param[in] model               Differential action model
    
         * @param[in] time_step           Step time (default 1e-3)
    
         * @param[in] with_cost_residual  Compute cost residual (default true)
    
         */
    
        IntegratedActionModelEulerTpl(
    
            std::shared_ptr<DifferentialActionModelAbstract> model,
    
            const Scalar time_step = Scalar(1e-3),
    
            const bool with_cost_residual = true);
    
      ✗
        virtual ~IntegratedActionModelEulerTpl() = default;
    
        /**
    
         * @brief Integrate the differential action model using symplectic Euler
    
         * scheme
    
         *
    
         * @param[in] data  Symplectic Euler data
    
         * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
    
         * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
    
         */
    
        virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
    
                          const Eigen::Ref<const VectorXs>& x,
    
                          const Eigen::Ref<const VectorXs>& u) override;
    
        /**
    
         * @brief Integrate the total cost value for nodes that depends only on the
    
         * state using symplectic Euler scheme
    
         *
    
         * It computes the total cost and defines the next state as the current one.
    
         * This function is used in the terminal nodes of an optimal control problem.
    
         *
    
         * @param[in] data  Symplectic Euler data
    
         * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
    
         */
    
        virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
    
                          const Eigen::Ref<const VectorXs>& x) override;
    
        /**
    
         * @brief Compute the partial derivatives of the symplectic Euler integrator
    
         *
    
         * @param[in] data  Symplectic Euler data
    
         * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
    
         * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
    
         */
    
        virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
    
                              const Eigen::Ref<const VectorXs>& x,
    
                              const Eigen::Ref<const VectorXs>& u) override;
    
        /**
    
         * @brief Compute the partial derivatives of the cost
    
         *
    
         * It updates the derivatives of the cost function with respect to the state
    
         * only. This function is used in the terminal nodes of an optimal control
    
         * problem.
    
         *
    
         * @param[in] data  Symplectic Euler data
    
         * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
    
         */
    
        virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
    
                              const Eigen::Ref<const VectorXs>& x) override;
    
        /**
    
         * @brief Create the symplectic Euler data
    
         *
    
         * @return the symplectic Euler data
    
         */
    
        virtual std::shared_ptr<ActionDataAbstract> createData() override;
    
        /**
    
         * @brief Cast the Euler integrated-action model to a different scalar type.
    
         *
    
         * It is useful for operations requiring different precision or scalar types.
    
         *
    
         * @tparam NewScalar The new scalar type to cast to.
    
         * @return IntegratedActionModelEulerTpl<NewScalar> An action model with the
    
         * new scalar type.
    
         */
    
        template <typename NewScalar>
    
        IntegratedActionModelEulerTpl<NewScalar> cast() const;
    
        /**
    
         * @brief Checks that a specific data belongs to this model
    
         */
    
        virtual bool checkData(
    
            const std::shared_ptr<ActionDataAbstract>& data) override;
    
        /**
    
         * @brief Computes the quasic static commands
    
         *
    
         * The quasic static commands are the ones produced for a the reference
    
         * posture as an equilibrium point, i.e. for
    
         * \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
    
         *
    
         * @param[in] data    Symplectic Euler data
    
         * @param[out] u      Quasic static commands
    
         * @param[in] x       State point (velocity has to be zero)
    
         * @param[in] maxiter Maximum allowed number of iterations
    
         * @param[in] tol     Tolerance
    
         */
    
        virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
    
                                 Eigen::Ref<VectorXs> u,
    
                                 const Eigen::Ref<const VectorXs>& x,
    
                                 const std::size_t maxiter = 100,
    
                                 const Scalar tol = Scalar(1e-9)) override;
    
        /**
    
         * @brief Print relevant information of the Euler integrator model
    
         *
    
         * @param[out] os  Output stream object
    
         */
    
        virtual void print(std::ostream& os) const override;
    
       protected:
    
        using Base::control_;       //!< Control parametrization
    
        using Base::differential_;  //!< Differential action model
    
        using Base::ng_;            //!< Number of inequality constraints
    
        using Base::nh_;            //!< Number of equality constraints
    
        using Base::nu_;            //!< Dimension of the control
    
        using Base::state_;         //!< Model of the state
    
        using Base::time_step2_;    //!< Square of the time step used for integration
    
        using Base::time_step_;     //!< Time step used for integration
    
        using Base::with_cost_residual_;  //!< Flag indicating whether a cost residual
    
                                          //!< is used
    
      };
    
      template <typename _Scalar>
    
      struct IntegratedActionDataEulerTpl
    
          : public IntegratedActionDataAbstractTpl<_Scalar> {
    
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
        typedef _Scalar Scalar;
    
        typedef MathBaseTpl<Scalar> MathBase;
    
        typedef IntegratedActionDataAbstractTpl<Scalar> Base;
    
        typedef DifferentialActionDataAbstractTpl<Scalar>
    
            DifferentialActionDataAbstract;
    
        typedef ControlParametrizationDataAbstractTpl<Scalar>
    
            ControlParametrizationDataAbstract;
    
        typedef typename MathBase::VectorXs VectorXs;
    
        typedef typename MathBase::MatrixXs MatrixXs;
    
        template <template <typename Scalar> class Model>
    
      ✗
        explicit IntegratedActionDataEulerTpl(Model<Scalar>* const model)
    
      ✗
            : Base(model) {
    
      ✗
          differential = model->get_differential()->createData();
    
      ✗
          control = model->get_control()->createData();
    
      ✗
          const std::size_t ndx = model->get_state()->get_ndx();
    
      ✗
          const std::size_t nv = model->get_state()->get_nv();
    
      ✗
          dx = VectorXs::Zero(ndx);
    
      ✗
          da_du = MatrixXs::Zero(nv, model->get_nu());
    
      ✗
          Lwu = MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu());
    
      ✗
        }
    
      ✗
        virtual ~IntegratedActionDataEulerTpl() = default;
    
        std::shared_ptr<DifferentialActionDataAbstract>
    
            differential;  //!< Differential model data
    
        std::shared_ptr<ControlParametrizationDataAbstract>
    
            control;  //!< Control parametrization data
    
        VectorXs dx;
    
        MatrixXs da_du;
    
        MatrixXs Lwu;  //!< Hessian of the cost function with respect to the control
    
                       //!< input (w) and control parameters (u)
    
        using Base::cost;
    
        using Base::Fu;
    
        using Base::Fx;
    
        using Base::Lu;
    
        using Base::Luu;
    
        using Base::Lx;
    
        using Base::Lxu;
    
        using Base::Lxx;
    
        using Base::r;
    
        using Base::xnext;
    
      };
    
      }  // namespace crocoddyl
    
      /* --- Details -------------------------------------------------------------- */
    
      /* --- Details -------------------------------------------------------------- */
    
      /* --- Details -------------------------------------------------------------- */
    
      #include "crocoddyl/core/integrator/euler.hxx"
    
      CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
    
          crocoddyl::IntegratedActionModelEulerTpl)
    
      CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
    
          crocoddyl::IntegratedActionDataEulerTpl)
    
      #endif  // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_

Line	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