Skip to content

Commit 554b2cc

Browse files
craigirobotccarigna
craigirobot
authored and
ccarigna
committed
Update based on feedback
1 parent 1ab74b7 commit 554b2cc

File tree

3 files changed

+63
-63
lines changed

3 files changed

+63
-63
lines changed

‎orocos_kdl/src/chainiksolvervel_pinv.cpp‎

Lines changed: 23 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -64,21 +64,23 @@ namespace KDL
6464

6565
void ChainIkSolverVel_pinv::setEps(const double eps_in)
6666
{
67-
if (0 < eps_in) eps = eps_in;
67+
if (eps_in > 0)
68+
eps = eps_in;
6869
// else silently ignore
6970
}
7071

7172
void ChainIkSolverVel_pinv::setMaxIter(const unsigned int maxiter_in)
7273
{
73-
if (1 <= maxiter_in) maxiter = maxiter_in;
74+
if (maxiter_in >= 1)
75+
maxiter = maxiter_in;
7476
// else silently ignore
7577
}
7678

7779
int ChainIkSolverVel_pinv::getSigma(JntArray& Sout)
7880
{
7981
if (Sout.rows() != Smaxtomin.rows())
8082
return (error = E_SIZE_MISMATCH);
81-
for (unsigned int i=0;i<Sout.rows();i++)
83+
for (unsigned int i=0; i<Sout.rows(); ++i)
8284
Sout(i)=Smaxtomin(i);
8385
return (error = E_NOERROR);
8486
}
@@ -114,15 +116,17 @@ namespace KDL
114116
}
115117

116118
// Sort S in descending order (S is not sorted in SVD_HH)
117-
// copied from svd_eigen_HH.cpp
119+
// Copied from svd_eigen_HH.cpp
118120
Smaxtomin = S;
119121
SortJntArrayMaxToMin(Smaxtomin);
120122
// Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
121-
if ( jac.columns() >= 6 ) {
123+
if (jac.columns() >= 6)
124+
{
122125
sigmaMin = Smaxtomin(5);
123126
}
124-
else {
125-
sigmaMin = 0.;
127+
else
128+
{
129+
sigmaMin = 0;
126130
}
127131

128132
// We have to calculate qdot_out = jac_pinv*v_in
@@ -168,25 +172,28 @@ namespace KDL
168172

169173
void ChainIkSolverVel_pinv::SortJntArrayMaxToMin(JntArray& Smaxtomin)
170174
{
171-
int n=Smaxtomin.rows();
172-
for (int i=0; i<n; i++){
175+
unsigned int n = Smaxtomin.rows();
176+
for (unsigned int i=0; i<n; ++i)
177+
{
173178
double S_max = Smaxtomin(i);
174-
int i_max = i;
175-
for (int j=i+1; j<n; j++){
179+
unsigned int i_max = i;
180+
for (unsigned int j=i+1; j<n; ++j)
181+
{
176182
double Sj = Smaxtomin(j);
177-
if (Sj > S_max){
183+
if (Sj > S_max)
184+
{
178185
S_max = Sj;
179186
i_max = j;
180187
}
181188
}
182-
if (i_max != i){
189+
if (i_max != i)
190+
{
183191
/* swap eigenvalues */
184192
double tmp = Smaxtomin(i);
185-
Smaxtomin(i)=Smaxtomin(i_max);
186-
Smaxtomin(i_max)=tmp;
193+
Smaxtomin(i) =Smaxtomin(i_max);
194+
Smaxtomin(i_max) = tmp;
187195
}
188196
}
189-
return;
190197
}
191198

192199
const char* ChainIkSolverVel_pinv::strError(const int error) const

‎orocos_kdl/src/chainiksolvervel_pinv.hpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ namespace KDL
101101
/**
102102
* Request the minimum of the first six singular values
103103
*/
104-
double getSigmaMin()const {return sigmaMin;};
104+
double getSigmaMin( )const { return sigmaMin; };
105105

106106
/**
107107
* Request the singular values of the Jacobian
@@ -111,13 +111,13 @@ namespace KDL
111111
/**
112112
* Request the value of eps
113113
*/
114-
double getEps()const {return eps;};
114+
double getEps() const { return eps; };
115115

116116
/**
117117
* Get maximum number of iterations
118-
* \pre 1 <= maxiter, otherwise maxiter is ignored
118+
* \pre maxiter >= 1, otherwise maxiter is ignored
119119
*/
120-
unsigned int getMaxIter()const { return maxiter; }
120+
unsigned int getMaxIter() const { return static_cast<unsigned int>(maxiter); }
121121

122122
/**
123123
* Retrieve the latest return code from the SVD algorithm

‎orocos_kdl/tests/solvertest.cpp‎

Lines changed: 36 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -695,47 +695,44 @@ void SolverTest::IkVelSolverWDLSTest()
695695
CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
696696
CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ; // full value
697697
}
698-
699-
700698
void SolverTest::IkVelSolverPinvTest()
701699
{
702700
unsigned int maxiter = 30;
703701
double eps = 1e-6;
704702
unsigned int maxiternew = 10;
705703
double epsnew = 0.1;
706704

707-
std::cout<<"KDL-IK Pinv Vel Solver Tests for Near Zero SVs"<<std::endl;
705+
std::cout << "KDL-IK Pinv Vel Solver Tests for Near Zero SVs" << std::endl;
708706

709-
KDL::ChainIkSolverVel_pinv ikvelsolver(motomansia10,eps,maxiter);
710-
CPPUNIT_ASSERT_EQUAL(eps,ikvelsolver.getEps());
711-
CPPUNIT_ASSERT_EQUAL(maxiter,ikvelsolver.getMaxIter());
707+
KDL::ChainIkSolverVel_pinv ikvelsolver(motomansia10, eps, maxiter);
708+
CPPUNIT_ASSERT_EQUAL(eps, ikvelsolver.getEps());
709+
CPPUNIT_ASSERT_EQUAL(maxiter, ikvelsolver.getMaxIter());
712710
ikvelsolver.setEps(epsnew);
713-
CPPUNIT_ASSERT_EQUAL(epsnew,ikvelsolver.getEps());
711+
CPPUNIT_ASSERT_EQUAL(epsnew, ikvelsolver.getEps());
714712
ikvelsolver.setMaxIter(maxiternew);
715-
CPPUNIT_ASSERT_EQUAL(maxiternew,ikvelsolver.getMaxIter());
713+
CPPUNIT_ASSERT_EQUAL(maxiternew, ikvelsolver.getMaxIter());
716714

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

720-
KDL::Vector v05(0.05,0.05,0.05);
721-
KDL::Twist dx(v05,v05);
718+
KDL::Vector v05(0.05, 0.05, 0.05);
719+
KDL::Twist dx(v05, v05);
722720

723721

724-
std::cout<<"smallest singular value is above threshold (no Pinv)"<<std::endl;
722+
std::cout << "smallest singular value is above threshold (no Pinv)" << std::endl;
725723

726-
q(0) = 0. ;
727-
q(1) = 0.5 ;
728-
q(2) = 0.4 ;
729-
q(3) = -PI_2 ;
730-
q(4) = 0. ;
731-
q(5) = 0. ;
732-
q(6) = 0. ;
724+
q(0) = 0.0;
725+
q(1) = 0.5;
726+
q(2) = 0.4;
727+
q(3) = -PI_2;
728+
q(4) = 0.0;
729+
q(5) = 0.0;
730+
q(6) = 0.0;
733731

734-
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
735-
ikvelsolver.CartToJnt(q, dx, dq)) ; // pinv mode
736-
CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ; // 1 singular value
732+
CPPUNIT_ASSERT_EQUAL(SolverI::E_NOERROR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode
733+
CPPUNIT_ASSERT_EQUAL(1u, ikvelsolver.getNrZeroSigmas()); // 1 singular value
737734

738-
std::cout<<"Test singular value function"<<std::endl;
735+
std::cout << "Test singular value function" << std::endl;
739736
JntArray S(nj), Sexp(nj);
740737
Sexp(0) = 1.86694;
741738
Sexp(1) = 1.61924;
@@ -744,38 +741,34 @@ void SolverTest::IkVelSolverPinvTest()
744741
Sexp(4) = 0.206596;
745742
Sexp(5) = 0.1163;
746743
Sexp(6) = 0.0;
747-
CPPUNIT_ASSERT_EQUAL(0,(int)ikvelsolver.getSigma(S)) ;
748-
for(unsigned int i=0;i<nj;i++) {
749-
CPPUNIT_ASSERT_DOUBLES_EQUAL(Sexp(i),S(i),0.0001);
744+
CPPUNIT_ASSERT_EQUAL(0, ikvelsolver.getSigma(S));
745+
for(unsigned int i=0; i<nj; ++i)
746+
{
747+
CPPUNIT_ASSERT_DOUBLES_EQUAL(Sexp(i), S(i), 0.0001);
750748
}
751749
double Smin = ikvelsolver.getSigmaMin();
752-
CPPUNIT_ASSERT_EQUAL(S(5),Smin);
753-
750+
CPPUNIT_ASSERT_EQUAL(S(5), Smin);
754751

755-
std::cout<<"smallest singular value is below threshold"<<std::endl;
752+
std::cout << "smallest singular value is below threshold" << std::endl;
756753

757-
q(1) = 0.2 ;
754+
q(1) = 0.2;
758755

759-
CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
760-
ikvelsolver.CartToJnt(q, dx, dq)) ; // pinv mode
761-
CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
756+
CPPUNIT_ASSERT_EQUAL(ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode
757+
CPPUNIT_ASSERT_EQUAL(2u, ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
762758

763-
q(1) = 0.0 ;
759+
q(1) = 0.0;
764760

765-
CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
766-
ikvelsolver.CartToJnt(q, dx, dq)) ; // pinv mode
767-
CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
761+
CPPUNIT_ASSERT_EQUAL(ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode
762+
CPPUNIT_ASSERT_EQUAL(2u, ikvelsolver.getNrZeroSigmas()); // 2 singular values
768763

769-
// fully singular
770-
q(2) = 0.0 ;
771-
q(3) = 0.0 ;
764+
// Fully singular
765+
q(2) = 0.0;
766+
q(3) = 0.0;
772767

773-
CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
774-
ikvelsolver.CartToJnt(q, dx, dq)) ; // pinv mode
775-
CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
768+
CPPUNIT_ASSERT_EQUAL(ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR, ikvelsolver.CartToJnt(q, dx, dq)); // pinv mode
769+
CPPUNIT_ASSERT_EQUAL(4u, ikvelsolver.getNrZeroSigmas());
776770
}
777771

778-
779772
void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
780773
{
781774
double deltaq = 1E-4;

0 commit comments

Comments
 (0)