GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/costs/residual.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 2 2 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2023, 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
43 typedef _Scalar Scalar;
44 typedef MathBaseTpl<Scalar> MathBase;
45 typedef CostModelAbstractTpl<Scalar> Base;
46 typedef CostDataResidualTpl<Scalar> Data;
47 typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
48 typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
49 typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
50 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::MatrixXs MatrixXs;
53
54 /**
55 * @brief Initialize the residual cost model
56 *
57 * @param[in] state State of the multibody system
58 * @param[in] activation Activation model
59 * @param[in] residual Residual model
60 */
61 CostModelResidualTpl(boost::shared_ptr<typename Base::StateAbstract> state,
62 boost::shared_ptr<ActivationModelAbstract> activation,
63 boost::shared_ptr<ResidualModelAbstract> residual);
64 /**
65 * @brief Initialize the residual cost model
66 *
67 * We use `ActivationModelQuadTpl` as a default activation model (i.e.
68 * \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$).
69 *
70 * @param[in] state State of the multibody system
71 * @param[in] residual Residual model
72 */
73 CostModelResidualTpl(boost::shared_ptr<typename Base::StateAbstract> state,
74 boost::shared_ptr<ResidualModelAbstract> residual);
75 virtual ~CostModelResidualTpl();
76
77 /**
78 * @brief Compute the residual cost
79 *
80 * @param[in] data Residual cost data
81 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
82 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
83 */
84 virtual void calc(const boost::shared_ptr<CostDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x,
86 const Eigen::Ref<const VectorXs>& u);
87
88 /**
89 * @brief Compute the residual cost based on state only
90 *
91 * It updates the total cost based on the state only. This function is used in
92 * the terminal nodes of an optimal control problem.
93 *
94 * @param[in] data Residual cost data
95 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
96 */
97 virtual void calc(const boost::shared_ptr<CostDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x);
99
100 /**
101 * @brief Compute the derivatives of the residual cost
102 *
103 * @param[in] data Residual cost data
104 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
105 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
106 */
107 virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
108 const Eigen::Ref<const VectorXs>& x,
109 const Eigen::Ref<const VectorXs>& u);
110
111 /**
112 * @brief Compute the derivatives of the residual cost with respect to the
113 * state only
114 *
115 * It updates the Jacobian and Hessian of the cost function based on the state
116 * only. This function is used in the terminal nodes of an optimal control
117 * problem.
118 *
119 * @param[in] data Residual cost data
120 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
121 */
122 virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
123 const Eigen::Ref<const VectorXs>& x);
124
125 /**
126 * @brief Create the residual cost data
127 */
128 virtual boost::shared_ptr<CostDataAbstract> createData(
129 DataCollectorAbstract* const data);
130
131 /**
132 * @brief Print relevant information of the cost-residual model
133 *
134 * @param[out] os Output stream object
135 */
136 virtual void print(std::ostream& os) const;
137
138 protected:
139 using Base::activation_;
140 using Base::nu_;
141 using Base::residual_;
142 using Base::state_;
143 using Base::unone_;
144 };
145
146 template <typename _Scalar>
147 struct CostDataResidualTpl : public CostDataAbstractTpl<_Scalar> {
148 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
149
150 typedef _Scalar Scalar;
151 typedef MathBaseTpl<Scalar> MathBase;
152 typedef CostDataAbstractTpl<Scalar> Base;
153 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
154
155 template <template <typename Scalar> class Model>
156 476960 CostDataResidualTpl(Model<Scalar>* const model,
157 DataCollectorAbstract* const data)
158 476960 : Base(model, data) {}
159
160 using Base::activation;
161 using Base::cost;
162 using Base::Lu;
163 using Base::Luu;
164 using Base::Lx;
165 using Base::Lxu;
166 using Base::Lxx;
167 using Base::residual;
168 using Base::shared;
169 };
170
171 } // namespace crocoddyl
172
173 /* --- Details -------------------------------------------------------------- */
174 /* --- Details -------------------------------------------------------------- */
175 /* --- Details -------------------------------------------------------------- */
176 #include "crocoddyl/core/costs/residual.hxx"
177
178 #endif // CROCODDYL_CORE_COSTS_RESIDUAL_COST_HPP_
179