GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/crocoddyl/multibody/impulses/multiple-impulses.hpp Lines: 20 23 87.0 %
Date: 2024-02-13 11:12:33 Branches: 15 28 53.6 %

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



29890
             pinocchio::ForceTpl<Scalar>::Zero()) {
285
4270
    Jc.setZero();
286
4270
    dv0_dq.setZero();
287
4270
    vnext.setZero();
288
4270
    dvnext_dx.setZero();
289
10231
    for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator
290
4270
             it = model->get_impulses().begin();
291
14501
         it != model->get_impulses().end(); ++it) {
292
10231
      const boost::shared_ptr<ImpulseItem>& item = it->second;
293
10231
      impulses.insert(
294

10231
          std::make_pair(item->name, item->impulse->createData(data)));
295
    }
296
4270
  }
297
298
  MatrixXs Jc;  //!< Contact Jacobian in frame coordinate
299
                //!< \f$\mathbf{J}_c\in\mathbb{R}^{ni_{total}\times{nv}}\f$
300
                //!< (memory defined for active and inactive impulses)
301
  MatrixXs
302
      dv0_dq;  //!< Jacobian of the desired spatial contact acceleration in
303
               //!< frame coordinate
304
               //!< \f$\frac{\partial\underline{\mathbf{v}}_0}{\partial\mathbf{q}}\in\mathbb{R}^{ni_{total}\times{nv}}\f$
305
               //!< (memory defined for active and inactive impulse)
306
  VectorXs vnext;  //!< Constrained system velocity after impact in generalized
307
                   //!< coordinates \f$\dot{\mathbf{v}'}\in\mathbb{R}^{nv}\f$
308
  MatrixXs
309
      dvnext_dx;  //!< Jacobian of the system velocity after impact in
310
                  //!< generalized coordinates
311
                  //!< \f$\frac{\partial\dot{\mathbf{v}'}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
312
                  //!< ndx}\f$
313
  typename ImpulseModelMultiple::ImpulseDataContainer
314
      impulses;  //!< Stack of impulse data
315
  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
316
      fext;  //!< External spatial forces in body coordinates
317
};
318
319
}  // namespace crocoddyl
320
321
/* --- Details -------------------------------------------------------------- */
322
/* --- Details -------------------------------------------------------------- */
323
/* --- Details -------------------------------------------------------------- */
324
#include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
325
326
#endif  // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_