crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-3d.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_CONTACTS_CONTACT_3D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_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 ContactModel3DTpl : 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 
50  ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Vector3s& xref,
51  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
52 
63  ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Vector3s& xref,
64  const Vector2s& gains = Vector2s::Zero());
65  DEPRECATED("Use constructor which is not based on FrameTranslation.",
66  ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslationTpl<Scalar>& xref,
67  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());)
68  DEPRECATED("Use constructor which is not based on FrameTranslation.",
69  ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslationTpl<Scalar>& xref,
70  const Vector2s& gains = Vector2s::Zero());)
71  virtual ~ContactModel3DTpl();
72 
80  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
81 
89  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
90 
97  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
98 
102  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
103 
107  const Vector3s& get_reference() const;
108 
109  DEPRECATED("Use get_reference() or get_id()", FrameTranslationTpl<Scalar> get_xref() const;)
110 
114  const Vector2s& get_gains() const;
115 
119  void set_reference(const Vector3s& reference);
120 
126  virtual void print(std::ostream& os) const;
127 
128  protected:
129  using Base::id_;
130  using Base::nc_;
131  using Base::nu_;
132  using Base::state_;
133 
134  private:
135  Vector3s xref_;
136  Vector2s gains_;
137 };
138 
139 template <typename _Scalar>
140 struct ContactData3DTpl : public ContactDataAbstractTpl<_Scalar> {
141  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142 
143  typedef _Scalar Scalar;
146  typedef typename MathBase::Matrix3s Matrix3s;
147  typedef typename MathBase::Matrix6xs Matrix6xs;
148  typedef typename MathBase::Vector3s Vector3s;
149 
150  template <template <typename Scalar> class Model>
151  ContactData3DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
152  : Base(model, data),
153  fJf(6, model->get_state()->get_nv()),
154  v_partial_dq(6, model->get_state()->get_nv()),
155  a_partial_dq(6, model->get_state()->get_nv()),
156  a_partial_dv(6, model->get_state()->get_nv()),
157  a_partial_da(6, model->get_state()->get_nv()),
158  fXjdv_dq(6, model->get_state()->get_nv()),
159  fXjda_dq(6, model->get_state()->get_nv()),
160  fXjda_dv(6, model->get_state()->get_nv()) {
161  frame = model->get_id();
162  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
163  fXj = jMf.inverse().toActionMatrix();
164  fJf.setZero();
165  v_partial_dq.setZero();
166  a_partial_dq.setZero();
167  a_partial_dv.setZero();
168  a_partial_da.setZero();
169  fXjdv_dq.setZero();
170  fXjda_dq.setZero();
171  fXjda_dv.setZero();
172  vv.setZero();
173  vw.setZero();
174  vv_skew.setZero();
175  vw_skew.setZero();
176  oRf.setZero();
177  }
178 
179  using Base::a0;
180  using Base::da0_dx;
181  using Base::df_du;
182  using Base::df_dx;
183  using Base::f;
184  using Base::frame;
185  using Base::fXj;
186  using Base::Jc;
187  using Base::jMf;
188  using Base::pinocchio;
189 
190  pinocchio::MotionTpl<Scalar> v;
191  pinocchio::MotionTpl<Scalar> a;
192  Matrix6xs fJf;
193  Matrix6xs v_partial_dq;
194  Matrix6xs a_partial_dq;
195  Matrix6xs a_partial_dv;
196  Matrix6xs a_partial_da;
197  Matrix6xs fXjdv_dq;
198  Matrix6xs fXjda_dq;
199  Matrix6xs fXjda_dv;
200  Vector3s vv;
201  Vector3s vw;
202  Matrix3s vv_skew;
203  Matrix3s vw_skew;
204  Matrix3s oRf;
205 };
206 
207 } // namespace crocoddyl
208 
209 /* --- Details -------------------------------------------------------------- */
210 /* --- Details -------------------------------------------------------------- */
211 /* --- Details -------------------------------------------------------------- */
212 #include "crocoddyl/multibody/contacts/contact-3d.hxx"
213 
214 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_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::ContactModel3DTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the 3d contact model.
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ForceDataAbstractTpl::frame
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
crocoddyl::ContactModel3DTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 3d contact holonomic constraint.
crocoddyl::ContactModel3DTpl::updateForce
virtual void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
Convert the force into a stack of spatial forces.
crocoddyl::ContactModel3DTpl::get_reference
const Vector3s & get_reference() const
Return the reference frame translation.
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::ContactModel3DTpl::calc
virtual void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the 3d contact Jacobian and drift.
crocoddyl::ContactModel3DTpl::set_reference
void set_reference(const Vector3s &reference)
Modify the reference frame translation.
crocoddyl::FrameTranslationTpl< Scalar >
crocoddyl::ContactModelAbstractTpl
Definition: contact-base.hpp:20
crocoddyl::ContactModel3DTpl::ContactModel3DTpl
ContactModel3DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector3s &xref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 3d contact model.
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::ContactModel3DTpl::createData
virtual boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 3d contact data.
crocoddyl::ContactModelAbstractTpl::id_
pinocchio::FrameIndex id_
Reference frame id of the contact.
Definition: contact-base.hpp:77
crocoddyl::ContactData3DTpl
Definition: contact-3d.hpp:140
crocoddyl::ContactModel3DTpl
Definition: contact-3d.hpp:27