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,37 @@ 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 (eps_in > 0)
+            eps = eps_in;
+        // else silently ignore
+    }
+
+    void ChainIkSolverVel_pinv::setMaxIter(const int maxiter_in)
+    {
+        if (maxiter_in >= 1)
+            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 +101,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 +115,20 @@ 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 +170,32 @@ namespace KDL
         }
     }
 
+    void ChainIkSolverVel_pinv::SortJntArrayMaxToMin(JntArray& Smaxtomin)
+    {
+        unsigned int n = Smaxtomin.rows();
+        for (unsigned int i=0; i<n; ++i)
+        {
+            double S_max = Smaxtomin(i);
+            unsigned int i_max = i;
+            for (unsigned 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;
+            }
+        }
+    }
+
     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 maxiter >= 1, otherwise maxiter is ignored
+         */
+        void setMaxIter(const 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 maxiter >= 1, otherwise maxiter is ignored
+         */
+        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;
 
@@ -111,7 +149,8 @@ namespace KDL
         int maxiter;
         unsigned int nrZeroSigmas;
         int svdResult;
-
+        double sigmaMin;
+        JntArray Smaxtomin;
     };
 }
 #endif

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 ;
@@ -695,8 +695,80 @@ void SolverTest::IkVelSolverWDLSTest()
 	CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
 	CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;	// full value
 }
+void SolverTest::IkVelSolverPinvTest()
+{
+    int maxiter = 30;
+    double eps = 1e-6;
+    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.0;
+    q(1) = 0.5;
+    q(2) = 0.4;
+    q(3) = -PI_2;
+    q(4) = 0.0;
+    q(5) = 0.0;
+    q(6) = 0.0;
+
+    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;
+    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, 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(2u, 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(2u, 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(4u, 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();