sot-talos-balance  1.5.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_qSIN)
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  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
87  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
88  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
89  }
90  catch (const std::exception& e)
91  {
92  std::cout << e.what();
93  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
94  return;
95  }
96  m_data = pinocchio::Data(m_model);
97  m_initSucceeded = true;
98  }
99 
100  /* ------------------------------------------------------------------- */
101  /* --- SIGNALS ------------------------------------------------------- */
102  /* ------------------------------------------------------------------- */
103 
105  {
106  if(!m_initSucceeded)
107  {
108  SEND_WARNING_STREAM_MSG("Cannot compute signal com before initialization!");
109  return s;
110  }
111  const Vector & q = m_qSIN(iter);
112  pinocchio::centerOfMass(m_model,m_data,q);
113  s = m_data.com[0];
114  return s;
115  }
116 
118  {
119  if(!m_initSucceeded)
120  {
121  SEND_WARNING_STREAM_MSG("Cannot compute signal dcom before initialization!");
122  return s;
123  }
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  void DcmEstimator::display(std::ostream& os) const
131  {
132  os << "DcmEstimator " << getName();
133  try
134  {
135  getProfiler().report_all(3, os);
136  }
137  catch (ExceptionSignal e) {}
138  }
139  } // namespace talos_balance
140  } // namespace sot
141 } // 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