crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-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_IMPULSES_IMPULSE_3D_HPP_
10 #define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_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/impulse-base.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
21 class ImpulseModel3DTpl : public ImpulseModelAbstractTpl<_Scalar> {
22  public:
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
26  typedef MathBaseTpl<Scalar> MathBase;
27  typedef ImpulseModelAbstractTpl<Scalar> Base;
28  typedef ImpulseData3DTpl<Scalar> Data;
29  typedef StateMultibodyTpl<Scalar> StateMultibody;
30  typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
31  typedef typename MathBase::Vector2s Vector2s;
32  typedef typename MathBase::Vector3s Vector3s;
33  typedef typename MathBase::VectorXs VectorXs;
34  typedef typename MathBase::MatrixXs MatrixXs;
35 
36  ImpulseModel3DTpl(boost::shared_ptr<StateMultibody> state, const std::size_t frame);
37  virtual ~ImpulseModel3DTpl();
38 
39  virtual void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
40  virtual void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
41  virtual void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force);
42  virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
43 
44  std::size_t get_frame() const;
45 
51  virtual void print(std::ostream& os) const;
52 
53  protected:
54  using Base::state_;
55 
56  private:
57  std::size_t frame_;
58 };
59 
60 template <typename _Scalar>
61 struct ImpulseData3DTpl : public ImpulseDataAbstractTpl<_Scalar> {
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 
64  typedef _Scalar Scalar;
65  typedef MathBaseTpl<Scalar> MathBase;
66  typedef ImpulseDataAbstractTpl<Scalar> Base;
67  typedef typename MathBase::Matrix6xs Matrix6xs;
68 
69  template <template <typename Scalar> class Model>
70  ImpulseData3DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
71  : Base(model, data),
72  fJf(6, model->get_state()->get_nv()),
73  v_partial_dq(6, model->get_state()->get_nv()),
74  v_partial_dv(6, model->get_state()->get_nv()) {
75  frame = model->get_frame();
76  jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
77  fXj = jMf.inverse().toActionMatrix();
78  fJf.setZero();
79  v_partial_dq.setZero();
80  v_partial_dv.setZero();
81  }
82 
83  using Base::df_dx;
84  using Base::dv0_dq;
85  using Base::f;
86  using Base::frame;
87  using Base::Jc;
88  using Base::jMf;
89  using Base::pinocchio;
90 
91  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
92  Matrix6xs fJf;
93  Matrix6xs v_partial_dq;
94  Matrix6xs v_partial_dv;
95 };
96 
97 } // namespace crocoddyl
98 
99 /* --- Details -------------------------------------------------------------- */
100 /* --- Details -------------------------------------------------------------- */
101 /* --- Details -------------------------------------------------------------- */
102 #include "crocoddyl/multibody/impulses/impulse-3d.hxx"
103 
104 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the 3d impulse model.