multiple-contacts.hpp
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef SOBEC_MULTIPLE_CONTACTS_HPP_
10 #define SOBEC_MULTIPLE_CONTACTS_HPP_
11 
12 #include <crocoddyl/core/utils/exception.hpp>
13 #include <crocoddyl/multibody/contact-base.hpp>
14 #include <crocoddyl/multibody/contacts/multiple-contacts.hpp>
15 #include <crocoddyl/multibody/fwd.hpp>
16 #include <map>
17 #include <set>
18 #include <string>
19 #include <utility>
20 
23 #include "sobec/fwd.hpp"
24 
25 namespace sobec {
26 namespace newcontacts {
27 
36 template <typename _Scalar>
38  : public crocoddyl::ContactModelMultipleTpl<_Scalar> {
39  public:
40  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 
42  typedef _Scalar Scalar;
43  typedef crocoddyl::ContactModelMultipleTpl<Scalar> Base;
44  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
45  typedef crocoddyl::StateMultibodyTpl<Scalar> StateMultibody;
46  typedef crocoddyl::ContactDataAbstractTpl<Scalar> ContactDataAbstract;
47  typedef crocoddyl::ContactDataMultipleTpl<Scalar> ContactDataMultiple;
48  typedef crocoddyl::ContactModelAbstractTpl<Scalar> ContactModelAbstract;
49 
50  typedef crocoddyl::ContactItemTpl<Scalar> ContactItem;
51 
52  typedef typename MathBase::Vector2s Vector2s;
53  typedef typename MathBase::Vector3s Vector3s;
54  typedef typename MathBase::VectorXs VectorXs;
55  typedef typename MathBase::MatrixXs MatrixXs;
56  typedef typename MathBase::Matrix3s Matrix3s;
57 
58  typedef std::map<std::string, boost::shared_ptr<ContactItem> >
60  typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> >
62  typedef typename pinocchio::container::aligned_vector<
63  pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
64 
71  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state,
72  const std::size_t nu);
73 
79  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
81 
93  void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data,
94  const boost::shared_ptr<MatrixXs> df_dx,
95  const boost::shared_ptr<MatrixXs> df_du) const;
96 
106  void updateRneaDerivatives(const boost::shared_ptr<ContactDataMultiple>& data,
107  pinocchio::DataTpl<Scalar>& pinocchio) const;
108 
109  // MatrixXs rotateJacobians(const boost::shared_ptr<MatrixXs> Jin);
110 };
111 
112 } // namespace newcontacts
113 } // namespace sobec
114 
115 /* --- Details -------------------------------------------------------------- */
116 /* --- Details -------------------------------------------------------------- */
117 /* --- Details -------------------------------------------------------------- */
119 
120 #endif // SOBEC_MULTIPLE_CONTACTS_HPP_
sobec::newcontacts::ContactModelMultipleTpl::updateForceDiff
void updateForceDiff(const boost::shared_ptr< ContactDataMultiple > &data, const boost::shared_ptr< MatrixXs > df_dx, const boost::shared_ptr< MatrixXs > df_du) const
Update the Jacobian of the spatial force defined in frame coordinate.
Definition: multiple-contacts.hxx:26
sobec::newcontacts::ContactModelMultipleTpl::ContactItem
crocoddyl::ContactItemTpl< Scalar > ContactItem
Definition: multiple-contacts.hpp:50
sobec::newcontacts::ContactModelMultipleTpl::StateMultibody
crocoddyl::StateMultibodyTpl< Scalar > StateMultibody
Definition: multiple-contacts.hpp:45
fwd.hpp
sobec::newcontacts::ContactModelMultipleTpl::~ContactModelMultipleTpl
~ContactModelMultipleTpl()
Definition: multiple-contacts.hxx:23
sobec::newcontacts::ContactModelMultipleTpl::ForceIterator
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > >::iterator ForceIterator
Definition: multiple-contacts.hpp:63
sobec::newcontacts::ContactModelMultipleTpl::Base
crocoddyl::ContactModelMultipleTpl< Scalar > Base
Definition: multiple-contacts.hpp:43
sobec::newcontacts::ContactModelMultipleTpl::ContactModelAbstract
crocoddyl::ContactModelAbstractTpl< Scalar > ContactModelAbstract
Definition: multiple-contacts.hpp:48
sobec::newcontacts::ContactModelMultipleTpl::Vector2s
MathBase::Vector2s Vector2s
Definition: multiple-contacts.hpp:52
sobec::newcontacts::ContactModelMultipleTpl::ContactDataMultiple
crocoddyl::ContactDataMultipleTpl< Scalar > ContactDataMultiple
Definition: multiple-contacts.hpp:47
sobec::newcontacts::ContactModelMultipleTpl::Vector3s
MathBase::Vector3s Vector3s
Definition: multiple-contacts.hpp:53
sobec
Definition: activation-quad-ref.hpp:19
sobec::newcontacts::ContactModelMultipleTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: multiple-contacts.hpp:44
contact3d.hpp
sobec::newcontacts::ContactModelMultipleTpl::VectorXs
MathBase::VectorXs VectorXs
Definition: multiple-contacts.hpp:54
sobec::newcontacts::ContactModelMultipleTpl::ContactDataContainer
std::map< std::string, boost::shared_ptr< ContactDataAbstract > > ContactDataContainer
Definition: multiple-contacts.hpp:61
sobec::newcontacts::ContactModelMultipleTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: multiple-contacts.hpp:42
sobec::newcontacts::ContactModelMultipleTpl::ContactDataAbstract
crocoddyl::ContactDataAbstractTpl< Scalar > ContactDataAbstract
Definition: multiple-contacts.hpp:46
contact1d.hpp
sobec::newcontacts::ContactModelMultipleTpl::MatrixXs
MathBase::MatrixXs MatrixXs
Definition: multiple-contacts.hpp:55
sobec::newcontacts::ContactModelMultipleTpl::Matrix3s
MathBase::Matrix3s Matrix3s
Definition: multiple-contacts.hpp:56
sobec::newcontacts::ContactModelMultipleTpl::ContactModelContainer
std::map< std::string, boost::shared_ptr< ContactItem > > ContactModelContainer
Definition: multiple-contacts.hpp:59
sobec::newcontacts::ContactModelMultipleTpl::ContactModelMultipleTpl
ContactModelMultipleTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the multi-contact model.
Definition: multiple-contacts.hxx:13
multiple-contacts.hxx
sobec::newcontacts::ContactModelMultipleTpl
Define a stack of contact models.
Definition: multiple-contacts.hpp:37
sobec::newcontacts::ContactModelMultipleTpl::updateRneaDerivatives
void updateRneaDerivatives(const boost::shared_ptr< ContactDataMultiple > &data, pinocchio::DataTpl< Scalar > &pinocchio) const
Update the RNEA derivatives dtau_dq by adding the skew term (necessary for contacts expressed in LOCA...
Definition: multiple-contacts.hxx:73