sot-talos-balance  1.5.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 <dynamic-graph/signal-helper.h>
33 #include <sot/core/matrix-geometry.hh>
34 #include <map>
35 #include "boost/assign.hpp"
36 #include <boost/math/distributions/normal.hpp> // for normal_distribution
37 
38 /* Pinocchio */
39 #include <pinocchio/multibody/model.hpp>
40 #include <pinocchio/parsers/urdf.hpp>
41 #include <pinocchio/algorithm/kinematics.hpp>
42 #include <sot/core/robot-utils.hh>
43 
44 namespace dynamicgraph
45 {
46  namespace sot
47  {
48  namespace talos_balance
49  {
50 
51  /* --------------------------------------------------------------------- */
52  /* --- CLASS ----------------------------------------------------------- */
53  /* --------------------------------------------------------------------- */
54 
56  : public::dynamicgraph::Entity
57  {
58  typedef pinocchio::SE3 SE3;
59  typedef Eigen::Vector2d Vector2;
60  typedef Eigen::Vector3d Vector3;
61  typedef Eigen::Vector4d Vector4;
62  typedef Vector6d Vector6;
63  typedef Vector7d Vector7;
64  typedef Eigen::Matrix3d Matrix3;
65  typedef boost::math::normal normal;
66 
67  DYNAMIC_GRAPH_ENTITY_DECL();
68 
69  public:
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 
72  /* --- CONSTRUCTOR ---- */
73  DcmEstimator(const std::string & name );
74 
75  void init(const double & dt, const std::string& urdfFile);
76  /* --- SIGNALS --- */
77  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
78  DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
79 
80  DECLARE_SIGNAL_OUT(c, dynamicgraph::Vector);
81  DECLARE_SIGNAL_OUT(dc, dynamicgraph::Vector);
82 
83  /* --- COMMANDS --- */
84  /* --- ENTITY INHERITANCE --- */
85  virtual void display( std::ostream& os ) const;
86 
87  protected:
89  RobotUtilShrPtr m_robot_util;
90  pinocchio::Data m_data;
91  Eigen::VectorXd m_q_pin;
92  Eigen::VectorXd m_v_pin;
93  double m_dt;
94  pinocchio::Model m_model;
95 
96  }; // class DCMEstimator
97 
98  } // namespace talos_balance
99  } // namespace sot
100 } // namespace dynamicgraph
101 
102 
103 
104 #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