Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Pinv velocity solver sigma output #359

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
pinv vel solver: add tests for get/set functions and sigma sorter
This adds a solver test for the new functions in the pseudoinverse velocity solver which include the set/get functions and the singular value sorter and retrieval functions.
  • Loading branch information
ccarigna committed Sep 28, 2021
commit 1ab74b7433800bb0f84a993ed9a85679885e4377
79 changes: 79 additions & 0 deletions orocos_kdl/tests/solvertest.cpp
Original file line number Diff line number Diff line change
@@ -697,6 +697,85 @@ void SolverTest::IkVelSolverWDLSTest()
}


craigirobot marked this conversation as resolved.
Show resolved Hide resolved
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;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

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());
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

unsigned int nj = motomansia10.getNrOfJoints();
JntArray q(nj), dq(nj);

KDL::Vector v05(0.05,0.05,0.05);
KDL::Twist dx(v05,v05);
craigirobot marked this conversation as resolved.
Show resolved Hide resolved


std::cout<<"smallest singular value is above threshold (no Pinv)"<<std::endl;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

q(0) = 0. ;
q(1) = 0.5 ;
q(2) = 0.4 ;
q(3) = -PI_2 ;
q(4) = 0. ;
q(5) = 0. ;
q(6) = 0. ;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
ikvelsolver.CartToJnt(q, dx, dq)) ; // pinv mode
CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ; // 1 singular value
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

std::cout<<"Test singular value function"<<std::endl;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved
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)) ;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved
for(unsigned int i=0;i<nj;i++) {
CPPUNIT_ASSERT_DOUBLES_EQUAL(Sexp(i),S(i),0.0001);
}
craigirobot marked this conversation as resolved.
Show resolved Hide resolved
double Smin = ikvelsolver.getSigmaMin();
CPPUNIT_ASSERT_EQUAL(S(5),Smin);
craigirobot marked this conversation as resolved.
Show resolved Hide resolved


std::cout<<"smallest singular value is below threshold"<<std::endl;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

q(1) = 0.2 ;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

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
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

q(1) = 0.0 ;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved

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
MatthijsBurgh marked this conversation as resolved.
Show resolved Hide resolved

// fully singular
MatthijsBurgh marked this conversation as resolved.
Show resolved Hide resolved
q(2) = 0.0 ;
q(3) = 0.0 ;
MatthijsBurgh marked this conversation as resolved.
Show resolved Hide resolved

CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
ikvelsolver.CartToJnt(q, dx, dq)) ; // pinv mode
CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
craigirobot marked this conversation as resolved.
Show resolved Hide resolved
}


craigirobot marked this conversation as resolved.
Show resolved Hide resolved
void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
{
double deltaq = 1E-4;
2 changes: 2 additions & 0 deletions orocos_kdl/tests/solvertest.hpp
Original file line number Diff line number Diff line change
@@ -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();