9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 #include "crocoddyl/multibody/contact-base.hpp"
22 template <
typename _Scalar>
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
30 ContactItemTpl(
const std::string& name, boost::shared_ptr<ContactModelAbstract> contact,
const bool active =
true)
31 : name(name), contact(contact), active(active) {}
34 boost::shared_ptr<ContactModelAbstract> contact;
45 template <
typename _Scalar>
48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 typedef _Scalar Scalar;
59 typedef typename MathBase::Vector2s Vector2s;
60 typedef typename MathBase::Vector3s Vector3s;
61 typedef typename MathBase::VectorXs VectorXs;
62 typedef typename MathBase::MatrixXs MatrixXs;
64 typedef std::map<std::string, boost::shared_ptr<ContactItem> > ContactModelContainer;
65 typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
66 typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
93 void addContact(
const std::string& name, boost::shared_ptr<ContactModelAbstract> contact,
const bool active =
true);
116 void calc(
const boost::shared_ptr<ContactDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
124 void calcDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
132 void updateAcceleration(
const boost::shared_ptr<ContactDataMultiple>& data,
const VectorXs& dv)
const;
141 void updateForce(
const boost::shared_ptr<ContactDataMultiple>& data,
const VectorXs& force);
150 void updateAccelerationDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
const MatrixXs& ddv_dx)
const;
161 void updateForceDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
const MatrixXs& df_dx,
162 const MatrixXs& df_du)
const;
170 boost::shared_ptr<ContactDataMultiple>
createData(pinocchio::DataTpl<Scalar>*
const data);
175 const boost::shared_ptr<StateMultibody>&
get_state()
const;
185 std::size_t
get_nc()
const;
195 std::size_t
get_nu()
const;
200 const std::vector<std::string>&
get_active()
const;
213 boost::shared_ptr<StateMultibody> state_;
214 ContactModelContainer contacts_;
216 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 a0(model->get_nc_total()),
248 da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
249 dv(model->get_state()->get_nv()),
250 ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
251 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
257 for (
typename ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
258 it != model->get_contacts().end(); ++it) {
259 const boost::shared_ptr<ContactItem>& item = it->second;
260 contacts.insert(std::make_pair(item->name, item->contact->createData(data)));
276 typename ContactModelMultiple::ContactDataContainer
contacts;
278 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
287 #include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
289 #endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_