sot-talos-balance  1.6.0
ft-calibration.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  * LAAS-CNRS
4  * F. Bailly
5  * T. Flayols
6  */
7 
8 
10 #include <sot/core/debug.hh>
11 #include <dynamic-graph/factory.h>
12 
13 
14 #include <dynamic-graph/all-commands.h>
15 #include <sot/core/stop-watch.hh>
17 
18 #define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating
19 
20 using namespace sot::talos_balance;
21 
22 namespace dynamicgraph
23 {
24  namespace sot
25  {
26  namespace talos_balance
27  {
28  namespace dynamicgraph = ::dynamicgraph;
29  using namespace dynamicgraph;
30  using namespace dynamicgraph::command;
31  using namespace dg::sot::talos_balance;
32 
33 #define INPUT_SIGNALS m_right_foot_force_inSIN << m_left_foot_force_inSIN
34 #define OUTPUT_SIGNALS m_right_foot_force_outSOUT << m_left_foot_force_outSOUT
35 
38  typedef FtCalibration EntityClassName;
39 
40  /* --- DG FACTORY ---------------------------------------------------- */
42  "FtCalibration");
43 
44  /* ------------------------------------------------------------------- */
45  /* --- CONSTRUCTION -------------------------------------------------- */
46  /* ------------------------------------------------------------------- */
48  FtCalibration(const std::string& name)
49  : Entity(name)
50  , CONSTRUCT_SIGNAL_IN(right_foot_force_in, dynamicgraph::Vector)
51  , CONSTRUCT_SIGNAL_IN(left_foot_force_in, dynamicgraph::Vector)
52  , CONSTRUCT_SIGNAL_OUT(right_foot_force_out, dynamicgraph::Vector, m_right_foot_force_inSIN)
53  , CONSTRUCT_SIGNAL_OUT(left_foot_force_out, dynamicgraph::Vector, m_left_foot_force_inSIN)
54  ,m_robot_util(RefVoidRobotUtil())
55  ,m_initSucceeded(false)
56  {
57 
58  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
59 
60  /* Commands. */
61  addCommand("init",
62  makeCommandVoid1(*this, &FtCalibration::init,
63  docCommandVoid1("Initialize the entity.",
64  "Robot reference (string)")));
65  addCommand("setRightFootWeight",
66  makeCommandVoid1(*this, &FtCalibration::setRightFootWeight,
67  docCommandVoid1("Set the weight of the right foot underneath the sensor",
68  "Vector of default forces in Newton")));
69  addCommand("setLeftFootWeight",
70  makeCommandVoid1(*this, &FtCalibration::setLeftFootWeight,
71  docCommandVoid1("Set the weight of the left foot underneath the sensor",
72  "Vector of default forces in Newton")));
73  addCommand("calibrateFeetSensor",
74  makeCommandVoid0(*this, &FtCalibration::calibrateFeetSensor,
75  docCommandVoid0("Calibrate the feet senors")));
76 
77  }
78 
79  void FtCalibration::init(const std::string &robotRef)
80  {
81  dgADD_OSTREAM_TO_RTLOG (std::cout);
82  std::string localName(robotRef);
83  m_initSucceeded = true;
84  if (!isNameInRobotUtil(localName))
85  {
86  m_robot_util = createRobotUtil(localName);
87  }
88  else
89  {
90  m_robot_util = getRobotUtil(localName);
91  }
92  m_right_FT_offset << 0,0,0,0,0,0;
93  m_left_FT_offset << 0,0,0,0,0,0;
94  m_right_FT_offset_calibration_sum << 0,0,0,0,0,0;
95  m_left_FT_offset_calibration_sum << 0,0,0,0,0,0;
96  SEND_MSG("Entity Initialized",MSG_TYPE_INFO);
97  }
98 
99 
100  /* ------------------------------------------------------------------- */
101  /* --- SIGNALS ------------------------------------------------------- */
102  /* ------------------------------------------------------------------- */
103 
105  {
106  if(!m_initSucceeded)
107  {
108  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
109  return s;
110  }
111  if(s.size()!=6)
112  s.resize(6);
113 
114  const Vector & right_foot_force = m_right_foot_force_inSIN(iter);
115 
116  assert(right_foot_force.size() == 6 && "Unexpected size of signal right_foot_force_in, should be 6.");
117 
118  //do offset calibration if needed
119  if (m_right_calibration_iter > 0)
120  {
121  m_right_FT_offset_calibration_sum += right_foot_force ;
123  }
124  else if (m_right_calibration_iter == 0)
125  {
126  SEND_INFO_STREAM_MSG("Calibrating ft sensors...");
128  }
129 
130  //remove offset and foot weight
131  s = right_foot_force - m_left_foot_weight - m_right_FT_offset;
132 
133  return s;
134  }
135 
137  {
138  if(!m_initSucceeded)
139  {
140  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
141  return s;
142  }
143  if(s.size()!=6)
144  s.resize(6);
145 
146  const Vector & left_foot_force = m_left_foot_force_inSIN(iter);
147 
148  assert(left_foot_force.size() == 6 && "Unexpected size of signal left_foot_force_in, should be 6.");
149 
150  //do offset calibration if needed
151  if (m_left_calibration_iter > 0)
152  {
153  m_left_FT_offset_calibration_sum += left_foot_force;
155  }
156  else if (m_left_calibration_iter == 0)
157  {
159  }
160  //remove offset and foot weight
161  s = left_foot_force - m_left_foot_weight - m_left_FT_offset;
162 
163  return s;
164  }
165  /* --- COMMANDS ---------------------------------------------------------- */
166 
167  void FtCalibration::setRightFootWeight(const double &rightW)
168  {
169  if(!m_initSucceeded)
170  {
171  SEND_WARNING_STREAM_MSG("Cannot set right foot weight before initialization!");
172  return;
173  }
174  m_right_foot_weight << 0,0,rightW,0,0,0;
175  }
176 
177  void FtCalibration::setLeftFootWeight(const double &leftW)
178  {
179  if(!m_initSucceeded)
180  {
181  SEND_WARNING_STREAM_MSG("Cannot set left foot weight before initialization!");
182  return;
183  }
184  m_left_foot_weight << 0,0,leftW,0,0,0;
185  }
186 
188  {
189  SEND_WARNING_STREAM_MSG("Sampling FT sensor for offset calibration... Robot should be in the air, with horizontal feet.");
192  m_right_FT_offset_calibration_sum << 0,0,0,0,0,0;
193  m_left_FT_offset_calibration_sum << 0,0,0,0,0,0;
194  }
195 
196  /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
197  /* ------------------------------------------------------------------- */
198  /* --- ENTITY -------------------------------------------------------- */
199  /* ------------------------------------------------------------------- */
200 
201 
202  void FtCalibration::display(std::ostream& os) const
203  {
204  os << "FtCalibration "<<getName();
205  try
206  {
207  getProfiler().report_all(3, os);
208  }
209  catch (ExceptionSignal e) {}
210  }
211 
212  } // namespace talos_balance
213  } // namespace sot
214 } // namespace dynamicgraph
virtual void display(std::ostream &os) const
Vector6d m_right_FT_offset_calibration_sum
Offset or bias to be removed from Left FT sensor.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
void setRightFootWeight(const double &rightW)
Commands for setting the feet weight.
#define OUTPUT_SIGNALS
void init(const std::string &robotRef)
Initialize.
Vector6d m_right_foot_weight
true if the entity has been successfully initialized
bool m_initSucceeded
Variable used durring average computation of the offset.
#define INPUT_SIGNALS
Vector6d m_left_FT_offset
Offset or bias to be removed from Right FT sensor.
Vector6d m_right_FT_offset
Number of iteration left for calibration (-1= not cailbrated, 0=caibration done)
int m_left_calibration_iter
Number of iteration left for calibration (-1= not cailbrated, 0=caibration done)
Vector6d m_left_FT_offset_calibration_sum
Variable used durring average computation of the offset.
#define CALIB_ITER_TIME
void calibrateFeetSensor()
Command to calibrate the foot sensors when the robot is standing in the air with horizontal feet...
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FtCalibration, "FtCalibration")