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::Block<Eigen::Matrix4f, 3, 1>
206  getPos(Eigen::Matrix4f& matrix)
207  {
208  return matrix.block<3, 1>(0, 3);
209  }
210 
211  PosePtr
212  dVecToPose(const mplib::core::DVec& dvec)
213  {
214  return new Pose(dVecToMat4(dvec));
215  }
216 
217  Eigen::Matrix4f
218  dVecToMat4(const mplib::core::DVec& dvec)
219  {
220  ARMARX_CHECK_EQUAL(dvec.size(), 7);
221  Eigen::Matrix4f mat = VirtualRobot::MathTools::quat2eigen4f(static_cast<float>(dvec[4]),
222  static_cast<float>(dvec[5]),
223  static_cast<float>(dvec[6]),
224  static_cast<float>(dvec[3]));
225  mat(0, 3) = static_cast<float>(dvec[0]);
226  mat(1, 3) = static_cast<float>(dvec[1]);
227  mat(2, 3) = static_cast<float>(dvec[2]);
228 
229  return mat;
230  }
231 
232  mplib::core::DVec
233  poseToDVec(PosePtr& pose)
234  {
235  Eigen::Matrix4f poseMat = pose->toEigen();
236  return mat4ToDVec(poseMat);
237  }
238 
239  mplib::core::DVec
240  mat4ToDVec(const Eigen::Matrix4f& mat)
241  {
242  std::vector<double> result;
243  result.resize(7);
244 
245  for (size_t i = 0; i < 3; ++i)
246  {
247  result.at(i) = static_cast<double>(mat(i, 3));
248  }
249 
250  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
251 
252  result.at(3) = static_cast<double>(quat.w);
253  result.at(4) = static_cast<double>(quat.x);
254  result.at(5) = static_cast<double>(quat.y);
255  result.at(6) = static_cast<double>(quat.z);
256 
257  return result;
258  }
259 
260  std::vector<float>
261  poseToFVec(PosePtr& pose)
262  {
263  Eigen::Matrix4f poseMat = pose->toEigen();
264  return mat4ToFVec(poseMat);
265  }
266 
267  std::vector<float>
268  mat4ToFVec(const Eigen::Matrix4f& mat)
269  {
270  std::vector<float> result;
271  result.resize(7);
272 
273  for (size_t i = 0; i < 3; ++i)
274  {
275  result.at(i) = mat(i, 3);
276  }
277 
278  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
279 
280  result.at(3) = quat.w;
281  result.at(4) = quat.x;
282  result.at(5) = quat.y;
283  result.at(6) = quat.z;
284 
285  return result;
286  }
287 
288  std::string
289  dVecToString(const mplib::core::DVec& dvec)
290  {
291  std::string s = "";
292  for (const double& d : dvec)
293  {
294  s = s + std::to_string(d) + " | ";
295  }
296  return s;
297  }
298 
299  Eigen::VectorXf
300  vecToEigen(std::vector<float>& vec)
301  {
302  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
303  }
304 
305  std::string
306  sVecToString(const std::vector<std::string>& vec, const std::string& delimiter)
307  {
308  std::string s = "";
309  for (size_t i = 0; i < vec.size(); ++i)
310  {
311  if (i != 0)
312  {
313  s = s + delimiter;
314  }
315  s + vec[i];
316  }
317  return s;
318  }
319 
320  mp::arondto::MPTraj
321  mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
322  {
323  std::vector<double> timesteps;
324  std::vector<std::vector<double>> trajPoses;
325 
326  std::ifstream f(filepath.c_str());
327 
328  if (!f.is_open())
329  {
330  ARMARX_ERROR << "Could not open file: " + filepath;
331  }
332 
333  using Tokenizer = boost::tokenizer<boost::escaped_list_separator<char>>;
334 
335 
336  // unsigned int stateDim = double().size();
337  unsigned int i = 1;
338 
339  double current_time = 0;
340  std::string line;
341  std::vector<std::string> stringVec;
342  while (getline(f, line))
343  {
344  if (containsHeader && i == 1)
345  {
346  i++;
347  continue;
348  }
349 
350  Tokenizer tok(line);
351  stringVec.assign(tok.begin(), tok.end());
352 
353  if (stringVec.size() == 0)
354  {
355  std::cout << "Line " << i << " is invalid skipping: " << line << std::endl;
356  continue;
357  }
358 
359  unsigned int j = 1;
360 
361  armarx::control::common::mp::arondto::MPTraj traj;
362 
363  //TrajData entry(*this);
364  int size = stringVec.size() - 1;
365  if (noTimeStamp)
366  {
367  j = 0;
368  //entry.timestamp = current_time;
369  timesteps.push_back(current_time);
370  current_time += 0.01;
371  size = stringVec.size();
372  }
373  else
374  {
375  //entry.timestamp = math::util::fromString<double>(stringVec[0]);
376  timesteps.push_back(std::stod(stringVec[0]));
377  }
378 
379  // DVec values(int(ceil(size/stateDim)*stateDim), 0.0);
380  //DVec values(size, 0.0);
381  std::vector<double> currentPose;
382 
383  for (unsigned int k = j; k < stringVec.size(); ++k)
384  {
385  currentPose.push_back(std::stod(stringVec[k]));
386  //values.at(sd) = (math::util::fromString<double>(stringVec[j]));
387  //math::util::checkValue(values.at(sd));
388  }
389  trajPoses.push_back(currentPose);
390 
391  i++;
392  }
393 
394  // convert to aron types
395  if (timesteps.size() != trajPoses.size())
396  {
397  ARMARX_ERROR << "consistency error in reading trajectory";
398  }
399  armarx::control::common::mp::arondto::MPTraj taskTraj;
400  taskTraj.time.setZero(timesteps.size());
401  taskTraj.traj.setZero(timesteps.size(), trajPoses.at(0).size());
402  ARMARX_INFO << "no timesteps" << timesteps.size();
403  ARMARX_INFO << "traj pose dim " << trajPoses.at(0).size();
404  for (unsigned int l = 0; l < timesteps.size(); l++)
405  {
406  taskTraj.time(l) = timesteps.at(l);
407  for (size_t j = 0; j < trajPoses.at(0).size(); ++j)
408  taskTraj.traj(l, j) = trajPoses.at(l).at(j);
409  }
410  return taskTraj;
411  }
412 
413  mp::arondto::MPTraj
414  createDummyTraj(const int dimension, const int timesteps)
415  {
416  mp::arondto::MPTraj traj;
417  traj.time = Eigen::VectorXd::LinSpaced(timesteps, 0, 1);
418  traj.traj = Eigen::MatrixXd::Zero(timesteps, dimension);
419  return traj;
420  }
421 
422  mp::arondto::MPConfig
423  getDefaultMPConfig(MPType mpType,
424  const std::string& name,
425  const std::string& nodeSetName,
426  const double& durationSec,
427  const std::vector<mp::arondto::MPTraj>& mpTraj,
428  const std::vector<mp::arondto::ListViaPoint>& viaPointList)
429  {
430  const std::string className = mpTypeToString.to_name(mpType);
431  const armarx::PackagePath configPath(
432  "armarx_control", "controller_config/mp/default_mp_" + className + ".json");
433  ARMARX_INFO << "using default config: " << configPath.toSystemPath();
434  auto cfg = armarx::readFromJson<mp::arondto::MPConfig>(configPath.toSystemPath());
435  cfg.name = name;
436  cfg.nodeSetName = nodeSetName;
437  if (durationSec > 0.0)
438  cfg.durationSec = durationSec;
439  if (mpTraj.size() > 0)
440  cfg.trajectoryList = mpTraj;
441  if (viaPointList.size() > 0)
442  cfg.viaPoints = viaPointList;
443  return cfg;
444  }
445 
446  mp::arondto::MPConfig
447  getDefaultMPConfig(MPType mpType,
448  const std::string& name,
449  const std::string& nodeSetName,
450  const double& durationSec,
451  const std::vector<std::string>& mpTraj,
452  const std::vector<mp::arondto::ListViaPoint>& viaPointList)
453  {
454  std::vector<mp::arondto::MPTraj> mpTrajList{};
455  for (auto& f : mpTraj)
456  {
457  mpTrajList.push_back(mpTrajFromFile(f));
458  }
459  return getDefaultMPConfig(mpType, name, nodeSetName, durationSec, mpTrajList, viaPointList);
460  }
461 
462  void
463  addViaPoint(mp::arondto::MPConfig& cfg,
464  const double& canValue,
465  const std::vector<double>& viaPoint)
466  {
467  auto vp = mp::arondto::ListViaPoint();
468  vp.canonicalValue = canValue;
469  vp.viaPointValue = viaPoint;
470  cfg.viaPoints.push_back(vp);
471  }
472 
473  void
474  addViaPoint(mp::arondto::MPConfig& cfg,
475  const double& canValue,
476  const std::vector<float>& viaPoint)
477  {
478  std::vector<double> viaPointVec;
479  std::transform(viaPoint.begin(),
480  viaPoint.end(),
481  std::back_inserter(viaPointVec),
482  [](float value) { return static_cast<double>(value); });
483  addViaPoint(cfg, canValue, viaPointVec);
484  }
485 
486  void
487  addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::VectorXf& viaPoint)
488  {
489  std::vector<double> viaPointVec;
490  std::transform(viaPoint.data(),
491  viaPoint.data() + viaPoint.size(),
492  std::back_inserter(viaPointVec),
493  [](float value) { return static_cast<double>(value); });
494  addViaPoint(cfg, canValue, viaPointVec);
495  }
496 
497  void
498  addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::Matrix4f& viaPose)
499  {
500  auto viaPointVec = mat4ToDVec(viaPose);
501  addViaPoint(cfg, canValue, viaPointVec);
502  }
503 
504  void
505  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
506  const double canonicalValue,
507  const std::vector<double>& viaPoint)
508  {
509  mp::arondto::ListViaPoint vp;
510  vp.canonicalValue = canonicalValue;
511  vp.viaPointValue = viaPoint;
512  vpList.push_back(vp);
513  }
514 
515  void
516  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
517  const double canonicalValue,
518  const std::vector<float>& viaPoint)
519  {
520  std::vector<double> viaPointVec;
521  std::transform(viaPoint.begin(),
522  viaPoint.end(),
523  std::back_inserter(viaPointVec),
524  [](float value) { return static_cast<double>(value); });
525  appendToViapointList(vpList, canonicalValue, viaPointVec);
526  }
527 
528  void
529  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
530  const double canonicalValue,
531  const Eigen::VectorXf& viaPoint)
532  {
533  std::vector<double> viaPointVec;
534  std::transform(viaPoint.data(),
535  viaPoint.data() + viaPoint.size(),
536  std::back_inserter(viaPointVec),
537  [](float value) { return static_cast<double>(value); });
538  appendToViapointList(vpList, canonicalValue, viaPointVec);
539  }
540 
541  void
542  appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
543  const double canonicalValue,
544  const Eigen::Matrix4f& viaPose)
545  {
546  auto viaPointVec = mat4ToDVec(viaPose);
547  appendToViapointList(vpList, canonicalValue, viaPointVec);
548  }
549 
550  float
551  getDeltaAngleBetweenRotMat(const Eigen::Matrix3f& rotMat1,
552  const Eigen::Matrix3f& rotMat2)
553  {
554  Eigen::Matrix3f poseDiffMatImp = rotMat1 * rotMat2.transpose();
555  Eigen::AngleAxisf oriDiffAngleAxis;
556  oriDiffAngleAxis.fromRotationMatrix(poseDiffMatImp);
557  float angle = VirtualRobot::MathTools::angleModPI(oriDiffAngleAxis.angle());
558  angle = std::fminf(angle, 2 * M_PI - angle);
559  return angle;
560  }
561 
562  float
563  getDeltaAngleBetweenPose(const Eigen::Matrix4f& pose1,
564  const Eigen::Matrix4f& pose2)
565  {
566  return getDeltaAngleBetweenRotMat(pose1.block<3, 3>(0, 0), pose2.block<3, 3>(0, 0));
567  }
568 
569  bool
570  poseDeviationTooLarge(const Eigen::Matrix4f& currentPose,
571  const Eigen::Matrix4f& desiredPose,
572  float positionThrMM,
573  float angleThrDeg)
574  {
575  float angle = getDeltaAngleBetweenPose(currentPose, desiredPose);
576  return (desiredPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)).norm() > positionThrMM or
577  angle > VirtualRobot::MathTools::deg2rad(angleThrDeg);
578  }
579 
580  void
581  reportPoseDeviationWarning(
582  const std::string& who,
583  const Eigen::Matrix4f& pose1,
584  const Eigen::Matrix4f& pose2,
585  const std::string& namePose1,
586  const std::string& namePose2)
587  {
588  auto quat1 = VirtualRobot::MathTools::eigen4f2quat(pose1);
589  auto quat2 = VirtualRobot::MathTools::eigen4f2quat(pose2);
590  ARMARX_INFO << deactivateSpam(2) << who << " " << namePose2 << " \n\n[ "
591  << pose2(0, 3) << ", " << pose2(1, 3) << ", " << pose2(2, 3) << ", "
592  << quat2.w << ", " << quat2.x << ", " << quat2.y << ", " << quat2.z
593  << " ]\n\n is too far away from the " << namePose1 << " \n\n[ "
594  << pose1(0, 3) << ", " << pose1(1, 3) << ", " << pose1(2, 3) << ", "
595  << quat1.w << ", " << quat1.x << ", " << quat1.y << ", " << quat1.z
596  << " ].";
597  }
598 
599  bool
600  detectAndReportPoseDeviationWarning(
601  const Eigen::Matrix4f& pose1,
602  const Eigen::Matrix4f& pose2,
603  const std::string& namePose1,
604  const std::string& namePose2,
605  float positionThrMM,
606  float angleThrDeg,
607  const std::string& who)
608  {
609  float angle = getDeltaAngleBetweenPose(pose1, pose2);
610  float dist = (pose2.block<3, 1>(0, 3) - pose1.block<3, 1>(0, 3)).norm();
611 
612  bool flagToReport = dist > positionThrMM or angle > VirtualRobot::MathTools::deg2rad(angleThrDeg);
613  if (flagToReport)
614  {
615  reportPoseDeviationWarning(who, pose1, pose2, namePose1, namePose2);
616  ARMARX_INFO << deactivateSpam(2) << "Delta distance is " << dist << " and angle is " << angle;
617  }
618  return flagToReport;
619  }
620 
621  Eigen::Matrix4f mirrorLeftRightPose(Eigen::Matrix4f& sourcePose)
622  {
623  Eigen::Matrix4f convert;
624  convert << 1, -1, -1, -1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1;
625  return sourcePose.cwiseProduct(convert);
626  }
627 
628  Eigen::Matrix4f mirrorLeftRightOri(Eigen::Matrix4f& sourcePose)
629  {
630  Eigen::Matrix4f convert;
631  convert << 1, -1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1;
632  return sourcePose.cwiseProduct(convert);
633  }
634 
635  Eigen::Matrix3f mirrorLeftRightOri(Eigen::Matrix3f& sourcePose)
636  {
637  Eigen::Matrix3f convert;
638  convert << 1, -1, -1, -1, 1, 1, -1, 1, 1;
639  return sourcePose.cwiseProduct(convert);
640  }
641 
642 } // namespace armarx::control::common
armarx::control::common::mirrorLeftRightPose
Eigen::Matrix4f mirrorLeftRightPose(Eigen::Matrix4f &sourcePose)
Definition: utils.cpp:621
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
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:111
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
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:414
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
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:600
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:628
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:581
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:261
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::control::common::dVecToString
std::string dVecToString(const mplib::core::DVec &dvec)
Definition: utils.cpp:289
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
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:570
armarx::control::common::mpTypeToString
const simox::meta::EnumNames< MPType > mpTypeToString
Definition: type.h:80
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:54
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
armarx::control::common::MPType
MPType
Definition: type.h:59
armarx::control::common::getPos
Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos(Eigen::Matrix4f &matrix)
Definition: utils.cpp:206
armarx::control::common::getDeltaAngleBetweenPose
float getDeltaAngleBetweenPose(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2)
Definition: utils.cpp:563
armarx::control::common::poseToDVec
mplib::core::DVec poseToDVec(PosePtr &pose)
Definition: utils.cpp:233
armarx::control::common::dVecToMat4
Eigen::Matrix4f dVecToMat4(const mplib::core::DVec &dvec)
create Eigen:Matrix4f from 7D double vector.
Definition: utils.cpp:218
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::control::common::dVecToPose
PosePtr dVecToPose(const mplib::core::DVec &dvec)
Definition: utils.cpp:212
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:40
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:300
armarx::control::common::getDeltaAngleBetweenRotMat
float getDeltaAngleBetweenRotMat(const Eigen::Matrix3f &rotMat1, const Eigen::Matrix3f &rotMat2)
Definition: utils.cpp:551
ExpressionException.h
armarx::control::common::mpTrajFromFile
mp::arondto::MPTraj mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
Definition: utils.cpp:321
armarx::Pose
The Pose class.
Definition: Pose.h:242
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
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:315
armarx::control::common::mat4ToFVec
std::vector< float > mat4ToFVec(const Eigen::Matrix4f &mat)
Definition: utils.cpp:268
armarx::control::common::appendToViapointList
void appendToViapointList(std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const std::vector< double > &viaPoint)
Definition: utils.cpp:505
armarx::control::common::mat4ToDVec
mplib::core::DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
Definition: utils.cpp:240
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
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:463
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
armarx::control::common::sVecToString
std::string sVecToString(const std::vector< std::string > &vec, const std::string &delimiter)
Definition: utils.cpp:306
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:186
armarx::PackagePath
Definition: PackagePath.h:55
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:423
PackagePath.h
norm
double norm(const Point &a)
Definition: point.hpp:94