sot-talos-balance  1.7.0
dcm-estimator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2019,
3  * LAAS-CNRS
4  * François Bailly,
5  *
6  * This file is part of sot-talos-balance.
7  * See license file.
8  */
9 
11 #include <sot/core/debug.hh>
12 #include <dynamic-graph/all-commands.h>
13 #include <dynamic-graph/factory.h>
14 #include <sot/core/stop-watch.hh>
15 #include "pinocchio/algorithm/frames.hpp"
16 #include "pinocchio/algorithm/center-of-mass.hpp"
17 
18 namespace dynamicgraph
19 {
20  namespace sot
21  {
22  namespace talos_balance
23  {
24  namespace dg = ::dynamicgraph;
25  using namespace dg;
26  using namespace dg::command;
27  using namespace std;
28  using namespace pinocchio;
29  using boost::math::normal; // typedef provides default type is double.
30  //Size to be aligned "-------------------------------------------------------"
31 
32 #define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"
33 #define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation"
34 #define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation"
35 
36 #define INPUT_SIGNALS m_qSIN << m_vSIN
37 #define OUTPUT_SIGNALS m_cSOUT << m_dcSOUT
38 
41  typedef DcmEstimator EntityClassName;
42  /* --- DG FACTORY ---------------------------------------------------- */
44  "DcmEstimator");
45  /* ------------------------------------------------------------------- */
46  /* --- CONSTRUCTION -------------------------------------------------- */
47  /* ------------------------------------------------------------------- */
48  DcmEstimator::DcmEstimator(const std::string& name)
49  : Entity(name)
50  ,CONSTRUCT_SIGNAL_IN( q, dynamicgraph::Vector)
51  ,CONSTRUCT_SIGNAL_IN( v, dynamicgraph::Vector)
52  ,CONSTRUCT_SIGNAL_OUT(c, dynamicgraph::Vector, m_dcSOUT)
53  ,CONSTRUCT_SIGNAL_OUT(dc, dynamicgraph::Vector, m_qSIN << m_vSIN)
54  ,m_data(pinocchio::Model())
55  {
56  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
57 
58  /* Commands. */
59  addCommand("init",
60  makeCommandVoid2(*this, &DcmEstimator::init,
61  docCommandVoid2("Initialize the entity.",
62  "time step (double)",
63  "URDF file path (string)")));
64  }
65  void DcmEstimator::init(const double & dt, const std::string& robotRef)
66  {
67  m_dt = dt;
68  try
69  {
70  // Retrieve m_robot_util informations
71  std::string localName(robotRef);
72  if (isNameInRobotUtil(localName))
73  {
74  m_robot_util = getRobotUtil(localName);
75  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
76  }
77  else
78  {
79  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
80  return;
81  }
82 
83  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
84  pinocchio::JointModelFreeFlyer(), m_model);
85  }
86  catch (const std::exception& e)
87  {
88  std::cout << e.what();
89  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
90  return;
91  }
92  m_data = pinocchio::Data(m_model);
93  m_initSucceeded = true;
94  }
95 
96  /* ------------------------------------------------------------------- */
97  /* --- SIGNALS ------------------------------------------------------- */
98  /* ------------------------------------------------------------------- */
99 
101  {
102  if(!m_initSucceeded)
103  {
104  SEND_WARNING_STREAM_MSG("Cannot compute signal com before initialization!");
105  return s;
106  }
107  if(s.size()!=3)
108  s.resize(3);
109  const Vector & dc = m_dcSOUT(iter);
110  (void) dc;
111  s = m_data.com[0];
112  return s;
113  }
114 
116  {
117  if(!m_initSucceeded)
118  {
119  SEND_WARNING_STREAM_MSG("Cannot compute signal dcom before initialization!");
120  return s;
121  }
122  if(s.size()!=3)
123  s.resize(3);
124  const Vector & q = m_qSIN(iter);
125  const Vector & v = m_vSIN(iter);
126  pinocchio::centerOfMass(m_model,m_data,q,v);
127  s = m_data.vcom[0];
128  return s;
129  }
130 
131  void DcmEstimator::display(std::ostream& os) const
132  {
133  os << "DcmEstimator " << getName();
134  try
135  {
136  getProfiler().report_all(3, os);
137  }
138  catch (ExceptionSignal e) {}
139  }
140  } // namespace talos_balance
141  } // namespace sot
142 } // namespace dynamicgraph
#define OUTPUT_SIGNALS
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
STL namespace.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define INPUT_SIGNALS
void init(const double &dt, const std::string &urdfFile)
double m_dt
robot velocities according to pinocchio convention
RobotUtilShrPtr m_robot_util
true if the entity has been successfully initialized
virtual void display(std::ostream &os) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DcmEstimator(const std::string &name)
pinocchio::Model m_model
sampling time step