9 #ifndef CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_ 10 #define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_ 16 #include "crocoddyl/multibody/fwd.hpp" 17 #include "crocoddyl/core/utils/exception.hpp" 18 #include "crocoddyl/multibody/impulse-base.hpp" 22 template <
typename _Scalar>
23 struct ImpulseItemTpl {
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
27 typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
30 ImpulseItemTpl(
const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
bool active =
true)
31 : name(name), impulse(impulse), active(active) {}
34 boost::shared_ptr<ImpulseModelAbstract> impulse;
38 template <
typename _Scalar>
39 class ImpulseModelMultipleTpl {
41 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 typedef _Scalar Scalar;
44 typedef MathBaseTpl<Scalar> MathBase;
45 typedef StateMultibodyTpl<Scalar> StateMultibody;
46 typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
47 typedef ImpulseDataMultipleTpl<Scalar> ImpulseDataMultiple;
48 typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
50 typedef ImpulseItemTpl<Scalar> ImpulseItem;
52 typedef typename MathBase::Vector2s Vector2s;
53 typedef typename MathBase::Vector3s Vector3s;
54 typedef typename MathBase::VectorXs VectorXs;
55 typedef typename MathBase::MatrixXs MatrixXs;
57 typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
58 typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
59 typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
61 explicit ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
62 ~ImpulseModelMultipleTpl();
64 void addImpulse(
const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
bool active =
true);
65 void removeImpulse(
const std::string& name);
66 void changeImpulseStatus(
const std::string& name,
bool active);
68 void calc(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
69 void calcDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
71 void updateVelocity(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const VectorXs& vnext)
const;
72 void updateForce(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const VectorXs& force);
73 boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>*
const data);
74 void updateVelocityDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const MatrixXs& dvnext_dx)
const;
75 void updateForceDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const MatrixXs& df_dx)
const;
77 const boost::shared_ptr<StateMultibody>& get_state()
const;
78 const ImpulseModelContainer& get_impulses()
const;
79 const std::size_t& get_ni()
const;
80 const std::size_t& get_ni_total()
const;
81 const std::vector<std::string>& get_active()
const;
82 const std::vector<std::string>& get_inactive()
const;
83 bool getImpulseStatus(
const std::string& name)
const;
86 boost::shared_ptr<StateMultibody> state_;
87 ImpulseModelContainer impulses_;
89 std::size_t ni_total_;
90 std::vector<std::string> active_;
91 std::vector<std::string> inactive_;
94 template <
typename _Scalar>
95 struct ImpulseDataMultipleTpl {
96 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98 typedef _Scalar Scalar;
99 typedef MathBaseTpl<Scalar> MathBase;
100 typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
101 typedef ImpulseItemTpl<Scalar> ImpulseItem;
102 typedef typename MathBase::VectorXs VectorXs;
103 typedef typename MathBase::MatrixXs MatrixXs;
105 template <
template <
typename Scalar>
class Model>
106 ImpulseDataMultipleTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
107 : Jc(model->get_ni_total(), model->get_state()->get_nv()),
108 dv0_dq(model->get_ni_total(), model->get_state()->get_nv()),
109 vnext(model->get_state()->get_nv()),
110 dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
111 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
116 for (
typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
117 it != model->get_impulses().end(); ++it) {
118 const boost::shared_ptr<ImpulseItem>& item = it->second;
119 impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
127 typename ImpulseModelMultiple::ImpulseDataContainer impulses;
128 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> > fext;
136 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx" 138 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_