crocoddyl  1.4.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  void setZeroForce(const boost::shared_ptr<ImpulseDataAbstract>& data) const;
42  void setZeroForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data) const;
43 
44  virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
45 
46  const boost::shared_ptr<StateMultibody>& get_state() const;
47  const std::size_t& get_ni() const;
48 
49  protected:
50  boost::shared_ptr<StateMultibody> state_;
51  std::size_t ni_;
52 };
53 
54 template <typename _Scalar>
55 struct ImpulseDataAbstractTpl {
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  typedef _Scalar Scalar;
59  typedef MathBaseTpl<Scalar> MathBase;
60  typedef typename MathBase::VectorXs VectorXs;
61  typedef typename MathBase::MatrixXs MatrixXs;
62 
63  template <template <typename Scalar> class Model>
64  ImpulseDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
65  : pinocchio(data),
66  joint(0),
67  frame(0),
68  jMf(pinocchio::SE3Tpl<Scalar>::Identity()),
69  Jc(model->get_ni(), model->get_state()->get_nv()),
70  dv0_dq(model->get_ni(), model->get_state()->get_nv()),
71  f(pinocchio::ForceTpl<Scalar>::Zero()),
72  df_dx(model->get_ni(), model->get_state()->get_ndx()) {
73  Jc.setZero();
74  dv0_dq.setZero();
75  df_dx.setZero();
76  }
77  virtual ~ImpulseDataAbstractTpl() {}
78 
79  pinocchio::DataTpl<Scalar>* pinocchio;
80  pinocchio::JointIndex joint;
81  pinocchio::FrameIndex frame;
82  typename pinocchio::SE3Tpl<Scalar> jMf;
83  MatrixXs Jc;
84  MatrixXs dv0_dq;
85  pinocchio::ForceTpl<Scalar> f;
86  MatrixXs df_dx;
87 };
88 
89 } // namespace crocoddyl
90 
91 /* --- Details -------------------------------------------------------------- */
92 /* --- Details -------------------------------------------------------------- */
93 /* --- Details -------------------------------------------------------------- */
94 #include "crocoddyl/multibody/impulse-base.hxx"
95 
96 #endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_