Kindr
Kinematics and Dynamics for Robotics
common.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 
29 #pragma once
30 
31 #include <cmath>
32 #include <cassert>
33 #include <iostream>
34 #include <limits>
35 #include <random>
36 #include <Eigen/Core>
37 
38 namespace kindr {
39 
45 template<typename T>
47 {
48  static_assert(!std::numeric_limits<T>::is_exact , "floatingPointModulo: floating-point type expected");
49 
50  if (y == 0.0)
51  return x;
52 
53  double m= x - y * floor(x/y);
54 
55  // handle boundary cases resulted from floating-point cut off:
56 
57  if (y > 0) // modulo range: [0..y)
58  {
59  if (m>=y) // mod(-1e-16 , 360. ): m= 360.
60  return 0;
61 
62  if (m<0 )
63  {
64  if (y+m == y)
65  return 0 ; // just in case...
66  else
67  return y+m; // Mod(106.81415022205296 , 2*M_PI ): m= -1.421e-14
68  }
69  }
70  else // modulo range: (y..0]
71  {
72  if (m<=y) // mod(1e-16 , -360. ): m= -360.
73  return 0;
74 
75  if (m>0 )
76  {
77  if (y+m == y)
78  return 0 ; // just in case...
79  else
80  return y+m; // mod(-106.81415022205296, -2*M_PI): m= 1.421e-14
81  }
82  }
83 
84  return m;
85 }
86 
88 template<typename T>
89 inline T wrapAngle(T angle, T x1, T x2)
90 {
91  return floatingPointModulo(angle-x1, x2-x1) + x1;
92 }
93 
95 template<typename T>
96 inline T wrapPosNegPI(T angle)
97 {
98  return floatingPointModulo(angle + T(M_PI), T(2.0*M_PI) ) - M_PI;
99 }
100 
102 template<typename T>
103 inline T wrapTwoPI(T angle)
104 {
105  return floatingPointModulo(angle, T(2.0*M_PI));
106 }
107 
108 
109 template <typename PrimType_, int Rows_>
110 static inline void setUniformRandom(Eigen::Matrix<PrimType_, Rows_, 1>& vector, PrimType_ min, PrimType_ max) {
111  std::random_device rand_dev;
112  std::mt19937 generator(rand_dev());
113  std::uniform_real_distribution<PrimType_> distr(min, max);
114  for (int i=0; i<vector.size(); i++) {
115  vector(i) = distr(generator);
116  }
117 }
118 
119 
120 namespace internal {
121 
126 template<typename Left_, typename Right_>
127 class MultiplicationTraits {
128  public:
129 // inline static Left_ mult(const Left_& lhs, const Right_& rhs);
130 };
131 
136 template<typename Left_, typename Right_>
137 class ComparisonTraits {
138  public:
139 // inline static bool isEqual(const Left_& left, const Right_& right);
140 };
141 
142 
146 template<typename Quantitiy_>
147 class get_scalar {
148  public:
149 // typedef PrimType Scalar;
150 };
151 
152 
153 template<typename T>
154 class GenericNumTraits {
155  public:
156  enum {
157  IsInteger = std::numeric_limits<T>::is_integer,
158  IsSigned = std::numeric_limits<T>::is_signed,
159  };
160 
161  typedef T Scalar;
162 
163 
164  static inline Scalar epsilon() { return std::numeric_limits<T>::epsilon(); }
165  static inline Scalar dummy_precision()
166  {
167  // make sure to override this for floating-point types
168  return Scalar(0);
169  }
170  static inline T highest() { return (std::numeric_limits<T>::max)(); }
171  static inline T lowest() { return IsInteger ? (std::numeric_limits<T>::min)() : (-(std::numeric_limits<T>::max)()); }
172 
173 };
174 
175 template<typename T>
176 class NumTraits : GenericNumTraits<T> {
177  public:
178 };
179 
180 template<>
181 class NumTraits<float> : GenericNumTraits<float>
182 {
183  public:
184  static inline float dummy_precision() { return 1e-5f; }
185 };
186 
187 template<>
188 class NumTraits<double> : GenericNumTraits<double>
189 {
190  public:
191  static inline double dummy_precision() { return 1e-12; }
192 };
193 
194 template<>
195 class NumTraits<long double> : GenericNumTraits<long double>
196 {
197  public:
198  static inline long double dummy_precision() { return 1e-15l; }
199 };
200 
201 
202 } // namespace internal
203 } // namespace kindr
204 
T wrapAngle(T angle, T x1, T x2)
wrap angle to [x1..x2)
Definition: common.hpp:89
T wrapPosNegPI(T angle)
wrap angle to [-PI..PI)
Definition: common.hpp:96
T floatingPointModulo(T x, T y)
Floating-point modulo.
Definition: common.hpp:46
static void setUniformRandom(Eigen::Matrix< PrimType_, Rows_, 1 > &vector, PrimType_ min, PrimType_ max)
Definition: common.hpp:110
T wrapTwoPI(T angle)
wrap angle to [0..2*PI)
Definition: common.hpp:103