Merge pull request #282 from orocos/fix/m_pi

Replace M_PI by custom PI

orocos_kdl/examples/chainiksolverpos_lma_demo.cpp

@@ -54,6 +54,8 @@ estimate of shortest time per invposkin (ms) 0.155544
 #include <models.hpp>
 #include <chainiksolverpos_lma.hpp>
 #include <chainfksolverpos_recursive.hpp>
+#include <utilities/utility.h>
+
 #include <boost/timer.hpp>
 
 /**
@@ -90,9 +92,9 @@ void test_inverseposkin(KDL::Chain& chain) {
 	JntArray q_sol(n);
 	for (int trial=0;trial<num_of_trials;++trial) {
 		q.data.setRandom();
-		q.data *= M_PI;
+		q.data *= PI;
 		q_init.data.setRandom();
-		q_init.data *= M_PI;
+		q_init.data *= PI;
 		Frame pos_goal,pos_reached;
 		fwdkin.JntToCart(q,pos_goal);
 		//solver.compute_fwdpos(q.data);

orocos_kdl/examples/geometry.cpp

@@ -1,5 +1,6 @@
 #include <frames.hpp>
 #include <frames_io.hpp>
+#include <utilities/utility.h>
 
 int main()
 {
@@ -87,18 +88,18 @@ int main()
     //Creating an Identity rotation matrix
     KDL::Rotation r4=KDL::Rotation::Identity();
     //Creating a Rotation matrix from a rotation around X
-    KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3);
+    KDL::Rotation r5=KDL::Rotation::RotX(PI/3);
     //Creating a Rotation matrix from a rotation around Y
-    KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3);
+    KDL::Rotation r6=KDL::Rotation::RotY(PI/3);
     //Creating a Rotation matrix from a rotation around Z
-    KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3);
+    KDL::Rotation r7=KDL::Rotation::RotZ(PI/3);
     //Creating a Rotation matrix from a rotation around a arbitrary
     //vector, the vector should not be normalised
-    KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
+    KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),PI_4);
     //Creating a Rotation matrix from a rotation around a arbitrary
     //vector, the vector should be normalised
     KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
-                                         M_PI/4);
+                                         PI_4);
     //Creating a Rotation matrix from Euler ZYZ rotation angles
     KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.);
     //Creating a Rotation matrix from Euler ZYX rotation angles

orocos_kdl/examples/trajectory_example.cpp

@@ -19,6 +19,7 @@
 #include <path_roundedcomposite.hpp>
 #include <rotational_interpolation_sa.hpp>
 #include <utilities/error.h>
+#include <utilities/utility.h>
 #include <trajectory_composite.hpp>
 
 int main(int argc,char* argv[]) {
@@ -35,13 +36,13 @@ int main(int argc,char* argv[]) {
 		// When the routines are parallel, no rounding is needed, and no attempt is made
 		// add constructing a rounding arc.
 		// (It is still not possible when the segments are on top of each other)
-		// Note that you can only rotate in a deterministic way over an angle less then M_PI!
-		// With an angle == M_PI, you cannot predict over which side will be rotated.
-		// With an angle > M_PI, the routine will rotate over 2*M_PI-angle.
+		// Note that you can only rotate in a deterministic way over an angle less then PI!
+		// With an angle == PI, you cannot predict over which side will be rotated.
+		// With an angle > PI, the routine will rotate over 2*PI-angle.
 		// If you need to rotate over a larger angle, you need to introduce intermediate points.
 		// So, there is a common use case for using parallel segments.
-		path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0)));
-		path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0)));
+		path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0)));
+		path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0)));
 		path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0)));
 		path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1)));
 		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[]) {
 		ctraject->Add(traject);
 		ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
 
-
-
 		// use the trajectory
 		double dt=0.1;
 		std::ofstream of("./trajectory.dat");
@@ -113,5 +112,3 @@ int main(int argc,char* argv[]) {
 	}
 
 }
-
-

orocos_kdl/models/puma560.cpp

@@ -21,28 +21,29 @@
 
 #include <chain.hpp>
 #include "models.hpp"
+#include <utilities/utility.h>
 
 namespace KDL{
     Chain Puma560(){
         Chain puma560;
         puma560.addSegment(Segment());
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0,M_PI_2,0.0,0.0),
+                                   Frame::DH(0.0,PI_2,0.0,0.0),
                                    RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));
         puma560.addSegment(Segment(Joint(Joint::RotZ),
                                    Frame::DH(0.4318,0.0,0.0,0.0),
                                    RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));
         puma560.addSegment(Segment());
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
+                                   Frame::DH(0.0203,-PI_2,0.15005,0.0),
                                    RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0,M_PI_2,0.4318,0.0),
+                                   Frame::DH(0.0,PI_2,0.4318,0.0),
                                    RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));
         puma560.addSegment(Segment());
         puma560.addSegment(Segment());
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0,-M_PI_2,0.0,0.0),
+                                   Frame::DH(0.0,-PI_2,0.0,0.0),
                                    RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));
         puma560.addSegment(Segment(Joint(Joint::RotZ),
                                    Frame::DH(0.0,0.0,0.0,0.0),

orocos_kdl/src/frames.cpp

@@ -26,9 +26,8 @@
  ***************************************************************************/
 
 #include "frames.hpp"
+#include "utilities/utility.h"
 
-#define _USE_MATH_DEFINES  // For MSVC
-#include <math.h>
 #include <algorithm>
 
 namespace KDL {
@@ -251,7 +250,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
     {
 		double epsilon=1E-12;
 		pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
-        if ( fabs(pitch) > (M_PI/2.0-epsilon) ) {
+        if ( fabs(pitch) > (PI_2-epsilon) ) {
             yaw = atan2(	-data[1], data[4]);
             roll  = 0.0 ;
         } else {
@@ -384,7 +383,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
         }
 
         // otherwise this singularity is angle = 180
-        angle = M_PI;
+        angle = PI;
         double xx = (data[0] + 1) / 2;
         double yy = (data[4] + 1) / 2;
         double zz = (data[8] + 1) / 2;

orocos_kdl/src/path_roundedcomposite.hpp

@@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path
 		 * - 3101 if the eq. radius <= 0
 		 * - 3102 if the first segment in a rounding has zero length.
 		 * - 3103 if the second segment in a rounding has zero length.
-		 * - 3104 if the angle between the first and the second segment is close to M_PI.
+		 * - 3104 if the angle between the first and the second segment is close to PI.
 		 *         (meaning that the segments are on top of each other)
 		 * - 3105 if the distance needed for the rounding is larger then the first segment.
 		 * - 3106 if the distance needed for the rounding is larger then the second segment.

orocos_kdl/src/utilities/utility.cxx

@@ -11,10 +11,12 @@
 
 namespace KDL {
 
-int STREAMBUFFERSIZE=10000;
+int STREAMBUFFERSIZE = 10000;
 int MAXLENFILENAME = 255;
-const double PI=       3.1415926535897932384626433832795;
-const double deg2rad = 0.01745329251994329576923690768488;
-const double rad2deg = 57.2957795130823208767981548141052;
-double epsilon = 0.000001;
+const double PI      = 3.141592653589793238462643383279502884; // PI
+const double PI_2    = 1.570796326794896619231321691639751442; // PI/2
+const double PI_4    = 0.785398163397448309615660845819875721; // PI/4
+const double deg2rad = 0.017453292519943295769236907684886127; // PI/180
+const double rad2deg = 57.29577951308232087679815481410517033; // 180/PI
+double epsilon = 1e-6;
 }

orocos_kdl/src/utilities/utility.h

@@ -179,6 +179,12 @@ extern int          MAXLENFILENAME;
 //! the value of pi
 extern const double PI;
 
+//! the value of pi/2
+extern const double PI_2;
+
+//! the value of pi/4
+extern const double PI_4;
+
 //! the value pi/180
 extern const double deg2rad;