Add degraded status chainiksolverpos

orocos_kdl/src/chainiksolverpos_nr.cpp

@@ -28,7 +28,10 @@ namespace KDL
         chain(_chain),nj (chain.getNrOfJoints()),
         iksolver(_iksolver),fksolver(_fksolver),
         delta_q(_chain.getNrOfJoints()),
-        maxiter(_maxiter),eps(_eps)
+        maxiter(_maxiter),
+        eps(_eps),
+        numiter(0),
+        ikvelstatus(E_NOERROR)
     {
     }
 
@@ -48,23 +51,49 @@ namespace KDL
             return (error = E_SIZE_MISMATCH);
 
         q_out = q_init;
+        bool degraded = false;
 
-        unsigned int i;
-        for(i=0;i<maxiter;i++){
+        for(numiter=1; numiter<=maxiter; ++numiter) {
+            degraded = false; // prove otherwise
             if (E_NOERROR > fksolver.JntToCart(q_out,f) )
                 return (error = E_FKSOLVERPOS_FAILED);
             delta_twist = diff(f,p_in);
-            const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
-            if (E_NOERROR > rc)
+            ikvelstatus = iksolver.CartToJnt(q_out,delta_twist,delta_q);
+            if (E_NOERROR > ikvelstatus)
                 return (error = E_IKSOLVER_FAILED);
-            // we chose to continue if the child solver returned a positive
+            // check for degraded solution (but can still continue motion)
+            else if (E_NOERROR < ikvelstatus)
+                degraded  = true;
+            // we choose to continue if the child solver returned a positive
             // "error", which may simply indicate a degraded solution
             Add(q_out,delta_q,q_out);
             if(Equal(delta_twist,Twist::Zero(),eps))
                 // converged, but possibly with a degraded solution
-                return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
+                return (ikvelstatus > E_NOERROR ? E_DEGRADED : E_NOERROR);
         }
-        return (error = E_MAX_ITERATIONS_EXCEEDED);        // failed to converge
+
+        // update cartesian output to be in sync with final joint solution
+        (void)fksolver.JntToCart(q_out,f);
+
+        // not converged but singularity avoidance is active so okay
+        if (degraded)
+            return (error = E_DEGRADED);
+        else
+            return (error = E_MAX_ITERATIONS_EXCEEDED);
+    }
+
+    void ChainIkSolverPos_NR::setEps(const double _eps)
+    {
+        if (0 < _eps)
+            eps = _eps;
+        // else silently ignore
+    }
+
+    void ChainIkSolverPos_NR::setMaxIter(const unsigned int _maxiter)
+    {
+        if (1 <= _maxiter)
+            maxiter = _maxiter;
+        // else silently ignore
     }
 
     ChainIkSolverPos_NR::~ChainIkSolverPos_NR()

orocos_kdl/src/chainiksolverpos_nr.hpp

@@ -73,6 +73,47 @@ namespace KDL {
          */
         virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
 
+        /**
+         * Set maximum number of iterations
+         */
+        void setMaxIter(const unsigned int maxiter_in);
+
+        /**
+         * Set epsilon
+         * \pre 0 < eps, otherwise eps is ignored
+         */
+        void setEps(const double eps_in);
+
+        /**
+         * Get maximum number of iterations
+         * \pre 1 <= maxiter, otherwise maxiter is ignored
+         */
+        unsigned int getMaxIter() const { return maxiter; }
+
+        /**
+         * Get epsilon
+         */
+        double getEps() const { return eps; }
+
+        /**
+         * Get delta twist from last call to CartToJnt()
+         */
+        void getDeltaTwist(KDL::Twist& _delta_twist)
+        {
+            _delta_twist = delta_twist;
+        }
+
+        /**
+         * Get status of ik velocity solver
+         */
+        int getVelSolverStatus() const { return ikvelstatus; }
+
+        /**
+         * Get number iterations spent in last call to CartToJnt()
+         * Defaults to 0 at construction
+         */
+        unsigned int getNumIters() const { return numiter; }
+
         /// @copydoc KDL::SolverI::strError()
         virtual const char* strError(const int error) const;
 
@@ -90,6 +131,8 @@ namespace KDL {
 
         unsigned int maxiter;
         double eps;
+        unsigned int numiter;
+        int ikvelstatus;
     };
 
 }

orocos_kdl/tests/solvertest.cpp

@@ -576,7 +576,7 @@ void SolverTest::IkSingularValueTest()
 	CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
 	F_des = F * dF ;
 
-	CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
+	CPPUNIT_ASSERT_EQUAL((int)SolverI::E_DEGRADED,
                          iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
 	CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
                          ikvelsolver1.getError());        	// truncated SV solution
@@ -627,7 +627,7 @@ void SolverTest::IkSingularValueTest()
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolver.JntToCart(q,F));
     F_des = F * dF ;
 
-    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_DEGRADED,
                          iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
                          ikvelsolver1.getError());        	// truncated SV solution