Flexiv RDK APIs  1.4
utility.hpp
Go to the documentation of this file.
1 
6 #ifndef FLEXIV_RDK_UTILITY_HPP_
7 #define FLEXIV_RDK_UTILITY_HPP_
8 
9 #include <Eigen/Eigen>
10 #include <array>
11 
12 namespace flexiv {
13 namespace rdk {
14 namespace utility {
15 
23 inline std::array<double, 3> Quat2EulerZYX(const std::array<double, 4>& quat)
24 {
25  // Form quaternion
26  Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
27 
28  // The returned array is in [z,y,x] order
29  auto euler_ZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
30 
31  // Convert to general [x,y,z] order
32  return (std::array<double, 3> {euler_ZYX[2], euler_ZYX[1], euler_ZYX[0]});
33 }
34 
38 inline double Rad2Deg(double rad)
39 {
40  constexpr double kPi = 3.14159265358979323846;
41  return (rad / kPi * 180.0);
42 }
43 
47 template <size_t N>
48 inline std::array<double, N> Rad2Deg(const std::array<double, N>& rad_arr)
49 {
50  std::array<double, N> deg_arr;
51  for (size_t i = 0; i < N; i++) {
52  deg_arr[i] = Rad2Deg(rad_arr[i]);
53  }
54  return deg_arr;
55 }
56 
66 template <typename T>
67 inline std::string Vec2Str(const std::vector<T>& vec, size_t decimal = 3,
68  bool trailing_space = true, const std::string& separator = " ")
69 {
70  std::string padding = "";
71  std::stringstream ss;
72  ss.precision(decimal);
73  ss << std::fixed;
74 
75  for (const auto& v : vec) {
76  ss << padding << v;
77  padding = separator;
78  }
79 
80  if (trailing_space) {
81  ss << " ";
82  }
83  return ss.str();
84 }
85 
95 template <typename T, size_t N>
96 inline std::string Arr2Str(const std::array<T, N>& arr, size_t decimal = 3,
97  bool trailing_space = true, const std::string& separator = " ")
98 {
99  std::vector<T> vec(N);
100  std::copy(arr.begin(), arr.end(), vec.begin());
101  return Vec2Str(vec, decimal, trailing_space, separator);
102 }
103 
112 inline bool ProgramArgsExistAny(int argc, char** argv, const std::vector<std::string>& ref_strings)
113 {
114  for (int i = 0; i < argc; i++) {
115  for (const auto& v : ref_strings) {
116  if (v == std::string(argv[i])) {
117  return true;
118  }
119  }
120  }
121  return false;
122 }
123 
132 inline bool ProgramArgsExist(int argc, char** argv, const std::string& ref_strings)
133 {
134  return ProgramArgsExistAny(argc, argv, {ref_strings});
135 }
136 
144 inline std::string ParsePtStates(
145  const std::vector<std::string>& pt_states, const std::string& parse_target)
146 {
147  for (const auto& state : pt_states) {
148  // Skip if empty
149  if (state.empty()) {
150  continue;
151  }
152  std::stringstream ss(state);
153  std::string buffer;
154  std::vector<std::string> parsed_state;
155  while (ss >> buffer) {
156  parsed_state.push_back(buffer);
157  }
158  if (parsed_state.front() == parse_target) {
159  return parsed_state.back();
160  }
161  }
162 
163  return "";
164 }
165 
166 } /* namespace utility */
167 } /* namespace rdk */
168 } /* namespace flexiv */
169 
170 #endif /* FLEXIV_RDK_UTILITY_HPP_ */
std::array< double, 3 > Quat2EulerZYX(const std::array< double, 4 > &quat)
Convert quaternion to Euler angles with ZYX axis rotations.
Definition: utility.hpp:23
std::string ParsePtStates(const std::vector< std::string > &pt_states, const std::string &parse_target)
Parse the value of a specified primitive state from the pt_states string list.
Definition: utility.hpp:144
bool ProgramArgsExist(int argc, char **argv, const std::string &ref_strings)
Check if one specific string exists in the program arguments.
Definition: utility.hpp:132
std::string Vec2Str(const std::vector< T > &vec, size_t decimal=3, bool trailing_space=true, const std::string &separator=" ")
Convert an std::vector to a string.
Definition: utility.hpp:67
std::string Arr2Str(const std::array< T, N > &arr, size_t decimal=3, bool trailing_space=true, const std::string &separator=" ")
Convert an std::array to a string.
Definition: utility.hpp:96
double Rad2Deg(double rad)
Convert radians to degrees for a single value.
Definition: utility.hpp:38
bool ProgramArgsExistAny(int argc, char **argv, const std::vector< std::string > &ref_strings)
Check if any provided strings exist in the program arguments.
Definition: utility.hpp:112