GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/frame-translation.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 11 0.0%
Branches: 0 22 0.0%
Line Branch Exec Source
1
2 ///////////////////////////////////////////////////////////////////////////////
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
6 // Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
12 #define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
13
14 #include "crocoddyl/core/residual-base.hpp"
15 #include "crocoddyl/multibody/data/multibody.hpp"
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/multibody/states/multibody.hpp"
18
19 namespace crocoddyl {
20
21 /**
22 * @brief Frame translation residual
23 *
24 * This residual function defines the tracking of a frame translation as
25 * \f$\mathbf{r}=\mathbf{t}-\mathbf{t}^*\f$, where
26 * \f$\mathbf{t},\mathbf{t}^*\in~\mathbb{R}^3\f$ are the current and reference
27 * frame translations, respectively. Note that the dimension of the residual
28 * vector is 3. Furthermore, the Jacobians of the residual function are computed
29 * analytically.
30 *
31 * As described in `ResidualModelAbstractTpl()`, the residual value and its
32 * Jacobians are calculated by `calc` and `calcDiff`, respectively.
33 *
34 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
35 */
36 template <typename _Scalar>
37 class ResidualModelFrameTranslationTpl
38 : public ResidualModelAbstractTpl<_Scalar> {
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelFrameTranslationTpl)
42
43 typedef _Scalar Scalar;
44 typedef MathBaseTpl<Scalar> MathBase;
45 typedef ResidualModelAbstractTpl<Scalar> Base;
46 typedef ResidualDataFrameTranslationTpl<Scalar> Data;
47 typedef StateMultibodyTpl<Scalar> StateMultibody;
48 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
49 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
50 typedef typename MathBase::VectorXs VectorXs;
51 typedef typename MathBase::Vector3s Vector3s;
52
53 /**
54 * @brief Initialize the frame translation residual model
55 *
56 * @param[in] state State of the multibody system
57 * @param[in] id Reference frame id
58 * @param[in] xref Reference frame translation
59 * @param[in] nu Dimension of the control vector
60 */
61 ResidualModelFrameTranslationTpl(std::shared_ptr<StateMultibody> state,
62 const pinocchio::FrameIndex id,
63 const Vector3s& xref, const std::size_t nu);
64
65 /**
66 * @brief Initialize the frame translation residual model
67 *
68 * The default `nu` is equals to StateAbstractTpl::get_nv().
69 *
70 * @param[in] state State of the multibody system
71 * @param[in] id Reference frame id
72 * @param[in] xref Reference frame translation
73 */
74 ResidualModelFrameTranslationTpl(std::shared_ptr<StateMultibody> state,
75 const pinocchio::FrameIndex id,
76 const Vector3s& xref);
77 ✗ virtual ~ResidualModelFrameTranslationTpl() = default;
78
79 /**
80 * @brief Compute the frame translation residual
81 *
82 * @param[in] data Frame translation residual data
83 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
84 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
85 */
86 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x,
88 const Eigen::Ref<const VectorXs>& u) override;
89
90 /**
91 * @brief Compute the derivatives of the frame translation residual
92 *
93 * @param[in] data Frame translation residual data
94 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
95 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
96 */
97 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x,
99 const Eigen::Ref<const VectorXs>& u) override;
100
101 /**
102 * @brief Create the frame translation residual data
103 */
104 virtual std::shared_ptr<ResidualDataAbstract> createData(
105 DataCollectorAbstract* const data) override;
106
107 /**
108 * @brief Cast the frame-translation residual model to a different scalar
109 * type.
110 *
111 * It is useful for operations requiring different precision or scalar types.
112 *
113 * @tparam NewScalar The new scalar type to cast to.
114 * @return ResidualModelFrameTranslationTpl<NewScalar> A residual model with
115 * the new scalar type.
116 */
117 template <typename NewScalar>
118 ResidualModelFrameTranslationTpl<NewScalar> cast() const;
119
120 /**
121 * @brief Return the reference frame id
122 */
123 pinocchio::FrameIndex get_id() const;
124
125 /**
126 * @brief Return the reference frame translation
127 */
128 const Vector3s& get_reference() const;
129
130 /**
131 * @brief Modify the reference frame id
132 */
133 void set_id(const pinocchio::FrameIndex id);
134
135 /**
136 * @brief Modify the reference frame translation reference
137 */
138 void set_reference(const Vector3s& reference);
139
140 /**
141 * @brief Print relevant information of the frame-translation residual
142 *
143 * @param[out] os Output stream object
144 */
145 virtual void print(std::ostream& os) const override;
146
147 protected:
148 using Base::nu_;
149 using Base::state_;
150 using Base::u_dependent_;
151 using Base::v_dependent_;
152
153 private:
154 pinocchio::FrameIndex id_; //!< Reference frame id
155 Vector3s xref_; //!< Reference frame translation
156 std::shared_ptr<typename StateMultibody::PinocchioModel>
157 pin_model_; //!< Pinocchio model
158 };
159
160 template <typename _Scalar>
161 struct ResidualDataFrameTranslationTpl
162 : public ResidualDataAbstractTpl<_Scalar> {
163 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
164
165 typedef _Scalar Scalar;
166 typedef MathBaseTpl<Scalar> MathBase;
167 typedef ResidualDataAbstractTpl<Scalar> Base;
168 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
169 typedef typename MathBase::Matrix6xs Matrix6xs;
170
171 template <template <typename Scalar> class Model>
172 ✗ ResidualDataFrameTranslationTpl(Model<Scalar>* const model,
173 DataCollectorAbstract* const data)
174 ✗ : Base(model, data), fJf(6, model->get_state()->get_nv()) {
175 ✗ fJf.setZero();
176 // Check that proper shared data has been passed
177 ✗ DataCollectorMultibodyTpl<Scalar>* d =
178 ✗ dynamic_cast<DataCollectorMultibodyTpl<Scalar>*>(shared);
179 ✗ if (d == NULL) {
180 ✗ throw_pretty(
181 "Invalid argument: the shared data should be derived from "
182 "DataCollectorMultibody");
183 }
184
185 // Avoids data casting at runtime
186 ✗ pinocchio = d->pinocchio;
187 }
188 ✗ virtual ~ResidualDataFrameTranslationTpl() = default;
189
190 pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
191 Matrix6xs fJf; //!< Local Jacobian of the frame
192
193 using Base::r;
194 using Base::Ru;
195 using Base::Rx;
196 using Base::shared;
197 };
198
199 } // namespace crocoddyl
200
201 /* --- Details -------------------------------------------------------------- */
202 /* --- Details -------------------------------------------------------------- */
203 /* --- Details -------------------------------------------------------------- */
204 #include "crocoddyl/multibody/residuals/frame-translation.hxx"
205
206 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
207 crocoddyl::ResidualModelFrameTranslationTpl)
208 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
209 crocoddyl::ResidualDataFrameTranslationTpl)
210
211 #endif // CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
212