GCC Code Coverage Report

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