crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multiple-impulses.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
10 #define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
11 
12 #include <string>
13 #include <map>
14 #include <utility>
15 
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 #include "crocoddyl/multibody/impulse-base.hpp"
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
23 struct ImpulseItemTpl {
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
27  typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
28 
29  ImpulseItemTpl() {}
30  ImpulseItemTpl(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, const bool active = true)
31  : name(name), impulse(impulse), active(active) {}
32 
36  friend std::ostream& operator<<(std::ostream& os, const ImpulseItemTpl<Scalar>& model) {
37  os << "{" << *model.impulse << "}";
38  return os;
39  }
40 
41  std::string name;
42  boost::shared_ptr<ImpulseModelAbstract> impulse;
43  bool active;
44 };
45 
53 template <typename _Scalar>
54 class ImpulseModelMultipleTpl {
55  public:
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  typedef _Scalar Scalar;
59  typedef MathBaseTpl<Scalar> MathBase;
60  typedef StateMultibodyTpl<Scalar> StateMultibody;
61  typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
62  typedef ImpulseDataMultipleTpl<Scalar> ImpulseDataMultiple;
63  typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
64 
65  typedef ImpulseItemTpl<Scalar> ImpulseItem;
66 
67  typedef typename MathBase::Vector2s Vector2s;
68  typedef typename MathBase::Vector3s Vector3s;
69  typedef typename MathBase::VectorXs VectorXs;
70  typedef typename MathBase::MatrixXs MatrixXs;
71 
72  typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
73  typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
74  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
75 
81  explicit ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
83 
93  void addImpulse(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, const bool active = true);
94 
100  void removeImpulse(const std::string& name);
101 
108  void changeImpulseStatus(const std::string& name, const bool active);
109 
116  void calc(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
117 
124  void calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
125 
132  void updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& vnext) const;
133 
141  void updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& impulse);
142 
150  void updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& dvnext_dx) const;
151 
159  void updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& df_dx) const;
160 
167  boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
168 
172  const boost::shared_ptr<StateMultibody>& get_state() const;
173 
177  const ImpulseModelContainer& get_impulses() const;
178 
182  std::size_t get_nc() const;
183  DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
184 
188  std::size_t get_nc_total() const;
189  DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const;)
190 
194  const std::vector<std::string>& get_active() const;
195 
199  const std::vector<std::string>& get_inactive() const;
200 
204  bool getImpulseStatus(const std::string& name) const;
205 
209  template <class Scalar>
210  friend std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model);
211 
212  private:
213  boost::shared_ptr<StateMultibody> state_;
214  ImpulseModelContainer impulses_;
215  std::size_t nc_;
216  std::size_t nc_total_;
217  std::vector<std::string> active_;
218  std::vector<std::string> inactive_;
219 };
220 
226 template <typename _Scalar>
227 struct ImpulseDataMultipleTpl {
228  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
229 
230  typedef _Scalar Scalar;
231  typedef MathBaseTpl<Scalar> MathBase;
232  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
233  typedef ImpulseItemTpl<Scalar> ImpulseItem;
234  typedef typename MathBase::VectorXs VectorXs;
235  typedef typename MathBase::MatrixXs MatrixXs;
236 
243  template <template <typename Scalar> class Model>
244  ImpulseDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
245  : Jc(model->get_nc_total(), model->get_state()->get_nv()),
246  dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
247  vnext(model->get_state()->get_nv()),
248  dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
249  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
250  Jc.setZero();
251  dv0_dq.setZero();
252  vnext.setZero();
253  dvnext_dx.setZero();
254  for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
255  it != model->get_impulses().end(); ++it) {
256  const boost::shared_ptr<ImpulseItem>& item = it->second;
257  impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
258  }
259  }
260 
261  MatrixXs Jc;
262  MatrixXs
265  VectorXs vnext;
268  MatrixXs dvnext_dx;
270  typename ImpulseModelMultiple::ImpulseDataContainer impulses;
272  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
274 };
275 
276 } // namespace crocoddyl
277 
278 /* --- Details -------------------------------------------------------------- */
279 /* --- Details -------------------------------------------------------------- */
280 /* --- Details -------------------------------------------------------------- */
281 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
282 
283 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
crocoddyl::ImpulseModelMultipleTpl::updateVelocityDiff
void updateVelocityDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &dvnext_dx) const
Update the Jacobian of the system velocity after impulse.
crocoddyl::ImpulseModelMultipleTpl::changeImpulseStatus
void changeImpulseStatus(const std::string &name, const bool active)
Change the impulse status.
crocoddyl::ImpulseModelMultipleTpl::updateForce
void updateForce(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &impulse)
Update the spatial impulse defined in frame coordinate.
crocoddyl::ImpulseModelMultipleTpl::addImpulse
void addImpulse(const std::string &name, boost::shared_ptr< ImpulseModelAbstract > impulse, const bool active=true)
Add impulse item.
crocoddyl::ImpulseModelMultipleTpl::updateForceDiff
void updateForceDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &df_dx) const
Update the Jacobian of the spatial impulse defined in frame coordinate.
crocoddyl::ImpulseDataMultipleTpl::vnext
VectorXs vnext
Definition: multiple-impulses.hpp:267
crocoddyl::ImpulseItemTpl::operator<<
friend std::ostream & operator<<(std::ostream &os, const ImpulseItemTpl< Scalar > &model)
Print information on the impulse item.
Definition: multiple-impulses.hpp:36
crocoddyl::ImpulseModelMultipleTpl::get_impulses
const ImpulseModelContainer & get_impulses() const
Return the impulse models.
crocoddyl::ImpulseDataMultipleTpl::impulses
ImpulseModelMultiple::ImpulseDataContainer impulses
Stack of impulse data.
Definition: multiple-impulses.hpp:271
crocoddyl::ImpulseModelMultipleTpl::get_state
const boost::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
crocoddyl::ImpulseModelMultipleTpl::get_nc
std::size_t get_nc() const
Return the dimension of active impulses.
crocoddyl::ImpulseModelMultipleTpl::calc
void calc(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total contact Jacobian and contact acceleration.
crocoddyl::ImpulseDataMultipleTpl::dv0_dq
MatrixXs dv0_dq
Definition: multiple-impulses.hpp:264
crocoddyl::ImpulseModelMultipleTpl::calcDiff
void calcDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the contact holonomic constraint.
crocoddyl::ImpulseDataMultipleTpl::Jc
MatrixXs Jc
Definition: multiple-impulses.hpp:261
crocoddyl::ImpulseModelMultipleTpl::removeImpulse
void removeImpulse(const std::string &name)
Remove impulse item.
crocoddyl::ImpulseModelMultipleTpl::updateVelocity
void updateVelocity(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &vnext) const
Update the system velocity after impulse.
crocoddyl::ImpulseModelMultipleTpl::ImpulseModelMultipleTpl
ImpulseModelMultipleTpl(boost::shared_ptr< StateMultibody > state)
Initialize the multi-impulse model.
crocoddyl::ImpulseModelMultipleTpl::getImpulseStatus
bool getImpulseStatus(const std::string &name) const
Return the status of a given impulse name.
crocoddyl::ImpulseDataMultipleTpl::fext
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.
Definition: multiple-impulses.hpp:273
crocoddyl::ImpulseItemTpl
Definition: fwd.hpp:336
crocoddyl::ImpulseModelMultipleTpl::createData
boost::shared_ptr< ImpulseDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-impulse data.
crocoddyl::ImpulseModelMultipleTpl::get_inactive
DEPRECATED("Use get_nc().", std::size_t get_ni() const ;) std const DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const ;) const std std::vector< std::string > & get_inactive() const
Return the dimension of all impulses.
crocoddyl::ImpulseDataMultipleTpl::ImpulseDataMultipleTpl
ImpulseDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-impulse data.
Definition: multiple-impulses.hpp:244
crocoddyl::ImpulseModelMultipleTpl::operator<<
friend std::ostream & operator<<(std::ostream &os, const ImpulseModelMultipleTpl< Scalar > &model)
Print information on the impulse models.
crocoddyl::ImpulseDataMultipleTpl::dvnext_dx
MatrixXs dvnext_dx
Definition: multiple-impulses.hpp:269