Cheetah Software  1.0
utilities.h
Go to the documentation of this file.
1 #ifndef PROJECT_UTILITIES_H
2 #define PROJECT_UTILITIES_H
3 
4 #include <algorithm>
5 #include <map>
6 #include <random>
7 #include <unordered_map>
8 #include <vector>
9 #include "cppTypes.h"
10 
14 template <typename T>
15 bool fpEqual(T a, T b, T tol) {
16  return std::abs(a - b) <= tol;
17 }
18 
22 template <typename T>
23 bool vectorEqual(const std::vector<T>& a, const std::vector<T>& b) {
24  if (a.size() != b.size()) return false;
25  for (size_t i = 0; i < a.size(); i++) {
26  if (a[i] != b[i]) return false;
27  }
28  return true;
29 }
30 
34 template <typename T>
35 T coerce(T in, T min, T max) {
36  if (in < min) {
37  in = min;
38  }
39  if (in > max) {
40  in = max;
41  }
42  return in;
43 }
44 
45 template <typename T>
46 T deadband(T x, T range) {
47  if (x < range && x > -range) x = T(0);
48  return x;
49 }
50 
51 template <typename T>
52 void eigenDeadband(Eigen::MatrixBase<T>& v, typename T::Scalar band) {
53  for (size_t i = 0; i < T::RowsAtCompileTime; i++) {
54  for (size_t j = 0; j < T::ColsAtCompileTime; j++) {
55  v(i, j) = deadband(v(i, j), band);
56  }
57  }
58 }
59 
64 template <typename T>
65 int sgn(T val) {
66  return (T(0) < val) - (val < T(0));
67 }
68 
74 template <typename T>
76  Eigen::MatrixBase<T>& v, std::mt19937& gen,
77  std::uniform_real_distribution<typename T::Scalar>& dist) {
78  for (size_t i = 0; i < T::RowsAtCompileTime; i++) {
79  for (size_t j = 0; j < T::ColsAtCompileTime; j++) {
80  v(i, j) = dist(gen);
81  }
82  }
83 }
84 
88 template <typename T>
89 T generator_gaussian_noise(T mean, T var) {
90  static bool hasSpare = false;
91  static T rand1, rand2;
92 
93  if (hasSpare) {
94  hasSpare = false;
95  return mean + sqrt(var * rand1) * sin(rand2);
96  }
97  hasSpare = true;
98 
99  rand1 = rand() / ((T)RAND_MAX);
100  if (rand1 < 1e-100) rand1 = 1e-100;
101  rand1 = -2 * log(rand1);
102  rand2 = rand() / ((T)RAND_MAX) * M_PI * 2.;
103 
104  // printf("rand: %f, %f\n", rand1, rand2);
105  return mean + sqrt(var * rand1) * cos(rand2);
106 }
107 
111 template <typename T1, typename T2>
112 bool uMapContains(const std::unordered_map<T1, T2>& set, T1 key) {
113  return set.find(key) != set.end();
114 }
115 
119 template <typename T1, typename T2>
120 bool mapContains(const std::map<T1, T2>& set, T1 key) {
121  return set.find(key) != set.end();
122 }
123 
129 template <typename T>
130 std::string numberToString(T number) {
131  static_assert(std::is_floating_point<T>::value,
132  "numberToString must use a floating point type!");
133  char buffer[100];
134  sprintf(buffer, "%g", number);
135  return std::string(buffer);
136 }
137 
141 template <typename T>
142 T mapToRange(T x, T inputMin, T inputMax, T outputMin, T outputMax) {
143  return outputMin +
144  (x - inputMin) * (outputMax - outputMin) / (inputMax - inputMin);
145 }
146 
147 template <typename T>
148 std::string eigenToString(Eigen::MatrixBase<T>& value) {
149  std::stringstream ss;
150  ss << value;
151  return ss.str();
152 }
153 
154 static inline std::string boolToString(bool b) {
155  return std::string(b ? "true" : "false");
156 }
157 
158 void writeStringToFile(const std::string& fileName,
159  const std::string& fileData);
160 std::string getCurrentTimeAndDate();
161 std::string getConfigDirectoryPath();
162 
167 template <typename T>
168 void EulerZYX_2_SO3(const Vec3<T>& euler_zyx, Mat3<T>& SO3) {
169  Mat3<T> Mat3_Z, Mat3_Y, Mat3_X;
170  Mat3_Z << cos(euler_zyx[0]), -sin(euler_zyx[0]), 0, sin(euler_zyx[0]),
171  cos(euler_zyx[0]), 0, 0, 0, 1;
172  Mat3_Y << cos(euler_zyx[1]), 0, sin(euler_zyx[1]), 0, 1, 0,
173  -sin(euler_zyx[1]), 0, cos(euler_zyx[1]);
174  Mat3_X << 1, 0, 0, 0, cos(euler_zyx[2]), -sin(euler_zyx[2]), 0,
175  sin(euler_zyx[2]), cos(euler_zyx[2]);
176 
177  SO3 = Mat3_Z * Mat3_Y * Mat3_X;
178 }
179 
180 // Smooth Changing
181 template <typename T>
182 T smooth_change(T ini, T end, T moving_duration, T curr_time) {
183  if (curr_time > moving_duration) {
184  return end;
185  }
186  return (ini +
187  (end - ini) * 0.5 * (1 - cos(curr_time / moving_duration * M_PI)));
188 }
189 
190 template <typename T>
191 T smooth_change_vel(T ini, T end, T moving_duration, T curr_time) {
192  if (curr_time > moving_duration) {
193  return 0.0;
194  }
195  return ((end - ini) * 0.5 * (M_PI / moving_duration) *
196  sin(curr_time / moving_duration * M_PI));
197 }
198 
199 template <typename T>
200 T smooth_change_acc(T ini, T end, T moving_duration, T curr_time) {
201  if (curr_time > moving_duration) {
202  return 0.0;
203  }
204  return ((end - ini) * 0.5 * (M_PI / moving_duration) *
205  (M_PI / moving_duration) * cos(curr_time / moving_duration * M_PI));
206 }
207 
208 template <typename T>
209 T stringToNumber(const std::string& str) {
210  static_assert(std::is_same<T, double>::value || std::is_same<T, float>::value,
211  "stringToNumber only works for double/float");
212 
213  if (std::is_same<T, double>::value) {
214  return std::stod(str);
215  } else if (std::is_same<T, float>::value) {
216  return std::stof(str);
217  }
218 }
219 
220 template <typename T>
221 T stringToNumber(const char* str) {
222  return stringToNumber<T>(std::string(str));
223 }
224 
225 template <typename T>
226 Vec3<T> stringToVec3(const std::string& str) {
227  Vec3<T> v;
228  size_t i = 0;
229 
230  // seek past whitespace
231  while (str.at(i) == ' ') i++;
232 
233  if (str.at(i) == '[') {
234  i++;
235  } else {
236  throw std::runtime_error("stringToVec3 didn't find open bracket");
237  }
238 
239  // seek past whitespace
240  while (str.at(i) == ' ') i++;
241  size_t start = i;
242 
243  // seek to end of first number
244  while (str.at(i) != ',') i++;
245  v[0] = stringToNumber<T>(str.substr(start, i - start));
246  i++;
247 
248  while (str.at(i) == ' ') i++;
249  start = i;
250  while (str.at(i) != ',') i++;
251  v[1] = stringToNumber<T>(str.substr(start, i - start));
252  i++;
253 
254  while (str.at(i) == ' ') i++;
255  start = i;
256  while (str.at(i) != ']') i++;
257  v[2] = stringToNumber<T>(str.substr(start, i - start));
258  return v;
259 }
260 
261 std::string getLcmUrl(s64 ttl);
262 
263 #endif // PROJECT_UTILITIES_H
T generator_gaussian_noise(T mean, T var)
Definition: utilities.h:89
bool fpEqual(T a, T b, T tol)
Definition: utilities.h:15
std::string getCurrentTimeAndDate()
Definition: utilities.cpp:18
T smooth_change(T ini, T end, T moving_duration, T curr_time)
Definition: utilities.h:182
typename Eigen::Matrix< T, 3, 3 > Mat3
Definition: cppTypes.h:54
std::string numberToString(T number)
Definition: utilities.h:130
typename Eigen::Matrix< T, 3, 1 > Vec3
Definition: cppTypes.h:26
int64_t s64
Definition: cTypes.h:24
std::string eigenToString(Eigen::MatrixBase< T > &value)
Definition: utilities.h:148
std::string getLcmUrl(s64 ttl)
Definition: utilities.cpp:32
T coerce(T in, T min, T max)
Definition: utilities.h:35
void writeStringToFile(const std::string &fileName, const std::string &fileData)
Definition: utilities.cpp:7
T stringToNumber(const std::string &str)
Definition: utilities.h:209
std::string getConfigDirectoryPath()
Definition: utilities.cpp:30
T smooth_change_acc(T ini, T end, T moving_duration, T curr_time)
Definition: utilities.h:200
T deadband(T x, T range)
Definition: utilities.h:46
Vec3< T > stringToVec3(const std::string &str)
Definition: utilities.h:226
void EulerZYX_2_SO3(const Vec3< T > &euler_zyx, Mat3< T > &SO3)
Definition: utilities.h:168
int sgn(T val)
Definition: utilities.h:65
bool vectorEqual(const std::vector< T > &a, const std::vector< T > &b)
Definition: utilities.h:23
void eigenDeadband(Eigen::MatrixBase< T > &v, typename T::Scalar band)
Definition: utilities.h:52
bool uMapContains(const std::unordered_map< T1, T2 > &set, T1 key)
Definition: utilities.h:112
bool mapContains(const std::map< T1, T2 > &set, T1 key)
Definition: utilities.h:120
void fillEigenWithRandom(Eigen::MatrixBase< T > &v, std::mt19937 &gen, std::uniform_real_distribution< typename T::Scalar > &dist)
Definition: utilities.h:75
T smooth_change_vel(T ini, T end, T moving_duration, T curr_time)
Definition: utilities.h:191
static std::string boolToString(bool b)
Definition: utilities.h:154
T mapToRange(T x, T inputMin, T inputMax, T outputMin, T outputMax)
Definition: utilities.h:142