utils.cpp
Go to the documentation of this file.
1 #include "utils.h"
2 
3 #include <filesystem>
4 #include <fstream>
5 #include <iostream>
6 #include <memory>
7 #include <optional>
8 #include <type_traits>
9 
10 #include <boost/tokenizer.hpp>
11 
12 #include <VirtualRobot/MathTools.h>
13 
14 #include "ArmarXCore/core/PackagePath.h"
15 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
16 
17 #include <armarx/control/common/aron_conversions.h>
18 
19 namespace armarx::control::common
20 {
21 
22  Eigen::VectorXf
23  getEigenVec(nlohmann::json& userConfig,
24  nlohmann::json& defaultConfig,
25  const std::string& entryName,
26  int size,
27  int value)
28  {
29  if (userConfig.find(entryName) != userConfig.end())
30  {
31  auto vec = userConfig[entryName].get<std::vector<float>>();
32  if (vec.size() > 0)
33  {
34  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
35  }
36  if (size > 0)
37  {
38  return Eigen::VectorXf::Ones(size) * value;
39  }
40  ARMARX_IMPORTANT << "user defined parameter " << entryName
41  << " have size 0, fall back to default";
42  }
43 
44  auto vec = defaultConfig[entryName].get<std::vector<float>>();
45  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
46  }
47 
48  void
49  getEigenMatrix(nlohmann::json& userConfig, const std::string& entryName, Eigen::MatrixXf& mat)
50  {
51  if (userConfig.find(entryName) != userConfig.end())
52  {
53  auto vec = userConfig[entryName].get<std::vector<std::vector<float>>>();
54  int rows = vec.size();
55  int cols = vec[0].size();
56  ARMARX_INFO << VAROUT(rows) << VAROUT(cols);
57  ARMARX_CHECK_GREATER_EQUAL(rows, 1);
58  ARMARX_CHECK_GREATER_EQUAL(cols, 1);
59  mat.resize(rows, cols);
60  for (int i = 0; i < rows; i++)
61  {
62  for (int j = 0; j < cols; j++)
63  {
64  mat(i, j) = vec[i][j];
65  }
66  }
67  // memcpy(mat.data(), vec.data(), rows * cols * sizeof(float));
68  }
69  else
70  {
71  ARMARX_ERROR << "values not found in user config";
72  }
73  }
74 
75  Eigen::VectorXf
76  getEigenVecWithDefault(nlohmann::json& userConfig,
77  Eigen::VectorXf defaultValue,
78  const std::string& entryName)
79  {
80  if (userConfig.find(entryName) != userConfig.end())
81  {
82  auto vec = userConfig[entryName].get<std::vector<float>>();
83  if (vec.size() == static_cast<unsigned>(defaultValue.size()))
84  {
85  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
86  }
87  ARMARX_WARNING << "size of the user defined parameter " << entryName
88  << " doesn't correspond to default size, use default value instead";
89  }
90  return defaultValue;
91  }
92 
93  void
94  getError(Eigen::Matrix4f& currentPose,
95  Eigen::Vector3d& targetPosition,
96  Eigen::Quaterniond& targetQuaternion,
97  double& posiError,
98  double& oriError)
99  {
100  Eigen::Matrix4d currentPosed = currentPose.cast<double>();
101  Eigen::Vector3d currentPosid = currentPosed.block<3, 1>(0, 3);
102  posiError = (currentPosid - targetPosition).norm();
103 
104  Eigen::Quaterniond currentOrientation = Eigen::Quaterniond{currentPosed.block<3, 3>(0, 0)};
105  oriError = targetQuaternion.angularDistance(currentOrientation);
106  if (oriError > M_PI)
107  {
108  oriError = 2 * M_PI - oriError;
109  }
110  }
111 
112  void
113  debugEigenPose(StringVariantBaseMap& datafields, const std::string& name, Eigen::Matrix4f pose)
114  {
115  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(pose);
116  datafields[name + "_x"] = new Variant(pose(0, 3));
117  datafields[name + "_y"] = new Variant(pose(1, 3));
118  datafields[name + "_z"] = new Variant(pose(2, 3));
119  datafields[name + "_rx"] = new Variant(rpy(0));
120  datafields[name + "_ry"] = new Variant(rpy(1));
121  datafields[name + "_rz"] = new Variant(rpy(2));
122  }
123 
124  void
125  debugEigenVec(StringVariantBaseMap& datafields, const std::string& name, Eigen::VectorXf vec)
126  {
127  for (int i = 0; i < vec.size(); i++)
128  {
129  datafields[name + "_" + std::to_string(i)] = new Variant(vec(i));
130  }
131  }
132 
133  void
134  debugStdVec(StringVariantBaseMap& datafields, const std::string& name, std::vector<float> vec)
135  {
136  for (size_t i = 0; i < vec.size(); i++)
137  {
138  datafields[name + "_" + std::to_string(i)] = new Variant(vec[i]);
139  }
140  }
141 
142  void
143  to_json(nlohmann::json& j, const Pose& fp)
144  {
145  j = nlohmann::json{{"qw", fp.orientation->qw},
146  {"qx", fp.orientation->qx},
147  {"qy", fp.orientation->qy},
148  {"qz", fp.orientation->qz},
149  {"x", fp.position->x},
150  {"y", fp.position->y},
151  {"z", fp.position->z}};
152  }
153 
154  void
155  from_json(const nlohmann::json& j, Pose& fp)
156  {
157  j.at("qw").get_to(fp.orientation->qw);
158  j.at("qx").get_to(fp.orientation->qx);
159  j.at("qy").get_to(fp.orientation->qy);
160  j.at("qz").get_to(fp.orientation->qz);
161  j.at("x").get_to(fp.position->x);
162  j.at("y").get_to(fp.position->y);
163  j.at("z").get_to(fp.position->z);
164  }
165 
166  void
167  to_json(nlohmann::json& j, const PosePtr& fp)
168  {
169  if (fp)
170  {
171  to_json(j, *fp);
172  }
173  }
174 
175  void
176  to_json(nlohmann::json& j, const PoseBasePtr& fp)
177  {
178  if (fp)
179  {
180  to_json(j, PosePtr::dynamicCast(fp));
181  }
182  }
183 
184  void
185  from_json(const nlohmann::json& j, PoseBasePtr& fp)
186  {
187  PosePtr p = new Pose();
188  from_json(j, p);
189  fp = p;
190  }
191 
192  void
193  from_json(const nlohmann::json& j, PosePtr& fp)
194  {
195  fp = new Pose();
196  from_json(j, *fp);
197  }
198 
199  Eigen::Block<Eigen::Matrix4f, 3, 3>
200  getOri(Eigen::Matrix4f& matrix)
201  {
202  return matrix.block<3, 3>(0, 0);
203  }
204 
205  Eigen::Matrix3f
206  getOri(const Eigen::Matrix4f& matrix)
207  {
208  return matrix.block<3, 3>(0, 0);
209  }
210 
211  Eigen::Block<Eigen::Matrix4f, 3, 1>
212  getPos(Eigen::Matrix4f& matrix)
213  {
214  return matrix.block<3, 1>(0, 3);
215  }
216 
217  Eigen::Vector3f
218  getPos(const Eigen::Matrix4f& matrix)
219  {
220  return matrix.block<3, 1>(0, 3);
221  }
222 
223  PosePtr
224  dVecToPose(const mplib::core::DVec& dvec)
225  {
226  return new Pose(dVecToMat4(dvec));
227  }
228 
229  Eigen::Matrix4f
230  dVecToMat4(const mplib::core::DVec& dvec)
231  {
232  ARMARX_CHECK_EQUAL(dvec.size(), 7);
233  Eigen::Matrix4f mat = VirtualRobot::MathTools::quat2eigen4f(static_cast<float>(dvec[4]),
234  static_cast<float>(dvec[5]),
235  static_cast<float>(dvec[6]),
236  static_cast<float>(dvec[3]));
237  mat(0, 3) = static_cast<float>(dvec[0]);
238  mat(1, 3) = static_cast<float>(dvec[1]);
239  mat(2, 3) = static_cast<float>(dvec[2]);
240 
241  return mat;
242  }
243 
244  mplib::core::DVec
245  poseToDVec(PosePtr& pose)
246  {
247  Eigen::Matrix4f poseMat = pose->toEigen();
248  return mat4ToDVec(poseMat);
249  }
250 
251  mplib::core::DVec
252  mat4ToDVec(const Eigen::Matrix4f& mat)
253  {
254  std::vector<double> result;
255  result.resize(7);
256 
257  for (size_t i = 0; i < 3; ++i)
258  {
259  result.at(i) = static_cast<double>(mat(i, 3));
260  }
261 
262  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
263 
264  result.at(3) = static_cast<double>(quat.w);
265  result.at(4) = static_cast<double>(quat.x);
266  result.at(5) = static_cast<double>(quat.y);
267  result.at(6) = static_cast<double>(quat.z);
268 
269  return result;
270  }
271 
272  std::vector<float>
273  poseToFVec(PosePtr& pose)
274  {
275  Eigen::Matrix4f poseMat = pose->toEigen();
276  return mat4ToFVec(poseMat);
277  }
278 
279  std::vector<float>
280  mat4ToFVec(const Eigen::Matrix4f& mat)
281  {
282  std::vector<float> result;
283  result.resize(7);
284 
285  for (size_t i = 0; i < 3; ++i)
286  {
287  result.at(i) = mat(i, 3);
288  }
289 
290  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
291 
292  result.at(3) = quat.w;
293  result.at(4) = quat.x;
294  result.at(5) = quat.y;
295  result.at(6) = quat.z;
296 
297  return result;
298  }
299 
300  std::string
301  dVecToString(const mplib::core::DVec& dvec)
302  {
303  std::string s = "";
304  for (const double& d : dvec)
305  {
306  s = s + std::to_string(d) + " | ";
307  }
308  return s;
309  }
310 
311  Eigen::VectorXf
312  vecToEigen(std::vector<float>& vec)
313  {
314  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
315  }
316 
317  std::string
318  sVecToString(const std::vector<std::string>& vec, const std::string& delimiter)
319  {
320  std::string s;
321  for (size_t i = 0; i < vec.size(); ++i)
322  {
323  if (i != 0)
324  {
325  s += delimiter;
326  }
327  s += vec[i];
328  }
329  return s;
330  }
331 
332  mp::arondto::MPTraj
333  mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
334  {
335  std::vector<double> timesteps;
336  std::vector<std::vector<double>> trajPoses;
337 
338  std::ifstream f(filepath.c_str());
339 
340  if (!f.is_open())
341  {
342  ARMARX_ERROR << "Could not open file: " + filepath;
343  }
344 
345  using Tokenizer = boost::tokenizer<boost::escaped_list_separator<char>>;
346 
347 
348  // unsigned int stateDim = double().size();
349  unsigned int i = 1;
350 
351  double current_time = 0;
352  std::string line;
353  std::vector<std::string> stringVec;
354  while (getline(f, line))
355  {
356  if (containsHeader && i == 1)
357  {
358  i++;
359  continue;
360  }
361 
362  Tokenizer tok(line);
363  stringVec.assign(tok.begin(), tok.end());
364 
365  if (stringVec.size() == 0)
366  {
367  std::cout << "Line " << i << " is invalid skipping: " << line << std::endl;
368  continue;
369  }
370 
371  unsigned int j = 1;
372 
373  armarx::control::common::mp::arondto::MPTraj traj;
374 
375  //TrajData entry(*this);
376  int size = stringVec.size() - 1;
377  if (noTimeStamp)
378  {
379  j = 0;
380  //entry.timestamp = current_time;
381  timesteps.push_back(current_time);
382  current_time += 0.01;
383  size = stringVec.size();
384  }
385  else
386  {
387  //entry.timestamp = math::util::fromString<double>(stringVec[0]);
388  timesteps.push_back(std::stod(stringVec[0]));
389  }
390 
391  // DVec values(int(ceil(size/stateDim)*stateDim), 0.0);
392  //DVec values(size, 0.0);
393  std::vector<double> currentPose;
394 
395  for (unsigned int k = j; k < stringVec.size(); ++k)
396  {
397  currentPose.push_back(std::stod(stringVec[k]));
398  //values.at(sd) = (math::util::fromString<double>(stringVec[j]));
399  //math::util::checkValue(values.at(sd));
400  }
401  trajPoses.push_back(currentPose);
402 
403  i++;
404  }
405 
406  // convert to aron types
407  if (timesteps.size() != trajPoses.size())
408  {
409  ARMARX_ERROR << "consistency error in reading trajectory";
410  }
411  armarx::control::common::mp::arondto::MPTraj taskTraj;
412  taskTraj.time.setZero(timesteps.size());
413  taskTraj.traj.setZero(timesteps.size(), trajPoses.at(0).size());
414  // ARMARX_INFO << "no timesteps" << timesteps.size();
415  // ARMARX_INFO << "traj pose dim " << trajPoses.at(0).size();
416  for (unsigned int l = 0; l < timesteps.size(); l++)
417  {
418  taskTraj.time(l) = timesteps.at(l);
419  for (size_t j = 0; j < trajPoses.at(0).size(); ++j)
420  taskTraj.traj(l, j) = trajPoses.at(l).at(j);
421  }
422  return taskTraj;
423  }
424 
425  mp::arondto::MPTraj
426  createDummyTraj(const int dimension, const int timesteps)
427  {
428  mp::arondto::MPTraj traj;
429  traj.time = Eigen::VectorXd::LinSpaced(timesteps, 0, 1);
430  traj.traj = Eigen::MatrixXd::Zero(timesteps, dimension);
431  return traj;
432  }
433 
434  mp::arondto::MPConfig
435  getDefaultMPConfig(MPType mpType,
436  const std::string& name,
437  const std::string& nodeSetName,
438  const double& durationSec,
439  const std::vector<mp::arondto::MPTraj>& mpTraj,
440  const std::vector<mp::arondto::ListViaPoint>& viaPointList)
441  {
442  const std::string className = mpTypeToString.to_name(mpType);
443  const armarx::PackagePath configPath(
444  "armarx_control", "controller_config/mp/default_mp_" + className + ".json");
445  // ARMARX_INFO << "using default config: " << configPath.toSystemPath();
446  auto cfg = armarx::readFromJson<mp::arondto::MPConfig>(configPath.toSystemPath());
447  cfg.name = name;
448  cfg.nodeSetName = nodeSetName;
449  if (durationSec > 0.0)
450  cfg.durationSec = durationSec;
451  if (mpTraj.size() > 0)
452  cfg.trajectoryList = mpTraj;
453  if (viaPointList.size() > 0)
454  cfg.viaPoints = viaPointList;
455  return cfg;
456  }
457 
458  mp::arondto::MPConfig
459  getDefaultMPConfig(MPType mpType,
460  const std::string& name,
461  const std::string& nodeSetName,
462  const double& durationSec,
463  const std::vector<std::string>& mpTraj,
464  const std::vector<mp::arondto::ListViaPoint>& viaPointList)
465  {
466  std::vector<mp::arondto::MPTraj> mpTrajList{};
467  for (auto& f : mpTraj)
468  {
469  mpTrajList.push_back(mpTrajFromFile(f));
470  }
471  return getDefaultMPConfig(mpType, name, nodeSetName, durationSec, mpTrajList, viaPointList);
472  }
473 
474  void
475  addViaPoint(mp::arondto::MPConfig& cfg,
476  const double& canValue,
477  const std::vector<double>& viaPoint)
478  {
479  auto vp = mp::arondto::ListViaPoint();
480  vp.canonicalValue = canValue;
481  vp.viaPointValue = viaPoint;
482  cfg.viaPoints.push_back(vp);
483  }
484 
485  void
486  addViaPoint(mp::arondto::MPConfig& cfg,
487  const double& canValue,
488  const std::vector<float>& viaPoint)
489  {
490  std::vector<double> viaPointVec;
491  std::transform(viaPoint.begin(),
492  viaPoint.end(),
493  std::back_inserter(viaPointVec),
494  [](float value) { return static_cast<double>(value); });
495  addViaPoint(cfg, canValue, viaPointVec);
496  }
497 
498  void
499  addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::VectorXf& viaPoint)
500  {
501  std::vector<double> viaPointVec;
502  std::transform(viaPoint.data(),
503  viaPoint.data() + viaPoint.size(),
504  std::back_inserter(viaPointVec),
505  [](float value) { return static_cast<double>(value); });
506  addViaPoint(cfg, canValue, viaPointVec);
507  }
508 
509  void
510  addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::Matrix4f& viaPose)
511  {
512  auto viaPointVec = mat4ToDVec(viaPose);
513  addViaPoint(cfg, canValue, viaPointVec);
514  }
515 
516  void
517  validateTSViaPoint(std::vector<double>& viaPoint, std::vector<double>& viaPointReference)
518  {
519  if (viaPoint.size() != 7 || viaPointReference.size() != 7)
520  {
521  ARMARX_ERROR << "Both viapoints must have exactly 7 elements";
522  }
523 
524  bool allDifferentSigns = true;
525 
526  // Check signs for the last 4 elements
527  for (int i = 3; i < 7; ++i)
528  {
529  if ((viaPoint[i] * viaPointReference[i]) >= 0)
530  {
531  allDifferentSigns = false;
532  break;
533  }
534  }
535 
536  // If all last 4 elements have different signs, flip their signs
537  if (allDifferentSigns)
538  {
539  ARMARX_INFO << "===> Viapoints for TS Orientation on two hemisphere, flipping sign";
540  for (int i = 3; i < 7; ++i)
541  {
542  viaPoint[i] = -viaPoint[i];
543  }
544  }
545  }
546 
547  void
548  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
549  const double canonicalValue,
550  const std::vector<double>& viaPoint)
551  {
552  mp::arondto::ListViaPoint vp;
553  vp.canonicalValue = canonicalValue;
554  vp.viaPointValue = viaPoint;
555  vpList.push_back(vp);
556  }
557 
558  void
559  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
560  const double canonicalValue,
561  const std::vector<float>& viaPoint)
562  {
563  std::vector<double> viaPointVec;
564  std::transform(viaPoint.begin(),
565  viaPoint.end(),
566  std::back_inserter(viaPointVec),
567  [](float value) { return static_cast<double>(value); });
568  appendToViapointList(vpList, canonicalValue, viaPointVec);
569  }
570 
571  void
572  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
573  const double canonicalValue,
574  const Eigen::VectorXf& viaPoint)
575  {
576  std::vector<double> viaPointVec;
577  std::transform(viaPoint.data(),
578  viaPoint.data() + viaPoint.size(),
579  std::back_inserter(viaPointVec),
580  [](float value) { return static_cast<double>(value); });
581  appendToViapointList(vpList, canonicalValue, viaPointVec);
582  }
583 
584  void
585  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
586  const double canonicalValue,
587  const Eigen::Matrix4f& viaPose)
588  {
589  auto viaPointVec = mat4ToDVec(viaPose);
590  appendToViapointList(vpList, canonicalValue, viaPointVec);
591  }
592 
593  float
594  getDeltaAngleBetweenRotMat(const Eigen::Matrix3f& rotMat1, const Eigen::Matrix3f& rotMat2)
595  {
596  Eigen::Matrix3f poseDiffMatImp = rotMat1 * rotMat2.transpose();
597  Eigen::AngleAxisf oriDiffAngleAxis;
598  oriDiffAngleAxis.fromRotationMatrix(poseDiffMatImp);
599  float angle = VirtualRobot::MathTools::angleModPI(oriDiffAngleAxis.angle());
600  angle = std::fminf(angle, 2 * M_PI - angle);
601  return angle;
602  }
603 
604  float
605  getDeltaAngleBetweenPose(const Eigen::Matrix4f& pose1, const Eigen::Matrix4f& pose2)
606  {
607  return getDeltaAngleBetweenRotMat(pose1.block<3, 3>(0, 0), pose2.block<3, 3>(0, 0));
608  }
609 
610  bool
611  poseDeviationTooLarge(const Eigen::Matrix4f& currentPose,
612  const Eigen::Matrix4f& desiredPose,
613  float positionThrMM,
614  float angleThrDeg)
615  {
616  float angle = getDeltaAngleBetweenPose(currentPose, desiredPose);
617  return (desiredPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)).norm() >
618  positionThrMM or
619  angle > VirtualRobot::MathTools::deg2rad(angleThrDeg);
620  }
621 
622  void
623  reportPoseDeviationWarning(const std::string& who,
624  const Eigen::Matrix4f& pose1,
625  const Eigen::Matrix4f& pose2,
626  const std::string& namePose1,
627  const std::string& namePose2)
628  {
629  auto quat1 = VirtualRobot::MathTools::eigen4f2quat(pose1);
630  auto quat2 = VirtualRobot::MathTools::eigen4f2quat(pose2);
631  ARMARX_INFO << deactivateSpam(2) << who << " "
632  << namePose2 << " (x, y, z, qw, qx, qy, qz) \n\n"
633  <<"[ "
634  << pose2(0, 3) << ", " << pose2(1, 3) << ", " << pose2(2, 3) << ", "
635  << quat2.w << ", " << quat2.x << ", " << quat2.y << ", " << quat2.z
636  << " ]\n\n"
637  << " is too far away from the "
638  << namePose1 << " (x, y, z, qw, qx, qy, qz) \n\n"
639  << "[ "
640  << pose1(0, 3) << ", " << pose1(1, 3) << ", " << pose1(2, 3) << ", "
641  << quat1.w << ", " << quat1.x << ", " << quat1.y << ", " << quat1.z
642  << " ].";
643  }
644 
645  bool
646  detectAndReportPoseDeviationWarning(const Eigen::Matrix4f& pose1,
647  const Eigen::Matrix4f& pose2,
648  const std::string& namePose1,
649  const std::string& namePose2,
650  float positionThrMM,
651  float angleThrDeg,
652  const std::string& who)
653  {
654  float angle = getDeltaAngleBetweenPose(pose1, pose2);
655  float dist = (pose2.block<3, 1>(0, 3) - pose1.block<3, 1>(0, 3)).norm();
656 
657  bool flagToReport =
658  dist > positionThrMM or angle > VirtualRobot::MathTools::deg2rad(angleThrDeg);
659  if (flagToReport)
660  {
661  reportPoseDeviationWarning(who, pose1, pose2, namePose1, namePose2);
662  ARMARX_INFO << deactivateSpam(2) << "Delta distance is " << dist << " and angle is "
663  << angle;
664  }
665  return flagToReport;
666  }
667 
668  Eigen::Matrix4f
669  mirrorLeftRightPose(Eigen::Matrix4f& sourcePose)
670  {
671  Eigen::Matrix4f convert;
672  convert << 1, -1, -1, -1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1;
673  return sourcePose.cwiseProduct(convert);
674  }
675 
676  Eigen::Matrix4f
677  mirrorLeftRightOri(Eigen::Matrix4f& sourcePose)
678  {
679  Eigen::Matrix4f convert;
680  convert << 1, -1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1;
681  return sourcePose.cwiseProduct(convert);
682  }
683 
684  Eigen::Matrix3f
685  mirrorLeftRightOri(Eigen::Matrix3f& sourcePose)
686  {
687  Eigen::Matrix3f convert;
688  convert << 1, -1, -1, -1, 1, 1, -1, 1, 1;
689  return sourcePose.cwiseProduct(convert);
690  }
691 
692 } // namespace armarx::control::common
armarx::control::common::mirrorLeftRightPose
Eigen::Matrix4f mirrorLeftRightPose(Eigen::Matrix4f &sourcePose)
Definition: utils.cpp:669
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::control::common::debugEigenPose
void debugEigenPose(StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
Definition: utils.cpp:113
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::control::common::getOri
Eigen::Block< Eigen::Matrix4f, 3, 3 > getOri(Eigen::Matrix4f &matrix)
Definition: utils.cpp:200
armarx::control::common::createDummyTraj
mp::arondto::MPTraj createDummyTraj(const int dimension, const int timesteps)
create a dummy trajectory that only has zeros for every dimension.
Definition: utils.cpp:426
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::common::getEigenVecWithDefault
Eigen::VectorXf getEigenVecWithDefault(nlohmann::json &userConfig, Eigen::VectorXf defaultValue, const std::string &entryName)
Definition: utils.cpp:76
armarx::control::common::detectAndReportPoseDeviationWarning
bool detectAndReportPoseDeviationWarning(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2, float positionThrMM, float angleThrDeg, const std::string &who)
Definition: utils.cpp:646
armarx::control::common::validateTSViaPoint
void validateTSViaPoint(std::vector< double > &viaPoint, std::vector< double > &viaPointReference)
Definition: utils.cpp:517
armarx::control::common::debugStdVec
void debugStdVec(StringVariantBaseMap &datafields, const std::string &name, std::vector< float > vec)
Definition: utils.cpp:134
armarx::control::common::mirrorLeftRightOri
Eigen::Matrix4f mirrorLeftRightOri(Eigen::Matrix4f &sourcePose)
Definition: utils.cpp:677
armarx::control::common::getError
void getError(Eigen::Matrix4f &currentPose, Eigen::Vector3d &targetPosition, Eigen::Quaterniond &targetQuaternion, double &posiError, double &oriError)
Definition: utils.cpp:94
armarx::control::common::from_json
void from_json(const nlohmann::json &j, Pose &fp)
Definition: utils.cpp:155
armarx::convert
armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot &obj, const armem::Time &timestamp)
Definition: ArticulatedObjectLocalizerDynamicSimulation.cpp:194
aron_conversions.h
armarx::control::common::reportPoseDeviationWarning
void reportPoseDeviationWarning(const std::string &who, const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2)
Definition: utils.cpp:623
armarx::control::common
This file is part of ArmarX.
Definition: aron_conversions.cpp:29
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
IceInternal::Handle< Pose >
armarx::control::common::poseToFVec
std::vector< float > poseToFVec(PosePtr &pose)
Definition: utils.cpp:273
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::control::common::dVecToString
std::string dVecToString(const mplib::core::DVec &dvec)
Definition: utils.cpp:301
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::control::common::poseDeviationTooLarge
bool poseDeviationTooLarge(const Eigen::Matrix4f &currentPose, const Eigen::Matrix4f &desiredPose, float positionThrMM, float angleThrDeg)
Definition: utils.cpp:611
armarx::control::common::mpTypeToString
const simox::meta::EnumNames< MPType > mpTypeToString
Definition: type.h:81
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:47
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:47
armarx::control::common::MPType
MPType
Definition: type.h:60
armarx::control::common::getPos
Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos(Eigen::Matrix4f &matrix)
Definition: utils.cpp:212
armarx::control::common::getDeltaAngleBetweenPose
float getDeltaAngleBetweenPose(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2)
Definition: utils.cpp:605
armarx::control::common::poseToDVec
mplib::core::DVec poseToDVec(PosePtr &pose)
Definition: utils.cpp:245
armarx::control::common::dVecToMat4
Eigen::Matrix4f dVecToMat4(const mplib::core::DVec &dvec)
create Eigen:Matrix4f from 7D double vector.
Definition: utils.cpp:230
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::control::common::dVecToPose
PosePtr dVecToPose(const mplib::core::DVec &dvec)
Definition: utils.cpp:224
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::control::common::debugEigenVec
void debugEigenVec(StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
Definition: utils.cpp:125
utils.h
armarx::control::common::vecToEigen
Eigen::VectorXf vecToEigen(std::vector< float > &vec)
Definition: utils.cpp:312
armarx::control::common::getDeltaAngleBetweenRotMat
float getDeltaAngleBetweenRotMat(const Eigen::Matrix3f &rotMat1, const Eigen::Matrix3f &rotMat2)
Definition: utils.cpp:594
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
ExpressionException.h
armarx::control::common::mpTrajFromFile
mp::arondto::MPTraj mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
Definition: utils.cpp:333
armarx::Pose
The Pose class.
Definition: Pose.h:242
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::control::common::mat4ToFVec
std::vector< float > mat4ToFVec(const Eigen::Matrix4f &mat)
Definition: utils.cpp:280
armarx::control::common::appendToViapointList
void appendToViapointList(std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const std::vector< double > &viaPoint)
Definition: utils.cpp:548
armarx::control::common::mat4ToDVec
mplib::core::DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
Definition: utils.cpp:252
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:109
armarx::control::common::to_json
void to_json(nlohmann::json &j, const Pose &fp)
Definition: utils.cpp:143
Eigen::Quaterniond
Quaternion< double, 0 > Quaterniond
Definition: EigenForwardDeclarations.h:62
armarx::control::common::addViaPoint
void addViaPoint(mp::arondto::MPConfig &cfg, const double &canValue, const std::vector< double > &viaPoint)
Definition: utils.cpp:475
armarx::control::common::getEigenVec
Eigen::VectorXf getEigenVec(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName, int size, int value)
Definition: utils.cpp:23
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::control::common::sVecToString
std::string sVecToString(const std::vector< std::string > &vec, const std::string &delimiter)
Definition: utils.cpp:318
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::PackagePath
Definition: PackagePath.h:52
armarx::control::common::getEigenMatrix
void getEigenMatrix(nlohmann::json &userConfig, const std::string &entryName, Eigen::MatrixXf &mat)
Definition: utils.cpp:49
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::control::common::getDefaultMPConfig
mp::arondto::MPConfig getDefaultMPConfig(MPType mpType, const std::string &name, const std::string &nodeSetName, const double &durationSec, const std::vector< mp::arondto::MPTraj > &mpTraj, const std::vector< mp::arondto::ListViaPoint > &viaPointList)
Definition: utils.cpp:435
PackagePath.h
norm
double norm(const Point &a)
Definition: point.hpp:102