Crocoddyl
 
Loading...
Searching...
No Matches
multiple-impulses.hpp
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.
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
16namespace crocoddyl {
17
18template <typename _Scalar>
20 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21
22 typedef _Scalar Scalar;
24
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
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
60template <typename _Scalar>
62 public:
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
71
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
91 explicit ImpulseModelMultipleTpl(std::shared_ptr<StateMultibody> state);
93
103 void addImpulse(const std::string& name,
104 std::shared_ptr<ImpulseModelAbstract> impulse,
105 const bool active = true);
106
112 void removeImpulse(const std::string& name);
113
120 void changeImpulseStatus(const std::string& name, const bool active);
121
128 void calc(const std::shared_ptr<ImpulseDataMultiple>& data,
129 const Eigen::Ref<const VectorXs>& x);
130
137 void calcDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
138 const Eigen::Ref<const VectorXs>& x);
139
147 void updateVelocity(const std::shared_ptr<ImpulseDataMultiple>& data,
148 const VectorXs& vnext) const;
149
157 void updateForce(const std::shared_ptr<ImpulseDataMultiple>& data,
158 const VectorXs& impulse);
159
168 void updateVelocityDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
169 const MatrixXs& dvnext_dx) const;
170
180 void updateForceDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
181 const MatrixXs& df_dx) const;
182
192 void updateRneaDiff(const std::shared_ptr<ImpulseDataMultiple>& data,
193 pinocchio::DataTpl<Scalar>& pinocchio) const;
194
201 std::shared_ptr<ImpulseDataMultiple> createData(
202 pinocchio::DataTpl<Scalar>* const data);
203
213 template <typename NewScalar>
215
219 const std::shared_ptr<StateMultibody>& get_state() const;
220
224 const ImpulseModelContainer& get_impulses() const;
225
229 std::size_t get_nc() const;
230
234 std::size_t get_nc_total() const;
235
239 const std::set<std::string>& get_active_set() const;
240
244 const std::set<std::string>& get_inactive_set() const;
245
249 bool getImpulseStatus(const std::string& name) const;
250
254 template <class Scalar>
255 friend std::ostream& operator<<(std::ostream& os,
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
272template <typename _Scalar>
274 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
275
276 typedef _Scalar Scalar;
280 typedef typename MathBase::VectorXs VectorXs;
281 typedef typename MathBase::MatrixXs MatrixXs;
282
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;
314 MatrixXs
319 VectorXs vnext;
321 MatrixXs
326 typename ImpulseModelMultiple::ImpulseDataContainer
328 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
330};
331
332} // namespace crocoddyl
333
334/* --- Details -------------------------------------------------------------- */
335/* --- Details -------------------------------------------------------------- */
336/* --- Details -------------------------------------------------------------- */
337#include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
338
339CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseItemTpl)
340CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModelMultipleTpl)
341CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseDataMultipleTpl)
342
343#endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
Define a stack of impulse models.
ImpulseModelMultipleTpl< NewScalar > cast() const
Cast the multi-impulse model to a different scalar type.
void updateVelocityDiff(const std::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &dvnext_dx) const
Update the Jacobian of the system velocity after impulse.
void updateRneaDiff(const std::shared_ptr< ImpulseDataMultiple > &data, pinocchio::DataTpl< Scalar > &pinocchio) const
Update the RNEA derivatives dtau_dq by adding the skew term (necessary for impulses expressed in LOCA...
std::size_t get_nc() const
Return the dimension of active impulses.
void calc(const std::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total impulse Jacobian and impulse velocity.
const std::set< std::string > & get_active_set() const
Return the names of the set of active impulses.
const std::set< std::string > & get_inactive_set() const
Return the names of the set of inactive impulses.
void calcDiff(const std::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the impulse holonomic constraint.
void addImpulse(const std::string &name, std::shared_ptr< ImpulseModelAbstract > impulse, const bool active=true)
Add impulse item.
ImpulseModelMultipleTpl(std::shared_ptr< StateMultibody > state)
Initialize the multi-impulse model.
std::shared_ptr< ImpulseDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-impulse data.
void updateForceDiff(const std::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &df_dx) const
Update the Jacobian of the spatial impulse defined in frame coordinate.
std::size_t get_nc_total() const
Return the dimension of all impulses.
void updateVelocity(const std::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &vnext) const
Update the system velocity after impulse.
const std::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
bool getImpulseStatus(const std::string &name) const
Return the status of a given impulse name.
void changeImpulseStatus(const std::string &name, const bool active)
Change the impulse status.
void removeImpulse(const std::string &name)
Remove impulse item.
const ImpulseModelContainer & get_impulses() const
Return the impulse models.
void updateForce(const std::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &impulse)
Update the spatial impulse defined in frame coordinate.
friend std::ostream & operator<<(std::ostream &os, const ImpulseModelMultipleTpl< Scalar > &model)
Print information on the impulse models.
State multibody representation.
Definition multibody.hpp:34
Define the multi-impulse data.
ImpulseDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-impulse data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.
ImpulseModelMultiple::ImpulseDataContainer impulses
Stack of impulse data.
friend std::ostream & operator<<(std::ostream &os, const ImpulseItemTpl< Scalar > &model)
Print information on the impulse item.