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_ |