sot-talos-balance  1.7.0
dcm-estimator.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2019,
3  * LAAS-CNRS,
4  * Gepetto team
5  *
6  * This file is part of sot-talos-balance.
7  * See license file
8  */
9 
10 #ifndef __sot_talos_balance_dcm_estimator_H__
11 #define __sot_talos_balance_dcm_estimator_H__
12 
13 /* --------------------------------------------------------------------- */
14 /* --- API ------------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
16 
17 #if defined (WIN32)
18 # if defined (dcm_estimator_EXPORTS)
19 # define DCMESTIMATOR_EXPORT __declspec(dllexport)
20 # else
21 # define DCMESTIMATOR_EXPORT __declspec(dllimport)
22 # endif
23 #else
24 # define DCMESTIMATOR_EXPORT
25 #endif
26 
27 
28 /* --------------------------------------------------------------------- */
29 /* --- INCLUDE --------------------------------------------------------- */
30 /* --------------------------------------------------------------------- */
31 
32 #include <pinocchio/fwd.hpp>
33 #include <dynamic-graph/signal-helper.h>
34 #include <sot/core/matrix-geometry.hh>
35 #include <map>
36 #include "boost/assign.hpp"
37 #include <boost/math/distributions/normal.hpp> // for normal_distribution
38 
39 /* Pinocchio */
40 #include <pinocchio/multibody/model.hpp>
41 #include <pinocchio/parsers/urdf.hpp>
42 #include <pinocchio/algorithm/kinematics.hpp>
43 #include <sot/core/robot-utils.hh>
44 
45 namespace dynamicgraph
46 {
47  namespace sot
48  {
49  namespace talos_balance
50  {
51 
52  /* --------------------------------------------------------------------- */
53  /* --- CLASS ----------------------------------------------------------- */
54  /* --------------------------------------------------------------------- */
55 
57  : public::dynamicgraph::Entity
58  {
59  typedef pinocchio::SE3 SE3;
60  typedef Eigen::Vector2d Vector2;
61  typedef Eigen::Vector3d Vector3;
62  typedef Eigen::Vector4d Vector4;
63  typedef Vector6d Vector6;
64  typedef Vector7d Vector7;
65  typedef Eigen::Matrix3d Matrix3;
66  typedef boost::math::normal normal;
67 
68  DYNAMIC_GRAPH_ENTITY_DECL();
69 
70  public:
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 
73  /* --- CONSTRUCTOR ---- */
74  DcmEstimator(const std::string & name );
75 
76  void init(const double & dt, const std::string& urdfFile);
77  /* --- SIGNALS --- */
78  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
79  DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
80 
81  DECLARE_SIGNAL_OUT(c, dynamicgraph::Vector);
82  DECLARE_SIGNAL_OUT(dc, dynamicgraph::Vector);
83 
84  /* --- COMMANDS --- */
85  /* --- ENTITY INHERITANCE --- */
86  virtual void display( std::ostream& os ) const;
87 
88  protected:
90  RobotUtilShrPtr m_robot_util;
91  pinocchio::Data m_data;
92  Eigen::VectorXd m_q_pin;
93  Eigen::VectorXd m_v_pin;
94  double m_dt;
95  pinocchio::Model m_model;
96 
97  }; // class DCMEstimator
98 
99  } // namespace talos_balance
100  } // namespace sot
101 } // namespace dynamicgraph
102 
103 
104 
105 #endif // #ifndef __sot_talos_balance_dcm_estimator_H__
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: math/fwd.hh:45
#define DCMESTIMATOR_EXPORT
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
Eigen::VectorXd m_q_pin
Pinocchio robot data.
Eigen::VectorXd m_v_pin
robot configuration according to pinocchio convention
double m_dt
robot velocities according to pinocchio convention
RobotUtilShrPtr m_robot_util
true if the entity has been successfully initialized
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: math/fwd.hh:46
pinocchio::Model m_model
sampling time step