From 509aa3718186b76b486c06a02916faf24d53a1b2 Mon Sep 17 00:00:00 2001 From: ccarigna Date: Thu, 5 Aug 2021 14:28:11 -0400 Subject: [PATCH 1/6] pinv vel solver: added get/set functions for parameters Add functions to set the input parameters (maxiter, eps) which can be done from the controller. Also add functions to retrieve the singular values and current value of eps. --- orocos_kdl/src/chainiksolvervel_pinv.cpp | 37 ++++++++++++++++++++++-- orocos_kdl/src/chainiksolvervel_pinv.hpp | 37 ++++++++++++++++++++++-- 2 files changed, 70 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp index 4ecc0cc49..afa9a02c0 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp @@ -36,7 +36,9 @@ namespace KDL eps(_eps), maxiter(_maxiter), nrZeroSigmas(0), - svdResult(0) + svdResult(0), + sigmaMin(0), + Smaxtomin(nj) { } @@ -52,6 +54,7 @@ namespace KDL for(unsigned int i = 0 ; i < V.size(); i++) V[i].resize(nj); tmp.resize(nj); + Smaxtomin.resize(nj); } ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv() @@ -59,6 +62,27 @@ namespace KDL } + void ChainIkSolverVel_pinv::setEps(const double eps_in) + { + if (0 < eps_in) eps = eps_in; + // else silently ignore + } + + void ChainIkSolverVel_pinv::setMaxIter(const unsigned int maxiter_in) + { + if (1 <= maxiter_in) maxiter = maxiter_in; + // else silently ignore + } + + int ChainIkSolverVel_pinv::getSigma(JntArray& Sout) + { + if (Sout.rows() != S.rows()) + return (error = E_SIZE_MISMATCH); + for (unsigned int i=0;i=6 and 0 for <6 + if ( jac.columns() >= 6 ) { + sigmaMin = S(5); + } + else { + sigmaMin = 0.; + } + // We have to calculate qdot_out = jac_pinv*v_in // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): // qdot_out = V*S_pinv*Ut*v_in diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp index b31f2dd4e..4e6d5b175 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.hpp @@ -79,6 +79,18 @@ namespace KDL */ virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;}; + /** + * Set eps + * \pre 0 < eps, otherwise eps is ignored + */ + void setEps(const double eps_in); + + /** + * Set maxIter + * \pre 1 <= maxiter, otherwise maxiter is ignored + */ + void setMaxIter(const unsigned int maxiter_in); + /** * Retrieve the number of singular values of the jacobian that are < eps; * if the number of near zero singular values is > jac.col()-jac.row(), @@ -86,6 +98,27 @@ namespace KDL */ unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; + /** + * Request the minimum of the first six singular values + */ + double getSigmaMin()const {return sigmaMin;}; + + /** + * Request the singular values of the Jacobian + */ + int getSigma(JntArray& Sout); + + /** + * Request the value of eps + */ + double getEps()const {return eps;}; + + /** + * Get maximum number of iterations + * \pre 1 <= maxiter, otherwise maxiter is ignored + */ + unsigned int getMaxIter()const { return maxiter; } + /** * Retrieve the latest return code from the SVD algorithm * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code. @@ -108,10 +141,10 @@ namespace KDL std::vector V; JntArray tmp; double eps; - int maxiter; + unsigned int maxiter; unsigned int nrZeroSigmas; int svdResult; - + double sigmaMin; }; } #endif From 2c76861cc7820d5a18ce5a014bdbe123de72bd68 Mon Sep 17 00:00:00 2001 From: ccarigna Date: Mon, 9 Aug 2021 16:18:08 -0400 Subject: [PATCH 2/6] pinv vel solver: added sort function for singular values The singular value decomposition in SVD_HH does not sort the singular values in descending order. So a sort function was added to do this based on the same code used in SVD_eigen_HH. The sorted version of S called Smaxtomin is then used to find the minimum of the six largest singular values and is also what is returned by the get Sigma function used to calculate the manipulability. --- orocos_kdl/src/chainiksolvervel_pinv.cpp | 33 +++++++++++++++++++++--- orocos_kdl/src/chainiksolvervel_pinv.hpp | 6 +++++ 2 files changed, 36 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp index afa9a02c0..9ceca0301 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp @@ -76,10 +76,10 @@ namespace KDL int ChainIkSolverVel_pinv::getSigma(JntArray& Sout) { - if (Sout.rows() != S.rows()) + if (Sout.rows() != Smaxtomin.rows()) return (error = E_SIZE_MISMATCH); for (unsigned int i=0;i=6 and 0 for <6 if ( jac.columns() >= 6 ) { - sigmaMin = S(5); + sigmaMin = Smaxtomin(5); } else { sigmaMin = 0.; @@ -162,6 +166,29 @@ namespace KDL } } + void ChainIkSolverVel_pinv::SortJntArrayMaxToMin(JntArray& Smaxtomin) + { + int n=Smaxtomin.rows(); + for (int i=0; i S_max){ + S_max = Sj; + i_max = j; + } + } + if (i_max != i){ + /* swap eigenvalues */ + double tmp = Smaxtomin(i); + Smaxtomin(i)=Smaxtomin(i_max); + Smaxtomin(i_max)=tmp; + } + } + return; + } + const char* ChainIkSolverVel_pinv::strError(const int error) const { if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular."; diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp index 4e6d5b175..11ad5d5ee 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.hpp @@ -125,6 +125,11 @@ namespace KDL */ int getSVDResult()const {return svdResult;}; + /** + * Sort a JntArray from maximum to minimum value + */ + void SortJntArrayMaxToMin(JntArray& Smaxtomin); + /// @copydoc KDL::SolverI::strError() virtual const char* strError(const int error) const; @@ -145,6 +150,7 @@ namespace KDL unsigned int nrZeroSigmas; int svdResult; double sigmaMin; + JntArray Smaxtomin; }; } #endif From 1ab74b7433800bb0f84a993ed9a85679885e4377 Mon Sep 17 00:00:00 2001 From: ccarigna Date: Mon, 27 Sep 2021 14:49:42 -0400 Subject: [PATCH 3/6] pinv vel solver: add tests for get/set functions and sigma sorter This adds a solver test for the new functions in the pseudoinverse velocity solver which include the set/get functions and the singular value sorter and retrieval functions. --- orocos_kdl/tests/solvertest.cpp | 79 +++++++++++++++++++++++++++++++++ orocos_kdl/tests/solvertest.hpp | 2 + 2 files changed, 81 insertions(+) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 24bde309b..23fcde88c 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -697,6 +697,85 @@ void SolverTest::IkVelSolverWDLSTest() } +void SolverTest::IkVelSolverPinvTest() +{ + unsigned int maxiter = 30; + double eps = 1e-6; + unsigned int maxiternew = 10; + double epsnew = 0.1; + + std::cout<<"KDL-IK Pinv Vel Solver Tests for Near Zero SVs"< Date: Fri, 7 Apr 2023 13:15:16 -0400 Subject: [PATCH 4/6] Update based on feedback --- orocos_kdl/src/chainiksolvervel_pinv.cpp | 39 +++++++----- orocos_kdl/src/chainiksolvervel_pinv.hpp | 8 +-- orocos_kdl/tests/solvertest.cpp | 79 +++++++++++------------- 3 files changed, 63 insertions(+), 63 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp index 9ceca0301..7c3a7a1a2 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp @@ -64,13 +64,15 @@ namespace KDL void ChainIkSolverVel_pinv::setEps(const double eps_in) { - if (0 < eps_in) eps = eps_in; + if (eps_in > 0) + eps = eps_in; // else silently ignore } void ChainIkSolverVel_pinv::setMaxIter(const unsigned int maxiter_in) { - if (1 <= maxiter_in) maxiter = maxiter_in; + if (maxiter_in >= 1) + maxiter = maxiter_in; // else silently ignore } @@ -78,7 +80,7 @@ namespace KDL { if (Sout.rows() != Smaxtomin.rows()) return (error = E_SIZE_MISMATCH); - for (unsigned int i=0;i=6 and 0 for <6 - if ( jac.columns() >= 6 ) { + if (jac.columns() >= 6) + { sigmaMin = Smaxtomin(5); } - else { - sigmaMin = 0.; + else + { + sigmaMin = 0; } // We have to calculate qdot_out = jac_pinv*v_in @@ -168,25 +172,28 @@ namespace KDL void ChainIkSolverVel_pinv::SortJntArrayMaxToMin(JntArray& Smaxtomin) { - int n=Smaxtomin.rows(); - for (int i=0; i S_max){ + if (Sj > S_max) + { S_max = Sj; i_max = j; } } - if (i_max != i){ + if (i_max != i) + { /* swap eigenvalues */ double tmp = Smaxtomin(i); - Smaxtomin(i)=Smaxtomin(i_max); - Smaxtomin(i_max)=tmp; + Smaxtomin(i) =Smaxtomin(i_max); + Smaxtomin(i_max) = tmp; } } - return; } const char* ChainIkSolverVel_pinv::strError(const int error) const diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp index 11ad5d5ee..1dadde1ad 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.hpp @@ -101,7 +101,7 @@ namespace KDL /** * Request the minimum of the first six singular values */ - double getSigmaMin()const {return sigmaMin;}; + double getSigmaMin( )const { return sigmaMin; }; /** * Request the singular values of the Jacobian @@ -111,13 +111,13 @@ namespace KDL /** * Request the value of eps */ - double getEps()const {return eps;}; + double getEps() const { return eps; }; /** * Get maximum number of iterations - * \pre 1 <= maxiter, otherwise maxiter is ignored + * \pre maxiter >= 1, otherwise maxiter is ignored */ - unsigned int getMaxIter()const { return maxiter; } + unsigned int getMaxIter() const { return static_cast(maxiter); } /** * Retrieve the latest return code from the SVD algorithm diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 23fcde88c..6de065c47 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -695,8 +695,6 @@ void SolverTest::IkVelSolverWDLSTest() CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ; CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ; // full value } - - void SolverTest::IkVelSolverPinvTest() { unsigned int maxiter = 30; @@ -704,38 +702,37 @@ void SolverTest::IkVelSolverPinvTest() unsigned int maxiternew = 10; double epsnew = 0.1; - std::cout<<"KDL-IK Pinv Vel Solver Tests for Near Zero SVs"< Date: Wed, 23 Oct 2024 14:38:03 -0400 Subject: [PATCH 5/6] changed maxiter back to int --- orocos_kdl/src/chainiksolvervel_pinv.cpp | 2 +- orocos_kdl/src/chainiksolvervel_pinv.hpp | 8 ++++---- orocos_kdl/tests/solvertest.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp index 7c3a7a1a2..bb5231e24 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp @@ -69,7 +69,7 @@ namespace KDL // else silently ignore } - void ChainIkSolverVel_pinv::setMaxIter(const unsigned int maxiter_in) + void ChainIkSolverVel_pinv::setMaxIter(const int maxiter_in) { if (maxiter_in >= 1) maxiter = maxiter_in; diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp index 1dadde1ad..84295d047 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv.hpp +++ b/orocos_kdl/src/chainiksolvervel_pinv.hpp @@ -87,9 +87,9 @@ namespace KDL /** * Set maxIter - * \pre 1 <= maxiter, otherwise maxiter is ignored + * \pre maxiter >= 1, otherwise maxiter is ignored */ - void setMaxIter(const unsigned int maxiter_in); + void setMaxIter(const int maxiter_in); /** * Retrieve the number of singular values of the jacobian that are < eps; @@ -117,7 +117,7 @@ namespace KDL * Get maximum number of iterations * \pre maxiter >= 1, otherwise maxiter is ignored */ - unsigned int getMaxIter() const { return static_cast(maxiter); } + int getMaxIter() const { return maxiter; } /** * Retrieve the latest return code from the SVD algorithm @@ -146,7 +146,7 @@ namespace KDL std::vector V; JntArray tmp; double eps; - unsigned int maxiter; + int maxiter; unsigned int nrZeroSigmas; int svdResult; double sigmaMin; diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 6de065c47..df9984cc6 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -517,7 +517,7 @@ void SolverTest::FkPosAndIkPosTest() void SolverTest::IkSingularValueTest() { - unsigned int maxiter = 30; + int maxiter = 30; double eps = 1e-6 ; int maxiter_vel = 30; double eps_vel = 0.1 ; @@ -697,9 +697,9 @@ void SolverTest::IkVelSolverWDLSTest() } void SolverTest::IkVelSolverPinvTest() { - unsigned int maxiter = 30; + int maxiter = 30; double eps = 1e-6; - unsigned int maxiternew = 10; + int maxiternew = 10; double epsnew = 0.1; std::cout << "KDL-IK Pinv Vel Solver Tests for Near Zero SVs" << std::endl; From b9f06c5c9422be99c42b19bcd750cc3659400b48 Mon Sep 17 00:00:00 2001 From: ccarigna Date: Wed, 23 Oct 2024 16:28:33 -0400 Subject: [PATCH 6/6] cast solver errors to int --- orocos_kdl/tests/solvertest.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index df9984cc6..a25b8dc99 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -729,7 +729,7 @@ void SolverTest::IkVelSolverPinvTest() q(5) = 0.0; q(6) = 0.0; - CPPUNIT_ASSERT_EQUAL(SolverI::E_NOERROR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode CPPUNIT_ASSERT_EQUAL(1u, ikvelsolver.getNrZeroSigmas()); // 1 singular value std::cout << "Test singular value function" << std::endl; @@ -753,19 +753,19 @@ void SolverTest::IkVelSolverPinvTest() q(1) = 0.2; - CPPUNIT_ASSERT_EQUAL(ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode + CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode CPPUNIT_ASSERT_EQUAL(2u, ikvelsolver.getNrZeroSigmas()) ; // 2 singular values q(1) = 0.0; - CPPUNIT_ASSERT_EQUAL(ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode + CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode CPPUNIT_ASSERT_EQUAL(2u, ikvelsolver.getNrZeroSigmas()); // 2 singular values // Fully singular q(2) = 0.0; q(3) = 0.0; - CPPUNIT_ASSERT_EQUAL(ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode + CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode CPPUNIT_ASSERT_EQUAL(4u, ikvelsolver.getNrZeroSigmas()); }