Pinv velocity solver sigma output

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,13 +54,35 @@ 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()
     {
     }
 
 
+    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() != Smaxtomin.rows())
+            return (error = E_SIZE_MISMATCH);
+        for (unsigned int i=0;i<Sout.rows();i++)
+            Sout(i)=Smaxtomin(i);
+        return (error = E_NOERROR);
+    }
+
     int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
     {
         if (nj != chain.getNrOfJoints())
@@ -75,8 +99,9 @@ namespace KDL
         double sum;
         unsigned int i,j;
 
-        // Initialize near zero singular value counter
+        // Initialize (internal) return values
         nrZeroSigmas = 0 ;
+        sigmaMin = 0.;
 
         //Do a singular value decomposition of "jac" with maximum
         //iterations "maxiter", put the results in "U", "S" and "V"
@@ -88,6 +113,18 @@ namespace KDL
             return (error = E_SVD_FAILED);
         }
 
+        // Sort S in descending order (S is not sorted in SVD_HH)
+        // copied from svd_eigen_HH.cpp
+        Smaxtomin = S;
+        SortJntArrayMaxToMin(Smaxtomin);
+        // Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
+        if ( jac.columns() >= 6 ) {
+            sigmaMin = Smaxtomin(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
@@ -129,6 +166,29 @@ namespace KDL
         }
     }
 
+    void ChainIkSolverVel_pinv::SortJntArrayMaxToMin(JntArray& Smaxtomin)
+    {
+        int n=Smaxtomin.rows();
+        for (int i=0; i<n; i++){
+            double S_max = Smaxtomin(i);
+            int i_max = i;
+            for (int j=i+1; j<n; j++){
+                double Sj = Smaxtomin(j);
+                if (Sj > 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.";

orocos_kdl/src/chainiksolvervel_pinv.hpp

@@ -79,19 +79,57 @@ 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(),
          * then the jacobian pseudoinverse is singular
          */
         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.
          */
         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;
 
@@ -108,10 +146,11 @@ namespace KDL
         std::vector<JntArray> V;
         JntArray tmp;
         double eps;
-        int maxiter;
+        unsigned int maxiter;
         unsigned int nrZeroSigmas;
         int svdResult;
-
+        double sigmaMin;
+        JntArray Smaxtomin;
     };
 }
 #endif

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"<<std::endl;
+
+    KDL::ChainIkSolverVel_pinv ikvelsolver(motomansia10,eps,maxiter);
+    CPPUNIT_ASSERT_EQUAL(eps,ikvelsolver.getEps());
+    CPPUNIT_ASSERT_EQUAL(maxiter,ikvelsolver.getMaxIter());
+    ikvelsolver.setEps(epsnew);
+    CPPUNIT_ASSERT_EQUAL(epsnew,ikvelsolver.getEps());
+    ikvelsolver.setMaxIter(maxiternew);
+    CPPUNIT_ASSERT_EQUAL(maxiternew,ikvelsolver.getMaxIter());
+
+    unsigned int nj = motomansia10.getNrOfJoints();
+    JntArray q(nj), dq(nj);
+
+    KDL::Vector v05(0.05,0.05,0.05);
+    KDL::Twist dx(v05,v05);
+
+
+    std::cout<<"smallest singular value is above threshold (no Pinv)"<<std::endl;
+
+    q(0) = 0. ;
+    q(1) = 0.5 ;
+    q(2) = 0.4 ;
+    q(3) = -PI_2 ;
+    q(4) = 0. ;
+    q(5) = 0. ;
+    q(6) = 0. ;
+
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
+                         ikvelsolver.CartToJnt(q, dx, dq)) ;    // pinv mode
+    CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ;          // 1 singular value
+
+    std::cout<<"Test singular value function"<<std::endl;
+    JntArray S(nj), Sexp(nj);
+    Sexp(0) = 1.86694;
+    Sexp(1) = 1.61924;
+    Sexp(2) = 1.3175;
+    Sexp(3) = 0.330559;
+    Sexp(4) = 0.206596;
+    Sexp(5) = 0.1163;
+    Sexp(6) = 0.0;
+    CPPUNIT_ASSERT_EQUAL(0,(int)ikvelsolver.getSigma(S)) ;
+    for(unsigned int i=0;i<nj;i++) {
+        CPPUNIT_ASSERT_DOUBLES_EQUAL(Sexp(i),S(i),0.0001);
+    }
+    double Smin = ikvelsolver.getSigmaMin();
+    CPPUNIT_ASSERT_EQUAL(S(5),Smin);
+
+
+    std::cout<<"smallest singular value is below threshold"<<std::endl;
+
+    q(1) = 0.2 ;
+
+    CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
+                         ikvelsolver.CartToJnt(q, dx, dq)) ;    // pinv mode
+    CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;        //    2 singular values
+
+    q(1) = 0.0 ;
+
+    CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
+                         ikvelsolver.CartToJnt(q, dx, dq)) ;    // pinv mode
+    CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;        //    2 singular values
+
+    // fully singular
+    q(2) = 0.0 ;
+    q(3) = 0.0 ;
+
+    CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
+                         ikvelsolver.CartToJnt(q, dx, dq)) ;    // pinv mode
+    CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
+}
+
+
 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
 {
     double deltaq = 1E-4;

orocos_kdl/tests/solvertest.hpp

@@ -37,6 +37,7 @@ class SolverTest : public CppUnit::TestFixture
     CPPUNIT_TEST(ExternalWrenchEstimatorTest );
     CPPUNIT_TEST(IkSingularValueTest );
     CPPUNIT_TEST(IkVelSolverWDLSTest );
+    CPPUNIT_TEST(IkVelSolverPinvTest );
     CPPUNIT_TEST(FkPosVectTest );
     CPPUNIT_TEST(FkVelVectTest );
     CPPUNIT_TEST(FdSolverDevelopmentTest );
@@ -58,6 +59,7 @@ class SolverTest : public CppUnit::TestFixture
     void ExternalWrenchEstimatorTest();
     void IkSingularValueTest() ;
     void IkVelSolverWDLSTest();
+    void IkVelSolverPinvTest();
     void FkPosVectTest();
     void FkVelVectTest();
     void FdSolverDevelopmentTest();