crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-2d.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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_CONTACTS_CONTACT_2D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
11 
12 #include <pinocchio/spatial/motion.hpp>
13 #include <pinocchio/multibody/data.hpp>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
16 
17 #include "crocoddyl/multibody/fwd.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 #include "crocoddyl/multibody/contact-base.hpp"
20 #include "crocoddyl/core/utils/deprecate.hpp"
21 
22 #include "crocoddyl/multibody/frames-deprecated.hpp"
23 
24 namespace crocoddyl {
25 
26 template <typename _Scalar>
27 class ContactModel2DTpl : public ContactModelAbstractTpl<_Scalar> {
28  public:
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 
31  typedef _Scalar Scalar;
37  typedef typename MathBase::Vector2s Vector2s;
38  typedef typename MathBase::Vector3s Vector3s;
39  typedef typename MathBase::VectorXs VectorXs;
40  typedef typename MathBase::Matrix3s Matrix3s;
41 
51  ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Vector2s& xref,
52  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
53 
64  ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Vector2s& xref,
65  const Vector2s& gains = Vector2s::Zero());
66  DEPRECATED("Use constructor which is not based on FrameTranslation.",
67  ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslationTpl<Scalar>& xref,
68  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());)
69  DEPRECATED("Use constructor which is not based on FrameTranslation.",
70  ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslationTpl<Scalar>& xref,
71  const Vector2s& gains = Vector2s::Zero());)
72  virtual ~ContactModel2DTpl();
73 
81  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
82 
90  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
91 
98  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
99 
103  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
104 
108  const Vector2s& get_reference() const;
109 
110  DEPRECATED("Use get_reference() or get_id()", FrameTranslationTpl<Scalar> get_xref() const;)
111 
115  const Vector2s& get_gains() const;
116 
120  void set_reference(const Vector2s& reference);
121 
127  virtual void print(std::ostream& os) const;
128 
129  protected:
130  using Base::id_;
131  using Base::nc_;
132  using Base::nu_;
133  using Base::state_;
134 
135  private:
136  Vector2s xref_;
137  Vector2s gains_;
138 };
139 
140 template <typename _Scalar>
141 struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
142  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 
144  typedef _Scalar Scalar;
147  typedef typename MathBase::Matrix2s Matrix2s;
148  typedef typename MathBase::Matrix3s Matrix3s;
149  typedef typename MathBase::Matrix6xs Matrix6xs;
150  typedef typename MathBase::Vector3s Vector3s;
151 
152  template <template <typename Scalar> class Model>
153  ContactData2DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
154  : Base(model, data),
155  fJf(6, model->get_state()->get_nv()),
156  v_partial_dq(6, model->get_state()->get_nv()),
157  a_partial_dq(6, model->get_state()->get_nv()),
158  a_partial_dv(6, model->get_state()->get_nv()),
159  a_partial_da(6, model->get_state()->get_nv()),
160  fXjdv_dq(6, model->get_state()->get_nv()),
161  fXjda_dq(6, model->get_state()->get_nv()),
162  fXjda_dv(6, model->get_state()->get_nv()) {
163  frame = model->get_id();
164  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
165  fXj = jMf.inverse().toActionMatrix();
166  fJf.setZero();
167  v_partial_dq.setZero();
168  a_partial_dq.setZero();
169  a_partial_dv.setZero();
170  a_partial_da.setZero();
171  fXjdv_dq.setZero();
172  fXjda_dq.setZero();
173  fXjda_dv.setZero();
174  vv.setZero();
175  vw.setZero();
176  vv_skew.setZero();
177  vw_skew.setZero();
178  oRf.setZero();
179  }
180 
181  using Base::a0;
182  using Base::da0_dx;
183  using Base::df_du;
184  using Base::df_dx;
185  using Base::f;
186  using Base::frame;
187  using Base::fXj;
188  using Base::Jc;
189  using Base::jMf;
190  using Base::pinocchio;
191 
192  pinocchio::MotionTpl<Scalar> v;
193  pinocchio::MotionTpl<Scalar> a;
194  Matrix6xs fJf;
195  Matrix6xs v_partial_dq;
196  Matrix6xs a_partial_dq;
197  Matrix6xs a_partial_dv;
198  Matrix6xs a_partial_da;
199  Matrix6xs fXjdv_dq;
200  Matrix6xs fXjda_dq;
201  Matrix6xs fXjda_dv;
202  Vector3s vv;
203  Vector3s vw;
204  Matrix3s vv_skew;
205  Matrix3s vw_skew;
206  Matrix2s oRf;
207 };
208 
209 } // namespace crocoddyl
210 
211 /* --- Details -------------------------------------------------------------- */
212 /* --- Details -------------------------------------------------------------- */
213 /* --- Details -------------------------------------------------------------- */
214 #include "crocoddyl/multibody/contacts/contact-2d.hxx"
215 
216 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
virtual boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 2d contact data.
virtual void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 2d contact holonomic constraint.
ContactModel2DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector2s &xref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 2d contact model.
State multibody representation.
Definition: fwd.hpp:305
virtual void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
Convert the force into a stack of spatial forces.
pinocchio::FrameIndex id_
Reference frame id of the contact.
virtual void print(std::ostream &os) const
Print relevant information of the 2d contact model.
void set_reference(const Vector2s &reference)
Modify the reference frame translation.
const Vector2s & get_reference() const
Return the reference frame translation.
virtual void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the 2d contact Jacobian and drift.