GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/costs/residual.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 4 5 80.0%
Branches: 0 2 0.0%
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 14074 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 477441 CostDataResidualTpl(Model<Scalar>* const model,
170 DataCollectorAbstract* const data)
171 477441 : Base(model, data) {}
172 949483 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