crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-1d.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_1D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_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 
21 #include "crocoddyl/multibody/frames-deprecated.hpp"
22 
23 namespace crocoddyl {
24 
25 template <typename _Scalar>
26 class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
27  public:
28  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 
30  typedef _Scalar Scalar;
36  typedef typename MathBase::Vector2s Vector2s;
37  typedef typename MathBase::Vector3s Vector3s;
38  typedef typename MathBase::VectorXs VectorXs;
39  typedef typename MathBase::Matrix3s Matrix3s;
40 
50  ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Scalar xref,
51  const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
52 
63  ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Scalar xref,
64  const Vector2s& gains = Vector2s::Zero());
65 
66  virtual ~ContactModel1DTpl();
67 
75  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
76 
84  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
85 
92  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
93 
97  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
98 
102  const Scalar get_reference() const;
103 
107  const Vector2s& get_gains() const;
108 
112  void set_reference(const Scalar reference);
113 
119  virtual void print(std::ostream& os) const;
120 
121  protected:
122  using Base::id_;
123  using Base::nc_;
124  using Base::nu_;
125  using Base::state_;
126 
127  private:
128  Scalar xref_;
129  Vector2s gains_;
130 };
131 
132 template <typename _Scalar>
133 struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
134  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135 
136  typedef _Scalar Scalar;
139  typedef typename MathBase::Matrix2s Matrix2s;
140  typedef typename MathBase::Matrix3s Matrix3s;
141  typedef typename MathBase::Matrix6xs Matrix6xs;
142  typedef typename MathBase::Vector3s Vector3s;
143 
144  template <template <typename Scalar> class Model>
145  ContactData1DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
146  : Base(model, data),
147  fJf(6, model->get_state()->get_nv()),
148  v_partial_dq(6, model->get_state()->get_nv()),
149  a_partial_dq(6, model->get_state()->get_nv()),
150  a_partial_dv(6, model->get_state()->get_nv()),
151  a_partial_da(6, model->get_state()->get_nv()),
152  fXjdv_dq(6, model->get_state()->get_nv()),
153  fXjda_dq(6, model->get_state()->get_nv()),
154  fXjda_dv(6, model->get_state()->get_nv()) {
155  frame = model->get_id();
156  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
157  fXj = jMf.inverse().toActionMatrix();
158  fJf.setZero();
159  v_partial_dq.setZero();
160  a_partial_dq.setZero();
161  a_partial_dv.setZero();
162  a_partial_da.setZero();
163  fXjdv_dq.setZero();
164  fXjda_dq.setZero();
165  fXjda_dv.setZero();
166  vv.setZero();
167  vw.setZero();
168  vv_skew.setZero();
169  vw_skew.setZero();
170  oRf.setZero();
171  }
172 
173  using Base::a0;
174  using Base::da0_dx;
175  using Base::df_du;
176  using Base::df_dx;
177  using Base::f;
178  using Base::frame;
179  using Base::fXj;
180  using Base::Jc;
181  using Base::jMf;
182  using Base::pinocchio;
183 
184  pinocchio::MotionTpl<Scalar> v;
185  pinocchio::MotionTpl<Scalar> a;
186  Matrix6xs fJf;
187  Matrix6xs v_partial_dq;
188  Matrix6xs a_partial_dq;
189  Matrix6xs a_partial_dv;
190  Matrix6xs a_partial_da;
191  Matrix6xs fXjdv_dq;
192  Matrix6xs fXjda_dq;
193  Matrix6xs fXjda_dv;
194  Vector3s vv;
195  Vector3s vw;
196  Matrix3s vv_skew;
197  Matrix3s vw_skew;
198  Matrix2s oRf;
199 };
200 
201 } // namespace crocoddyl
202 
203 /* --- Details -------------------------------------------------------------- */
204 /* --- Details -------------------------------------------------------------- */
205 /* --- Details -------------------------------------------------------------- */
206 #include "crocoddyl/multibody/contacts/contact-1d.hxx"
207 
208 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_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::ContactModel1DTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the 1d contact model.
crocoddyl::ContactModel1DTpl::get_reference
const Scalar get_reference() const
Return the reference frame translation.
crocoddyl::ContactModel1DTpl
Definition: contact-1d.hpp:26
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ForceDataAbstractTpl::frame
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
crocoddyl::ContactModel1DTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 1d contact holonomic constraint.
crocoddyl::ContactModel1DTpl::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::ContactModel1DTpl::calc
virtual void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the 1d contact Jacobian and drift.
crocoddyl::ContactModel1DTpl::set_reference
void set_reference(const Scalar reference)
Modify the reference frame translation.
crocoddyl::ContactModelAbstractTpl
Definition: contact-base.hpp:20
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::ContactModel1DTpl::ContactModel1DTpl
ContactModel1DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Scalar xref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 1d contact model.
crocoddyl::ForceDataAbstractTpl::pinocchio
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44
crocoddyl::ContactModel1DTpl::createData
virtual boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 1d contact data.
crocoddyl::ContactModel1DTpl::get_gains
const Vector2s & get_gains() const
Create the 1d contact data.
crocoddyl::ContactData1DTpl
Definition: contact-1d.hpp:133
crocoddyl::ContactModelAbstractTpl::id_
pinocchio::FrameIndex id_
Reference frame id of the contact.
Definition: contact-base.hpp:77