GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_CORE_COSTS_RESIDUAL_COST_HPP_
10			#define CROCODDYL_CORE_COSTS_RESIDUAL_COST_HPP_
11
12			#include "crocoddyl/core/cost-base.hpp"
13			#include "crocoddyl/core/fwd.hpp"
14			#include "crocoddyl/core/residual-base.hpp"
15
16			namespace crocoddyl {
17
18			/**
19			* @brief Residual-based cost
20			*
21			* This cost function uses a residual model to compute the cost, i.e., \f[ cost
22			* = a(\mathbf{r}(\mathbf{x}, \mathbf{u})), \f] where \f$\mathbf{r}(\cdot)\f$
23			* and \f$a(\cdot)\f$ define the residual and activation functions,
24			* respectively.
25			*
26			* Note that we only compute the Jacobians of the residual function. Therefore,
27			* this cost model computes its Hessians through a Gauss-Newton approximation,
28			* e.g., \f$\mathbf{l_{xu}} = \mathbf{R_x}^T \mathbf{A_{rr}} \mathbf{R_u} \f$,
29			* where \f$\mathbf{R_x}\f$ and \f$\mathbf{R_u}\f$ are the Jacobians of the
30			* residual function, and \f$\mathbf{A_{rr}}\f$ is the Hessian of the activation
31			* model.
32			*
33			* As described in `CostModelAbstractTpl()`, the cost value and its derivatives
34			* are calculated by `calc` and `calcDiff`, respectively.
35			*
36			* \sa `CostModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
37			*/
38			template <typename _Scalar>
39			class CostModelResidualTpl : public CostModelAbstractTpl<_Scalar> {
40			public:
41			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42		✗	CROCODDYL_DERIVED_CAST(CostModelBase, CostModelResidualTpl)
43
44			typedef _Scalar Scalar;
45			typedef MathBaseTpl<Scalar> MathBase;
46			typedef CostModelAbstractTpl<Scalar> Base;
47			typedef CostDataResidualTpl<Scalar> Data;
48			typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
49			typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
50			typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
51			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
52			typedef typename MathBase::VectorXs VectorXs;
53			typedef typename MathBase::MatrixXs MatrixXs;
54
55			/**
56			* @brief Initialize the residual cost model
57			*
58			* @param[in] state State of the multibody system
59			* @param[in] activation Activation model
60			* @param[in] residual Residual model
61			*/
62			CostModelResidualTpl(std::shared_ptr<typename Base::StateAbstract> state,
63			std::shared_ptr<ActivationModelAbstract> activation,
64			std::shared_ptr<ResidualModelAbstract> residual);
65			/**
66			* @brief Initialize the residual cost model
67			*
68			* We use `ActivationModelQuadTpl` as a default activation model (i.e.
69			* \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$).
70			*
71			* @param[in] state State of the multibody system
72			* @param[in] residual Residual model
73			*/
74			CostModelResidualTpl(std::shared_ptr<typename Base::StateAbstract> state,
75			std::shared_ptr<ResidualModelAbstract> residual);
76		✗	virtual ~CostModelResidualTpl() = default;
77
78			/**
79			* @brief Compute the residual cost
80			*
81			* @param[in] data Residual cost data
82			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
83			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
84			*/
85			virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
86			const Eigen::Ref<const VectorXs>& x,
87			const Eigen::Ref<const VectorXs>& u) override;
88
89			/**
90			* @brief Compute the residual cost based on state only
91			*
92			* It updates the total cost based on the state only. This function is used in
93			* the terminal nodes of an optimal control problem.
94			*
95			* @param[in] data Residual cost data
96			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
97			*/
98			virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
99			const Eigen::Ref<const VectorXs>& x) override;
100
101			/**
102			* @brief Compute the derivatives of the residual cost
103			*
104			* @param[in] data Residual cost data
105			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
106			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
107			*/
108			virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
109			const Eigen::Ref<const VectorXs>& x,
110			const Eigen::Ref<const VectorXs>& u) override;
111
112			/**
113			* @brief Compute the derivatives of the residual cost with respect to the
114			* state only
115			*
116			* It updates the Jacobian and Hessian of the cost function based on the state
117			* only. This function is used in the terminal nodes of an optimal control
118			* problem.
119			*
120			* @param[in] data Residual cost data
121			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
122			*/
123			virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
124			const Eigen::Ref<const VectorXs>& x) override;
125
126			/**
127			* @brief Create the residual cost data
128			*/
129			virtual std::shared_ptr<CostDataAbstract> createData(
130			DataCollectorAbstract* const data) override;
131
132			/**
133			* @brief Cast the residual cost model to a different scalar type.
134			*
135			* It is useful for operations requiring different precision or scalar types.
136			*
137			* @tparam NewScalar The new scalar type to cast to.
138			* @return CostModelResidualTpl<NewScalar> A cost model with the
139			* new scalar type.
140			*/
141			template <typename NewScalar>
142			CostModelResidualTpl<NewScalar> cast() const;
143
144			/**
145			* @brief Print relevant information of the cost-residual model
146			*
147			* @param[out] os Output stream object
148			*/
149			virtual void print(std::ostream& os) const override;
150
151			protected:
152			using Base::activation_;
153			using Base::nu_;
154			using Base::residual_;
155			using Base::state_;
156			using Base::unone_;
157			};
158
159			template <typename _Scalar>
160			struct CostDataResidualTpl : public CostDataAbstractTpl<_Scalar> {
161			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
162
163			typedef _Scalar Scalar;
164			typedef MathBaseTpl<Scalar> MathBase;
165			typedef CostDataAbstractTpl<Scalar> Base;
166			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
167
168			template <template <typename Scalar> class Model>
169		✗	CostDataResidualTpl(Model<Scalar>* const model,
170			DataCollectorAbstract* const data)
171		✗	: Base(model, data) {}
172		✗	virtual ~CostDataResidualTpl() = default;
173
174			using Base::activation;
175			using Base::cost;
176			using Base::Lu;
177			using Base::Luu;
178			using Base::Lx;
179			using Base::Lxu;
180			using Base::Lxx;
181			using Base::residual;
182			using Base::shared;
183			};
184
185			} // namespace crocoddyl
186
187			/* --- Details -------------------------------------------------------------- */
188			/* --- Details -------------------------------------------------------------- */
189			/* --- Details -------------------------------------------------------------- */
190			#include "crocoddyl/core/costs/residual.hxx"
191
192			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CostModelResidualTpl)
193			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostDataResidualTpl)
194
195			#endif // CROCODDYL_CORE_COSTS_RESIDUAL_COST_HPP_
196