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) {}
36 template <
class Scalar>
37 friend std::ostream& operator<<(std::ostream& os, const ContactItemTpl<Scalar>& model) {
38 os <<
"{" << *model.contact <<
"}";
43 boost::shared_ptr<ContactModelAbstract> contact;
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<ContactItem> > ContactModelContainer;
74 typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
75 typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
102 void addContact(
const std::string& name, boost::shared_ptr<ContactModelAbstract> contact,
const bool active =
true);
109 void removeContact(
const std::string& name);
117 void changeContactStatus(
const std::string& name,
const bool active);
125 void calc(
const boost::shared_ptr<ContactDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
133 void calcDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
const Eigen::Ref<const VectorXs>& x);
141 void updateAcceleration(
const boost::shared_ptr<ContactDataMultiple>& data,
const VectorXs& dv)
const;
150 void updateForce(
const boost::shared_ptr<ContactDataMultiple>& data,
const VectorXs& force);
159 void updateAccelerationDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
const MatrixXs& ddv_dx)
const;
170 void updateForceDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
const MatrixXs& df_dx,
171 const MatrixXs& df_du)
const;
179 boost::shared_ptr<ContactDataMultiple> createData(pinocchio::DataTpl<Scalar>*
const data);
184 const boost::shared_ptr<StateMultibody>& get_state()
const;
189 const ContactModelContainer& get_contacts()
const;
194 std::size_t get_nc()
const;
199 std::size_t get_nc_total()
const;
204 std::size_t get_nu()
const;
209 const std::vector<std::string>& get_active()
const;
214 const std::vector<std::string>& get_inactive()
const;
219 bool getContactStatus(
const std::string& name)
const;
224 template <
class Scalar>
225 friend std::ostream& operator<<(std::ostream& os, const ContactModelMultipleTpl<Scalar>& model);
228 boost::shared_ptr<StateMultibody> state_;
229 ContactModelContainer contacts_;
231 std::size_t nc_total_;
233 std::vector<std::string> active_;
234 std::vector<std::string> inactive_;
242 template <
typename _Scalar>
244 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
246 typedef _Scalar Scalar;
250 typedef typename MathBase::VectorXs VectorXs;
251 typedef typename MathBase::MatrixXs MatrixXs;
259 template <
template <
typename Scalar>
class Model>
261 : Jc(model->get_nc_total(), model->get_state()->get_nv()),
262 a0(model->get_nc_total()),
263 da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
264 dv(model->get_state()->get_nv()),
265 ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
266 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
272 for (
typename ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
273 it != model->get_contacts().end(); ++it) {
274 const boost::shared_ptr<ContactItem>& item = it->second;
275 contacts.insert(std::make_pair(item->name, item->contact->createData(data)));
291 typename ContactModelMultiple::ContactDataContainer contacts;
293 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
302 #include "crocoddyl/multibody/contacts/multiple-contacts.hxx" 304 #endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
State multibody representation.