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,
const bool active =
true)
31 : name(name), impulse(impulse), active(active) {}
36 template <
class Scalar>
37 friend std::ostream& operator<<(std::ostream& os, const ImpulseItemTpl<Scalar>& model) {
38 os <<
"{" << *model.impulse <<
"}";
43 boost::shared_ptr<ImpulseModelAbstract> impulse;
54 template <
typename _Scalar>
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 typedef _Scalar Scalar;
68 typedef typename MathBase::Vector2s Vector2s;
69 typedef typename MathBase::Vector3s Vector3s;
70 typedef typename MathBase::VectorXs VectorXs;
71 typedef typename MathBase::MatrixXs MatrixXs;
73 typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
74 typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
75 typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
94 void addImpulse(
const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
const bool active =
true);
101 void removeImpulse(
const std::string& name);
109 void changeImpulseStatus(
const std::string& name,
const bool active);
117 void calc(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
125 void calcDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
133 void updateVelocity(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const VectorXs& vnext)
const;
142 void updateForce(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const VectorXs& impulse);
151 void updateVelocityDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const MatrixXs& dvnext_dx)
const;
160 void updateForceDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const MatrixXs& df_dx)
const;
168 boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>*
const data);
173 const boost::shared_ptr<StateMultibody>& get_state()
const;
178 const ImpulseModelContainer& get_impulses()
const;
183 std::size_t get_nc()
const;
184 DEPRECATED(
"Use get_nc().", std::size_t get_ni()
const;)
189 std::size_t get_nc_total()
const;
190 DEPRECATED(
"Use get_nc_total().", std::size_t get_ni_total()
const;)
195 const std::vector<std::string>& get_active()
const;
200 const std::vector<std::string>& get_inactive()
const;
205 bool getImpulseStatus(
const std::string& name)
const;
210 template <
class Scalar>
211 friend std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model);
214 boost::shared_ptr<StateMultibody> state_;
215 ImpulseModelContainer impulses_;
217 std::size_t nc_total_;
218 std::vector<std::string> active_;
219 std::vector<std::string> inactive_;
227 template <
typename _Scalar>
229 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
231 typedef _Scalar Scalar;
235 typedef typename MathBase::VectorXs VectorXs;
236 typedef typename MathBase::MatrixXs MatrixXs;
244 template <
template <
typename Scalar>
class Model>
246 : Jc(model->get_nc_total(), model->get_state()->get_nv()),
247 dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
248 vnext(model->get_state()->get_nv()),
249 dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
250 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
255 for (
typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
256 it != model->get_impulses().end(); ++it) {
257 const boost::shared_ptr<ImpulseItem>& item = it->second;
258 impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
271 typename ImpulseModelMultiple::ImpulseDataContainer impulses;
273 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
282 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx" 284 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
Define the multi-impulse data.
State multibody representation.
Define a stack of impulse models.
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.