diff --git a/orocos_kdl/tests/inertiatest.cpp b/orocos_kdl/tests/inertiatest.cpp
index bff2ddb..add2318 100644
--- a/orocos_kdl/tests/inertiatest.cpp
+++ b/orocos_kdl/tests/inertiatest.cpp
@@ -77,7 +77,7 @@ void InertiaTest::TestRigidBodyInertia() {
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I3.getRotationalInertia().data)));
//Check if multiplication and addition works fine
RigidBodyInertia I4=-2*I2 +I3+I3;
- CPPUNIT_ASSERT_EQUAL(I4.getMass(),0.0);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon);
CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero());
CPPUNIT_ASSERT(Map<Matrix3d>(I4.getRotationalInertia().data).isZero());
@@ -88,13 +88,13 @@ void InertiaTest::TestRigidBodyInertia() {
random(R);
I3 = R*I2;
I4 = R.Inverse()*I3;
- CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon);
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
//rotation and total with p=0
Frame T(R);
I4 = T*I2;
- CPPUNIT_ASSERT_EQUAL(I3.getMass(),I4.getMass());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon);
CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
@@ -103,12 +103,12 @@ void InertiaTest::TestRigidBodyInertia() {
random(p);
I3 = I2.RefPoint(p);
I4 = I3.RefPoint(-p);
- CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
T=Frame(-p);
I4 = T*I2;
- CPPUNIT_ASSERT_EQUAL(I3.getMass(),I4.getMass());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon);
CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
@@ -124,7 +124,7 @@ void InertiaTest::TestRigidBodyInertia() {
random(T);
I3 = T*I2;
I4 = T.Inverse()*I3;
- CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
@@ -150,18 +150,18 @@ void InertiaTest::TestArticulatedBodyInertia() {
CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
CPPUNIT_ASSERT(!I2.I.isZero());
- //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()))));
- //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())));
+ //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));
+ //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));
ArticulatedBodyInertia I3=I2;
//check if copying works fine
- CPPUNIT_ASSERT_EQUAL(I2.M,I3.M);
- CPPUNIT_ASSERT_EQUAL(I2.H,I3.H);
- CPPUNIT_ASSERT_EQUAL(I2.I,I3.I);
+ CPPUNIT_ASSERT(I2.M.isApprox(I3.M));
+ CPPUNIT_ASSERT(I2.H.isApprox(I3.H));
+ CPPUNIT_ASSERT(I2.I.isApprox(I3.I));
//Check if multiplication and addition works fine
ArticulatedBodyInertia I4=-2*I2 +I3+I3;
- CPPUNIT_ASSERT_EQUAL(I4.M,Matrix3d::Zero().eval());
- CPPUNIT_ASSERT_EQUAL(I4.H,Matrix3d::Zero().eval());
- CPPUNIT_ASSERT_EQUAL(I4.I,Matrix3d::Zero().eval());
+ CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval()));
+ CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval()));
+ CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval()));
//Check if transformations work fine
//Check only rotation transformation
@@ -173,7 +173,7 @@ void InertiaTest::TestArticulatedBodyInertia() {
Matrix3d tmp = E.transpose()*I2.M*E;
CPPUNIT_ASSERT(I3.M.isApprox(tmp));
tmp = E.transpose()*I2.H*E;
- CPPUNIT_ASSERT_EQUAL(I3.H,tmp);
+ CPPUNIT_ASSERT(I3.H.isApprox(tmp));
tmp = E.transpose()*I2.I*E;
CPPUNIT_ASSERT(I3.I.isApprox(tmp));
@@ -184,23 +184,23 @@ void InertiaTest::TestArticulatedBodyInertia() {
//rotation and total with p=0
Frame T(R);
I4 = T*I2;
- CPPUNIT_ASSERT_EQUAL(I3.M,I4.M);
- CPPUNIT_ASSERT_EQUAL(I3.H,I4.H);
- CPPUNIT_ASSERT_EQUAL(I3.I,I4.I);
+ CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
+ CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
+ CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
//Check only transformation
Vector p;
random(p);
I3 = I2.RefPoint(p);
I4 = I3.RefPoint(-p);
- CPPUNIT_ASSERT_EQUAL(I2.M,I4.M);
+ CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
T=Frame(-p);
I4 = T*I2;
- CPPUNIT_ASSERT_EQUAL(I3.M,I4.M);
- CPPUNIT_ASSERT_EQUAL(I3.H,I4.H);
- CPPUNIT_ASSERT_EQUAL(I3.I,I4.I);
+ CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
+ CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
+ CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
random(T);
diff --git a/orocos_kdl/tests/velocityprofiletest.cpp b/orocos_kdl/tests/velocityprofiletest.cpp
index 991c7a4..149de6b 100644
--- a/orocos_kdl/tests/velocityprofiletest.cpp
+++ b/orocos_kdl/tests/velocityprofiletest.cpp
@@ -20,53 +20,53 @@ void VelocityProfileTest::TestTrap_MaxVelocity1()
VelocityProfile_Trap v(2, 1);
double time;
v.SetProfile(2, 10);
- CPPUNIT_ASSERT_EQUAL(6.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
// start
time = 0;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
// end of ramp up
time = 2;
- CPPUNIT_ASSERT_EQUAL(4.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// middle of flat velocity
time = 3;
- CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// end of flat velocity
time = 4;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(-1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
// middle of ramp down
time = 5;
- CPPUNIT_ASSERT_EQUAL(9.5, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(-1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
// end
time = 6;
- CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(-1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
// fenceposts - before and after
time = -1;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
time = 11;
- CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
}
void VelocityProfileTest::TestTrap_MaxVelocity2()
@@ -76,7 +76,7 @@ void VelocityProfileTest::TestTrap_MaxVelocity2()
// 2 second ramp down (cover -2 distance),
VelocityProfile_Trap v(2, 1);
v.SetProfile(2, -6);
- CPPUNIT_ASSERT_EQUAL(6.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
}
void VelocityProfileTest::TestTrap_MaxVelocity3()
@@ -86,15 +86,15 @@ void VelocityProfileTest::TestTrap_MaxVelocity3()
// 2 second ramp down (cover 4 distance),
VelocityProfile_Trap v(4, 2);
v.SetProfile(2, 10);
- CPPUNIT_ASSERT_EQUAL(4.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
// new profile
v.SetProfile(2, -6);
- CPPUNIT_ASSERT_EQUAL(4.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
// another new profile : ramp + 2 sec + ramp
v.SetProfile(13, 13 + 4 + 8 + 4);
- CPPUNIT_ASSERT_EQUAL(6.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
}
void VelocityProfileTest::TestTrap_SetDuration1()
@@ -104,43 +104,43 @@ void VelocityProfileTest::TestTrap_SetDuration1()
VelocityProfile_Trap v(2, 1);
double time;
v.SetProfileDuration(2, 10, 12.0);
- CPPUNIT_ASSERT_EQUAL(12.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(12.0, v.Duration(),epsilon);
// start
time = 0;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.25, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.25, v.Acc(time),epsilon);
// end of ramp up
time = 4;
- CPPUNIT_ASSERT_EQUAL(4.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// middle of flat velocity
time = 6;
- CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// end of flat velocity
time = 8;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
// middle of ramp down
time = 10;
- CPPUNIT_ASSERT_EQUAL(9.5, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.5, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
// end
time = 12;
- CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
}
void VelocityProfileTest::TestTrapHalf_SetProfile_Start()
@@ -150,41 +150,41 @@ void VelocityProfileTest::TestTrapHalf_SetProfile_Start()
VelocityProfile_TrapHalf v(2, 1, true);
double time;
v.SetProfile(2, 2+6);
- CPPUNIT_ASSERT_EQUAL(4.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
// start
time = 0;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
// end of ramp up
time = 2;
- CPPUNIT_ASSERT_EQUAL(4.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// middle of flat velocity
time = 3;
- CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// end
time = 4;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// fenceposts - before and after
time = -1;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
time = 5;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
}
void VelocityProfileTest::TestTrapHalf_SetProfile_End()
@@ -194,41 +194,41 @@ void VelocityProfileTest::TestTrapHalf_SetProfile_End()
VelocityProfile_TrapHalf v(2, 1, false);
double time;
v.SetProfile(9, 9-6);
- CPPUNIT_ASSERT_EQUAL(4.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
// start - flat velocity
time = 0;
- CPPUNIT_ASSERT_EQUAL(9.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(-2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(9.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// end of flat velocity
time = 2;
- CPPUNIT_ASSERT_EQUAL(5.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(-2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
// middle of ramp down
time = 3;
- CPPUNIT_ASSERT_EQUAL(3.5, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(-1.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(3.5, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
// end
time = 4;
- CPPUNIT_ASSERT_EQUAL(3.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// fenceposts - before and after
time = -1;
- CPPUNIT_ASSERT_EQUAL(9.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(9.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
time = 5;
- CPPUNIT_ASSERT_EQUAL(3.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
}
void VelocityProfileTest::TestTrapHalf_SetDuration_Start()
@@ -239,24 +239,24 @@ void VelocityProfileTest::TestTrapHalf_SetDuration_Start()
VelocityProfile_TrapHalf v(2, 1, true);
double time;
v.SetProfileDuration(2, 2+6, 8);
- CPPUNIT_ASSERT_EQUAL(8.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Duration(),epsilon);
// start - no motion
time = 0;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// no motion
time = 1.9;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// begin ramp at scaled acceleration
time = 2;
CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time), 0.001);
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.333, v.Acc(time), 0.001);
// middle of ramp up
@@ -267,19 +267,19 @@ void VelocityProfileTest::TestTrapHalf_SetDuration_Start()
// end - continue with given velocity
time = 8;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// fenceposts - before and after
time = -1;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
time = 9;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
}
@@ -291,24 +291,24 @@ void VelocityProfileTest::TestTrapHalf_SetDuration_End()
VelocityProfile_TrapHalf v(2, 1, true);
double time;
v.SetProfileDuration(2+6, 2, 8);
- CPPUNIT_ASSERT_EQUAL(8.0, v.Duration());
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Duration(),epsilon);
// start - no motion
time = 0;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// no motion
time = 1.9;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// begin ramp at scaled acceleration
time = 2;
CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time), 0.001);// WRONG, backwards!
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.333, v.Acc(time), 0.001);
// middle of ramp up
@@ -319,18 +319,18 @@ void VelocityProfileTest::TestTrapHalf_SetDuration_End()
// end - continue with given velocity
time = 8;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(-2.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
// fenceposts - before and after
time = -1;
- CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
time = 9;
- CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
- CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
+ CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
}