9 #ifndef CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_ 10 #define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_ 17 #include "crocoddyl/multibody/fwd.hpp" 18 #include "crocoddyl/core/utils/exception.hpp" 19 #include "crocoddyl/multibody/impulse-base.hpp" 23 template <
typename _Scalar>
24 struct ImpulseItemTpl {
25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 typedef _Scalar Scalar;
28 typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
31 ImpulseItemTpl(
const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
const bool active =
true)
32 : name(name), impulse(impulse), active(active) {}
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>
55 class ImpulseModelMultipleTpl {
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 typedef _Scalar Scalar;
60 typedef MathBaseTpl<Scalar> MathBase;
61 typedef StateMultibodyTpl<Scalar> StateMultibody;
62 typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
63 typedef ImpulseDataMultipleTpl<Scalar> ImpulseDataMultiple;
64 typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
66 typedef ImpulseItemTpl<Scalar> ImpulseItem;
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);
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;
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::set<std::string>& get_active_set()
const;
202 DEPRECATED(
"get_active() is deprecated and will be replaced with get_active_set()",
203 const std::vector<std::string>& get_active() {
205 active_.reserve(active_set_.size());
206 for (
const auto& contact : active_set_) {
207 active_.push_back(contact);
212 DEPRECATED(
"get_inactive() is deprecated and will be replaced with get_inactive_set()",
213 const std::vector<std::string>& get_inactive() {
215 inactive_.reserve(inactive_set_.size());
216 for (
const auto& contact : inactive_set_) {
217 inactive_.push_back(contact);
230 template <
class Scalar>
231 friend std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model);
234 boost::shared_ptr<StateMultibody> state_;
235 ImpulseModelContainer impulses_;
237 std::size_t nc_total_;
238 std::set<std::string> active_set_;
239 std::set<std::string> inactive_set_;
243 std::vector<std::string> active_;
244 std::vector<std::string> inactive_;
252 template <
typename _Scalar>
253 struct ImpulseDataMultipleTpl {
254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
256 typedef _Scalar Scalar;
257 typedef MathBaseTpl<Scalar> MathBase;
258 typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
259 typedef ImpulseItemTpl<Scalar> ImpulseItem;
260 typedef typename MathBase::VectorXs VectorXs;
261 typedef typename MathBase::MatrixXs MatrixXs;
269 template <
template <
typename Scalar>
class Model>
271 :
Jc(model->get_nc_total(), model->get_state()->get_nv()),
272 dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
273 vnext(model->get_state()->get_nv()),
274 dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
275 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
280 for (
typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
281 it != model->get_impulses().end(); ++it) {
282 const boost::shared_ptr<ImpulseItem>& item = it->second;
283 impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
296 typename ImpulseModelMultiple::ImpulseDataContainer
impulses;
298 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
307 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx" 309 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_ void changeImpulseStatus(const std::string &name, const bool active)
Change the impulse status.
const boost::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
std::size_t get_nc() const
Return the dimension of active impulses.
void updateVelocityDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &dvnext_dx) const
Update the Jacobian of the system velocity after impulse.
void calc(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total contact Jacobian and contact acceleration.
void updateForce(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &impulse)
Update the spatial impulse defined in frame coordinate.
void calcDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the contact holonomic constraint.
ImpulseModelMultiple::ImpulseDataContainer impulses
Stack of impulse data.
const ImpulseModelContainer & get_impulses() const
Return the impulse models.
bool getImpulseStatus(const std::string &name) const
Return the status of a given impulse name.
void updateVelocity(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &vnext) const
Update the system velocity after impulse.
void updateForceDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &df_dx) const
Update the Jacobian of the spatial impulse defined in frame coordinate.
DEPRECATED("Use get_nc().", std::size_t get_ni() const ;) std DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const ;) const std const std::set< std::string > & get_inactive_set() const
Return the dimension of all impulses.
void removeImpulse(const std::string &name)
Remove impulse item.
ImpulseModelMultipleTpl(boost::shared_ptr< StateMultibody > state)
Initialize the multi-impulse model.
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.
void addImpulse(const std::string &name, boost::shared_ptr< ImpulseModelAbstract > impulse, const bool active=true)
Add impulse item.
boost::shared_ptr< ImpulseDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-impulse data.