From ba2e003b92c563c9e6aa1a3c1fb4d9f26caca635 Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Wed, 14 Oct 2015 17:54:59 -0300 Subject: [PATCH] Added changes to KDL brought after the SVN merge, where applicable Taken from commits 470880e, df4c99f, cd0ae20, efb0823, c519989 --- src/Mod/Robot/App/kdl_cp/chaindynparam.hpp | 2 +- .../App/kdl_cp/chainiksolvervel_pinv_nso.hpp | 6 +++++ src/Mod/Robot/App/kdl_cp/frameacc.hpp | 24 +++++++++---------- src/Mod/Robot/App/kdl_cp/frames.cpp | 2 +- src/Mod/Robot/App/kdl_cp/frames.inl | 10 ++++---- src/Mod/Robot/App/kdl_cp/jacobian.cpp | 4 ++-- src/Mod/Robot/App/kdl_cp/jacobian.hpp | 2 +- .../App/kdl_cp/jntspaceinertiamatrix.cpp | 2 +- src/Mod/Robot/App/kdl_cp/path_composite.cpp | 4 ++-- src/Mod/Robot/App/kdl_cp/tree.cpp | 2 +- 10 files changed, 32 insertions(+), 26 deletions(-) diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp index 287d08cf5547..2592de3184ea 100644 --- 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 index 20760f859ef4..b9f21751373c 100644 --- 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 index eb54cfdac405..4bda4cd3fa18 100644 --- 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 index 4b9ec2de50e4..0b42a30d0085 100644 --- 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 index d85a25935640..e9eccc3cbef6 100644 --- 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 index 40018d6cba04..7c49c18c0707 100644 --- 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 index 35b491b08174..4c34e8c3bb17 100644 --- 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 index dea5cb646073..b1bc10528274 100644 --- 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 index 62e099d51b6b..6bae1de3e744 100644 --- 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())); return gv[i].first; } double Path_Composite::GetLengthToEndOfSegment(int i) { assert(i>=0); - assert(i(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 index 1ca75148994e..50425e298c3a 100644 --- 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)));