Skip to content

Commit a2a2dff

Browse files
MatthijsBurghMatthijsBurgh
authored
Merge pull request #282 from orocos/fix/m_pi
Replace M_PI by custom PI
2 parents 29f2329 + 00b7533 commit a2a2dff

10 files changed

+126
-118
lines changed

‎orocos_kdl/examples/chainiksolverpos_lma_demo.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,8 @@ estimate of shortest time per invposkin (ms) 0.155544
5454
#include <models.hpp>
5555
#include <chainiksolverpos_lma.hpp>
5656
#include <chainfksolverpos_recursive.hpp>
57+
#include <utilities/utility.h>
58+
5759
#include <boost/timer.hpp>
5860

5961
/**
@@ -90,9 +92,9 @@ void test_inverseposkin(KDL::Chain& chain) {
9092
JntArray q_sol(n);
9193
for (int trial=0;trial<num_of_trials;++trial) {
9294
q.data.setRandom();
93-
q.data *= M_PI;
95+
q.data *= PI;
9496
q_init.data.setRandom();
95-
q_init.data *= M_PI;
97+
q_init.data *= PI;
9698
Frame pos_goal,pos_reached;
9799
fwdkin.JntToCart(q,pos_goal);
98100
//solver.compute_fwdpos(q.data);

‎orocos_kdl/examples/geometry.cpp

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include <frames.hpp>
22
#include <frames_io.hpp>
3+
#include <utilities/utility.h>
34

45
int main()
56
{
@@ -87,18 +88,18 @@ int main()
8788
//Creating an Identity rotation matrix
8889
KDL::Rotation r4=KDL::Rotation::Identity();
8990
//Creating a Rotation matrix from a rotation around X
90-
KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3);
91+
KDL::Rotation r5=KDL::Rotation::RotX(PI/3);
9192
//Creating a Rotation matrix from a rotation around Y
92-
KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3);
93+
KDL::Rotation r6=KDL::Rotation::RotY(PI/3);
9394
//Creating a Rotation matrix from a rotation around Z
94-
KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3);
95+
KDL::Rotation r7=KDL::Rotation::RotZ(PI/3);
9596
//Creating a Rotation matrix from a rotation around a arbitrary
9697
//vector, the vector should not be normalised
97-
KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
98+
KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),PI_4);
9899
//Creating a Rotation matrix from a rotation around a arbitrary
99100
//vector, the vector should be normalised
100101
KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
101-
M_PI/4);
102+
PI_4);
102103
//Creating a Rotation matrix from Euler ZYZ rotation angles
103104
KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.);
104105
//Creating a Rotation matrix from Euler ZYX rotation angles

‎orocos_kdl/examples/trajectory_example.cpp

Lines changed: 6 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <path_roundedcomposite.hpp>
2020
#include <rotational_interpolation_sa.hpp>
2121
#include <utilities/error.h>
22+
#include <utilities/utility.h>
2223
#include <trajectory_composite.hpp>
2324

2425
int main(int argc,char* argv[]) {
@@ -35,13 +36,13 @@ int main(int argc,char* argv[]) {
3536
// When the routines are parallel, no rounding is needed, and no attempt is made
3637
// add constructing a rounding arc.
3738
// (It is still not possible when the segments are on top of each other)
38-
// Note that you can only rotate in a deterministic way over an angle less then M_PI!
39-
// With an angle == M_PI, you cannot predict over which side will be rotated.
40-
// With an angle > M_PI, the routine will rotate over 2*M_PI-angle.
39+
// Note that you can only rotate in a deterministic way over an angle less then PI!
40+
// With an angle == PI, you cannot predict over which side will be rotated.
41+
// With an angle > PI, the routine will rotate over 2*PI-angle.
4142
// If you need to rotate over a larger angle, you need to introduce intermediate points.
4243
// So, there is a common use case for using parallel segments.
43-
path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0)));
44-
path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0)));
44+
path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0)));
45+
path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0)));
4546
path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0)));
4647
path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1)));
4748
path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0)));
@@ -61,8 +62,6 @@ int main(int argc,char* argv[]) {
6162
ctraject->Add(traject);
6263
ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
6364

64-
65-
6665
// use the trajectory
6766
double dt=0.1;
6867
std::ofstream of("./trajectory.dat");
@@ -113,5 +112,3 @@ int main(int argc,char* argv[]) {
113112
}
114113

115114
}
116-
117-

‎orocos_kdl/models/puma560.cpp

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -21,28 +21,29 @@
2121

2222
#include <chain.hpp>
2323
#include "models.hpp"
24+
#include <utilities/utility.h>
2425

2526
namespace KDL{
2627
Chain Puma560(){
2728
Chain puma560;
2829
puma560.addSegment(Segment());
2930
puma560.addSegment(Segment(Joint(Joint::RotZ),
30-
Frame::DH(0.0,M_PI_2,0.0,0.0),
31+
Frame::DH(0.0,PI_2,0.0,0.0),
3132
RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));
3233
puma560.addSegment(Segment(Joint(Joint::RotZ),
3334
Frame::DH(0.4318,0.0,0.0,0.0),
3435
RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));
3536
puma560.addSegment(Segment());
3637
puma560.addSegment(Segment(Joint(Joint::RotZ),
37-
Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
38+
Frame::DH(0.0203,-PI_2,0.15005,0.0),
3839
RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));
3940
puma560.addSegment(Segment(Joint(Joint::RotZ),
40-
Frame::DH(0.0,M_PI_2,0.4318,0.0),
41+
Frame::DH(0.0,PI_2,0.4318,0.0),
4142
RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));
4243
puma560.addSegment(Segment());
4344
puma560.addSegment(Segment());
4445
puma560.addSegment(Segment(Joint(Joint::RotZ),
45-
Frame::DH(0.0,-M_PI_2,0.0,0.0),
46+
Frame::DH(0.0,-PI_2,0.0,0.0),
4647
RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));
4748
puma560.addSegment(Segment(Joint(Joint::RotZ),
4849
Frame::DH(0.0,0.0,0.0,0.0),

‎orocos_kdl/src/frames.cpp

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,8 @@
2626
***************************************************************************/
2727

2828
#include "frames.hpp"
29+
#include "utilities/utility.h"
2930

30-
#define _USE_MATH_DEFINES // For MSVC
31-
#include <math.h>
3231
#include <algorithm>
3332

3433
namespace KDL {
@@ -251,7 +250,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
251250
{
252251
double epsilon=1E-12;
253252
pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) );
254-
if ( fabs(pitch) > (M_PI/2.0-epsilon) ) {
253+
if ( fabs(pitch) > (PI_2-epsilon) ) {
255254
yaw = atan2( -data[1], data[4]);
256255
roll = 0.0 ;
257256
} else {
@@ -384,7 +383,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
384383
}
385384

386385
// otherwise this singularity is angle = 180
387-
angle = M_PI;
386+
angle = PI;
388387
double xx = (data[0] + 1) / 2;
389388
double yy = (data[4] + 1) / 2;
390389
double zz = (data[8] + 1) / 2;

‎orocos_kdl/src/path_roundedcomposite.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path
9696
* - 3101 if the eq. radius <= 0
9797
* - 3102 if the first segment in a rounding has zero length.
9898
* - 3103 if the second segment in a rounding has zero length.
99-
* - 3104 if the angle between the first and the second segment is close to M_PI.
99+
* - 3104 if the angle between the first and the second segment is close to PI.
100100
* (meaning that the segments are on top of each other)
101101
* - 3105 if the distance needed for the rounding is larger then the first segment.
102102
* - 3106 if the distance needed for the rounding is larger then the second segment.

‎orocos_kdl/src/utilities/utility.cxx

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,12 @@
1111

1212
namespace KDL {
1313

14-
int STREAMBUFFERSIZE=10000;
14+
int STREAMBUFFERSIZE = 10000;
1515
int MAXLENFILENAME = 255;
16-
const double PI= 3.1415926535897932384626433832795;
17-
const double deg2rad = 0.01745329251994329576923690768488;
18-
const double rad2deg = 57.2957795130823208767981548141052;
19-
double epsilon = 0.000001;
16+
const double PI = 3.141592653589793238462643383279502884; // PI
17+
const double PI_2 = 1.570796326794896619231321691639751442; // PI/2
18+
const double PI_4 = 0.785398163397448309615660845819875721; // PI/4
19+
const double deg2rad = 0.017453292519943295769236907684886127; // PI/180
20+
const double rad2deg = 57.29577951308232087679815481410517033; // 180/PI
21+
double epsilon = 1e-6;
2022
}

‎orocos_kdl/src/utilities/utility.h

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -179,6 +179,12 @@ extern int MAXLENFILENAME;
179179
//! the value of pi
180180
extern const double PI;
181181

182+
//! the value of pi/2
183+
extern const double PI_2;
184+
185+
//! the value of pi/4
186+
extern const double PI_4;
187+
182188
//! the value pi/180
183189
extern const double deg2rad;
184190

0 commit comments

Comments
 (0)