00001 #include "velocityprofiletest.hpp"
00002 #include <frames_io.hpp>
00003 CPPUNIT_TEST_SUITE_REGISTRATION( VelocityProfileTest );
00004
00005 using namespace KDL;
00006
00007 void VelocityProfileTest::setUp()
00008 {
00009 }
00010
00011 void VelocityProfileTest::tearDown()
00012 {
00013 }
00014
00015 void VelocityProfileTest::TestTrap_MaxVelocity1()
00016 {
00017
00018
00019
00020 VelocityProfile_Trap v(2, 1);
00021 double time;
00022 v.SetProfile(2, 10);
00023 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
00024
00025
00026 time = 0;
00027 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00028 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00029 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
00030
00031
00032 time = 2;
00033 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
00034 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00035 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00036
00037
00038 time = 3;
00039 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
00040 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00041 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00042
00043
00044 time = 4;
00045 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00046 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00047 CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
00048
00049
00050 time = 5;
00051 CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time),epsilon);
00052 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
00053 CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
00054
00055
00056 time = 6;
00057 CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
00058 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00059 CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
00060
00061
00062 time = -1;
00063 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00064 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00065 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00066 time = 11;
00067 CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
00068 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00069 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00070 }
00071
00072 void VelocityProfileTest::TestTrap_MaxVelocity2()
00073 {
00074
00075
00076
00077 VelocityProfile_Trap v(2, 1);
00078 v.SetProfile(2, -6);
00079 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
00080 }
00081
00082 void VelocityProfileTest::TestTrap_MaxVelocity3()
00083 {
00084
00085
00086
00087 VelocityProfile_Trap v(4, 2);
00088 v.SetProfile(2, 10);
00089 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
00090
00091
00092 v.SetProfile(2, -6);
00093 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
00094
00095
00096 v.SetProfile(13, 13 + 4 + 8 + 4);
00097 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
00098 }
00099
00100 void VelocityProfileTest::TestTrap_SetDuration1()
00101 {
00102
00103
00104 VelocityProfile_Trap v(2, 1);
00105 double time;
00106 v.SetProfileDuration(2, 10, 12.0);
00107 CPPUNIT_ASSERT_DOUBLES_EQUAL(12.0, v.Duration(),epsilon);
00108
00109
00110 time = 0;
00111 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00112 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00113 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.25, v.Acc(time),epsilon);
00114
00115
00116 time = 4;
00117 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
00118 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
00119 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00120
00121
00122 time = 6;
00123 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
00124 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
00125 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00126
00127
00128 time = 8;
00129 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00130 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
00131 CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
00132
00133
00134 time = 10;
00135 CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time),epsilon);
00136 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, v.Vel(time),epsilon);
00137 CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
00138
00139
00140 time = 12;
00141 CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
00142 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00143 CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
00144 }
00145
00146 void VelocityProfileTest::TestTrapHalf_SetProfile_Start()
00147 {
00148
00149
00150 VelocityProfile_TrapHalf v(2, 1, true);
00151 double time;
00152 v.SetProfile(2, 2+6);
00153 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
00154
00155
00156 time = 0;
00157 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00158 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00159 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
00160
00161
00162 time = 2;
00163 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
00164 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00165 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00166
00167
00168 time = 3;
00169 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
00170 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00171 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00172
00173
00174 time = 4;
00175 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00176 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00177 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00178
00179
00180 time = -1;
00181 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00182 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00183 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00184 time = 5;
00185 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00186 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00187 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00188 }
00189
00190 void VelocityProfileTest::TestTrapHalf_SetProfile_End()
00191 {
00192
00193
00194 VelocityProfile_TrapHalf v(2, 1, false);
00195 double time;
00196 v.SetProfile(9, 9-6);
00197 CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
00198
00199
00200 time = 0;
00201 CPPUNIT_ASSERT_DOUBLES_EQUAL(9.0, v.Pos(time),epsilon);
00202 CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
00203 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00204
00205
00206 time = 2;
00207 CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, v.Pos(time),epsilon);
00208 CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
00209 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
00210
00211
00212 time = 3;
00213 CPPUNIT_ASSERT_DOUBLES_EQUAL(3.5, v.Pos(time),epsilon);
00214 CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Vel(time),epsilon);
00215 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
00216
00217
00218 time = 4;
00219 CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, v.Pos(time),epsilon);
00220 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00221 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00222
00223
00224 time = -1;
00225 CPPUNIT_ASSERT_DOUBLES_EQUAL(9.0, v.Pos(time),epsilon);
00226 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00227 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00228 time = 5;
00229 CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, v.Pos(time),epsilon);
00230 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00231 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00232 }
00233
00234 void VelocityProfileTest::TestTrapHalf_SetDuration_Start()
00235 {
00236
00237
00238
00239 VelocityProfile_TrapHalf v(2, 1, true);
00240 double time;
00241 v.SetProfileDuration(2, 2+6, 8);
00242 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Duration(),epsilon);
00243
00244
00245 time = 0;
00246 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00247 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00248 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00249
00250
00251 time = 1.9;
00252 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00253 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00254 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00255
00256
00257 time = 2;
00258 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time), 0.001);
00259 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00260 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.333, v.Acc(time), 0.001);
00261
00262
00263 time = 5;
00264 CPPUNIT_ASSERT_DOUBLES_EQUAL(3.5, v.Pos(time), 0.001);
00265 CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time), 0.001);
00266 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3333, v.Acc(time), 0.001);
00267
00268
00269 time = 8;
00270 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00271 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
00272 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00273
00274
00275 time = -1;
00276 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00277 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00278 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00279 time = 9;
00280 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00281 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00282 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00283
00284 }
00285
00286 void VelocityProfileTest::TestTrapHalf_SetDuration_End()
00287 {
00288
00289
00290
00291 VelocityProfile_TrapHalf v(2, 1, true);
00292 double time;
00293 v.SetProfileDuration(2+6, 2, 8);
00294 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Duration(),epsilon);
00295
00296
00297 time = 0;
00298 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00299 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00300 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00301
00302
00303 time = 1.9;
00304 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00305 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00306 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00307
00308
00309 time = 2;
00310 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time), 0.001);
00311 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00312 CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.333, v.Acc(time), 0.001);
00313
00314
00315 time = 5;
00316 CPPUNIT_ASSERT_DOUBLES_EQUAL(6.5, v.Pos(time), 0.001);
00317 CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Vel(time), 0.001);
00318 CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.3333, v.Acc(time), 0.001);
00319
00320
00321 time = 8;
00322 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00323 CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
00324 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00325
00326
00327 time = -1;
00328 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
00329 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00330 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00331 time = 9;
00332 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
00333 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
00334 CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
00335
00336 }