From e51d2e351d46c2215ff97045500737bb49369203 Mon Sep 17 00:00:00 2001 From: Stephen Roderick Date: Thu, 13 Jan 2011 08:50:00 -0500 Subject: [PATCH 1/3] Add velocity profile tests for trap. --- tests/CMakeLists.txt | 7 ++ tests/velocityprofiletest.cpp | 144 +++++++++++++++++++++++++++++++++++++++++ tests/velocityprofiletest.hpp | 28 ++++++++ 3 files changed, 179 insertions(+), 0 deletions(-) create mode 100644 tests/velocityprofiletest.cpp create mode 100644 tests/velocityprofiletest.hpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e6c1a19..d1a8bf6 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -37,6 +37,13 @@ IF(BUILD_TESTING) COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") ADD_TEST(jacobiantest jacobiantest) + ADD_EXECUTABLE(velocityprofiletest velocityprofiletest.cpp test-runner.cpp) + SET(TESTNAME "velocityprofiletest") + TARGET_LINK_LIBRARIES(velocityprofiletest orocos-kdl ${CPPUNIT}) + SET_TARGET_PROPERTIES( velocityprofiletest PROPERTIES + COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") + ADD_TEST(velocityprofiletest velocityprofiletest) + # ADD_EXECUTABLE(rframestest rframestest.cpp) # TARGET_LINK_LIBRARIES(rframestest orocos-kdl) # ADD_TEST(rframestest rframestest) diff --git a/tests/velocityprofiletest.cpp b/tests/velocityprofiletest.cpp new file mode 100644 index 0000000..8d16be5 --- /dev/null +++ b/tests/velocityprofiletest.cpp @@ -0,0 +1,144 @@ +#include "velocityprofiletest.hpp" +#include +CPPUNIT_TEST_SUITE_REGISTRATION( VelocityProfileTest ); + +using namespace KDL; + +void VelocityProfileTest::setUp() +{ +} + +void VelocityProfileTest::tearDown() +{ +} + +void VelocityProfileTest::TestTrap_MaxVelocity1() +{ + // 2 second ramp up (cover 2 distance), + // 2 second flat velocity (cover 4 distance) + // 2 second ramp down (cover 2 distance), + VelocityProfile_Trap v(2, 1); + double time; + v.SetProfile(2, 10); + 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_MaxVelocity2() +{ + // 2 second ramp up (cover -2 distance), + // 2 second flat velocity (cover -4 distance) + // 2 second ramp down (cover -2 distance), + VelocityProfile_Trap v(2, 1); + v.SetProfile(2, -6); + CPPUNIT_ASSERT_EQUAL(6.0, v.Duration()); +} + +void VelocityProfileTest::TestTrap_MaxVelocity3() +{ + // 2 second ramp up (cover 4 distance), + // 0 second flat velocity (cover 0 distance) + // 2 second ramp down (cover 4 distance), + VelocityProfile_Trap v(4, 2); + v.SetProfile(2, 10); + CPPUNIT_ASSERT_EQUAL(4.0, v.Duration()); + + // new profile + v.SetProfile(2, -6); + CPPUNIT_ASSERT_EQUAL(4.0, v.Duration()); + + // another new profile : ramp + 2 sec + ramp + v.SetProfile(13, 13 + 4 + 8 + 4); + CPPUNIT_ASSERT_EQUAL(6.0, v.Duration()); +} + +void VelocityProfileTest::TestTrap_SetDuration1() +{ + // same as first max velocity test, but twice as + // long (max velocity gives 6 seconds) + VelocityProfile_Trap v(2, 1); + double time; + v.SetProfileDuration(2, 10, 12.0); + 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)); +} diff --git a/tests/velocityprofiletest.hpp b/tests/velocityprofiletest.hpp new file mode 100644 index 0000000..fdecf23 --- /dev/null +++ b/tests/velocityprofiletest.hpp @@ -0,0 +1,28 @@ +#ifndef VELOCITYPROFILETEST_HPP +#define VELOCITYPROFILETEST_HPP + +#include +#include + +class VelocityProfileTest : public CppUnit::TestFixture +{ + CPPUNIT_TEST_SUITE(VelocityProfileTest); + CPPUNIT_TEST(TestTrap_MaxVelocity1); + CPPUNIT_TEST(TestTrap_MaxVelocity2); + CPPUNIT_TEST(TestTrap_MaxVelocity3); + CPPUNIT_TEST(TestTrap_SetDuration1); + + CPPUNIT_TEST_SUITE_END(); + +public: + void setUp(); + void tearDown(); + + void TestTrap_MaxVelocity1(); + void TestTrap_MaxVelocity2(); + void TestTrap_MaxVelocity3(); + void TestTrap_SetDuration1(); +}; + +#endif + -- 1.7.0.2