GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/state.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 1 2 50.0%
Branches: 0 2 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2022-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_STATE_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_STATE_HPP_
12
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/residual-base.hpp"
15 #include "crocoddyl/core/state-base.hpp"
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/multibody/states/multibody.hpp"
18
19 namespace crocoddyl {
20
21 /**
22 * @brief State residual
23 *
24 * This residual function defines the state tracking as
25 * \f$\mathbf{r}=\mathbf{x}\ominus\mathbf{x}^*\f$, where
26 * \f$\mathbf{x},\mathbf{x}^*\in~\mathcal{X}\f$ are the current and reference
27 * states, respectively, which belong to the state manifold \f$\mathcal{X}\f$.
28 * Note that the dimension of the residual vector is obtained from
29 * `StateAbstract::get_ndx()`. Furthermore, the Jacobians of the residual
30 * function are computed analytically.
31 *
32 * As described in `ResidualModelAbstractTpl()`, the residual value and its
33 * derivatives are calculated by `calc` and `calcDiff`, respectively.
34 *
35 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
36 */
37 template <typename _Scalar>
38 class ResidualModelStateTpl : public ResidualModelAbstractTpl<_Scalar> {
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelStateTpl)
42
43 typedef _Scalar Scalar;
44 typedef MathBaseTpl<Scalar> MathBase;
45 typedef ResidualModelAbstractTpl<Scalar> Base;
46 typedef StateMultibodyTpl<Scalar> StateMultibody;
47 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
48 typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
49 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
50 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::MatrixXs MatrixXs;
53
54 /**
55 * @brief Initialize the state residual model
56 *
57 * @param[in] state State of the multibody system
58 * @param[in] xref Reference state
59 * @param[in] nu Dimension of the control vector
60 */
61 ResidualModelStateTpl(std::shared_ptr<typename Base::StateAbstract> state,
62 const VectorXs& xref, const std::size_t nu);
63
64 /**
65 * @brief Initialize the state residual model
66 *
67 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
68 *
69 * @param[in] state State of the multibody system
70 * @param[in] xref Reference state
71 */
72 ResidualModelStateTpl(std::shared_ptr<typename Base::StateAbstract> state,
73 const VectorXs& xref);
74
75 /**
76 * @brief Initialize the state residual model
77 *
78 * The default reference state is obtained from `StateAbstractTpl::zero()`.
79 *
80 * @param[in] state State of the multibody system
81 * @param[in] nu Dimension of the control vector
82 */
83 ResidualModelStateTpl(std::shared_ptr<typename Base::StateAbstract> state,
84 const std::size_t nu);
85
86 /**
87 * @brief Initialize the state residual model
88 *
89 * The default state reference is obtained from `StateAbstractTpl::zero()`,
90 * and `nu` from `StateAbstractTpl::get_nv()`.
91 *
92 * @param[in] state State of the multibody system
93 * @param[in] activation Activation model
94 */
95 ResidualModelStateTpl(std::shared_ptr<typename Base::StateAbstract> state);
96 3002 virtual ~ResidualModelStateTpl() = default;
97
98 /**
99 * @brief Compute the state residual
100 *
101 * @param[in] data State residual data
102 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
103 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
104 */
105 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
106 const Eigen::Ref<const VectorXs>& x,
107 const Eigen::Ref<const VectorXs>& u) override;
108
109 /**
110 * @brief Compute the Jacobians of the state residual
111 *
112 * @param[in] data State residual data
113 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
114 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
115 */
116 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
117 const Eigen::Ref<const VectorXs>& x,
118 const Eigen::Ref<const VectorXs>& u) override;
119
120 /**
121 * @brief Compute the derivative of the state-cost function and store it in
122 * cost data
123 *
124 * This function assumes that the derivatives of the activation and residual
125 * are computed via calcDiff functions.
126 *
127 * @param cdata Cost data
128 * @param rdata Residual data
129 * @param adata Activation data
130 * @param update_u Update the derivative of the cost function w.r.t. to the
131 * control if True.
132 */
133 virtual void calcCostDiff(
134 const std::shared_ptr<CostDataAbstract>& cdata,
135 const std::shared_ptr<ResidualDataAbstract>& rdata,
136 const std::shared_ptr<ActivationDataAbstract>& adata,
137 const bool update_u = true) override;
138
139 /**
140 * @brief Cast the state residual model to a different scalar type.
141 *
142 * It is useful for operations requiring different precision or scalar types.
143 *
144 * @tparam NewScalar The new scalar type to cast to.
145 * @return ResidualModelStateTpl<NewScalar> A residual model with the
146 * new scalar type.
147 */
148 template <typename NewScalar>
149 ResidualModelStateTpl<NewScalar> cast() const;
150
151 /**
152 * @brief Return the reference state
153 */
154 const VectorXs& get_reference() const;
155
156 /**
157 * @brief Modify the reference state
158 */
159 void set_reference(const VectorXs& reference);
160
161 /**
162 * @brief Print relevant information of the state residual
163 *
164 * @param[out] os Output stream object
165 */
166 virtual void print(std::ostream& os) const override;
167
168 protected:
169 using Base::nr_;
170 using Base::nu_;
171 using Base::state_;
172 using Base::u_dependent_;
173
174 private:
175 VectorXs xref_; //!< Reference state
176 std::shared_ptr<typename StateMultibody::PinocchioModel>
177 pin_model_; //!< Pinocchio model
178 };
179
180 } // namespace crocoddyl
181
182 /* --- Details -------------------------------------------------------------- */
183 /* --- Details -------------------------------------------------------------- */
184 /* --- Details -------------------------------------------------------------- */
185 #include "crocoddyl/multibody/residuals/state.hxx"
186
187 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
188 crocoddyl::ResidualModelStateTpl<double>;
189 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
190 crocoddyl::ResidualModelStateTpl<float>;
191
192 #endif // CROCODDYL_MULTIBODY_RESIDUALS_STATE_HPP_
193