crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-base.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_IMPULSE_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
11 
12 #include <pinocchio/multibody/data.hpp>
13 #include <pinocchio/spatial/force.hpp>
14 
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/states/multibody.hpp"
17 #include "crocoddyl/core/utils/to-string.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
22 class ImpulseModelAbstractTpl {
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
27  typedef MathBaseTpl<Scalar> MathBase;
28  typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
29  typedef StateMultibodyTpl<Scalar> StateMultibody;
30  typedef typename MathBase::VectorXs VectorXs;
31  typedef typename MathBase::MatrixXs MatrixXs;
32 
33  ImpulseModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& ni);
34  virtual ~ImpulseModelAbstractTpl();
35 
36  virtual void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
37  virtual void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
38 
39  virtual void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force) = 0;
40  void updateForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const MatrixXs& df_dx) const;
41 
42  virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
43 
44  const boost::shared_ptr<StateMultibody>& get_state() const;
45  const std::size_t& get_ni() const;
46 
47  protected:
48  boost::shared_ptr<StateMultibody> state_;
49  std::size_t ni_;
50 };
51 
52 template <typename _Scalar>
53 struct ImpulseDataAbstractTpl {
54  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55 
56  typedef _Scalar Scalar;
57  typedef MathBaseTpl<Scalar> MathBase;
58  typedef typename MathBase::VectorXs VectorXs;
59  typedef typename MathBase::MatrixXs MatrixXs;
60 
61  template <template <typename Scalar> class Model>
62  ImpulseDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
63  : pinocchio(data),
64  joint(0),
65  frame(0),
66  jMf(pinocchio::SE3Tpl<Scalar>::Identity()),
67  Jc(model->get_ni(), model->get_state()->get_nv()),
68  dv0_dq(model->get_ni(), model->get_state()->get_nv()),
69  f(pinocchio::ForceTpl<Scalar>::Zero()),
70  df_dx(model->get_ni(), model->get_state()->get_ndx()) {
71  Jc.setZero();
72  dv0_dq.setZero();
73  df_dx.setZero();
74  }
75  virtual ~ImpulseDataAbstractTpl() {}
76 
77  pinocchio::DataTpl<Scalar>* pinocchio;
78  pinocchio::JointIndex joint;
79  pinocchio::FrameIndex frame;
80  typename pinocchio::SE3Tpl<Scalar> jMf;
81  MatrixXs Jc;
82  MatrixXs dv0_dq;
83  pinocchio::ForceTpl<Scalar> f;
84  MatrixXs df_dx;
85 };
86 
87 } // namespace crocoddyl
88 
89 /* --- Details -------------------------------------------------------------- */
90 /* --- Details -------------------------------------------------------------- */
91 /* --- Details -------------------------------------------------------------- */
92 #include "crocoddyl/multibody/impulse-base.hxx"
93 
94 #endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_