crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-6d.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_6D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
11 
12 #include <pinocchio/spatial/motion.hpp>
13 #include <pinocchio/multibody/data.hpp>
14 
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/contact-base.hpp"
17 #include "crocoddyl/core/utils/deprecate.hpp"
18 
19 #include "crocoddyl/multibody/frames-deprecated.hpp"
20 
21 namespace crocoddyl {
22 
23 template <typename _Scalar>
24 class ContactModel6DTpl : public ContactModelAbstractTpl<_Scalar> {
25  public:
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
28  typedef _Scalar Scalar;
34  typedef pinocchio::SE3Tpl<Scalar> SE3;
35  typedef typename MathBase::Vector2s Vector2s;
36  typedef typename MathBase::Vector3s Vector3s;
37  typedef typename MathBase::VectorXs VectorXs;
38 
48  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const SE3& pref,
49  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
50 
61  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const SE3& pref,
62  const Vector2s& gains = Vector2s::Zero());
63  DEPRECATED("Use constructor which is not based on FramePlacement.",
64  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacementTpl<Scalar>& Mref,
65  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());)
66  DEPRECATED("Use constructor which is not based on FramePlacement.",
67  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacementTpl<Scalar>& Mref,
68  const Vector2s& gains = Vector2s::Zero());)
69  virtual ~ContactModel6DTpl();
70 
78  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
79 
87  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
88 
95  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
96 
100  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
101 
105  const SE3& get_reference() const;
106  DEPRECATED("Use get_reference() or get_id()", FramePlacementTpl<Scalar> get_Mref() const;)
107 
111  const Vector2s& get_gains() const;
112 
116  void set_reference(const SE3& reference);
117 
123  virtual void print(std::ostream& os) const;
124 
125  protected:
126  using Base::id_;
127  using Base::nc_;
128  using Base::nu_;
129  using Base::state_;
130 
131  private:
132  SE3 pref_;
133  Vector2s gains_;
134 };
135 
136 template <typename _Scalar>
137 struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
138  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139 
140  typedef _Scalar Scalar;
143  typedef typename MathBase::Matrix6xs Matrix6xs;
144  typedef typename MathBase::Matrix6s Matrix6s;
145 
146  template <template <typename Scalar> class Model>
147  ContactData6DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
148  : Base(model, data),
149  rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
150  v_partial_dq(6, model->get_state()->get_nv()),
151  a_partial_dq(6, model->get_state()->get_nv()),
152  a_partial_dv(6, model->get_state()->get_nv()),
153  a_partial_da(6, model->get_state()->get_nv()) {
154  frame = model->get_id();
155  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
156  fXj = jMf.inverse().toActionMatrix();
157  v_partial_dq.setZero();
158  a_partial_dq.setZero();
159  a_partial_dv.setZero();
160  a_partial_da.setZero();
161  rMf_Jlog6.setZero();
162  }
163 
164  using Base::a0;
165  using Base::da0_dx;
166  using Base::df_du;
167  using Base::df_dx;
168  using Base::f;
169  using Base::frame;
170  using Base::fXj;
171  using Base::Jc;
172  using Base::jMf;
173  using Base::pinocchio;
174 
175  pinocchio::SE3Tpl<Scalar> rMf;
176  pinocchio::MotionTpl<Scalar> v;
177  pinocchio::MotionTpl<Scalar> a;
178  Matrix6xs v_partial_dq;
179  Matrix6xs a_partial_dq;
180  Matrix6xs a_partial_dv;
181  Matrix6xs a_partial_da;
182  Matrix6s rMf_Jlog6;
183 };
184 
185 } // namespace crocoddyl
186 /* --- Details -------------------------------------------------------------- */
187 /* --- Details -------------------------------------------------------------- */
188 /* --- Details -------------------------------------------------------------- */
189 #include "crocoddyl/multibody/contacts/contact-6d.hxx"
190 
191 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
crocoddyl::ForceDataAbstractTpl
Definition: force-base.hpp:21
crocoddyl::ContactModel6DTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the 6d contact model.
crocoddyl::ForceDataAbstractTpl::df_du
MatrixXs df_du
Jacobian of the contact forces.
Definition: force-base.hpp:51
crocoddyl::ContactModel6DTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 6d contact holonomic constraint.
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ForceDataAbstractTpl::frame
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
crocoddyl::ContactModel6DTpl::updateForce
virtual void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
Convert the force into a stack of spatial forces.
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::ContactModel6DTpl::calc
virtual void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the 3d contact Jacobian and drift.
crocoddyl::ContactModelAbstractTpl
Definition: contact-base.hpp:20
crocoddyl::FramePlacementTpl< Scalar >
crocoddyl::ContactData6DTpl
Definition: contact-6d.hpp:137
crocoddyl::ContactModel6DTpl::ContactModel6DTpl
ContactModel6DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const SE3 &pref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 6d 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::ContactModel6DTpl::createData
virtual boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 6d contact data.
crocoddyl::ContactModel6DTpl::set_reference
void set_reference(const SE3 &reference)
Modify the reference frame placement.
crocoddyl::ContactModel6DTpl::get_reference
const SE3 & get_reference() const
Return the reference frame placement.
crocoddyl::ContactModelAbstractTpl::id_
pinocchio::FrameIndex id_
Reference frame id of the contact.
Definition: contact-base.hpp:77
crocoddyl::ContactModel6DTpl
Definition: contact-6d.hpp:24