orocos_kdl: velocityprofiletest.cpp Source File

Go to the documentation of this file.
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         // 2 second ramp up (cover 2 distance),
00018         // 2 second flat velocity (cover 4 distance)
00019         // 2 second ramp down (cover 2 distance),
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         // start
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         // end of ramp up
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         // middle of flat velocity
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         // end of flat velocity
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         // middle of ramp down
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         // end
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         // fenceposts - before and after
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         // 2 second ramp up (cover -2 distance),
00075         // 2 second flat velocity (cover -4 distance)
00076         // 2 second ramp down (cover -2 distance),
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         // 2 second ramp up (cover 4 distance),
00085         // 0 second flat velocity (cover 0 distance)
00086         // 2 second ramp down (cover 4 distance),
00087         VelocityProfile_Trap    v(4, 2);
00088         v.SetProfile(2, 10);
00089         CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
00090 
00091         // new profile
00092         v.SetProfile(2, -6);
00093         CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
00094 
00095         // another new profile : ramp + 2 sec + ramp
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         // same as first max velocity test, but twice as
00103         // long (max velocity gives 6 seconds)
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         // start
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         // end of ramp up
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         // middle of flat velocity
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         // end of flat velocity
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         // middle of ramp down
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         // end
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         // 2 second ramp up (cover 2 distance),
00149         // 2 second flat velocity (cover 4 distance)
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         // start
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         // end of ramp up
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         // middle of flat velocity
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         // end
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         // fenceposts - before and after
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         // 2 second flat velocity (cover 4 distance)
00193         // 2 second ramp up (cover 2 distance),
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         // start - flat velocity
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         // end of flat velocity
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         // middle of ramp down
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         // end
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         // fenceposts - before and after
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         // same as TestTrapHalf__SetProfile_Start() but twice as slow
00237         // Lingers at start position with zero velocity for a period of time,
00238         // as does not scale the velocity; only scales the acceleration!?
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         // start - no motion
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         // no motion
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         // begin ramp at scaled acceleration
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         // middle of ramp up
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         // end - continue with given velocity
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         // fenceposts - before and after
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         // same as TestTrapHalf__SetProfile_Start() but twice as slow
00289         // Lingers at start position with zero velocity for a period of time,
00290         // as does not scale the velocity; only scales the acceleration!?
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         // start - no motion
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         // no motion
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         // begin ramp at scaled acceleration
00309         time = 2;
00310         CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time), 0.001);// WRONG, backwards!
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         // middle of ramp up
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         // end - continue with given velocity
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         // fenceposts - before and after
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 }
00337 
00338 void VelocityProfileTest::TestDirac_SetProfile()
00339 {
00340     double time;
00341     double pos1 = 10.0;
00342     double pos2 = -1.0;
00343 
00344     VelocityProfile_Dirac v;
00345     v.SetProfile(pos1, pos2);
00346 
00347     time = 0;
00348     CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
00349 
00350     time = 1.0;
00351     CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
00352 
00353     time = -1.0;
00354     CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
00355 }
00356 
00357 void VelocityProfileTest::TestDirac_SetProfileDuration()
00358 {
00359     double time;
00360     double pos1 = 10.0;
00361     double pos2 = -1.0;
00362     double duration = 5.0;
00363     VelocityProfile_Dirac v;
00364     v.SetProfileDuration(pos1, pos2, duration);
00365 
00366     time = -1.0;
00367     CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
00368 
00369     time = duration/2;
00370     CPPUNIT_ASSERT_DOUBLES_EQUAL((pos1 + pos2)/2, v.Pos(time), epsilon);
00371     CPPUNIT_ASSERT_DOUBLES_EQUAL((pos2-pos1)/duration, v.Vel(time), epsilon);
00372 
00373     time = duration;
00374     CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
00375 
00376     time = duration + 1.0;
00377     CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
00378 }