diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index 79747bf77..bdb0a1edc 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/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,50 @@ namespace KDL return (error = E_SIZE_MISMATCH); q_out = q_init; + bool degraded = false; - unsigned int i; - for(i=0;i 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() diff --git a/orocos_kdl/src/chainiksolverpos_nr.hpp b/orocos_kdl/src/chainiksolverpos_nr.hpp index a94c5a471..2c952841f 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.hpp +++ b/orocos_kdl/src/chainiksolverpos_nr.hpp @@ -73,6 +73,50 @@ 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; } + + /** + * Request the delta twist from last call to CartToJnt() + * + * + * @return const reference to the delta twist + */ + const Twist& getDeltaTwist()const + { + return 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 +134,8 @@ namespace KDL { unsigned int maxiter; double eps; + unsigned int numiter; + int ikvelstatus; }; } diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 24bde309b..55ecf0f61 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/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