crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-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_IMPULSES_IMPULSE_6D_HPP_
10 #define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_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/impulse-base.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
21 class ImpulseModel6DTpl : 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 ImpulseData6DTpl<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  ImpulseModel6DTpl(boost::shared_ptr<StateMultibody> state, const std::size_t frame);
37  virtual ~ImpulseModel6DTpl();
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 ImpulseData6DTpl : public ImpulseDataAbstractTpl<_Scalar> {
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63  typedef _Scalar Scalar;
64  typedef MathBaseTpl<Scalar> MathBase;
65  typedef ImpulseDataAbstractTpl<Scalar> Base;
66  typedef typename MathBase::Matrix6xs Matrix6xs;
67 
68  template <template <typename Scalar> class Model>
69  ImpulseData6DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
70  : Base(model, data),
71  fJf(6, model->get_state()->get_nv()),
72  v_partial_dq(6, model->get_state()->get_nv()),
73  v_partial_dv(6, model->get_state()->get_nv()) {
74  frame = model->get_frame();
75  jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
76  fXj = jMf.inverse().toActionMatrix();
77  fJf.setZero();
78  v_partial_dq.setZero();
79  v_partial_dv.setZero();
80  }
81 
82  using Base::df_dx;
83  using Base::dv0_dq;
84  using Base::f;
85  using Base::frame;
86  using Base::Jc;
87  using Base::jMf;
88  using Base::pinocchio;
89 
90  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
91  Matrix6xs fJf;
92  Matrix6xs v_partial_dq;
93  Matrix6xs v_partial_dv;
94 };
95 
96 } // namespace crocoddyl
97 
98 /* --- Details -------------------------------------------------------------- */
99 /* --- Details -------------------------------------------------------------- */
100 /* --- Details -------------------------------------------------------------- */
101 #include "crocoddyl/multibody/impulses/impulse-6d.hxx"
102 
103 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the 6d impulse model.