sot-talos-balance  1.5.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 
138  const Vector &q = m_qSIN(iter);
139  assert(q.size() == m_model.nq && "Unexpected size of signal q");
140 
141  // Get sensorPlacement
142  pinocchio::framesForwardKinematics(m_model, *m_data, q);
143  const pinocchio::SE3 &sensorPlacement = m_data->oMf[m_rightSensorId];
144 
145  Vector leverArm = sensorPlacement.rotation() * m_rightLeverArm;
146 
147  m_rightHandWeight[3] = leverArm(1)*m_rightHandWeight(2);
148  m_rightHandWeight[4] = -leverArm(0)*m_rightHandWeight(2);
149 
150  Vector weight(6);
151 
152  weight.head<3>() = sensorPlacement.rotation().transpose() * m_rightHandWeight.head<3>();
153  weight.tail<3>() = sensorPlacement.rotation().transpose() * m_rightHandWeight.tail<3>();
154 
155  s = weight;
156 
157  return s;
158  }
159 
161  {
162  if (!m_initSucceeded)
163  {
164  SEND_WARNING_STREAM_MSG("Cannot compute signal rightWeight before initialization!");
165  return s;
166  }
167 
168  const Vector &q = m_qSIN(iter);
169  assert(q.size() == m_model.nq && "Unexpected size of signal q");
170 
171  pinocchio::framesForwardKinematics(m_model, *m_data, q);
172  const pinocchio::SE3 &sensorPlacement = m_data->oMf[m_leftSensorId];
173 
174  Vector leverArm = sensorPlacement.rotation() * m_leftLeverArm;
175 
176  m_leftHandWeight[3] = leverArm(1) * m_leftHandWeight(2);
177  m_leftHandWeight[4] = -leverArm(0) * m_leftHandWeight(2);
178 
179  Vector weight(6);
180 
181  weight.head<3>() = sensorPlacement.rotation().transpose() * m_leftHandWeight.head<3>();
182  weight.tail<3>() = sensorPlacement.rotation().transpose() * m_leftHandWeight.tail<3>();
183 
184  s = weight;
185 
186  return s;
187  }
188 
190  {
191  if(!m_initSucceeded)
192  {
193  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
194  return s;
195  }
196  const Vector & rightWristForce = m_rightWristForceInSIN(iter);
197  assert(rightWristForce.size() == 6 && "Unexpected size of signal rightWristForceIn, should be 6.");
198  const Vector & rightWeight = m_rightWeightSINNER(iter);
199  assert(rightWeight.size() == 6 && "Unexpected size of signal rightWeight, should be 6.");
200 
201  //do offset calibration if needed
202  if (m_rightCalibrationIter > 0)
203  {
204  m_right_FT_offset_calibration_sum += rightWristForce ;
205  m_right_weight_calibration_sum += rightWeight ;
207  }
208  else if (m_rightCalibrationIter == 0)
209  {
210  SEND_INFO_STREAM_MSG("Calibrating ft sensors...");
214  }
215 
216  s = rightWristForce - m_right_FT_offset;
217 
218  if (m_removeWeight)
219  {
220  s -= rightWeight;
221  }
222  return s;
223  }
224 
226  {
227  if(!m_initSucceeded)
228  {
229  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
230  return s;
231  }
232  const Vector & leftWristForce = m_leftWristForceInSIN(iter);
233  assert(leftWristForce.size() == 6 && "Unexpected size of signal leftWristForceIn, should be 6.");
234  const Vector & leftWeight = m_leftWeightSINNER(iter);
235  assert(leftWeight.size() == 6 && "Unexpected size of signal leftWeight, should be 6.");
236 
237  //do offset calibration if needed
238  if (m_leftCalibrationIter > 0)
239  {
240  m_left_FT_offset_calibration_sum += leftWristForce;
241  m_left_weight_calibration_sum += leftWeight ;
243  }
244  else if (m_leftCalibrationIter == 0)
245  {
249  }
250  //remove offset and hand weight
251  s = leftWristForce - m_left_FT_offset;
252 
253  if (m_removeWeight)
254  {
255  s -= leftWeight;
256  }
257  return s;
258  }
259 
260  /* --- COMMANDS ---------------------------------------------------------- */
261 
262  void FtWristCalibration::setRightHandConf(const double &rightW, const Vector &rightLeverArm)
263  {
264  if(!m_initSucceeded)
265  {
266  SEND_WARNING_STREAM_MSG("Cannot set right hand weight before initialization!");
267  return;
268  }
269  m_rightHandWeight << 0, 0, rightW, 0, 0, 0;
270  m_rightLeverArm << rightLeverArm[0],rightLeverArm[1],rightLeverArm[2];
271  }
272 
273  void FtWristCalibration::setLeftHandConf(const double &leftW, const Vector &leftLeverArm)
274  {
275  if(!m_initSucceeded)
276  {
277  SEND_WARNING_STREAM_MSG("Cannot set left hand weight before initialization!");
278  return;
279  }
280  m_leftHandWeight << 0, 0, leftW, 0, 0, 0;
281  m_leftLeverArm << leftLeverArm[0],leftLeverArm[1],leftLeverArm[2];
282  }
283 
285  {
286  SEND_WARNING_STREAM_MSG("Sampling FT sensor for offset calibration... Robot should be in the air, with horizontal hand.");
289  m_right_FT_offset_calibration_sum << 0,0,0,0,0,0;
290  m_left_FT_offset_calibration_sum << 0,0,0,0,0,0;
291  }
292 
293  void FtWristCalibration::setRemoveWeight(const bool &removeWeight)
294  {
295  m_removeWeight = removeWeight;
296  }
297 
298  /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
299  /* ------------------------------------------------------------------- */
300  /* --- ENTITY -------------------------------------------------------- */
301  /* ------------------------------------------------------------------- */
302 
303 
304  void FtWristCalibration::display(std::ostream& os) const
305  {
306  os << "FtWristCalibration "<<getName();
307  try
308  {
309  getProfiler().report_all(3, os);
310  }
311  catch (ExceptionSignal e) {}
312  }
313 
314  } // namespace talos_balance
315  } // namespace sot
316 } // 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.