Added changes to KDL brought after the SVN merge, where applicable

Taken from commits 470880e, df4c99f, cd0ae20, efb0823, c519989
FreeCAD · Oct 14, 2015 · ba2e003 · ba2e003
1 parent 9304f7a
commit ba2e003
Show file tree

Hide file tree

Showing 10 changed files with 32 additions and 26 deletions.
diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp
@@ -57,7 +57,7 @@ namespace KDL {
         const Chain chain;
 	int nr;
 	unsigned int nj;
-        unsigned int ns;	
+        unsigned int ns;
 	Vector grav;
 	Vector vectornull;
 	JntArray jntarraynull;

diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp
@@ -28,6 +28,10 @@
 
 namespace KDL
 {
+
+    // FIXME: seems this class is unused/unmaintained/unfinished for several years
+    //        it supposed to be either fixer or removed
+
     /**
      * Implementation of a inverse velocity kinematics algorithm based
      * on the generalize pseudo inverse to calculate the velocity
@@ -60,6 +64,8 @@ namespace KDL
          * @param alpha the null-space velocity gain
          *
          */
+
+        // FIXME: alpha is int but is initialized with a float value.
         ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, double alpha = 0.25);
         explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25);
         ~ChainIkSolverVel_pinv_nso();

diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.hpp b/src/Mod/Robot/App/kdl_cp/frameacc.hpp
@@ -45,18 +45,18 @@ class FrameAcc;
 class RotationAcc;
 class VectorAcc;
 
-IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
-IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
-IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
-IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
-IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
-IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
-IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
-IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
-IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
-IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
-IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
-IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
+IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
+IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
+IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
+IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
+IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
+IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
+IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
+IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps);
+IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps);
+IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps);
+IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps);
+IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps);
 
 class VectorAcc
 {

diff --git a/src/Mod/Robot/App/kdl_cp/frames.cpp b/src/Mod/Robot/App/kdl_cp/frames.cpp
@@ -387,7 +387,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
 
 bool operator==(const Rotation& a,const Rotation& b) {
 #ifdef KDL_USE_EQUAL
-    return Equal(a,b);
+    return Equal(a,b,epsilon);
 #else
     return ( a.data[0]==b.data[0] &&
              a.data[1]==b.data[1] &&

diff --git a/src/Mod/Robot/App/kdl_cp/frames.inl b/src/Mod/Robot/App/kdl_cp/frames.inl
@@ -1267,7 +1267,7 @@ IMETHOD void posrandom(Frame& F) {
 
 IMETHOD bool operator==(const Frame& a,const Frame& b ) {
 #ifdef KDL_USE_EQUAL
-    return Equal(a,b);
+    return Equal(a,b,epsilon);
 #else
         return (a.p == b.p &&
                 a.M == b.M );
@@ -1280,7 +1280,7 @@ IMETHOD bool operator!=(const Frame& a,const Frame& b) {
 
 IMETHOD bool operator==(const Vector& a,const Vector& b) {
 #ifdef KDL_USE_EQUAL
-    return Equal(a,b);
+    return Equal(a,b,epsilon);
 #else
         return (a.data[0]==b.data[0]&&
                 a.data[1]==b.data[1]&&
@@ -1294,7 +1294,7 @@ IMETHOD bool operator!=(const Vector& a,const Vector& b) {
 
 IMETHOD bool operator==(const Twist& a,const Twist& b) {
 #ifdef KDL_USE_EQUAL
-    return Equal(a,b);
+    return Equal(a,b,epsilon);
 #else
         return (a.rot==b.rot &&
                 a.vel==b.vel  );
@@ -1307,7 +1307,7 @@ IMETHOD bool operator!=(const Twist& a,const Twist& b) {
 
 IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
 #ifdef KDL_USE_EQUAL
-    return Equal(a,b);
+    return Equal(a,b,epsilon);
 #else
     return (a.force==b.force &&
             a.torque==b.torque );
@@ -1323,7 +1323,7 @@ IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
 
 IMETHOD bool operator==(const Vector2& a,const Vector2& b) {
 #ifdef KDL_USE_EQUAL
-    return Equal(a,b);
+    return Equal(a,b,epsilon);
 #else
         return (a.data[0]==b.data[0]&&
                 a.data[1]==b.data[1] );

diff --git a/src/Mod/Robot/App/kdl_cp/jacobian.cpp b/src/Mod/Robot/App/kdl_cp/jacobian.cpp
@@ -126,12 +126,12 @@ namespace KDL
 
     bool Jacobian::operator ==(const Jacobian& arg)const
     {
-        return Equal((*this),arg);
+        return Equal((*this),arg,epsilon);
     }
 
     bool Jacobian::operator!=(const Jacobian& arg)const
     {
-        return !Equal((*this),arg);
+        return !Equal((*this),arg,epsilon);
     }
 
     bool Equal(const Jacobian& a,const Jacobian& b,double eps)

diff --git a/src/Mod/Robot/App/kdl_cp/jacobian.hpp b/src/Mod/Robot/App/kdl_cp/jacobian.hpp
@@ -29,7 +29,7 @@ namespace KDL
 {
     // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
     class Jacobian;
-    bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon);
+    bool Equal(const Jacobian& a,const Jacobian& b,double eps);
 
 
     class Jacobian

diff --git a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp
@@ -115,7 +115,7 @@ namespace KDL
         return src1.data.isApprox(src2.data,eps);
     }
 
-    bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
+    bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2,epsilon);};
     //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
 
 }

diff --git a/src/Mod/Robot/App/kdl_cp/path_composite.cpp b/src/Mod/Robot/App/kdl_cp/path_composite.cpp
@@ -132,13 +132,13 @@ int Path_Composite::GetNrOfSegments() {
 
 Path* Path_Composite::GetSegment(int i) {
 	assert(i>=0);
-	assert(i<dv.size());
+	assert(i<static_cast<int>(dv.size()));
 	return gv[i].first;
 }
 
 double Path_Composite::GetLengthToEndOfSegment(int i) {
 	assert(i>=0);
-	assert(i<dv.size());
+	assert(i<static_cast<int>(dv.size()));
 	return dv[i];
 }
 

diff --git a/src/Mod/Robot/App/kdl_cp/tree.cpp b/src/Mod/Robot/App/kdl_cp/tree.cpp
@@ -33,8 +33,8 @@ Tree::Tree(const std::string& _root_name) :
 
 Tree::Tree(const Tree& in) {
     segments.clear();
-    nrOfSegments = 0;
     nrOfJoints = 0;
+    nrOfSegments = 0;
     root_name = in.root_name;
 
     segments.insert(make_pair(root_name, TreeElement::Root(root_name)));