From 69f2e00b5d47407947375822a183f502689f43c5 Mon Sep 17 00:00:00 2001 From: Stephen Roderick Date: Thu, 13 Jan 2011 08:50:56 -0500 Subject: [PATCH 3/3] tests: Add tests for specified velocity in trap. profile --- tests/velocityprofiletest.cpp | 137 +++++++++++++++++++++++++++++++++++++++++ tests/velocityprofiletest.hpp | 6 ++ 2 files changed, 143 insertions(+), 0 deletions(-) diff --git a/tests/velocityprofiletest.cpp b/tests/velocityprofiletest.cpp index 8d16be5..8ca4eed 100644 --- a/tests/velocityprofiletest.cpp +++ b/tests/velocityprofiletest.cpp @@ -142,3 +142,140 @@ void VelocityProfileTest::TestTrap_SetDuration1() CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time)); CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time)); } + +void VelocityProfileTest::TestTrap_SetVelocity1() +{ + // same as first max velocity test, but twice as + // slow + VelocityProfile_Trap v(2, 1); + double time; + v.SetProfileVelocity(2, 10, 0.5); + CPPUNIT_ASSERT_EQUAL(12.0, v.Duration()); + + // 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)); + + // 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)); + + // 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)); + + // 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)); + + // 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)); + + // 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)); +} + +void VelocityProfileTest::TestTrap_SetVelocity2() +{ + // same as first max velocity test, but twice as + // slow + VelocityProfile_Trap v(2, 1); + double time; + v.SetProfileVelocity(2, 10, 1.0); // max velocity + CPPUNIT_ASSERT_EQUAL(6.0, v.Duration()); + + // 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)); + + // 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)); + + // 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)); + + // 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)); + + // 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)); + + // 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)); + + // 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)); + 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)); +} + +void VelocityProfileTest::TestTrap_SetVelocity3() +{ + // same as first max velocity test, but twice as + // slow + VelocityProfile_Trap v(2, 1); + double time; + v.SetProfileVelocity(2, 10, KDL::epsilon); // min velocity + CPPUNIT_ASSERT_EQUAL(6.0 / KDL::epsilon, v.Duration()); + + // start + time = 0; + CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time)); + CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time)); + // O(eps^2) as a*t^2 + CPPUNIT_ASSERT_EQUAL(1.0*(KDL::epsilon*KDL::epsilon), v.Acc(time)); + + // middle of flat velocity + time = 3/KDL::epsilon; + CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time)); + CPPUNIT_ASSERT_EQUAL(2.0*KDL::epsilon, v.Vel(time)); + CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time)); + + // middle of ramp down + time = 5/KDL::epsilon; + CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time), KDL::epsilon); + CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0*KDL::epsilon, v.Vel(time), KDL::epsilon); + CPPUNIT_ASSERT_EQUAL(-1.0*(KDL::epsilon*KDL::epsilon), v.Acc(time)); + + // end + time = 6/KDL::epsilon; + CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time)); + CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time)); + CPPUNIT_ASSERT_EQUAL(-1.0*(KDL::epsilon*KDL::epsilon), v.Acc(time)); +} diff --git a/tests/velocityprofiletest.hpp b/tests/velocityprofiletest.hpp index fdecf23..15e6ef1 100644 --- a/tests/velocityprofiletest.hpp +++ b/tests/velocityprofiletest.hpp @@ -11,6 +11,9 @@ class VelocityProfileTest : public CppUnit::TestFixture CPPUNIT_TEST(TestTrap_MaxVelocity2); CPPUNIT_TEST(TestTrap_MaxVelocity3); CPPUNIT_TEST(TestTrap_SetDuration1); + CPPUNIT_TEST(TestTrap_SetVelocity1); + CPPUNIT_TEST(TestTrap_SetVelocity2); + CPPUNIT_TEST(TestTrap_SetVelocity3); CPPUNIT_TEST_SUITE_END(); @@ -22,6 +25,9 @@ public: void TestTrap_MaxVelocity2(); void TestTrap_MaxVelocity3(); void TestTrap_SetDuration1(); + void TestTrap_SetVelocity1(); + void TestTrap_SetVelocity2(); + void TestTrap_SetVelocity3(); }; #endif -- 1.7.0.2