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) {}
37 os <<
"{" << *model.impulse <<
"}";
42 boost::shared_ptr<ImpulseModelAbstract> impulse;
53 template <
typename _Scalar>
54 class ImpulseModelMultipleTpl {
56 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
65 typedef ImpulseItemTpl<Scalar> ImpulseItem;
67 typedef typename MathBase::Vector2s Vector2s;
68 typedef typename MathBase::Vector3s Vector3s;
69 typedef typename MathBase::VectorXs VectorXs;
70 typedef typename MathBase::MatrixXs MatrixXs;
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;
93 void addImpulse(
const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
const bool active =
true);
116 void calc(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
124 void calcDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
132 void updateVelocity(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const VectorXs& vnext)
const;
141 void updateForce(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const VectorXs& impulse);
150 void updateVelocityDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const MatrixXs& dvnext_dx)
const;
159 void updateForceDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
const MatrixXs& df_dx)
const;
167 boost::shared_ptr<ImpulseDataMultiple>
createData(pinocchio::DataTpl<Scalar>*
const data);
172 const boost::shared_ptr<StateMultibody>&
get_state()
const;
182 std::size_t
get_nc()
const;
183 DEPRECATED(
"Use get_nc().", std::size_t get_ni()
const;)
188 std::size_t get_nc_total()
const;
189 DEPRECATED(
"Use get_nc_total().", std::size_t get_ni_total()
const;)
194 const std::vector<std::string>& get_active()
const;
209 template <
class Scalar>
210 friend std::ostream&
operator<<(std::ostream& os,
const ImpulseModelMultipleTpl<Scalar>& model);
213 boost::shared_ptr<StateMultibody> state_;
214 ImpulseModelContainer impulses_;
216 std::size_t nc_total_;
217 std::vector<std::string> active_;
218 std::vector<std::string> inactive_;
226 template <
typename _Scalar>
227 struct ImpulseDataMultipleTpl {
228 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
243 template <
template <
typename Scalar>
class Model>
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()) {
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)));
270 typename ImpulseModelMultiple::ImpulseDataContainer
impulses;
272 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
281 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
283 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_