sot-talos-balance  1.6.0
ft-wrist-calibration.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  * LAAS-CNRS
4  * F. Bailly
5  * T. Flayols
6  * F. Risbourg
7  */
8 
9 
11 #include <sot/core/debug.hh>
12 #include <dynamic-graph/factory.h>
13 
14 
15 #include <dynamic-graph/all-commands.h>
16 #include <sot/core/stop-watch.hh>
18 
19 #define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating
20 
21 using namespace sot::talos_balance;
22 
23 namespace dynamicgraph
24 {
25  namespace sot
26  {
27  namespace talos_balance
28  {
29  namespace dynamicgraph = ::dynamicgraph;
30  using namespace dynamicgraph;
31  using namespace dynamicgraph::command;
32  using namespace dg::sot::talos_balance;
33 
34 #define INPUT_SIGNALS m_rightWristForceInSIN << m_leftWristForceInSIN << m_qSIN
35 #define INNER_SIGNALS m_rightWeightSINNER << m_leftWeightSINNER
36 #define OUTPUT_SIGNALS m_rightWristForceOutSOUT << m_leftWristForceOutSOUT
37 
40  typedef FtWristCalibration EntityClassName;
41 
42  /* --- DG FACTORY ---------------------------------------------------- */
43  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FtWristCalibration,
44  "FtWristCalibration");
45 
46  /* ------------------------------------------------------------------- */
47  /* --- CONSTRUCTION -------------------------------------------------- */
48  /* ------------------------------------------------------------------- */
50  FtWristCalibration(const std::string& name)
51  : Entity(name)
52  , CONSTRUCT_SIGNAL_IN(rightWristForceIn, dynamicgraph::Vector)
53  , CONSTRUCT_SIGNAL_IN(leftWristForceIn, dynamicgraph::Vector)
54  , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
55  , CONSTRUCT_SIGNAL_INNER(rightWeight, dynamicgraph::Vector, INPUT_SIGNALS)
56  , CONSTRUCT_SIGNAL_INNER(leftWeight, dynamicgraph::Vector, INPUT_SIGNALS)
57  , CONSTRUCT_SIGNAL_OUT(rightWristForceOut, dynamicgraph::Vector, INPUT_SIGNALS << INNER_SIGNALS)
58  , CONSTRUCT_SIGNAL_OUT(leftWristForceOut, dynamicgraph::Vector, INPUT_SIGNALS << INNER_SIGNALS)
59  , m_robot_util(RefVoidRobotUtil())
60  , m_model()
61  , m_data()
62  , m_rightSensorId()
63  , m_leftSensorId()
64  , m_initSucceeded(false)
65  , m_removeWeight(false)
66  {
67 
68  Entity::signalRegistration(INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS);
69 
70  /* Commands. */
71  addCommand("init",
72  makeCommandVoid1(*this, &FtWristCalibration::init,
73  docCommandVoid1("Initialize the entity.",
74  "Robot reference (string)")));
75  addCommand("setRightHandConf",
76  makeCommandVoid2(*this, &FtWristCalibration::setRightHandConf,
77  docCommandVoid2("Set the data of the right hand",
78  "Vector of default forces in Newton",
79  "Vector of the weight lever arm")));
80  addCommand("setLeftHandConf",
81  makeCommandVoid2(*this, &FtWristCalibration::setLeftHandConf,
82  docCommandVoid2("Set the data of the left hand",
83  "Vector of default forces in Newton",
84  "Vector of the weight lever arm")));
85  addCommand("calibrateWristSensor",
86  makeCommandVoid0(*this, &FtWristCalibration::calibrateWristSensor,
87  docCommandVoid0("Calibrate the wrist sensors")));
88 
89  addCommand("setRemoveWeight",
90  makeCommandVoid1(*this, &FtWristCalibration::setRemoveWeight,
91  docCommandVoid1("set RemoveWeight", "desired removeWeight")));
92 
93 
94  }
95 
96  void FtWristCalibration::init(const std::string &robotRef)
97  {
98  dgADD_OSTREAM_TO_RTLOG (std::cout);
99  std::string localName(robotRef);
100  if (!isNameInRobotUtil(localName))
101  {
102  m_robot_util = createRobotUtil(localName);
103  }
104  else
105  {
106  m_robot_util = getRobotUtil(localName);
107  }
108  m_right_FT_offset << 0, 0, 0, 0, 0, 0;
109  m_left_FT_offset << 0, 0, 0, 0, 0, 0;
110  m_right_FT_offset_calibration_sum << 0, 0, 0, 0, 0, 0;
111  m_left_FT_offset_calibration_sum << 0, 0, 0, 0, 0, 0;
112  m_right_weight_calibration_sum << 0, 0, 0, 0, 0, 0;
113  m_left_weight_calibration_sum << 0, 0, 0, 0, 0, 0;
114 
115  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
116  m_data = new pinocchio::Data(m_model);
117 
118  m_rightSensorId = m_model.getFrameId("wrist_right_ft_link");
119  m_leftSensorId = m_model.getFrameId("wrist_left_ft_link");
120 
121  m_initSucceeded = true;
122  SEND_MSG("Entity Initialized", MSG_TYPE_INFO);
123  }
124 
125 
126  /* ------------------------------------------------------------------- */
127  /* --- SIGNALS ------------------------------------------------------- */
128  /* ------------------------------------------------------------------- */
129 
131  {
132  if (!m_initSucceeded)
133  {
134  SEND_WARNING_STREAM_MSG("Cannot compute signal rightWeight before initialization!");
135  return s;
136  }
137  if(s.size()!=6)
138  s.resize(6);
139 
140  const Vector &q = m_qSIN(iter);
141  assert(q.size() == m_model.nq && "Unexpected size of signal q");
142 
143  // Get sensorPlacement
144  pinocchio::framesForwardKinematics(m_model, *m_data, q);
145  const pinocchio::SE3 &sensorPlacement = m_data->oMf[m_rightSensorId];
146 
147  Eigen::Vector3d leverArm = sensorPlacement.rotation() * m_rightLeverArm;
148 
149  m_rightHandWeight[3] = leverArm(1)*m_rightHandWeight(2);
150  m_rightHandWeight[4] = -leverArm(0)*m_rightHandWeight(2);
151 
152  Eigen::Matrix<double,6,1> weight;
153 
154  weight.head<3>() = sensorPlacement.rotation().transpose() * m_rightHandWeight.head<3>();
155  weight.tail<3>() = sensorPlacement.rotation().transpose() * m_rightHandWeight.tail<3>();
156 
157  s = weight;
158 
159  return s;
160  }
161 
163  {
164  if (!m_initSucceeded)
165  {
166  SEND_WARNING_STREAM_MSG("Cannot compute signal rightWeight before initialization!");
167  return s;
168  }
169  if(s.size()!=6)
170  s.resize(6);
171 
172  const Vector &q = m_qSIN(iter);
173  assert(q.size() == m_model.nq && "Unexpected size of signal q");
174 
175  pinocchio::framesForwardKinematics(m_model, *m_data, q);
176  const pinocchio::SE3 &sensorPlacement = m_data->oMf[m_leftSensorId];
177 
178  Eigen::Vector3d leverArm = sensorPlacement.rotation() * m_leftLeverArm;
179 
180  m_leftHandWeight[3] = leverArm(1) * m_leftHandWeight(2);
181  m_leftHandWeight[4] = -leverArm(0) * m_leftHandWeight(2);
182 
183  Eigen::Matrix<double,6,1> weight;
184 
185  weight.head<3>() = sensorPlacement.rotation().transpose() * m_leftHandWeight.head<3>();
186  weight.tail<3>() = sensorPlacement.rotation().transpose() * m_leftHandWeight.tail<3>();
187 
188  s = weight;
189 
190  return s;
191  }
192 
194  {
195  if(!m_initSucceeded)
196  {
197  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
198  return s;
199  }
200  if(s.size()!=6)
201  s.resize(6);
202  const Vector & rightWristForce = m_rightWristForceInSIN(iter);
203  assert(rightWristForce.size() == 6 && "Unexpected size of signal rightWristForceIn, should be 6.");
204  const Vector & rightWeight = m_rightWeightSINNER(iter);
205  assert(rightWeight.size() == 6 && "Unexpected size of signal rightWeight, should be 6.");
206 
207  //do offset calibration if needed
208  if (m_rightCalibrationIter > 0)
209  {
210  m_right_FT_offset_calibration_sum += rightWristForce ;
211  m_right_weight_calibration_sum += rightWeight ;
213  }
214  else if (m_rightCalibrationIter == 0)
215  {
216  SEND_INFO_STREAM_MSG("Calibrating ft sensors...");
220  }
221 
222  s = rightWristForce - m_right_FT_offset;
223 
224  if (m_removeWeight)
225  {
226  s -= rightWeight;
227  }
228  return s;
229  }
230 
232  {
233  if(!m_initSucceeded)
234  {
235  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
236  return s;
237  }
238  if(s.size()!=6)
239  s.resize(6);
240  const Vector & leftWristForce = m_leftWristForceInSIN(iter);
241  assert(leftWristForce.size() == 6 && "Unexpected size of signal leftWristForceIn, should be 6.");
242  const Vector & leftWeight = m_leftWeightSINNER(iter);
243  assert(leftWeight.size() == 6 && "Unexpected size of signal leftWeight, should be 6.");
244 
245  //do offset calibration if needed
246  if (m_leftCalibrationIter > 0)
247  {
248  m_left_FT_offset_calibration_sum += leftWristForce;
249  m_left_weight_calibration_sum += leftWeight ;
251  }
252  else if (m_leftCalibrationIter == 0)
253  {
257  }
258  //remove offset and hand weight
259  s = leftWristForce - m_left_FT_offset;
260 
261  if (m_removeWeight)
262  {
263  s -= leftWeight;
264  }
265  return s;
266  }
267 
268  /* --- COMMANDS ---------------------------------------------------------- */
269 
270  void FtWristCalibration::setRightHandConf(const double &rightW, const Vector &rightLeverArm)
271  {
272  if(!m_initSucceeded)
273  {
274  SEND_WARNING_STREAM_MSG("Cannot set right hand weight before initialization!");
275  return;
276  }
277  m_rightHandWeight << 0, 0, rightW, 0, 0, 0;
278  m_rightLeverArm << rightLeverArm[0],rightLeverArm[1],rightLeverArm[2];
279  }
280 
281  void FtWristCalibration::setLeftHandConf(const double &leftW, const Vector &leftLeverArm)
282  {
283  if(!m_initSucceeded)
284  {
285  SEND_WARNING_STREAM_MSG("Cannot set left hand weight before initialization!");
286  return;
287  }
288  m_leftHandWeight << 0, 0, leftW, 0, 0, 0;
289  m_leftLeverArm << leftLeverArm[0],leftLeverArm[1],leftLeverArm[2];
290  }
291 
293  {
294  SEND_WARNING_STREAM_MSG("Sampling FT sensor for offset calibration... Robot should be in the air, with horizontal hand.");
297  m_right_FT_offset_calibration_sum << 0,0,0,0,0,0;
298  m_left_FT_offset_calibration_sum << 0,0,0,0,0,0;
299  }
300 
301  void FtWristCalibration::setRemoveWeight(const bool &removeWeight)
302  {
303  m_removeWeight = removeWeight;
304  }
305 
306  /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
307  /* ------------------------------------------------------------------- */
308  /* --- ENTITY -------------------------------------------------------- */
309  /* ------------------------------------------------------------------- */
310 
311 
312  void FtWristCalibration::display(std::ostream& os) const
313  {
314  os << "FtWristCalibration "<<getName();
315  try
316  {
317  getProfiler().report_all(3, os);
318  }
319  catch (ExceptionSignal e) {}
320  }
321 
322  } // namespace talos_balance
323  } // namespace sot
324 } // namespace dynamicgraph
pinocchio::Data * m_data
Pinocchio robot data.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FtWristCalibration, "FtWristCalibration")
Vector6d m_left_FT_offset_calibration_sum
Variable used during average computation of the offset.
Vector6d m_right_FT_offset
Offset or bias to be removed from Right FT sensor.
void setLeftHandConf(const double &leftW, const Vector &leftLeverArm)
Vector6d m_right_weight_calibration_sum
Variable used during average computation of the weight.
void init(const std::string &robotRef)
Initialize.
#define CALIB_ITER_TIME
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
bool m_removeWeight
If true, the weight of the end effector is removed from the force.
Vector6d m_right_FT_offset_calibration_sum
Variable used during average computation of the offset.
#define INNER_SIGNALS
#define INPUT_SIGNALS
RobotUtilShrPtr m_robot_util
Robot Util instance to get the sensor frame.
Vector6d m_left_FT_offset
Offset or bias to be removed from Left FT sensor.
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
#define OUTPUT_SIGNALS
void setRemoveWeight(const bool &removeWeight)
Set to true if you want to remove the weight from the force.
Vector6d m_left_weight_calibration_sum
Variable used during average computation of the weight.
pinocchio::FrameIndex m_rightSensorId
Id of the force sensor frame.
pinocchio::FrameIndex m_leftSensorId
Id of the joint of the end-effector.
pinocchio::Model m_model
Pinocchio robot model.
void calibrateWristSensor()
Command to calibrate the wrist sensors when the robot is in half sitting with the hands aligned...
int m_leftCalibrationIter
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done) ...
int m_rightCalibrationIter
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done) ...
bool m_initSucceeded
true if the entity has been successfully initialized
void setRightHandConf(const double &rightW, const Vector &rightLeverArm)
Commands for setting the hand weight.