Skip to content

Commit 546d04d

Browse files
MatthijsBurghMatthijsBurgh
authored
Merge pull request #262 from orocos/fix/is_approx
Don't use isApprox, but isZero
2 parents e6c7059 + 20304ce commit 546d04d

File tree

7 files changed

+107
-83
lines changed

7 files changed

+107
-83
lines changed

‎orocos_kdl/src/jacobian.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -138,7 +138,7 @@ namespace KDL
138138
bool Equal(const Jacobian& a,const Jacobian& b,double eps)
139139
{
140140
if(a.rows()==b.rows()&&a.columns()==b.columns()){
141-
return a.data.isApprox(b.data,eps);
141+
return (a.data-b.data).isZero(eps);
142142
}else
143143
return false;
144144
}

‎orocos_kdl/src/jntarray.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,7 @@ namespace KDL
114114
{
115115
if(src1.rows()!=src2.rows())
116116
return false;
117-
return src1.data.isApprox(src2.data,eps);
117+
return (src1.data-src2.data).isZero(eps);
118118
}
119119

120120
bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}

‎orocos_kdl/src/jntspaceinertiamatrix.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -112,7 +112,7 @@ namespace KDL
112112
{
113113
if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns())
114114
return false;
115-
return src1.data.isApprox(src2.data,eps);
115+
return (src1.data-src2.data).isZero(eps);
116116
}
117117

118118
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}

‎orocos_kdl/tests/framestest.cpp‎

Lines changed: 51 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -635,50 +635,63 @@ void FramesTest::TestFrame() {
635635
CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v));
636636
}
637637

638+
JntArray CreateRandomJntArray(int size)
639+
{
640+
JntArray j(size);
641+
for (int i = 0; i<size; ++i)
642+
random(j(i));
643+
return j;
644+
}
645+
638646
void FramesTest::TestJntArray()
639647
{
640-
JntArray a1(4);
641-
random(a1(0));
642-
random(a1(1));
643-
random(a1(2));
644-
random(a1(3));
645-
JntArray a2(a1);
646-
CPPUNIT_ASSERT(Equal(a2,a1));
648+
JntArray random1 = CreateRandomJntArray(4);
649+
JntArray random1_copy(random1);
650+
CPPUNIT_ASSERT(Equal(random1_copy,random1));
647651

648-
SetToZero(a2);
649-
CPPUNIT_ASSERT(!Equal(a1,a2));
650-
651-
JntArray a3(4);
652-
CPPUNIT_ASSERT(Equal(a2,a3));
653-
654-
a1=a2;
655-
CPPUNIT_ASSERT(Equal(a1,a3));
652+
JntArray zero_set_to_zero(4);
653+
SetToZero(zero_set_to_zero);
654+
CPPUNIT_ASSERT(!Equal(random1,zero_set_to_zero));
655+
656+
JntArray zero(4);
657+
CPPUNIT_ASSERT(Equal(zero_set_to_zero,zero));
658+
659+
JntArray almost_zero = CreateRandomJntArray(4);
660+
almost_zero(0) = almost_zero(0)*1e-7;
661+
almost_zero(1) = almost_zero(1)*1e-7;
662+
almost_zero(2) = almost_zero(2)*1e-7;
663+
almost_zero(3) = almost_zero(3)*1e-7;
664+
665+
// This should obviously be equal, but fails in old buggy implementation
666+
CPPUNIT_ASSERT(Equal(almost_zero,zero,1));
667+
CPPUNIT_ASSERT(Equal(almost_zero,zero,1e-6));
668+
CPPUNIT_ASSERT(!Equal(almost_zero,zero,1e-8));
669+
670+
JntArray sum_random_zero(4);
671+
672+
Add(random1,zero_set_to_zero,sum_random_zero);
673+
CPPUNIT_ASSERT(Equal(random1,sum_random_zero));
674+
675+
JntArray add_subtract(4);
676+
JntArray random2 = CreateRandomJntArray(4);
677+
678+
Add(random1,random2,add_subtract);
679+
Subtract(add_subtract,random2,add_subtract);
680+
CPPUNIT_ASSERT(Equal(random1,add_subtract));
681+
682+
JntArray random_multiply_by_2(4);
683+
JntArray sum_random_same_random(4);
684+
Multiply(random1,2,random_multiply_by_2);
685+
Add(random1,random1,sum_random_same_random);
686+
CPPUNIT_ASSERT(Equal(sum_random_same_random,random_multiply_by_2));
656687

657-
random(a1(0));
658-
random(a1(1));
659-
random(a1(2));
660-
random(a1(3));
661-
662-
Add(a1,a2,a3);
663-
CPPUNIT_ASSERT(Equal(a1,a3));
664-
665-
random(a2(0));
666-
random(a2(1));
667-
random(a2(2));
668-
random(a2(3));
669-
Add(a1,a2,a3);
670-
Subtract(a3,a2,a3);
671-
CPPUNIT_ASSERT(Equal(a1,a3));
672-
673-
Multiply(a1,2,a3);
674-
Add(a1,a1,a2);
675-
CPPUNIT_ASSERT(Equal(a2,a3));
676-
677688
double a;
678689
random(a);
679-
Multiply(a1,a,a3);
680-
Divide(a3,a,a2);
681-
CPPUNIT_ASSERT(Equal(a2,a1));
690+
691+
JntArray random_multiply_devide(4);
692+
Multiply(random1,a,random_multiply_devide);
693+
Divide(random_multiply_devide,a,random_multiply_devide);
694+
CPPUNIT_ASSERT(Equal(random_multiply_devide,random1));
682695
}
683696

684697

‎orocos_kdl/tests/inertiatest.cpp‎

Lines changed: 38 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -25,14 +25,14 @@ void InertiaTest::TestRotationalInertia() {
2525
CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isZero());
2626
RotationalInertia I1;
2727
CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
28-
CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isApprox(Map<Matrix3d>(I1.data)));
28+
CPPUNIT_ASSERT((Map<Matrix3d>(I0.data)-Map<Matrix3d>(I1.data)).isZero());
2929
RotationalInertia I2(1,2,3,4,5,6);
3030

3131
//Check if copying works fine
3232
RotationalInertia I3=I2;
33-
CPPUNIT_ASSERT(Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
33+
CPPUNIT_ASSERT((Map<Matrix3d>(I3.data)-Map<Matrix3d>(I2.data)).isZero());
3434
I2.data[0]=0.0;
35-
CPPUNIT_ASSERT(!Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
35+
CPPUNIT_ASSERT(!(Map<Matrix3d>(I3.data)-Map<Matrix3d>(I2.data)).isZero());
3636

3737
//Check if addition and multiplication works fine
3838
Map<Matrix3d>(I0.data).setRandom();
@@ -42,15 +42,14 @@ void InertiaTest::TestRotationalInertia() {
4242
CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
4343

4444
//Check if matrix is symmetric
45-
CPPUNIT_ASSERT(Map<Matrix3d>(I2.data).isApprox(Map<Matrix3d>(I2.data).transpose()));
45+
CPPUNIT_ASSERT((Map<Matrix3d>(I2.data)-Map<Matrix3d>(I2.data).transpose()).isZero());
4646

4747
//Check if angular momentum is correctly calculated:
4848
Vector omega;
4949
random(omega);
5050
Vector momentum=I2*omega;
51-
52-
CPPUNIT_ASSERT(Map<Vector3d>(momentum.data).isApprox(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data)));
5351

52+
CPPUNIT_ASSERT((Map<Vector3d>(momentum.data)-(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data))).isZero());
5453
}
5554

5655
void InertiaTest::TestRigidBodyInertia() {
@@ -68,13 +67,13 @@ void InertiaTest::TestRigidBodyInertia() {
6867
CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass);
6968
CPPUNIT_ASSERT(!Map<Matrix3d>(I2.getRotationalInertia().data).isZero());
7069
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c);
71-
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))));
70+
CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())))).isZero());
7271

7372
RigidBodyInertia I3=I2;
7473
//check if copying works fine
7574
CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass());
7675
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG());
77-
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I3.getRotationalInertia().data)));
76+
CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I3.getRotationalInertia().data)).isZero());
7877
//Check if multiplication and addition works fine
7978
RigidBodyInertia I4=-2*I2 +I3+I3;
8079
CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon);
@@ -90,13 +89,13 @@ void InertiaTest::TestRigidBodyInertia() {
9089
I4 = R.Inverse()*I3;
9190
CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon);
9291
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
93-
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
92+
CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
9493
//rotation and total with p=0
9594
Frame T(R);
9695
I4 = T*I2;
9796
CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon);
9897
CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
99-
CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
98+
CPPUNIT_ASSERT((Map<Matrix3d>(I3.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
10099

101100
//Check only transformation
102101
Vector p;
@@ -105,12 +104,12 @@ void InertiaTest::TestRigidBodyInertia() {
105104
I4 = I3.RefPoint(-p);
106105
CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
107106
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
108-
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
107+
CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
109108
T=Frame(-p);
110109
I4 = T*I2;
111110
CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon);
112111
CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
113-
CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
112+
CPPUNIT_ASSERT((Map<Matrix3d>(I3.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
114113

115114
random(T);
116115
I3=T*I2;
@@ -126,7 +125,7 @@ void InertiaTest::TestRigidBodyInertia() {
126125
I4 = T.Inverse()*I3;
127126
CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
128127
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
129-
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
128+
CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
130129

131130
}
132131
void InertiaTest::TestArticulatedBodyInertia() {
@@ -150,18 +149,18 @@ void InertiaTest::TestArticulatedBodyInertia() {
150149

151150
CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
152151
CPPUNIT_ASSERT(!I2.I.isZero());
153-
//CPPUNIT_ASSERT(I2.I.isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())),epsilon));
154-
//CPPUNIT_ASSERT(I2.H.isApprox(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()),epsilon));
152+
// CPPUNIT_ASSERT((I2.I-(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())))).isZero());
153+
// CPPUNIT_ASSERT((I2.H-(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))).isZero());
155154
ArticulatedBodyInertia I3=I2;
156155
//check if copying works fine
157-
CPPUNIT_ASSERT(I2.M.isApprox(I3.M));
158-
CPPUNIT_ASSERT(I2.H.isApprox(I3.H));
159-
CPPUNIT_ASSERT(I2.I.isApprox(I3.I));
156+
CPPUNIT_ASSERT((I2.M-I3.M).isZero());
157+
CPPUNIT_ASSERT((I2.H-I3.H).isZero());
158+
CPPUNIT_ASSERT((I2.I-I3.I).isZero());
160159
//Check if multiplication and addition works fine
161160
ArticulatedBodyInertia I4=-2*I2 +I3+I3;
162-
CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval()));
163-
CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval()));
164-
CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval()));
161+
CPPUNIT_ASSERT(I4.M.isZero());
162+
CPPUNIT_ASSERT(I4.H.isZero());
163+
CPPUNIT_ASSERT(I4.I.isZero());
165164

166165
//Check if transformations work fine
167166
//Check only rotation transformation
@@ -171,36 +170,36 @@ void InertiaTest::TestArticulatedBodyInertia() {
171170
I3 = R*I2;
172171
Map<Matrix3d> E(R.data);
173172
Matrix3d tmp = E.transpose()*I2.M*E;
174-
CPPUNIT_ASSERT(I3.M.isApprox(tmp));
173+
CPPUNIT_ASSERT((I3.M-tmp).isZero());
175174
tmp = E.transpose()*I2.H*E;
176-
CPPUNIT_ASSERT(I3.H.isApprox(tmp));
175+
CPPUNIT_ASSERT((I3.H-tmp).isZero());
177176
tmp = E.transpose()*I2.I*E;
178-
CPPUNIT_ASSERT(I3.I.isApprox(tmp));
177+
CPPUNIT_ASSERT((I3.I-tmp).isZero());
179178

180179
I4 = R.Inverse()*I3;
181-
CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
182-
CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
183-
CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
180+
CPPUNIT_ASSERT((I2.M-I4.M).isZero());
181+
CPPUNIT_ASSERT((I2.H-I4.H).isZero());
182+
CPPUNIT_ASSERT((I2.I-I4.I).isZero());
184183
//rotation and total with p=0
185184
Frame T(R);
186185
I4 = T*I2;
187-
CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
188-
CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
189-
CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
186+
CPPUNIT_ASSERT((I3.M-I4.M).isZero());
187+
CPPUNIT_ASSERT((I3.H-I4.H).isZero());
188+
CPPUNIT_ASSERT((I3.I-I4.I).isZero());
190189

191190
//Check only transformation
192191
Vector p;
193192
random(p);
194193
I3 = I2.RefPoint(p);
195194
I4 = I3.RefPoint(-p);
196-
CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
197-
CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
198-
CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
195+
CPPUNIT_ASSERT((I2.M-I4.M).isZero());
196+
CPPUNIT_ASSERT((I2.H-I4.H).isZero());
197+
CPPUNIT_ASSERT((I2.I-I4.I).isZero());
199198
T=Frame(-p);
200199
I4 = T*I2;
201-
CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
202-
CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
203-
CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
200+
CPPUNIT_ASSERT((I3.M-I4.M).isZero());
201+
CPPUNIT_ASSERT((I3.H-I4.H).isZero());
202+
CPPUNIT_ASSERT((I3.I-I4.I).isZero());
204203

205204

206205
random(T);
@@ -215,7 +214,7 @@ void InertiaTest::TestArticulatedBodyInertia() {
215214
random(T);
216215
I3 = T*I2;
217216
I4 = T.Inverse()*I3;
218-
CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
219-
CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
220-
CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
217+
CPPUNIT_ASSERT((I2.M-I4.M).isZero());
218+
CPPUNIT_ASSERT((I2.H-I4.H).isZero());
219+
CPPUNIT_ASSERT((I2.I-I4.I).isZero());
221220
}

‎orocos_kdl/tests/jacobiantest.cpp‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,18 @@ void JacobianTest::TestConstructor(){
9292
CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)5);
9393
}
9494

95-
void JacobianTest::TestGetSetColumn(){}
95+
void JacobianTest::TestEqual(){
96+
Jacobian j1(1);
97+
Jacobian j2(j1);
98+
for (unsigned int i=0; i<6; ++i)
99+
{
100+
random(j1(i,0));
101+
j1(i,0) = j1(i,0)*1e-7;
102+
}
103+
CPPUNIT_ASSERT(Equal(j1,j2,1));
104+
CPPUNIT_ASSERT(Equal(j1,j2,1e-6));
105+
CPPUNIT_ASSERT(!Equal(j1,j2,1e-8));
106+
107+
}
96108

97109

‎orocos_kdl/tests/jacobiantest.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ class JacobianTest : public CppUnit::TestFixture
1313
CPPUNIT_TEST(TestChangeRefFrame);
1414
CPPUNIT_TEST(TestChangeBase);
1515
CPPUNIT_TEST(TestConstructor);
16-
CPPUNIT_TEST(TestGetSetColumn);
16+
CPPUNIT_TEST(TestEqual);
1717
CPPUNIT_TEST_SUITE_END();
1818

1919
public:
@@ -24,7 +24,7 @@ class JacobianTest : public CppUnit::TestFixture
2424
void TestChangeRefFrame();
2525
void TestChangeBase();
2626
void TestConstructor();
27-
void TestGetSetColumn();
27+
void TestEqual();
2828
};
2929

3030
#endif

0 commit comments

Comments
 (0)