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_EQUAL(6.0, v.Duration());
00024
00025
00026 time = 0;
00027 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00028 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00029 CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
00030
00031
00032 time = 2;
00033 CPPUNIT_ASSERT_EQUAL(4.0, v.Pos(time));
00034 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00035 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00036
00037
00038 time = 3;
00039 CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time));
00040 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00041 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00042
00043
00044 time = 4;
00045 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00046 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00047 CPPUNIT_ASSERT_EQUAL(-1.0, v.Acc(time));
00048
00049
00050 time = 5;
00051 CPPUNIT_ASSERT_EQUAL(9.5, v.Pos(time));
00052 CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
00053 CPPUNIT_ASSERT_EQUAL(-1.0, v.Acc(time));
00054
00055
00056 time = 6;
00057 CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time));
00058 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00059 CPPUNIT_ASSERT_EQUAL(-1.0, v.Acc(time));
00060
00061
00062 time = -1;
00063 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00064 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00065 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00066 time = 11;
00067 CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time));
00068 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00069 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
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_EQUAL(6.0, v.Duration());
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_EQUAL(4.0, v.Duration());
00090
00091
00092 v.SetProfile(2, -6);
00093 CPPUNIT_ASSERT_EQUAL(4.0, v.Duration());
00094
00095
00096 v.SetProfile(13, 13 + 4 + 8 + 4);
00097 CPPUNIT_ASSERT_EQUAL(6.0, v.Duration());
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_EQUAL(12.0, v.Duration());
00108
00109
00110 time = 0;
00111 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00112 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00113 CPPUNIT_ASSERT_EQUAL(0.25, v.Acc(time));
00114
00115
00116 time = 4;
00117 CPPUNIT_ASSERT_EQUAL(4.0, v.Pos(time));
00118 CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
00119 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00120
00121
00122 time = 6;
00123 CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time));
00124 CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
00125 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00126
00127
00128 time = 8;
00129 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00130 CPPUNIT_ASSERT_EQUAL(1.0, v.Vel(time));
00131 CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time));
00132
00133
00134 time = 10;
00135 CPPUNIT_ASSERT_EQUAL(9.5, v.Pos(time));
00136 CPPUNIT_ASSERT_EQUAL(0.5, v.Vel(time));
00137 CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time));
00138
00139
00140 time = 12;
00141 CPPUNIT_ASSERT_EQUAL(10.0, v.Pos(time));
00142 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00143 CPPUNIT_ASSERT_EQUAL(-0.25, v.Acc(time));
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_EQUAL(4.0, v.Duration());
00154
00155
00156 time = 0;
00157 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00158 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00159 CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
00160
00161
00162 time = 2;
00163 CPPUNIT_ASSERT_EQUAL(4.0, v.Pos(time));
00164 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00165 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00166
00167
00168 time = 3;
00169 CPPUNIT_ASSERT_EQUAL(6.0, v.Pos(time));
00170 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00171 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00172
00173
00174 time = 4;
00175 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00176 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00177 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00178
00179
00180 time = -1;
00181 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00182 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00183 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00184 time = 5;
00185 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00186 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00187 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
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_EQUAL(4.0, v.Duration());
00198
00199
00200 time = 0;
00201 CPPUNIT_ASSERT_EQUAL(9.0, v.Pos(time));
00202 CPPUNIT_ASSERT_EQUAL(-2.0, v.Vel(time));
00203 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00204
00205
00206 time = 2;
00207 CPPUNIT_ASSERT_EQUAL(5.0, v.Pos(time));
00208 CPPUNIT_ASSERT_EQUAL(-2.0, v.Vel(time));
00209 CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
00210
00211
00212 time = 3;
00213 CPPUNIT_ASSERT_EQUAL(3.5, v.Pos(time));
00214 CPPUNIT_ASSERT_EQUAL(-1.0, v.Vel(time));
00215 CPPUNIT_ASSERT_EQUAL(1.0, v.Acc(time));
00216
00217
00218 time = 4;
00219 CPPUNIT_ASSERT_EQUAL(3.0, v.Pos(time));
00220 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00221 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00222
00223
00224 time = -1;
00225 CPPUNIT_ASSERT_EQUAL(9.0, v.Pos(time));
00226 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00227 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00228 time = 5;
00229 CPPUNIT_ASSERT_EQUAL(3.0, v.Pos(time));
00230 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00231 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
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_EQUAL(8.0, v.Duration());
00243
00244
00245 time = 0;
00246 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00247 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00248 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00249
00250
00251 time = 1.9;
00252 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00253 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00254 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00255
00256
00257 time = 2;
00258 CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time), 0.001);
00259 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
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_EQUAL(8.0, v.Pos(time));
00271 CPPUNIT_ASSERT_EQUAL(2.0, v.Vel(time));
00272 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00273
00274
00275 time = -1;
00276 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00277 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00278 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00279 time = 9;
00280 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00281 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00282 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
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_EQUAL(8.0, v.Duration());
00295
00296
00297 time = 0;
00298 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00299 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00300 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00301
00302
00303 time = 1.9;
00304 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00305 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00306 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00307
00308
00309 time = 2;
00310 CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time), 0.001);
00311 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
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_EQUAL(2.0, v.Pos(time));
00323 CPPUNIT_ASSERT_EQUAL(-2.0, v.Vel(time));
00324 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00325
00326
00327 time = -1;
00328 CPPUNIT_ASSERT_EQUAL(8.0, v.Pos(time));
00329 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00330 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00331 time = 9;
00332 CPPUNIT_ASSERT_EQUAL(2.0, v.Pos(time));
00333 CPPUNIT_ASSERT_EQUAL(0.0, v.Vel(time));
00334 CPPUNIT_ASSERT_EQUAL(0.0, v.Acc(time));
00335
00336 }