DHParameterOptimizationLogger.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotComponents::ArmarXObjects::DHParameterOptimizationLogger
17  * @author Stefan Reither ( stef dot reither at web dot de )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
26 
31 
32 #include <VirtualRobot/Nodes/Sensor.h>
33 
34 #include <Eigen/Geometry>
35 
36 #include <iomanip>
37 #include <ctime>
38 
39 using namespace armarx;
40 
41 namespace fs = std::filesystem;
42 
43 
44 void DHParameterOptimizationLogger::logData(const Ice::Current&)
45 {
46  std::vector<float> data;
47  if (_logRepeatAccuracy)
48  {
49  data.push_back(-1.0f);
50  data.push_back(-1.0f);
51  }
52  logData(data);
53  if (!data.empty())
54  {
55  CsvWriter writer(_filePath, std::vector<std::string>(), true);
56  writer.write(data);
57  writer.close();
58  }
59 
60  ARMARX_DEBUG << "Logging data finished";
61 }
62 
64 {
65  if (!_logRepeatAccuracy)
66  {
67  ARMARX_WARNING << "Loging of repeatAccuracy is not activated!";
68  return;
69  }
70 
71  std::vector<float> data;
72  // placeholder for error of repeat accuracy
73  data.push_back(0.0f);
74  data.push_back(0.0f);
75  logData(data);
76  std::pair<float, float> repeatErrors;
77  repeatErrors = calculateRepeatError();
78  data[0] = repeatErrors.first;
79  data[1] = repeatErrors.second;
80 
81  if (data.size() > 2)
82  {
83  CsvWriter writer(_filePath, std::vector<std::string>(), true);
84  writer.write(data);
85  writer.close();
86  }
87 
88  ARMARX_DEBUG << "Logging data with repeat accuracy finished";
89 }
90 
92 {
93  _viconLoggingActive = true;
94 }
95 
97 {
98  _viconLoggingActive = false;
99 }
100 
101 void DHParameterOptimizationLogger::init(const std::string& kinematicChainName,
102  const std::map<std::string, std::string>& neckMarkerMapping,
103  const std::map<std::string, std::string>& handMarkerMapping,
104  const std::string& loggingFileName,
105  bool logRepeatAccuracy,
106  const Ice::Current&)
107 {
108  ARMARX_CHECK_EXPRESSION(!neckMarkerMapping.empty());
109  ARMARX_CHECK_EXPRESSION(!handMarkerMapping.empty());
110  ARMARX_CHECK_EXPRESSION(!kinematicChainName.empty());
111 
112  _neckMarkerMapping = neckMarkerMapping;
113  _handMarkerMapping = handMarkerMapping;
114 
115  _neckMarker.clear();
116  _handMarker.clear();
117 
118  for (auto& pair : neckMarkerMapping)
119  {
120  _neckMarker.push_back(pair.first);
121  }
122  for (auto& pair : handMarkerMapping)
123  {
124  _handMarker.push_back(pair.first);
125  }
126  for (auto& s : _neckMarker)
127  {
128  if (std::find(_handMarker.begin(), _handMarker.end(), s) != _handMarker.end())
129  {
130  ARMARX_ERROR << "HandMarker and NeckMarker can not contain the same marker!! Abort.";
131  return;
132  }
133  }
134  std::vector<VirtualRobot::SensorPtr> sensors = localRobot->getSensors();
135  for (auto& pair : neckMarkerMapping)
136  {
137  auto& i = pair.second;
138  ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(), sensors.end(), [i](VirtualRobot::SensorPtr s)
139  {
140  return s->getName() == i;
141  }) != sensors.end()) <<
142  "Robot does not have a sensor with name '" + i + "'";
143  }
144  for (auto& pair : handMarkerMapping)
145  {
146  auto& i = pair.second;
147  ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(), sensors.end(), [i](VirtualRobot::SensorPtr s)
148  {
149  return s->getName() == i;
150  }) != sensors.end()) <<
151  "Robot does not have a sensor with name '" + i + "'";
152  }
153 
154  _kcName = kinematicChainName;
155  ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(_kcName)) << "Robot " + localRobot->getName() + " does not have a kinematic chain with name " + _kcName;
156  _kc = localRobot->getRobotNodeSet(_kcName);
157  _jointNames = _kc->getNodeNames();
158  _logRepeatAccuracy = logRepeatAccuracy;
159 
160  setupFile(loggingFileName);
161  setupHeader();
162  ARMARX_INFO << VAROUT(_header);
163  CsvWriter writer(_filePath, _header, true);
164  writer.close();
165 
166  _initialized = true;
167 }
168 
169 void DHParameterOptimizationLogger::reportLabeledViconMarkerFrame(const StringVector3fMap& markerPosMap, const Ice::Current&)
170 {
171  // Format of received data: ObjectName:MarkerName x y z
172 
173  StringVector3fMap copyMap;
174  for (auto& it : markerPosMap)
175  {
176  std::string key = it.first;
177  int i = key.find(':') + 1;
178  copyMap[key.erase(0, i)] = it.second;
179  Vector3f v = it.second;
180  // ARMARX_INFO << it.first << " " << v.e0 << " " << v.e1 << " " << v.e2;
181  debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_x", new Variant(v.e0));
182  debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_y", new Variant(v.e1));
183  debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_z", new Variant(v.e2));
184  }
185 
186  if (_viconLoggingActive)
187  {
188  std::unique_lock lock(_bufferMutex);
189  _viconMarkerBuffer = copyMap;
190  _timeStampLastViconDataUpdate = TimeUtil::GetTime();
191  _viconBufferUpdated = true;
192 
193  }
194 }
195 
196 void DHParameterOptimizationLogger::reportUnlabeledViconMarkerFrame(const Vector3fSeq& markerPos, const Ice::Current&)
197 {
198  // Not needed
199 }
200 
201 void DHParameterOptimizationLogger::setupFile(const std::string& fileName)
202 {
203  std::string localFileName = fileName;
204  using namespace std::filesystem;
205 
206  auto timeToString = []()
207  {
208  std::time_t now = std::time(nullptr);
209  std::tm* nowTm = std::localtime(&now);
210 
211  std::stringstream ss;
212  ss << std::put_time(nowTm, "%Y%m%d_%H%M%S");
213  return ss.str();
214  };
215 
216  // Setup logging file
217  if (ends_with(localFileName, ".csv"))
218  {
219  localFileName = localFileName.substr(0, localFileName.size() - 4);
220  }
221  localFileName += "_";
222  localFileName += timeToString();
223  localFileName += ".csv";
224 
225  std::string pathOrig = getProperty<std::string>("LoggingFilePath").getValue();
226  if (!pathOrig.empty() && pathOrig[0] == '~')
227  {
228  ARMARX_CHECK_EXPRESSION(pathOrig.size() == 1 || pathOrig[1] == '/') << "LoggingFilePath isviconValues not valid: '~' has to be followed by ’/’";
229  char const* home = std::getenv("HOME");
230  ARMARX_CHECK_EXPRESSION(home) << "HOME environment variable is not set!";
231  pathOrig.replace(0, 1, home);
232  }
233  fs::path filePath(pathOrig);
234  if (!fs::exists(filePath))
235  {
236  fs::create_directory(filePath);
237  }
238  filePath /= path(localFileName);
239  _filePath = filePath.string();
240 }
241 
243 {
244  usingTopic(getProperty<std::string>("ViconDataTopicName").getValue());
245  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
246  usingProxy(getProperty<std::string>("RobotUnitObserverName").getValue());
247 
248  _initialized = false;
249 
250  int size = getProperty<int>("FilterWindowSize").getValue();
251  ARMARX_CHECK_EXPRESSION(size > 0);
252  _filterSize = size;
253 
254  _viconObjectName_neck = getProperty<std::string>("Neck_ViconObjectName").getValue();
255  _viconObjectName_hand = getProperty<std::string>("Hand_ViconObjectName").getValue();
256 }
257 
258 
260 {
261  getProxy(robotStateComponent, getProperty<std::string>("RobotStateComponentName").getValue());
262  getProxy(robotUnitObserver, getProperty<std::string>("RobotUnitObserverName").getValue());
263  _viconLoggingActive = true;
264 
265  debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
266  debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
267 
268 
269  localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
270 
271  std::vector<VirtualRobot::SensorPtr> s = localRobot->getSensors();
272  for (auto& sp : s)
273  {
274  ARMARX_INFO << sp->getName();
275  }
276 }
277 
278 
280 {
281  _viconLoggingActive = false;
282 }
283 
284 
286 {
287 
288 }
289 
291 {
294 }
295 
296 void DHParameterOptimizationLogger::setupHeader()
297 {
298  std::vector<std::string> header;
299 
300  // order is important!! do not change without adapting the order of logging calls in logData(std::vector<std::string> &data) !!
301 
302  // Accuracy columns
303  if (_logRepeatAccuracy)
304  {
305  header.push_back("Error_Translation");
306  header.push_back("Error_Orientation");
307  }
308 
309  // ViconMarker columns neck --> 12
310  for (auto& m : _neckMarker)
311  {
312  header.push_back(m + "_x");
313  header.push_back(m + "_y");
314  header.push_back(m + "_z");
315  }
316 
317  // ViconMarker columns hand --> 12
318  for (auto& m : _handMarker)
319  {
320  header.push_back(m + "_x");
321  header.push_back(m + "_y");
322  header.push_back(m + "_z");
323  }
324 
325  // JointValues --> ~8
326  for (auto& node : _jointNames)
327  {
328  header.push_back("JointValue_" + node);
329  }
330  header.push_back("JointValue_TorsoJoint");
331 
332  // ForwardKinematic to vicon marker --> 12
333  for (auto& m : _handMarker)
334  {
335  header.push_back("FK_" + m + "_x");
336  header.push_back("FK_" + m + "_y");
337  header.push_back("FK_" + m + "_z");
338  }
339 
340  // Error between model and vicon data --> 2
341  header.push_back("Error_translation");
342  header.push_back("Error_orientation");
343 
344  // Raw torque ticks --> ~8
345  for (auto& node : _jointNames)
346  {
347  header.push_back(node + "_torqueTicks");
348  }
349 
350  // TorqueValues --> ~8
351  for (auto& node : _jointNames)
352  {
353  header.push_back(node + "_torque");
354  }
355 
356  // FT Sensor torque --> 3
357  if (_kc->getTCP()->getName() == "Hand R TCP")
358  {
359  header.push_back("FT R_FT_torque_x");
360  header.push_back("FT R_FT_torque_y");
361  header.push_back("FT R_FT_torque_z");
362  }
363  else if (_kc->getTCP()->getName() == "Hand L TCP")
364  {
365  header.push_back("FT L_FT_torque_x");
366  header.push_back("FT L_FT_torque_y");
367  header.push_back("FT L_FT_torque_z");
368  }
369 
370  _header = header;
371 }
372 
373 void DHParameterOptimizationLogger::logData(std::vector<float>& data)
374 {
375  // order is important!! do not change without adapting the order of setting up the header in setupHeader() !!
376  logViconMarkerPositions(data);
377  logJointValues(data);
378  logForwardKinematicToViconMarker(data);
379  logErrorBetweenModelAndVicon(data);
380  logRawTorqueTicks(data);
381  logTorqueValues(data);
382  logFTSensorValues(data);
383 }
384 
385 void DHParameterOptimizationLogger::logViconMarkerPositions(std::vector<float>& data)
386 {
387  ARMARX_DEBUG << "Logging viconMarkerPositions";
388 
389  if (!_viconLoggingActive)
390  {
391  ARMARX_DEBUG << deactivateSpam(1) << "ViconLogging not active";
392  size_t sum = 3 * (_neckMarker.size() + _handMarker.size());
393  for (size_t i = 0; i < sum; i++)
394  {
395  data.push_back(0.0f);
396  }
397  }
398  else
399  {
400  // blocking wait until all marker are visible --> no marker has 0 values, and enough data frames for filtering arrived
401  StringVector3fMap values = waitBlockingForAllMarkersFiltered();
402  if (values.empty())
403  {
404  size_t sum = 3 * (_neckMarker.size() + _handMarker.size());
405  for (size_t i = 0; i < sum; i++)
406  {
407  data.push_back(0.0f);
408  }
409  }
410  else
411  {
412  values = transformViconMarkerPositionsIntoRobotRootFrame(values);
413  for (auto& m : _neckMarker)
414  {
415  data.push_back(values.at(m).e0);
416  data.push_back(values.at(m).e1);
417  data.push_back(values.at(m).e2);
418  }
419  for (auto& m : _handMarker)
420  {
421  data.push_back(values.at(m).e0);
422  data.push_back(values.at(m).e1);
423  data.push_back(values.at(m).e2);
424  }
425 
426  _filteredViconMarkerPositions_robotRootFrame = values;
427  _filteredViconMarkerPositionsUpdated = true;
428  }
429  }
430 }
431 
432 void DHParameterOptimizationLogger::logJointValues(std::vector<float>& data) const
433 {
434  ARMARX_DEBUG << "Logging jointValues";
435 
436  RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
437  for (auto& node : _kc->getAllRobotNodes())
438  {
439  data.push_back(node->getJointValue());
440  }
441  data.push_back(localRobot->getRobotNode("TorsoJoint")->getJointValue());
442 }
443 
444 void DHParameterOptimizationLogger::logForwardKinematicToViconMarker(std::vector<float>& data) const
445 {
446  ARMARX_DEBUG << "Logging forwardKinematicToViconMarker";
447 
448  RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
449  VirtualRobot::RobotNodePtr hand = getHandMarkerRootNode();
450 
451  for (VirtualRobot::SensorPtr const& sensor : hand->getSensors())
452  {
453  Eigen::Matrix4f m = sensor->getTransformationFrom(localRobot->getRootNode());
454  data.push_back(m(0, 3)); // x
455  data.push_back(m(1, 3)); // y
456  data.push_back(m(2, 3)); // z
457  }
458 }
459 
460 void DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<float>& data)
461 {
462  ARMARX_DEBUG << "Logging errorBetweenModelAndVicon";
463 
464  if (!_viconLoggingActive || !_filteredViconMarkerPositionsUpdated)
465  {
466  data.push_back(0.0f);
467  data.push_back(0.0f);
468  }
469  else
470  {
471  RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
472  VirtualRobot::RobotNodePtr neck = getNeckMarkerRootNode();
473  VirtualRobot::RobotNodePtr hand = getHandMarkerRootNode();
474  VirtualRobot::RobotNodePtr root = localRobot->getRootNode();
475 
476  _lastObservedHandRootPose = _newObservedHandRootPose;
477 
478  // Eigen::Matrix4f modelRelativePose = hand->getTransformationFrom(neck);
479 
480  // Calculation of neck pose from vicon data
481  Eigen::MatrixXf observedNeckPositions_robotRootFrame(3, _neckMarker.size());
482  Eigen::MatrixXf modelNeckPositions_neckFrame(3, _neckMarker.size());
483 
484 
485 
486 
487 
488  size_t i = 0;
489  for (auto& it : _neckMarker)
490  {
491  observedNeckPositions_robotRootFrame(0, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
492  observedNeckPositions_robotRootFrame(1, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
493  observedNeckPositions_robotRootFrame(2, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
494 
495  modelNeckPositions_neckFrame(0, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
496  modelNeckPositions_neckFrame(1, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
497  modelNeckPositions_neckFrame(2, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
498 
499  // visu
500  Eigen::Vector3f position = (root->getGlobalPose() * observedNeckPositions_robotRootFrame).block<3, 1>(0, i);
501  debugDrawerPrx->setSphereVisu("markerVisu", it, new Vector3(position), DrawColor {0, 1, 1, 1}, 7);
502 
503  i++;
504  }
505 
506 
507 
508  // Calculation of hand pose from vicon data
509  Eigen::MatrixXf observedHandPositions_robotRootFrame(3, _handMarker.size());
510  Eigen::MatrixXf modelHandPositions_handFrame(3, _handMarker.size());
511 
512 
513  i = 0;
514  for (auto& it : _handMarker)
515  {
516  observedHandPositions_robotRootFrame(0, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
517  observedHandPositions_robotRootFrame(1, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
518  observedHandPositions_robotRootFrame(2, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
519 
520  modelHandPositions_handFrame(0, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(0, 3);
521  modelHandPositions_handFrame(1, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(1, 3);
522  modelHandPositions_handFrame(2, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(2, 3);
523 
524  // visu
525  Eigen::Vector3f position = (root->getGlobalPose() * observedHandPositions_robotRootFrame).block<3, 1>(0, i);
526  debugDrawerPrx->setSphereVisu("markerVisu", it, new Vector3(position), DrawColor {0, 1, 0, 1}, 7);
527 
528 
529  i++;
530  }
531 
532 
533  ARMARX_DEBUG << VAROUT(modelHandPositions_handFrame) << VAROUT(observedHandPositions_robotRootFrame);
534  Eigen::Matrix4f observedHandRootPose_robotRootFrame = registration3d(modelHandPositions_handFrame.transpose(), observedHandPositions_robotRootFrame.transpose());
535 
536  _newObservedHandRootPose = observedHandRootPose_robotRootFrame;
537  _observedHandPosesUpdated = true;
538 
539  debugDrawerPrx->setPoseVisu("MarkerVisu", "observedTCP", new Pose(root->getGlobalPose() * observedHandRootPose_robotRootFrame));
540  debugDrawerPrx->setPoseVisu("MarkerVisu", "kinematicTCP", new Pose(hand->getGlobalPose()));
541 
542  // Eigen::Matrix4f viconRelativePose = observedNeckRootPose.inverse() * observedHandRootPose;
543  float errorPos = (hand->getPoseInRootFrame().block<3, 1>(0, 3) - observedHandRootPose_robotRootFrame.block<3, 1>(0, 3)).norm();
544  // Calculate error
545  // Eigen::Vector3f errorV = viconRelativePose.block<3, 1>(0, 3) - modelRelativePose.block<3, 1>(0, 3);
546  // float errorPos = errorV.norm();
547 
548  Eigen::Matrix3f oriDir = hand->getPoseInRootFrame().block<3, 3>(0, 0) * observedHandRootPose_robotRootFrame.block<3, 3>(0, 0).inverse();
549  Eigen::AngleAxisf aa(oriDir);
550  float errorOri = aa.angle();
551 
552  ARMARX_INFO << "Error (Hand) in rootFrame between model and vicon ---- Position-Error: " << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
553 
554  data.push_back(errorPos);
555  data.push_back(errorOri);
556 
557  _filteredViconMarkerPositionsUpdated = false;
558  }
559 }
560 
561 void DHParameterOptimizationLogger::logRawTorqueTicks(std::vector<float>& data) const
562 {
563  ARMARX_DEBUG << "Logging rawTorqueTicks";
564 
565  if (robotUnitObserver->existsChannel("SensorDevices"))
566  {
567  for (auto& node : _jointNames)
568  {
569  if (robotUnitObserver->existsDataField("SensorDevices", node + "_SensorValueArmar6Actuator_torqueTicks"))
570  {
571  data.push_back((float) robotUnitObserver->getDatafieldByName("SensorDevices", node + "_SensorValueArmar6Actuator_torqueTicks")->getInt());
572  }
573  else
574  {
575  data.push_back(0.0f);
576  }
577  }
578  }
579  else
580  {
581  for (size_t i = 0; i < _jointNames.size(); i++)
582  {
583  data.push_back(0.0f);
584  }
585  }
586 }
587 
588 void DHParameterOptimizationLogger::logTorqueValues(std::vector<float>& data) const
589 {
590  ARMARX_DEBUG << "Logging torqueValues";
591 
592  if (robotUnitObserver->existsChannel("SensorDevices"))
593  {
594  for (auto& node : _jointNames)
595  {
596  if (robotUnitObserver->existsDataField("SensorDevices", node + "_SensorValueArmar6Actuator_torque"))
597  {
598  data.push_back(robotUnitObserver->getDatafieldByName("SensorDevices", node + "_SensorValueArmar6Actuator_torque")->getFloat());
599  }
600  else
601  {
602  data.push_back(0.0f);
603  }
604  }
605  }
606  else
607  {
608  for (size_t i = 0; i < _jointNames.size(); i++)
609  {
610  data.push_back(0.0f);
611  }
612  }
613 }
614 
615 void DHParameterOptimizationLogger::logFTSensorValues(std::vector<float>& data) const
616 {
617  ARMARX_DEBUG << "Logging FTSensorValues";
618 
619  std::string datafieldName ;
620  if (_kc->getTCP()->getName() == "Hand R TCP")
621  {
622  datafieldName = "FT R_FTSensorDataValue_torque";
623  }
624  else if (_kc->getTCP()->getName() == "Hand L TCP")
625  {
626  datafieldName = "FT L_FTSensorDataValue_torque";
627  }
628 
629  if (robotUnitObserver->existsChannel("SensorDevices"))
630  {
631  if (robotUnitObserver->existsDataField("SensorDevices", datafieldName))
632  {
633  TimedVariantPtr tv = TimedVariantPtr::dynamicCast(robotUnitObserver->getDatafieldByName("SensorDevices", datafieldName));
635  Vector3Ptr v = tv->get<Vector3>();
636 
637  data.push_back(v->x);
638  data.push_back(v->y);
639  data.push_back(v->z);
640  }
641  else
642  {
643  data.push_back(0.0f);
644  data.push_back(0.0f);
645  data.push_back(0.0f);
646  }
647  }
648  else
649  {
650  data.push_back(0.0f);
651  data.push_back(0.0f);
652  data.push_back(0.0f);
653  }
654 }
655 
656 StringVector3fMap DHParameterOptimizationLogger::waitBlockingForAllMarkersFiltered()
657 {
658  TIMING_START(waitBlockingForAllMarkersFiltered);
659  std::map<std::string, armarx::rtfilters::AverageFilter> viconDataFilters;
660 
661  // Setup filter
662  viconDataFilters.clear();
663  for (auto& s : _neckMarker)
664  {
665  viconDataFilters.insert(std::make_pair(s + "_x", rtfilters::AverageFilter(_filterSize)));
666  viconDataFilters.insert(std::make_pair(s + "_y", rtfilters::AverageFilter(_filterSize)));
667  viconDataFilters.insert(std::make_pair(s + "_z", rtfilters::AverageFilter(_filterSize)));
668  }
669  for (auto& s : _handMarker)
670  {
671  viconDataFilters.insert(std::make_pair(s + "_x", rtfilters::AverageFilter(_filterSize)));
672  viconDataFilters.insert(std::make_pair(s + "_y", rtfilters::AverageFilter(_filterSize)));
673  viconDataFilters.insert(std::make_pair(s + "_z", rtfilters::AverageFilter(_filterSize)));
674  }
675 
676  size_t i = 0; // Counter to make sure the whole filter is filled
677  StringVector3fMap viconValues;
678  bool viconValuesSet = false;
679  while (_viconLoggingActive && !viconValuesSet)
680  {
681  // Waiting for new vicon data
682  if (!_viconBufferUpdated)
683  {
684  TimeUtil::USleep(10);
685  }
686  else
687  {
688  _viconBufferUpdated = false;
689  // copying viconData
690  {
691  std::unique_lock lock(_bufferMutex);
692  viconValues = _viconMarkerBuffer;
693  }
694  // Check if all data is valid, if not, continue waiting for next data frame
695  for (const std::string& s : _neckMarker)
696  {
697  ARMARX_CHECK_EXPRESSION(viconValues.count(s) > 0) << armarx::getMapKeys(viconValues) << "\n" << s;
698  Vector3f v = viconValues.at(s);
699  if (v.e0 == 0.0f && v.e1 == 0.0f && v.e2 == 0.0f)
700  {
701  goto invalidData; // rare case where goto is actually useful
702  }
703  }
704  for (const std::string& s : _handMarker)
705  {
706  ARMARX_CHECK_EXPRESSION(viconValues.count(s) > 0);
707  Vector3f v = viconValues.at(s);
708  if (v.e0 == 0.0f && v.e1 == 0.0f && v.e2 == 0.0f)
709  {
710  goto invalidData; // rare case where goto is actually useful
711  }
712  }
713 
714  // All data is valid, we can push it to the filters
715  for (const std::string& s : _neckMarker)
716  {
717  Vector3f v = viconValues.at(s);
718  viconDataFilters.at(s + "_x").update(_timeStampLastViconDataUpdate, v.e0);
719  viconDataFilters.at(s + "_y").update(_timeStampLastViconDataUpdate, v.e1);
720  viconDataFilters.at(s + "_z").update(_timeStampLastViconDataUpdate, v.e2);
721  }
722  for (const std::string& s : _handMarker)
723  {
724  Vector3f v = viconValues.at(s);
725  viconDataFilters.at(s + "_x").update(_timeStampLastViconDataUpdate, v.e0);
726  viconDataFilters.at(s + "_y").update(_timeStampLastViconDataUpdate, v.e1);
727  viconDataFilters.at(s + "_z").update(_timeStampLastViconDataUpdate, v.e2);
728  }
729  if (i >= _filterSize)
730  {
731  viconValues.clear();
732  for (const std::string& s : _neckMarker)
733  {
734  float x = viconDataFilters.at(s + "_x").calculate();
735  float y = viconDataFilters.at(s + "_y").calculate();
736  float z = viconDataFilters.at(s + "_z").calculate();
737  Vector3f v;
738  v.e0 = x;
739  v.e1 = y;
740  v.e2 = z;
741  viconValues.insert(std::make_pair(s, v));
742 
743  }
744  for (const std::string& s : _handMarker)
745  {
746  float x = viconDataFilters.at(s + "_x").calculate();
747  float y = viconDataFilters.at(s + "_y").calculate();
748  float z = viconDataFilters.at(s + "_z").calculate();
749  Vector3f v;
750  v.e0 = x;
751  v.e1 = y;
752  v.e2 = z;
753  viconValues.insert(std::make_pair(s, v));
754  }
755  viconValuesSet = true;
756  }
757  else
758  {
759  i++;
760  }
761  }
762 invalidData:; // rare case where goto is actually useful
763  }
764  TIMING_END(waitBlockingForAllMarkersFiltered);
765  viconDataFilters.clear();
766  return viconValues;
767 }
768 
769 VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getNeckMarkerRootNode() const
770 {
772  VirtualRobot::RobotNodePtr node;
773  if (_kc->getTCP()->getName() == "Hand R TCP")
774  {
775  node = localRobot->getRobotNode("DHParameterOptimization_NeckRight");
776  }
777  else if (_kc->getTCP()->getName() == "Hand L TCP")
778  {
779  node = localRobot->getRobotNode("DHParameterOptimization_NeckLeft");
780  }
781  else
782  {
783  ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L TCP'. Maybe the used robot is not Armar-6?!";
784  }
785  return node;
786 }
787 
788 VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getHandMarkerRootNode() const
789 {
791  VirtualRobot::RobotNodePtr node;
792  if (_kc->getTCP()->getName() == "Hand R TCP")
793  {
794  node = localRobot->getRobotNode("DHParameterOptimization_HandRight");
795  }
796  else if (_kc->getTCP()->getName() == "Hand L TCP")
797  {
798  node = localRobot->getRobotNode("DHParameterOptimization_HandLeft");
799  }
800  else
801  {
802  ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L TCP'. Maybe the used robot is not Armar-6?!";
803  }
804  return node;
805 }
806 
807 std::pair<float, float> DHParameterOptimizationLogger::calculateRepeatError()
808 {
809  if (!_observedHandPosesUpdated)
810  {
811  ARMARX_WARNING << "Pose of HandViconMarker have not been updated. No repeat accuracy calculated. (writes -1.0 in log)";
812  return std::make_pair(-1.0f, -1.0f);
813  }
814  _observedHandPosesUpdated = false;
815 
816  Eigen::Matrix4f oldPose = _lastObservedHandRootPose;
817  Eigen::Matrix4f newPose = _newObservedHandRootPose;
818 
819  // Calculate error
820  Eigen::Vector3f errorV = oldPose.block<3, 1>(0, 3) - newPose.block<3, 1>(0, 3);
821  float errorPos = errorV.norm();
822 
823  Eigen::Matrix3f oriDir = oldPose.block<3, 3>(0, 0) * newPose.block<3, 3>(0, 0).inverse();
824  Eigen::AngleAxisf aa(oriDir);
825  float errorOri = aa.angle();
826 
827  ARMARX_INFO << "Repeat Accuracy: Error between old pose and new pose ---- Position-Error: " << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
828 
829  return std::make_pair(errorPos, errorOri);
830 }
831 
832 
833 Eigen::Matrix4f DHParameterOptimizationLogger::registration3d(const Eigen::MatrixXf& A, const Eigen::MatrixXf& B) const
834 {
835 
836  // std::cout << "A:\n" << A<< std::endl;
837  // std::cout << "B:\n" << B<< std::endl;
838  assert(A.rows() == B.rows()); // Same number of points
839  assert(A.cols() == B.cols()); // Same dimension
840  // assert(A.cols()==3);
841  assert(A.cols() == 3 || A.cols() == 4); // Homogeneous coordinates?
842  assert(A.rows() >= 3);
843 
844 
845  Eigen::Vector3f centroid_A = A.block(0, 0, A.rows(), 3).colwise().mean();
846  Eigen::Vector3f centroid_B = B.block(0, 0, A.rows(), 3).colwise().mean();
847  // std::cout << "cA & cB: \n" << centroid_A << std::endl << std::endl << centroid_B << std::endl;
848  Eigen::MatrixXf AA = A.block(0, 0, A.rows(), 3).rowwise() - centroid_A.transpose();
849  Eigen::MatrixXf BB = (B.block(0, 0, A.rows(), 3).rowwise() - centroid_B.transpose());
850  // std::cout << VAROUT(AA) << VAROUT(BB);
851  Eigen::MatrixXf H = (AA).transpose() * BB;
852  // std::cout << "H: " << H<< std::endl;
853 
854  Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
855  Eigen::Matrix3f U = svd.matrixU();
856  // std::cout << "U: " << U<< std::endl;
857  Eigen::Matrix3f V = svd.matrixV();
858  // std::cout << "V: " << V<< std::endl;
859  Eigen::Matrix3f R = V * U.transpose();
860  // std::cout << "R: " << R<< std::endl;
861 
862  if (R.determinant() < 0)
863  {
864  // Reflection detected
865  // abort();
866  // ARMARX_INFO << "Reflection detected";
867  V.col(2) *= -1;
868  R = V * U.transpose();
869  // R.col(2) *= -1;
870  }
871 
872  Eigen::Vector3f t = -R * centroid_A + centroid_B;
873  // std::cout << "t: " << t << std::endl;
875  T.block(0, 0, 3, 3) = R;
876  T.block(0, 3, 3, 1) = t;
877 
878  return T;
879 }
880 
881 StringVector3fMap DHParameterOptimizationLogger::transformViconMarkerPositionsIntoRobotRootFrame(const StringVector3fMap& values) const
882 {
883  RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
884  VirtualRobot::RobotNodePtr neck = getNeckMarkerRootNode();
885 
886  Eigen::MatrixXf observedNeckPositions_viconRootFrame(3, _neckMarker.size());
887  Eigen::MatrixXf modelNeckPositions_neckFrame(3, _neckMarker.size());
888 
889  size_t i = 0;
890  for (auto& it : _neckMarker)
891  {
892  observedNeckPositions_viconRootFrame(0, i) = values.at(it).e0;
893  observedNeckPositions_viconRootFrame(1, i) = values.at(it).e1;
894  observedNeckPositions_viconRootFrame(2, i) = values.at(it).e2;
895  modelNeckPositions_neckFrame(0, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
896  modelNeckPositions_neckFrame(1, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
897  modelNeckPositions_neckFrame(2, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
898 
899  i++;
900  }
901  Eigen::Matrix4f observedNeckRootPose_viconRootFrame = registration3d(modelNeckPositions_neckFrame.transpose(), observedNeckPositions_viconRootFrame.transpose());
902  localRobot->setGlobalPoseForRobotNode(neck, observedNeckRootPose_viconRootFrame);
903  Eigen::Matrix4f realRobotToVicon = localRobot->getGlobalPose().inverse();
904 
905 
906  // Eigen::Matrix4f realGlobalRootPose = localRobot->getGlobalPose();
907  // ARMARX_DEBUG << VAROUT(observedNeckRootPose_robotRootFrame) << VAROUT(modelNeckPositions_neckFrame) << VAROUT(observedNeckPositions_robotRootFrame);
908  // Eigen::Matrix4f realGlobalRootPose = localRobot->getGlobalPose();
909  // localRobot->setGlobalPoseForRobotNode(neck, observedNeckRootPose_robotRootFrame);
910 
911  // debugDrawerPrx->setRobotVisu("markerVisu", "robot", robotStateComponent->getRobotFilename(), "armar6_rt", FullModel);
912  // debugDrawerPrx->updateRobotPose("markerVisu", "robot", new Pose(localRobot->getGlobalPose()));
913  // debugDrawerPrx->updateRobotConfig("markerVisu", "robot", localRobot->getConfig()->getRobotNodeJointValueMap());
914 
915  // Eigen::Matrix4f realRobotToVicon = realGlobalRootPose.inverse() * localRobot->getGlobalPose();
916  // ARMARX_INFO << VAROUT(realRobotToVicon) << VAROUT(realGlobalRootPose) << VAROUT(localRobot->getGlobalPose()) << VAROUT(neck->getGlobalPose()) << VAROUT(root->getGlobalPose());
917 
918 
919 
920  StringVector3fMap transformedValues;
921  for (auto& it : values)
922  {
923  std::string markerName = it.first;
924  armarx::Vector3f pos_viconRootFrame = it.second;
925 
927  mat(0, 3) = pos_viconRootFrame.e0;
928  mat(1, 3) = pos_viconRootFrame.e1;
929  mat(2, 3) = pos_viconRootFrame.e2;
930 
931  mat = realRobotToVicon * mat;
932 
933  armarx::Vector3f r;
934  r.e0 = mat(0, 3);
935  r.e1 = mat(1, 3);
936  r.e2 = mat(2, 3);
937  transformedValues[markerName] = r;
938  }
939 
940  return transformedValues;
941 }
armarx::DHParameterOptimizationLogger::logDataWithRepeatAccuracy
void logDataWithRepeatAccuracy(const Ice::Current &=Ice::emptyCurrent) override
Definition: DHParameterOptimizationLogger.cpp:63
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::DHParameterOptimizationLogger::logData
void logData(const Ice::Current &=Ice::emptyCurrent) override
logData logs data for the current position.
Definition: DHParameterOptimizationLogger.cpp:44
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:280
algorithm.h
armarx::CsvWriter
Definition: CsvWriter.h:36
TIMING_END
#define TIMING_END(name)
Definition: TimeUtil.h:296
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:448
armarx::RemoteRobot::createLocalCloneFromFile
static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
This is a convenience function for createLocalClone, which automatically gets the filename from the R...
Definition: RemoteRobot.cpp:441
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::VariantType::Vector3
const VariantTypeId Vector3
Definition: Pose.h:38
armarx::rtfilters::AverageFilter
Definition: AverageFilter.h:32
CsvWriter.h
armarx::DHParameterOptimizationLogger::init
void init(const std::string &kinematicChainName, const std::map< std::string, std::string > &neckMarkerMapping, const std::map< std::string, std::string > &handMarkerMapping, const std::string &loggingFileName, bool logRepeatAccuracy, const Ice::Current &=Ice::emptyCurrent) override
Definition: DHParameterOptimizationLogger.cpp:101
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::DHParameterOptimizationLogger::onConnectComponent
void onConnectComponent() override
Definition: DHParameterOptimizationLogger.cpp:259
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::CsvWriter::close
void close()
Definition: CsvWriter.cpp:77
armarx::TimeUtil::USleep
static int USleep(long usec)
like timed_wait on boost condition_variables, but with timeserver support
Definition: TimeUtil.cpp:148
DHParameterOptimizationLogger.h
FramedPose.h
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
armarx::DHParameterOptimizationLogger::onInitComponent
void onInitComponent() override
Definition: DHParameterOptimizationLogger.cpp:242
armarx::DHParameterOptimizationLogger::startViconLogging
void startViconLogging(const Ice::Current &=Ice::emptyCurrent) override
Definition: DHParameterOptimizationLogger.cpp:91
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::DHParameterOptimizationLogger::onExitComponent
void onExitComponent() override
Definition: DHParameterOptimizationLogger.cpp:285
armarx::DHParameterOptimizationLogger::reportUnlabeledViconMarkerFrame
void reportUnlabeledViconMarkerFrame(const Vector3fSeq &markerPos, const Ice::Current &=Ice::emptyCurrent) override
Definition: DHParameterOptimizationLogger.cpp:196
armarx::DHParameterOptimizationLogger::reportLabeledViconMarkerFrame
void reportLabeledViconMarkerFrame(const StringVector3fMap &markerPosMap, const Ice::Current &=Ice::emptyCurrent) override
Definition: DHParameterOptimizationLogger.cpp:169
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::CsvWriter::write
void write(const std::vector< std::string > &row)
Definition: CsvWriter.cpp:39
A
class A(deque< T, A >)) ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE((class T
Enables hashing of std::list.
TimedVariant.h
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::DHParameterOptimizationLogger::onDisconnectComponent
void onDisconnectComponent() override
Definition: DHParameterOptimizationLogger.cpp:279
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::ends_with
bool ends_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:50
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::DHParameterOptimizationLogger::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: DHParameterOptimizationLogger.cpp:290
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
armarx::DHParameterOptimizationLoggerPropertyDefinitions
Definition: DHParameterOptimizationLogger.h:43
T
float T
Definition: UnscentedKalmanFilterTest.cpp:35
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
armarx::DHParameterOptimizationLogger::stopViconLogging
void stopViconLogging(const Ice::Current &=Ice::emptyCurrent) override
Definition: DHParameterOptimizationLogger.cpp:96
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition: ManagedIceObject.cpp:393
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::getMapKeys
void getMapKeys(const MapType &map, OutputIteratorType it)
Definition: algorithm.h:157
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
norm
double norm(const Point &a)
Definition: point.hpp:94