crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/mathbase.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/force-base.hpp"
16 
17 namespace crocoddyl {
18 
19 template <typename _Scalar>
21  public:
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
28  typedef typename MathBase::VectorXs VectorXs;
29  typedef typename MathBase::MatrixXs MatrixXs;
30 
31  ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc, const std::size_t nu);
32  ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc);
33  virtual ~ContactModelAbstractTpl();
34 
35  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
36  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
37 
38  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) = 0;
39  void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
40  const MatrixXs& df_du) const;
41  void setZeroForce(const boost::shared_ptr<ContactDataAbstract>& data) const;
42  void setZeroForceDiff(const boost::shared_ptr<ContactDataAbstract>& data) const;
43 
44  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
45 
46  const boost::shared_ptr<StateMultibody>& get_state() const;
47  std::size_t get_nc() const;
48  std::size_t get_nu() const;
49 
53  pinocchio::FrameIndex get_id() const;
54 
58  void set_id(const pinocchio::FrameIndex id);
59 
63  template <class Scalar>
64  friend std::ostream& operator<<(std::ostream& os, const ContactModelAbstractTpl<Scalar>& model);
65 
71  virtual void print(std::ostream& os) const;
72 
73  protected:
74  boost::shared_ptr<StateMultibody> state_;
75  std::size_t nc_;
76  std::size_t nu_;
77  pinocchio::FrameIndex id_;
78 };
79 
80 template <typename _Scalar>
81 struct ContactDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
82  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 
84  typedef _Scalar Scalar;
87  typedef typename MathBase::VectorXs VectorXs;
88  typedef typename MathBase::MatrixXs MatrixXs;
89 
90  template <template <typename Scalar> class Model>
91  ContactDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
92  : Base(model, data),
93  fXj(jMf.inverse().toActionMatrix()),
94  a0(model->get_nc()),
95  da0_dx(model->get_nc(), model->get_state()->get_ndx()) {
96  a0.setZero();
97  da0_dx.setZero();
98  }
99  virtual ~ContactDataAbstractTpl() {}
100 
101  using Base::df_du;
102  using Base::df_dx;
103  using Base::f;
104  using Base::frame;
105  using Base::Jc;
106  using Base::jMf;
107  using Base::pinocchio;
108  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
109  VectorXs a0;
110  MatrixXs da0_dx;
111 };
112 
113 } // namespace crocoddyl
114 
115 /* --- Details -------------------------------------------------------------- */
116 /* --- Details -------------------------------------------------------------- */
117 /* --- Details -------------------------------------------------------------- */
118 #include "crocoddyl/multibody/contact-base.hxx"
119 
120 #endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
crocoddyl::ForceDataAbstractTpl
Definition: force-base.hpp:21
crocoddyl::ForceDataAbstractTpl::df_du
MatrixXs df_du
Jacobian of the contact forces.
Definition: force-base.hpp:51
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ForceDataAbstractTpl::frame
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
crocoddyl::ForceDataAbstractTpl::df_dx
MatrixXs df_dx
Jacobian of the contact forces.
Definition: force-base.hpp:50
crocoddyl::ForceDataAbstractTpl::jMf
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:46
crocoddyl::ContactModelAbstractTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the contact model.
crocoddyl::ContactModelAbstractTpl
Definition: contact-base.hpp:20
crocoddyl::ContactModelAbstractTpl::get_id
pinocchio::FrameIndex get_id() const
Return the reference frame id.
crocoddyl::StateMultibodyTpl
State multibody representation.
Definition: fwd.hpp:305
crocoddyl::ForceDataAbstractTpl::Jc
MatrixXs Jc
Contact Jacobian.
Definition: force-base.hpp:47
crocoddyl::ForceDataAbstractTpl::f
pinocchio::ForceTpl< Scalar > f
Definition: force-base.hpp:48
crocoddyl::ContactDataAbstractTpl
Definition: contact-base.hpp:81
crocoddyl::ForceDataAbstractTpl::pinocchio
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44
crocoddyl::ContactModelAbstractTpl::operator<<
friend std::ostream & operator<<(std::ostream &os, const ContactModelAbstractTpl< Scalar > &model)
Print information on the contact model.
crocoddyl::ContactModelAbstractTpl::id_
pinocchio::FrameIndex id_
Reference frame id of the contact.
Definition: contact-base.hpp:77
crocoddyl::ContactModelAbstractTpl::set_id
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.