From 7e0e38480912495f652570ce59c8fc98b8d17f30 Mon Sep 17 00:00:00 2001 From: ccarigna Date: Thu, 29 Jul 2021 16:43:26 -0400 Subject: [PATCH 01/13] nr pos solver: added degraded check and get/set functions for parameters The position solver was causing an IK failure if it did not converge after the maximum number of iterations was exceeded. However, when singularity avoidance is active (degraded), the solution may not converge but the IK should be allowed to continue. So a degraded flag was added and is checked when the maximum number of iterations is exceeded. If it is degraded, the error is set to E_DEGRADED to allow it to continue. Otherwise, it is set to E_MAX_ITERATIONS_EXCEEDED which will cause the IK to fail. Added functions to set the input parameters (maxiter, eps) and to retrieve the twist error, number of iterations performed, and the velocity solver status. --- orocos_kdl/src/chainiksolverpos_nr.cpp | 46 +++++++++++++++++++++----- orocos_kdl/src/chainiksolverpos_nr.hpp | 43 ++++++++++++++++++++++++ 2 files changed, 81 insertions(+), 8 deletions(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index 79747bf77..d8eed7d7b 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..f5fbfff6d 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.hpp +++ b/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; }; } From efbc1658120eb6100877e76bee4723bb7e1c2f37 Mon Sep 17 00:00:00 2001 From: ccarigna Date: Fri, 17 Sep 2021 16:23:21 -0400 Subject: [PATCH 02/13] nr pos solver: nonconvergent solver tests now flagged as degraded if singular If the maximum iterations is reached but the velocity solver returns a singular status, it is now flagged as degraded and allowed to continue. Because it was singular, it could not reduce the error to zero which is why it kept iterating to the limit. This does not constitute a failure of the inverse kinematics. --- orocos_kdl/tests/solvertest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From 258b44850015c55d3481d885fc9a71ea1b128f64 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:09:52 -0500 Subject: [PATCH 03/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index d8eed7d7b..170f91169 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -78,11 +78,10 @@ namespace KDL (void)fksolver.JntToCart(q_out,f); // not converged but singularity avoidance is active so okay - if (degraded) { + if (degraded) return (error = E_DEGRADED); - } else { + else return (error = E_MAX_ITERATIONS_EXCEEDED); - } } void ChainIkSolverPos_NR::setEps(const double _eps) From 110e50bf9e1acd927b70e84d86ab8f2b6fcb105a Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:27:12 -0500 Subject: [PATCH 04/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index 170f91169..1803b6c42 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -53,8 +53,8 @@ namespace KDL q_out = q_init; bool degraded = false; - for(numiter=1;numiter<=maxiter;numiter++){ - degraded = false; // prove otherwise + 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); From c985bdcc12c1a29b50b0764d059de8a4034af914 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:33:49 -0500 Subject: [PATCH 05/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index 1803b6c42..38786ab52 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -59,7 +59,7 @@ namespace KDL return (error = E_FKSOLVERPOS_FAILED); delta_twist = diff(f,p_in); ikvelstatus = iksolver.CartToJnt(q_out,delta_twist,delta_q); - if (E_NOERROR > ikvelstatus) { + if (E_NOERROR > ikvelstatus) return (error = E_IKSOLVER_FAILED); } // check for degraded solution (but can still continue motion) From 69323ebe9e16de57d92c08126e7ab0a9519c754b Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:34:03 -0500 Subject: [PATCH 06/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index 38786ab52..e6f206676 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -61,11 +61,9 @@ namespace KDL ikvelstatus = iksolver.CartToJnt(q_out,delta_twist,delta_q); if (E_NOERROR > ikvelstatus) return (error = E_IKSOLVER_FAILED); - } // check for degraded solution (but can still continue motion) - else if (E_NOERROR < ikvelstatus) { + 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); From af09d668f82a4d523885ed3e3dc023e267008011 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:34:29 -0500 Subject: [PATCH 07/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index e6f206676..14b933d9d 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -84,7 +84,8 @@ namespace KDL void ChainIkSolverPos_NR::setEps(const double _eps) { - if (0 < _eps) eps = _eps; + if (0 < _eps) + eps = _eps; // else silently ignore } From a106c6d1f7b6915efa6159d0af7e59ac15f761e4 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:34:44 -0500 Subject: [PATCH 08/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index 14b933d9d..b9cc5dbdb 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -91,7 +91,8 @@ namespace KDL void ChainIkSolverPos_NR::setMaxIter(const unsigned int _maxiter) { - if (1 <= _maxiter) maxiter = _maxiter; + if (1 <= _maxiter) + maxiter = _maxiter; // else silently ignore } From c05de07898710b7f17852c1abec751761adbd41f Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:35:05 -0500 Subject: [PATCH 09/13] Update orocos_kdl/src/chainiksolverpos_nr.hpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.hpp b/orocos_kdl/src/chainiksolverpos_nr.hpp index f5fbfff6d..5b4c97752 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.hpp +++ b/orocos_kdl/src/chainiksolverpos_nr.hpp @@ -88,7 +88,7 @@ namespace KDL { * Get maximum number of iterations * \pre 1 <= maxiter, otherwise maxiter is ignored */ - unsigned int getMaxIter()const { return maxiter; } + unsigned int getMaxIter() const { return maxiter; } /** * Get epsilon From a02488d493f2f408607867c8163742d4047949e1 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:35:35 -0500 Subject: [PATCH 10/13] Update orocos_kdl/src/chainiksolverpos_nr.hpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.hpp b/orocos_kdl/src/chainiksolverpos_nr.hpp index 5b4c97752..6c3996fb2 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.hpp +++ b/orocos_kdl/src/chainiksolverpos_nr.hpp @@ -93,7 +93,7 @@ namespace KDL { /** * Get epsilon */ - double getEps()const { return eps; } + double getEps() const { return eps; } /** * Get delta twist from last call to CartToJnt() From cc5021c06e1d1614cc66c4487f6df93a79b0a564 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 24 Nov 2021 10:37:33 -0500 Subject: [PATCH 11/13] Update orocos_kdl/src/chainiksolverpos_nr.hpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.hpp b/orocos_kdl/src/chainiksolverpos_nr.hpp index 6c3996fb2..808b8e6a0 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.hpp +++ b/orocos_kdl/src/chainiksolverpos_nr.hpp @@ -100,7 +100,7 @@ namespace KDL { */ void getDeltaTwist(KDL::Twist& _delta_twist) { - _delta_twist = delta_twist; + _delta_twist = delta_twist; } /** From 434e34771d60c96c963588250107004e679c5904 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Fri, 26 Nov 2021 09:48:42 -0500 Subject: [PATCH 12/13] Update orocos_kdl/src/chainiksolverpos_nr.cpp Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainiksolverpos_nr.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.cpp b/orocos_kdl/src/chainiksolverpos_nr.cpp index b9cc5dbdb..bdb0a1edc 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.cpp +++ b/orocos_kdl/src/chainiksolverpos_nr.cpp @@ -53,7 +53,8 @@ namespace KDL q_out = q_init; bool degraded = false; - for(numiter=1; numiter<=maxiter; ++numiter) { + for(numiter=1; numiter<=maxiter; ++numiter) + { degraded = false; // prove otherwise if (E_NOERROR > fksolver.JntToCart(q_out,f) ) return (error = E_FKSOLVERPOS_FAILED); From c734cadcf72f6ef435c644198b69692ae14ebc3c Mon Sep 17 00:00:00 2001 From: ccarigna Date: Fri, 26 Nov 2021 10:24:35 -0500 Subject: [PATCH 13/13] chainiksolverpos_nr.hpp: convert getDeltaTwist to a simple return --- orocos_kdl/src/chainiksolverpos_nr.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/chainiksolverpos_nr.hpp b/orocos_kdl/src/chainiksolverpos_nr.hpp index 808b8e6a0..2c952841f 100644 --- a/orocos_kdl/src/chainiksolverpos_nr.hpp +++ b/orocos_kdl/src/chainiksolverpos_nr.hpp @@ -96,11 +96,14 @@ namespace KDL { double getEps() const { return eps; } /** - * Get delta twist from last call to CartToJnt() + * Request the delta twist from last call to CartToJnt() + * + * + * @return const reference to the delta twist */ - void getDeltaTwist(KDL::Twist& _delta_twist) + const Twist& getDeltaTwist()const { - _delta_twist = delta_twist; + return delta_twist; } /**