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_
ContactModelMultipleTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the multi-contact model.
Definition: multiple-contacts.hxx:13
MathBase::Matrix3s Matrix3s
Definition: multiple-contacts.hpp:56
crocoddyl::ContactModelMultipleTpl< Scalar > Base
Definition: multiple-contacts.hpp:43
crocoddyl::ContactDataMultipleTpl< Scalar > ContactDataMultiple
Definition: multiple-contacts.hpp:47
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
MathBase::VectorXs VectorXs
Definition: multiple-contacts.hpp:54
crocoddyl::ContactItemTpl< Scalar > ContactItem
Definition: multiple-contacts.hpp:50
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > >::iterator ForceIterator
Definition: multiple-contacts.hpp:63
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: multiple-contacts.hpp:42
Define a stack of contact models.
Definition: multiple-contacts.hpp:37
Definition: activation-quad-ref.hpp:19
std::map< std::string, boost::shared_ptr< ContactItem > > ContactModelContainer
Definition: multiple-contacts.hpp:59
crocoddyl::ContactModelAbstractTpl< Scalar > ContactModelAbstract
Definition: multiple-contacts.hpp:48
MathBase::Vector2s Vector2s
Definition: multiple-contacts.hpp:52
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: multiple-contacts.hpp:44
MathBase::MatrixXs MatrixXs
Definition: multiple-contacts.hpp:55
MathBase::Vector3s Vector3s
Definition: multiple-contacts.hpp:53
crocoddyl::ContactDataAbstractTpl< Scalar > ContactDataAbstract
Definition: multiple-contacts.hpp:46
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
std::map< std::string, boost::shared_ptr< ContactDataAbstract > > ContactDataContainer
Definition: multiple-contacts.hpp:61
crocoddyl::StateMultibodyTpl< Scalar > StateMultibody
Definition: multiple-contacts.hpp:45
~ContactModelMultipleTpl()
Definition: multiple-contacts.hxx:23