1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
|
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Simulation, Systems Optimization and Robotics
// group, TU Darmstadt nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#ifndef utilfunctions_h__
#define utilfunctions_h__
#include <cmath>
namespace util{
static inline float normalize_angle_pos(float angle)
{
return fmod(fmod(angle, 2.0f*M_PI) + 2.0f*M_PI, 2.0f*M_PI);
}
static inline float normalize_angle(float angle)
{
float a = normalize_angle_pos(angle);
if (a > M_PI){
a -= 2.0f*M_PI;
}
return a;
}
static inline float sqr(float val)
{
return val*val;
}
static inline int sign(int x)
{
return x > 0 ? 1 : -1;
}
template<typename T>
static T toDeg(const T radVal)
{
return radVal * static_cast<T>(180.0 / M_PI);
}
template<typename T>
static T toRad(const T degVal)
{
return degVal * static_cast<T>(M_PI / 180.0);
}
static bool poseDifferenceLargerThan(const Eigen::Vector3f& pose1, const Eigen::Vector3f& pose2, float distanceDiffThresh, float angleDiffThresh)
{
//check distance
if ( ( (pose1.head<2>() - pose2.head<2>()).norm() ) > distanceDiffThresh){
return true;
}
float angleDiff = (pose1.z() - pose2.z());
if (angleDiff > M_PI) {
angleDiff -= M_PI * 2.0f;
} else if (angleDiff < -M_PI) {
angleDiff += M_PI * 2.0f;
}
if (abs(angleDiff) > angleDiffThresh){
return true;
}
return false;
}
}
#endif
| cs |
namespace util{
namespace는 util이다.
static inline float normalize_angle_pos(float angle)
{
return fmod(fmod(angle, 2.0f*M_PI) + 2.0f*M_PI, 2.0f*M_PI);
}
fmod는 부동 소수점 나머지 연산이다. 예를 들면 더 쉬울테니 아래 내용을 보자.
#include <math.h>
#include <stdio.h>
int main(void)
{
double x, y, z;
x = -10.0;
y = 3.0;
z = fmod(x,y); /* z = -1.0 */
printf("fmod( %lf, %lf) = %lf\n", x, y, z);
}
/******************* Output should be similar to: ***************
fmod( -10.000000, 3.000000) = -1.000000
*/즉, narmalize_angle_pos(float angle) 함수는
robot의 pos 중 angle 값을 normalize 하는 것이다.
static inline float normalize_angle(float angle)
{
float a = normalize_angle_pos(angle);
if (a > M_PI){
a -= 2.0f*M_PI;
}
return a;
}
normalize 한다는 것은 어려운 의미는 아니고,
angle의 값이 radian 값으로 ...~ -6.28 ~ -3.14 ~ -1.57 ~ 0.0 ~ 1.57 ~ 3.14 ~ 6.28 ~ ... 등의 값으로 존재할 텐데,
이 값들을 -3.14에서 +3.14 사이의 값으로 nomarize를 한다는 것이다.
static inline float sqr(float val)
{
return val*val;
}
제곱연산이다.
static inline int sign(int x)
{
return x > 0 ? 1 : -1;
}
0보다 크면 1을 0보다 작으면 -1을 반환하는 함수이다.
template<typename T>
static T toDeg(const T radVal)
{
return radVal * static_cast<T>(180.0 / M_PI);
}
T에 자료형을 주면서 함수를 호출하면 radVal이 어떤 자료형이든 Degree로 cast 시킬 수 있다.
template<typename T>
static T toRad(const T degVal)
{
return degVal * static_cast<T>(M_PI / 180.0);
}
Degree를 Radian으로 cast한다.
static bool poseDifferenceLargerThan(const Eigen::Vector3f& pose1,
const Eigen::Vector3f& pose2,
float distanceDiffThresh,
float angleDiffThresh)
const Eigen::Vector3f& pose2,
float distanceDiffThresh,
float angleDiffThresh)
{
//check distance
if ( ( (pose1.head<2>() - pose2.head<2>()).norm() ) > distanceDiffThresh){
return true;
}
float angleDiff = (pose1.z() - pose2.z());
if (angleDiff > M_PI) {
angleDiff -= M_PI * 2.0f;
} else if (angleDiff < -M_PI) {
angleDiff += M_PI * 2.0f;
}
if (abs(angleDiff) > angleDiffThresh){
return true;
}
return false;
}
해당 내용은 pose1과 pose2의 거리와 각도를 비교하여 임계치보다 클 시에 true를 반환하는 것인데, 로봇의 움직임이 갑자기 크게 변할 시에 이를 알 수 있게 되는 것이다.
댓글 없음:
댓글 쓰기