GCC Code Coverage Report

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