GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/impulses/multiple-impulses.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 26 0.0%
Branches: 0 58 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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_IMPULSES_MULTIPLE_IMPULSES_HPP_
11 #define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
12
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/multibody/impulse-base.hpp"
15
16 namespace crocoddyl {
17
18 template <typename _Scalar>
19 struct ImpulseItemTpl {
20 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21
22 typedef _Scalar Scalar;
23 typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
24
25 ✗ ImpulseItemTpl() {}
26 ✗ ImpulseItemTpl(const std::string& name,
27 std::shared_ptr<ImpulseModelAbstract> impulse,
28 const bool active = true)
29 ✗ : name(name), impulse(impulse), active(active) {}
30
31 template <typename NewScalar>
32 ✗ ImpulseItemTpl<NewScalar> cast() const {
33 typedef ImpulseItemTpl<NewScalar> ReturnType;
34 ✗ ReturnType ret(name, impulse->template cast<NewScalar>(), active);
35 ✗ return ret;
36 }
37
38 /**
39 * @brief Print information on the impulse item
40 */
41 ✗ friend std::ostream& operator<<(std::ostream& os,
42 const ImpulseItemTpl<Scalar>& model) {
43 ✗ os << "{" << *model.impulse << "}";
44 ✗ return os;
45 }
46
47 std::string name;
48 std::shared_ptr<ImpulseModelAbstract> impulse;
49 bool active;
50 };
51
52 /**
53 * @brief Define a stack of impulse models
54 *
55 * The impulse models can be defined with active and inactive status. The idea
56 * behind this design choice is to be able to create a mechanism that allocates
57 * the entire data needed for the computations. Then, there are designed
58 * routines that update the only active impulse.
59 */
60 template <typename _Scalar>
61 class ImpulseModelMultipleTpl {
62 public:
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
66 typedef MathBaseTpl<Scalar> MathBase;
67 typedef StateMultibodyTpl<Scalar> StateMultibody;
68 typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
69 typedef ImpulseDataMultipleTpl<Scalar> ImpulseDataMultiple;
70 typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
71
72 typedef ImpulseItemTpl<Scalar> ImpulseItem;
73
74 typedef typename MathBase::Vector2s Vector2s;
75 typedef typename MathBase::Vector3s Vector3s;
76 typedef typename MathBase::VectorXs VectorXs;
77 typedef typename MathBase::MatrixXs MatrixXs;
78
79 typedef std::map<std::string, std::shared_ptr<ImpulseItem> >
80 ImpulseModelContainer;
81 typedef std::map<std::string, std::shared_ptr<ImpulseDataAbstract> >
82 ImpulseDataContainer;
83 typedef typename pinocchio::container::aligned_vector<
84 pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
85
86 /**
87 * @brief Initialize the multi-impulse model
88 *
89 * @param[in] state Multibody state
90 */
91 explicit ImpulseModelMultipleTpl(std::shared_ptr<StateMultibody> state);
92 ~ImpulseModelMultipleTpl();
93
94 /**
95 * @brief Add impulse item
96 *
97 * Note that the memory is allocated for inactive impulses as well.
98 *
99 * @param[in] name Impulse name
100 * @param[in] impulse Impulse model
101 * @param[in] active Impulse status (active by default)
102 */
103 void addImpulse(const std::string& name,
104 std::shared_ptr<ImpulseModelAbstract> impulse,
105 const bool active = true);
106
107 /**
108 * @brief Remove impulse item
109 *
110 * @param[in] name Impulse name
111 */
112 void removeImpulse(const std::string& name);
113
114 /**
115 * @brief Change the impulse status
116 *
117 * @param[in] name Impulse name
118 * @param[in] active Impulse status (True for active)
119 */
120 void changeImpulseStatus(const std::string& name, const bool active);
121
122 /**
123 * @brief Compute the total impulse Jacobian and impulse velocity
124 *
125 * @param[in] data Multi-impulse data
126 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
127 */
128 void calc(const std::shared_ptr<ImpulseDataMultiple>& data,
129 const Eigen::Ref<const VectorXs>& x);
130
131 /**
132 * @brief Compute the derivatives of the impulse holonomic constraint
133 *
134 * @param[in] data Multi-impulse data
135 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
136 */
137 void calcDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
138 const Eigen::Ref<const VectorXs>& x);
139
140 /**
141 * @brief Update the system velocity after impulse
142 *
143 * @param[in] data Multi-impulse data
144 * @param[in] vnext System velocity after impulse
145 * \f$\mathbf{v}'\in\mathbb{R}^{nv}\f$
146 */
147 void updateVelocity(const std::shared_ptr<ImpulseDataMultiple>& data,
148 const VectorXs& vnext) const;
149
150 /**
151 * @brief Update the spatial impulse defined in frame coordinate
152 *
153 * @param[in] data Multi-impulse data
154 * @param[in] impulse Spatial impulse defined in frame coordinate
155 * \f${}^o\underline{\boldsymbol{\Lambda}}_c\in\mathbb{R}^{nc}\f$
156 */
157 void updateForce(const std::shared_ptr<ImpulseDataMultiple>& data,
158 const VectorXs& impulse);
159
160 /**
161 * @brief Update the Jacobian of the system velocity after impulse
162 *
163 * @param[in] data Multi-impulse data
164 * @param[in] dvnext_dx Jacobian of the system velocity after impact in
165 * generalized coordinates
166 * \f$\frac{\partial\dot{\mathbf{v}'}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times{ndx}}\f$
167 */
168 void updateVelocityDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
169 const MatrixXs& dvnext_dx) const;
170
171 /**
172 * @brief Update the Jacobian of the spatial impulse defined in frame
173 * coordinate
174 *
175 * @param[in] data Multi-contact data
176 * @param[in] df_dx Jacobian of the spatial impulse defined in frame
177 * coordinate
178 * \f$\frac{\partial{}^o\underline{\boldsymbol{\Lambda}}_c}{\partial\mathbf{x}}\in\mathbb{R}^{nc\times{ndx}}\f$
179 */
180 void updateForceDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
181 const MatrixXs& df_dx) const;
182
183 /**
184 * @brief Update the RNEA derivatives dtau_dq by adding the skew term
185 * (necessary for impulses expressed in LOCAL_WORLD_ALIGNED / WORLD)
186 * @brief as explained in this document :
187 * https://www.overleaf.com/read/tzvrrxxtntwk
188 *
189 * @param[in] data Multi-contact data
190 * @param[in] pinocchio Pinocchio data
191 */
192 void updateRneaDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
193 pinocchio::DataTpl<Scalar>& pinocchio) const;
194
195 /**
196 * @brief Create the multi-impulse data
197 *
198 * @param[in] data Pinocchio data
199 * @return the multi-impulse data.
200 */
201 std::shared_ptr<ImpulseDataMultiple> createData(
202 pinocchio::DataTpl<Scalar>* const data);
203
204 /**
205 * @brief Cast the multi-impulse model to a different scalar type.
206 *
207 * It is useful for operations requiring different precision or scalar types.
208 *
209 * @tparam NewScalar The new scalar type to cast to.
210 * @return ImpulseModelMultipleTpl<NewScalar> A multi-impulse model with the
211 * new scalar type.
212 */
213 template <typename NewScalar>
214 ImpulseModelMultipleTpl<NewScalar> cast() const;
215
216 /**
217 * @brief Return the multibody state
218 */
219 const std::shared_ptr<StateMultibody>& get_state() const;
220
221 /**
222 * @brief Return the impulse models
223 */
224 const ImpulseModelContainer& get_impulses() const;
225
226 /**
227 * @brief Return the dimension of active impulses
228 */
229 std::size_t get_nc() const;
230
231 /**
232 * @brief Return the dimension of all impulses
233 */
234 std::size_t get_nc_total() const;
235
236 /**
237 * @brief Return the names of the set of active impulses
238 */
239 const std::set<std::string>& get_active_set() const;
240
241 /**
242 * @brief Return the names of the set of inactive impulses
243 */
244 const std::set<std::string>& get_inactive_set() const;
245
246 /**
247 * @brief Return the status of a given impulse name
248 */
249 bool getImpulseStatus(const std::string& name) const;
250
251 /**
252 * @brief Print information on the impulse models
253 */
254 template <class Scalar>
255 friend std::ostream& operator<<(std::ostream& os,
256 const ImpulseModelMultipleTpl<Scalar>& model);
257
258 private:
259 std::shared_ptr<StateMultibody> state_;
260 ImpulseModelContainer impulses_;
261 std::size_t nc_;
262 std::size_t nc_total_;
263 std::set<std::string> active_set_;
264 std::set<std::string> inactive_set_;
265 };
266
267 /**
268 * @brief Define the multi-impulse data
269 *
270 * \sa ImpulseModelMultipleTpl
271 */
272 template <typename _Scalar>
273 struct ImpulseDataMultipleTpl {
274 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
275
276 typedef _Scalar Scalar;
277 typedef MathBaseTpl<Scalar> MathBase;
278 typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
279 typedef ImpulseItemTpl<Scalar> ImpulseItem;
280 typedef typename MathBase::VectorXs VectorXs;
281 typedef typename MathBase::MatrixXs MatrixXs;
282
283 /**
284 * @brief Initialized a multi-impulse data
285 *
286 * @param[in] model Multi-impulse model
287 * @param[in] data Pinocchio data
288 */
289 template <template <typename Scalar> class Model>
290 ✗ ImpulseDataMultipleTpl(Model<Scalar>* const model,
291 pinocchio::DataTpl<Scalar>* const data)
292 ✗ : Jc(model->get_nc_total(), model->get_state()->get_nv()),
293 ✗ dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
294 ✗ vnext(model->get_state()->get_nv()),
295 ✗ dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
296 ✗ fext(model->get_state()->get_pinocchio()->njoints,
297 ✗ pinocchio::ForceTpl<Scalar>::Zero()) {
298 ✗ Jc.setZero();
299 ✗ dv0_dq.setZero();
300 ✗ vnext.setZero();
301 ✗ dvnext_dx.setZero();
302 ✗ for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator
303 ✗ it = model->get_impulses().begin();
304 ✗ it != model->get_impulses().end(); ++it) {
305 ✗ const std::shared_ptr<ImpulseItem>& item = it->second;
306 ✗ impulses.insert(
307 ✗ std::make_pair(item->name, item->impulse->createData(data)));
308 }
309 }
310
311 MatrixXs Jc; //!< Contact Jacobian in frame coordinate
312 //!< \f$\mathbf{J}_c\in\mathbb{R}^{ni_{total}\times{nv}}\f$
313 //!< (memory defined for active and inactive impulses)
314 MatrixXs
315 dv0_dq; //!< Jacobian of the desired spatial contact acceleration in
316 //!< frame coordinate
317 //!< \f$\frac{\partial\underline{\mathbf{v}}_0}{\partial\mathbf{q}}\in\mathbb{R}^{ni_{total}\times{nv}}\f$
318 //!< (memory defined for active and inactive impulse)
319 VectorXs vnext; //!< Constrained system velocity after impact in generalized
320 //!< coordinates \f$\dot{\mathbf{v}'}\in\mathbb{R}^{nv}\f$
321 MatrixXs
322 dvnext_dx; //!< Jacobian of the system velocity after impact in
323 //!< generalized coordinates
324 //!< \f$\frac{\partial\dot{\mathbf{v}'}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
325 //!< ndx}\f$
326 typename ImpulseModelMultiple::ImpulseDataContainer
327 impulses; //!< Stack of impulse data
328 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
329 fext; //!< External spatial forces in body coordinates
330 };
331
332 } // namespace crocoddyl
333
334 /* --- Details -------------------------------------------------------------- */
335 /* --- Details -------------------------------------------------------------- */
336 /* --- Details -------------------------------------------------------------- */
337 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
338
339 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseItemTpl)
340 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModelMultipleTpl)
341 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseDataMultipleTpl)
342
343 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
344