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 
15 
17 {
18 
19  Eigen::VectorXf
20  getEigenVec(nlohmann::json& userConfig,
21  nlohmann::json& defaultConfig,
22  const std::string& entryName,
23  int size,
24  int value)
25  {
26  if (userConfig.find(entryName) != userConfig.end())
27  {
28  auto vec = userConfig[entryName].get<std::vector<float>>();
29  if (vec.size() > 0)
30  {
31  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
32  }
33  if (size > 0)
34  {
35  return Eigen::VectorXf::Ones(size) * value;
36  }
37  ARMARX_IMPORTANT << "user defined parameter " << entryName
38  << " have size 0, fall back to default";
39  }
40 
41  auto vec = defaultConfig[entryName].get<std::vector<float>>();
42  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
43  }
44 
45  void
46  getEigenMatrix(nlohmann::json& userConfig, const std::string& entryName, Eigen::MatrixXf& mat)
47  {
48  if (userConfig.find(entryName) != userConfig.end())
49  {
50  auto vec = userConfig[entryName].get<std::vector<std::vector<float>>>();
51  int rows = vec.size();
52  int cols = vec[0].size();
53  ARMARX_INFO << VAROUT(rows) << VAROUT(cols);
56  mat.resize(rows, cols);
57  for (int i = 0; i < rows; i++)
58  {
59  for (int j = 0; j < cols; j++)
60  {
61  mat(i, j) = vec[i][j];
62  }
63  }
64  // memcpy(mat.data(), vec.data(), rows * cols * sizeof(float));
65  }
66  else
67  {
68  ARMARX_ERROR << "values not found in user config";
69  }
70  }
71 
72  Eigen::VectorXf
73  getEigenVecWithDefault(nlohmann::json& userConfig,
74  Eigen::VectorXf defaultValue,
75  const std::string& entryName)
76  {
77  if (userConfig.find(entryName) != userConfig.end())
78  {
79  auto vec = userConfig[entryName].get<std::vector<float>>();
80  if (vec.size() == static_cast<unsigned>(defaultValue.size()))
81  {
82  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
83  }
84  ARMARX_WARNING << "size of the user defined parameter " << entryName
85  << " doesn't correspond to default size, use default value instead";
86  }
87  return defaultValue;
88  }
89 
90  void
91  getError(Eigen::Matrix4f& currentPose,
92  Eigen::Vector3d& targetPosition,
93  Eigen::Quaterniond& targetQuaternion,
94  double& posiError,
95  double& oriError)
96  {
97  Eigen::Matrix4d currentPosed = currentPose.cast<double>();
98  Eigen::Vector3d currentPosid = currentPosed.block<3, 1>(0, 3);
99  posiError = (currentPosid - targetPosition).norm();
100 
101  Eigen::Quaterniond currentOrientation = Eigen::Quaterniond{currentPosed.block<3, 3>(0, 0)};
102  oriError = targetQuaternion.angularDistance(currentOrientation);
103  if (oriError > M_PI)
104  {
105  oriError = 2 * M_PI - oriError;
106  }
107  }
108 
109  void
110  debugEigenPose(StringVariantBaseMap& datafields, const std::string& name, Eigen::Matrix4f pose)
111  {
112  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(pose);
113  datafields[name + "_x"] = new Variant(pose(0, 3));
114  datafields[name + "_y"] = new Variant(pose(1, 3));
115  datafields[name + "_z"] = new Variant(pose(2, 3));
116  datafields[name + "_rx"] = new Variant(rpy(0));
117  datafields[name + "_ry"] = new Variant(rpy(1));
118  datafields[name + "_rz"] = new Variant(rpy(2));
119  }
120 
121  void
122  debugEigenVec(StringVariantBaseMap& datafields, const std::string& name, Eigen::VectorXf vec)
123  {
124  for (int i = 0; i < vec.size(); i++)
125  {
126  datafields[name + "_" + std::to_string(i)] = new Variant(vec(i));
127  }
128  }
129 
130  void
131  debugStdVec(StringVariantBaseMap& datafields, const std::string& name, std::vector<float> vec)
132  {
133  for (size_t i = 0; i < vec.size(); i++)
134  {
135  datafields[name + "_" + std::to_string(i)] = new Variant(vec[i]);
136  }
137  }
138 
139  void
140  to_json(nlohmann::json& j, const Pose& fp)
141  {
142  j = nlohmann::json{{"qw", fp.orientation->qw},
143  {"qx", fp.orientation->qx},
144  {"qy", fp.orientation->qy},
145  {"qz", fp.orientation->qz},
146  {"x", fp.position->x},
147  {"y", fp.position->y},
148  {"z", fp.position->z}};
149  }
150 
151  void
152  from_json(const nlohmann::json& j, Pose& fp)
153  {
154  j.at("qw").get_to(fp.orientation->qw);
155  j.at("qx").get_to(fp.orientation->qx);
156  j.at("qy").get_to(fp.orientation->qy);
157  j.at("qz").get_to(fp.orientation->qz);
158  j.at("x").get_to(fp.position->x);
159  j.at("y").get_to(fp.position->y);
160  j.at("z").get_to(fp.position->z);
161  }
162 
163  void
164  to_json(nlohmann::json& j, const PosePtr& fp)
165  {
166  if (fp)
167  {
168  to_json(j, *fp);
169  }
170  }
171 
172  void
173  to_json(nlohmann::json& j, const PoseBasePtr& fp)
174  {
175  if (fp)
176  {
177  to_json(j, PosePtr::dynamicCast(fp));
178  }
179  }
180 
181  void
182  from_json(const nlohmann::json& j, PoseBasePtr& fp)
183  {
184  PosePtr p = new Pose();
185  from_json(j, p);
186  fp = p;
187  }
188 
189  void
190  from_json(const nlohmann::json& j, PosePtr& fp)
191  {
192  fp = new Pose();
193  from_json(j, *fp);
194  }
195 
196  PosePtr
198  {
199  return new Pose(dVecToMat4(dvec));
200  }
201 
204  {
205  ARMARX_CHECK_EQUAL(dvec.size(), 7);
206  Eigen::Matrix4f mat = VirtualRobot::MathTools::quat2eigen4f(static_cast<float>(dvec[4]),
207  static_cast<float>(dvec[5]),
208  static_cast<float>(dvec[6]),
209  static_cast<float>(dvec[3]));
210  mat(0, 3) = static_cast<float>(dvec[0]);
211  mat(1, 3) = static_cast<float>(dvec[1]);
212  mat(2, 3) = static_cast<float>(dvec[2]);
213 
214  return mat;
215  }
216 
219  {
220  Eigen::Matrix4f poseMat = pose->toEigen();
221  return mat4ToDVec(poseMat);
222  }
223 
226  {
227  std::vector<double> result;
228  result.resize(7);
229 
230  for (size_t i = 0; i < 3; ++i)
231  {
232  result.at(i) = static_cast<double>(mat(i, 3));
233  }
234 
235  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
236 
237  result.at(3) = static_cast<double>(quat.w);
238  result.at(4) = static_cast<double>(quat.x);
239  result.at(5) = static_cast<double>(quat.y);
240  result.at(6) = static_cast<double>(quat.z);
241 
242  return result;
243  }
244 
245  std::vector<float>
247  {
248  Eigen::Matrix4f poseMat = pose->toEigen();
249  return mat4ToFVec(poseMat);
250  }
251 
252  std::vector<float>
254  {
255  std::vector<float> result;
256  result.resize(7);
257 
258  for (size_t i = 0; i < 3; ++i)
259  {
260  result.at(i) = mat(i, 3);
261  }
262 
263  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
264 
265  result.at(3) = quat.w;
266  result.at(4) = quat.x;
267  result.at(5) = quat.y;
268  result.at(6) = quat.z;
269 
270  return result;
271  }
272 
273  std::string
275  {
276  std::string s = "";
277  for (const double& d : dvec)
278  {
279  s = s + std::to_string(d) + " | ";
280  }
281  return s;
282  }
283 
284  Eigen::VectorXf
285  vecToEigen(std::vector<float>& vec)
286  {
287  return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
288  }
289 
290  std::string
291  sVecToString(const std::vector<std::string>& vec, const std::string& delimiter)
292  {
293  std::string s = "";
294  for (size_t i = 0; i < vec.size(); ++i)
295  {
296  if (i != 0)
297  {
298  s = s + delimiter;
299  }
300  s + vec[i];
301  }
302  return s;
303  }
304 
305  mp::arondto::MPTraj
306  mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
307  {
308  std::vector<double> timesteps;
309  std::vector<std::vector<double>> trajPoses;
310 
311  std::ifstream f(filepath.c_str());
312 
313  if (!f.is_open())
314  {
315  ARMARX_ERROR << "Could not open file: " + filepath;
316  }
317 
318  using Tokenizer = boost::tokenizer<boost::escaped_list_separator<char>>;
319 
320 
321  // unsigned int stateDim = double().size();
322  unsigned int i = 1;
323 
324  double current_time = 0;
325  std::string line;
326  std::vector<std::string> stringVec;
327  while (getline(f, line))
328  {
329  if (containsHeader && i == 1)
330  {
331  i++;
332  continue;
333  }
334 
335  Tokenizer tok(line);
336  stringVec.assign(tok.begin(), tok.end());
337 
338  if (stringVec.size() == 0)
339  {
340  std::cout << "Line " << i << " is invalid skipping: " << line << std::endl;
341  continue;
342  }
343 
344  unsigned int j = 1;
345 
346  armarx::control::common::mp::arondto::MPTraj traj;
347 
348  //TrajData entry(*this);
349  int size = stringVec.size() - 1;
350  if (noTimeStamp)
351  {
352  j = 0;
353  //entry.timestamp = current_time;
354  timesteps.push_back(current_time);
355  current_time += 0.01;
356  size = stringVec.size();
357  }
358  else
359  {
360  //entry.timestamp = math::util::fromString<double>(stringVec[0]);
361  timesteps.push_back(std::stod(stringVec[0]));
362  }
363 
364  // DVec values(int(ceil(size/stateDim)*stateDim), 0.0);
365  //DVec values(size, 0.0);
366  std::vector<double> currentPose;
367 
368  for (unsigned int k = j; k < stringVec.size(); ++k)
369  {
370  currentPose.push_back(std::stod(stringVec[k]));
371  //values.at(sd) = (math::util::fromString<double>(stringVec[j]));
372  //math::util::checkValue(values.at(sd));
373  }
374  trajPoses.push_back(currentPose);
375 
376  i++;
377  }
378 
379  // convert to aron types
380  if (timesteps.size() != trajPoses.size())
381  {
382  ARMARX_ERROR << "consistency error in reading trajectory";
383  }
384  armarx::control::common::mp::arondto::MPTraj taskTraj;
385  taskTraj.time.setZero(timesteps.size());
386  taskTraj.traj.setZero(timesteps.size(), trajPoses.at(0).size());
387  ARMARX_INFO << "no timesteps" << timesteps.size();
388  ARMARX_INFO << "traj pose dim " << trajPoses.at(0).size();
389  for (unsigned int l = 0; l < timesteps.size(); l++)
390  {
391  taskTraj.time(l) = timesteps.at(l);
392  for (size_t j = 0; j < trajPoses.at(0).size(); ++j)
393  taskTraj.traj(l, j) = trajPoses.at(l).at(j);
394  }
395  return taskTraj;
396  }
397 
398 
399 } // namespace armarx::control::common
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:110
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::control::common::mat4ToFVec
std::vector< float > mat4ToFVec(Eigen::Matrix4f &mat)
Definition: utils.cpp:253
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:73
armarx::control::common::mat4ToDVec
mplib::core::DVec mat4ToDVec(Eigen::Matrix4f &mat)
Definition: utils.cpp:225
armarx::control::common::debugStdVec
void debugStdVec(StringVariantBaseMap &datafields, const std::string &name, std::vector< float > vec)
Definition: utils.cpp:131
armarx::control::common::getError
void getError(Eigen::Matrix4f &currentPose, Eigen::Vector3d &targetPosition, Eigen::Quaterniond &targetQuaternion, double &posiError, double &oriError)
Definition: utils.cpp:91
armarx::control::common::from_json
void from_json(const nlohmann::json &j, Pose &fp)
Definition: utils.cpp:152
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:246
armarx::control::common::dVecToString
std::string dVecToString(const mplib::core::DVec &dvec)
Definition: utils.cpp:274
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
armarx::control::common::poseToDVec
mplib::core::DVec poseToDVec(PosePtr &pose)
Definition: utils.cpp:218
armarx::control::common::dVecToMat4
Eigen::Matrix4f dVecToMat4(const mplib::core::DVec &dvec)
Definition: utils.cpp:203
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::control::common::dVecToPose
PosePtr dVecToPose(const mplib::core::DVec &dvec)
Definition: utils.cpp:197
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:122
utils.h
armarx::control::common::vecToEigen
Eigen::VectorXf vecToEigen(std::vector< float > &vec)
Definition: utils.cpp:285
ExpressionException.h
armarx::control::common::mpTrajFromFile
mp::arondto::MPTraj mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
Definition: utils.cpp:306
armarx::Pose
The Pose class.
Definition: Pose.h:242
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::control::common::to_json
void to_json(nlohmann::json &j, const Pose &fp)
Definition: utils.cpp:140
Eigen::Quaterniond
Quaternion< double, 0 > Quaterniond
Definition: EigenForwardDeclarations.h:62
armarx::control::common::getEigenVec
Eigen::VectorXf getEigenVec(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName, int size, int value)
Definition: utils.cpp:20
armarx::control::common::sVecToString
std::string sVecToString(const std::vector< std::string > &vec, const std::string &delimiter)
Definition: utils.cpp:291
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::control::common::getEigenMatrix
void getEigenMatrix(nlohmann::json &userConfig, const std::string &entryName, Eigen::MatrixXf &mat)
Definition: utils.cpp:46
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
norm
double norm(const Point &a)
Definition: point.hpp:94