GCC Code Coverage Report

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