crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-3d.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, 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 "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/impulse-base.hpp"
14 
15 #include <pinocchio/spatial/motion.hpp>
16 #include <pinocchio/multibody/data.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  const std::size_t& get_frame() const;
45 
46  protected:
47  using Base::ni_;
48  using Base::state_;
49 
50  private:
51  std::size_t frame_;
52 };
53 
54 template <typename _Scalar>
55 struct ImpulseData3DTpl : public ImpulseDataAbstractTpl<_Scalar> {
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  typedef _Scalar Scalar;
59  typedef MathBaseTpl<Scalar> MathBase;
60  typedef ImpulseDataAbstractTpl<Scalar> Base;
61  typedef typename MathBase::Matrix6xs Matrix6xs;
62 
63  template <template <typename Scalar> class Model>
64  ImpulseData3DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
65  : Base(model, data),
66  fJf(6, model->get_state()->get_nv()),
67  v_partial_dq(6, model->get_state()->get_nv()),
68  v_partial_dv(6, model->get_state()->get_nv()) {
69  frame = model->get_frame();
70  joint = model->get_state()->get_pinocchio()->frames[frame].parent;
71  jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
72  fXj = jMf.inverse().toActionMatrix();
73  fJf.setZero();
74  v_partial_dq.setZero();
75  v_partial_dv.setZero();
76  }
77 
78  using Base::df_dx;
79  using Base::dv0_dq;
80  using Base::f;
81  using Base::frame;
82  using Base::Jc;
83  using Base::jMf;
84  using Base::joint;
85  using Base::pinocchio;
86 
87  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
88  Matrix6xs fJf;
89  Matrix6xs v_partial_dq;
90  Matrix6xs v_partial_dv;
91 };
92 
93 } // namespace crocoddyl
94 
95 /* --- Details -------------------------------------------------------------- */
96 /* --- Details -------------------------------------------------------------- */
97 /* --- Details -------------------------------------------------------------- */
98 #include "crocoddyl/multibody/impulses/impulse-3d.hxx"
99 
100 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_