add a proper test on the riccati equation

tests/CMakeLists.txt

@@ -55,12 +55,16 @@ INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/src)
 ##########################
 ## Test Ricatti Equation #
 ##########################
+CONFIG_FILES_CMAKE(TestRiccatiEquationWeightsWithoutInitialPose.datref)
+CONFIG_FILES_CMAKE(TestRiccatiEquationWeightsWithInitialPose.datref)
 ADD_EXECUTABLE(TestRiccatiEquation
   TestRiccatiEquation.cpp
   ../src/PreviewControl/OptimalControllerSolver.cpp
   )
-
+# Need this dependency for the use of the fortran functions (it seems)
 PKG_CONFIG_USE_DEPENDENCY(TestRiccatiEquation jrl-mal)
+# Add test on the ricatti equation
+ADD_TEST(TestRiccatiEquation TestRiccatiEquation)
 
 #######################
 ## Test Morisawa 2007 #

tests/TestRiccatiEquation.cpp

@@ -25,12 +25,128 @@
 /*! \file TestRiccatiEquation.cpp
   \brief Example to solve the Riccati Equation for the preview control. */
 
+#define NB_OF_FIELDS 1
+
 #include <iostream>
 #include <fstream>
+#include <Debug.hh>
 #include "PreviewControl/OptimalControllerSolver.hh"
 
 using namespace std;
 
+bool compareDebugFiles( string fileName )
+{
+  ifstream alif;
+  unsigned max_nb_of_pbs=100;
+  unsigned nb_of_pbs = 0;
+  ODEBUG("Report : " << fileName);
+  alif.open(fileName.c_str(),ifstream::in);
+  if (!alif.is_open())
+  {
+    std::cerr << "Unable to open "<< fileName << std::endl;
+    return -1;
+  }
+
+  ifstream arif;
+  string refFileName = fileName + "ref" ;
+  arif.open(refFileName.c_str(),ifstream::in);
+  ODEBUG("ReportRef : " << refFileName);
+
+  if (!arif.is_open())
+  {
+    std::cerr << "Unable to open "<< refFileName << std::endl;
+    return -1;
+  }
+
+
+  ofstream areportof;
+  string aFileName ;
+  aFileName = fileName ;
+  aFileName += ".report";
+  areportof.open(aFileName.c_str(),ofstream::out);
+
+  // Time
+  double LocalInput[NB_OF_FIELDS], ReferenceInput[NB_OF_FIELDS];
+  bool finalreport = true;
+  unsigned long int nblines = 0;
+  bool endofinspection=false;
+
+  // Find size of the two files.
+  alif.seekg (0, alif.end);
+  unsigned long int alif_length = (unsigned long int)alif.tellg();
+  alif.seekg (0, alif.beg);
+
+  arif.seekg (0, arif.end);
+  unsigned long int arif_length = (unsigned long int)arif.tellg();
+  arif.seekg (0, arif.beg);
+
+  while ((!alif.eof()) &&
+         (!arif.eof()) &&
+         (!endofinspection))
+  {
+    for (unsigned int i=0;i<NB_OF_FIELDS;i++)
+    {
+      alif >> LocalInput[i];
+      if (alif.eof())
+      {
+        endofinspection =true;
+        break;
+      }
+    }
+    if (endofinspection)
+      break;
+
+    for (unsigned int i=0;i<NB_OF_FIELDS;i++)
+    {
+      arif >> ReferenceInput[i];
+      if (arif.eof())
+      {
+        endofinspection =true;
+        break;
+      }
+    }
+    if (endofinspection)
+      break;
+
+
+    for (unsigned int i=0;i<NB_OF_FIELDS;i++)
+    {
+      if  (fabs(LocalInput[i]-
+                ReferenceInput[i])>=1e-6)
+      {
+        finalreport = false;
+        ostringstream oss;
+        oss << "l: " << nblines
+            << " col:" << i
+            << " ref: " << ReferenceInput[i]
+               << " now: " << LocalInput[i]
+                  << " " << nb_of_pbs
+                  <<std::endl;
+        areportof << oss.str();
+        std::cout << oss.str();
+        nb_of_pbs++;
+        if(nb_of_pbs>max_nb_of_pbs)
+        {
+          endofinspection=true;
+        }
+      }
+    }
+
+    nblines++;
+    if ((nblines*2> alif_length) ||
+        (nblines*2> arif_length))
+    {
+      endofinspection=true;
+      break;
+    }
+  }
+
+  alif.close();
+  arif.close();
+  areportof.close();
+  return finalreport;
+}
+
 int main()
 {
   PatternGeneratorJRL::OptimalControllerSolver *anOCS;
@@ -110,14 +226,14 @@ int main()
 
   anOCS->GetF(lF);
 
-  aof.open("WeightsOutputWithoutInitialPose.dat",ofstream::out);
+  aof.open("TestRiccatiEquationWeightsWithoutInitialPose.dat",ofstream::out);
 
   for(unsigned int li=0;li<MAL_MATRIX_NB_ROWS(lF);li++)
     {
       aof << lF(li,0) << endl;
     }
   aof.close();
-
+  bool sameFile = compareDebugFiles( "TestRiccatiEquationWeightsWithoutInitialPose.dat" );
   delete anOCS;
 
 
@@ -155,7 +271,7 @@ int main()
 
   anOCS->GetF(lF);
 
-  aof.open("WeightsOutputWithInitialPose.dat",ofstream::out);
+  aof.open("TestRiccatiEquationWeightsWithInitialPose.dat",ofstream::out);
 
   for(unsigned int li=0;li<MAL_MATRIX_NB_ROWS(lF);li++)
     {
@@ -163,6 +279,16 @@ int main()
     }
   aof.close();
 
+  bool sameFile2 = compareDebugFiles( "TestRiccatiEquationWeightsWithInitialPose.dat" );
   delete anOCS;
 
+  if (sameFile && sameFile2){
+    cout << "Passed test " << endl;
+    return 0 ;
+  }
+  else
+  {
+    cout << "Failed test " << endl;
+    return -1;
+  }
 }

tests/TestRiccatiEquationWeightsWithInitialPose.datref.cmake

+-59.8616
+-42.0874
+-28.4901
+-18.1176
+-10.2337
+-4.26928
+0.21549
+3.56057
+6.02869
+7.82285
+9.09987
+9.98087
+10.5594
+10.9076
+11.0812
+11.1231
+11.0665
+10.9367
+10.7534
+10.5317
+10.2829
+10.0161
+9.7378
+9.4532
+9.16615
+8.87955
+8.59555
+8.31574
+8.04128
+7.77299
+7.51143
+7.25696
+7.0098
+6.77005
+6.53773
+6.31279
+6.09515
+5.88467
+5.6812
+5.48457
+5.29461
+5.11112
+4.93392
+4.7628
+4.59759
+4.43808
+4.2841
+4.13545
+3.99196
+3.85345
+3.71976
+3.59071
+3.46616
+3.34593
+3.22989
+3.11789
+3.00979
+2.90545
+2.80474
+2.70753
+2.61371
+2.52315
+2.43574
+2.35138
+2.26995
+2.19135
+2.11548
+2.04225
+1.97157
+1.90334
+1.83749
+1.77392
+1.71256
+1.65334
+1.59617
+1.54099
+1.48772
+1.4363
+1.38667
+1.33876
+1.29251
+1.24787
+1.20477
+1.16317
+1.12302
+1.08425
+1.04683
+1.01071
+0.97584
+0.942177
+0.909681
+0.878311
+0.848027
+0.818792
+0.790569
+0.763324
+0.737021
+0.711629
+0.687116
+0.663451
+0.640604
+0.618548
+0.597254
+0.576697
+0.55685
+0.53769
+0.519192
+0.501333
+0.484091
+0.467444
+0.451373
+0.435857
+0.420876
+0.406412
+0.392448
+0.378966
+0.365949
+0.353381
+0.341247
+0.329531
+0.318219
+0.307297
+0.296752
+0.28657
+0.276739
+0.267247
+0.258082
+0.249233
+0.240688
+0.232438
+0.224471
+0.216779
+0.209352
+0.20218
+0.195256
+0.188569
+0.182112
+0.175878
+0.169857
+0.164044
+0.158431
+0.15301
+0.147776
+0.142722
+0.137841
+0.133128
+0.128577
+0.124182
+0.119938
+0.11584
+0.111882
+0.10806
+0.104369
+0.100805
+0.0973633
+0.0940395
+0.0908296
+0.0877298
+0.0847362
+0.0818453

tests/TestRiccatiEquationWeightsWithoutInitialPose.datref.cmake

+618.556
+777.45
+952.24
+1082.22
+1155.16
+1181.2
+1176.71
+1156.25
+1129.84
+1103.09
+1078.4
+1056.27
+1036.29
+1017.84
+1000.33
+983.379
+966.75
+950.346
+934.141
+918.145
+902.379
+886.859
+871.596
+856.594
+841.852
+827.367
+813.134
+799.148
+785.403
+771.895
+758.619
+745.571
+732.748
+720.145
+707.759
+695.586
+683.623
+671.865
+660.309
+648.952
+637.79
+626.821
+616.04
+605.444
+595.031
+584.797
+574.739
+564.853
+555.138
+545.59
+536.206
+526.984
+517.92
+509.012
+500.257
+491.653
+483.197
+474.886
+466.719
+458.691
+450.802
+443.049
+435.428
+427.939
+420.579
+413.345
+406.236
+399.249
+392.382
+385.633
+379.001
+372.482
+366.076
+359.779
+353.591
+347.51
+341.533
+335.659
+329.885
+324.212
+318.635
+313.155
+307.769
+302.475
+297.273
+292.16
+287.135
+282.197
+277.343
+272.573
+267.885
+263.277
+258.749
+254.299
+249.925
+245.626
+241.402
+237.25
+233.169
+229.159
+225.217
+221.344
+217.537
+213.795
+210.118
+206.504
+202.952
+199.462
+196.031
+192.66
+189.346
+186.089
+182.889
+179.743
+176.652
+173.613
+170.627
+167.693
+164.808
+161.974
+159.188
+156.45
+153.759
+151.114
+148.515
+145.961
+143.451
+140.983
+138.558
+136.175
+133.833
+131.531
+129.269
+127.046
+124.861
+122.713
+120.602
+118.528
+116.49
+114.486
+112.517
+110.582
+108.68
+106.811
+104.973
+103.168
+101.394
+99.6496
+97.9357
+96.2513
+94.5958
+92.9688
+91.3698
+89.7983
+88.2538
+86.7359
+85.2441
+83.7779
+82.337
+80.9208
+79.529
+78.1612
+76.8169
+75.4957
+74.1972
+72.921
+71.6668
+70.4342
+69.2228
+68.0322
+66.8621
+65.7121
+64.5819
+63.4711
+62.3794
+61.3065
+60.2521
+59.2158
+58.1973
+57.1963
+56.2126
+55.2458
+54.2956
+53.3617
+52.4439
+51.5419
+50.6554
+49.7842
+48.9279
+48.0864
+47.2593
+46.4465
+45.6477
+44.8625
+44.0909
+43.3326
+42.5873
+41.8548
+41.1349
+40.4274
+39.7321
+39.0487
+38.3771
+37.7171
+37.0683
+36.4308
+35.8042
+35.1884
+34.5832
+33.9884
+33.4038
+32.8293
+32.2646
+31.7097
+31.1643
+30.6283
+30.1015
+29.5838
+29.0749
+28.5749
+28.0834
+27.6004
+27.1257
+26.6591
+26.2006
+25.75
+25.3071
+24.8718
+24.444
+24.0236
+23.6104
+23.2043
+22.8052
+22.413
+22.0275
+21.6486
+21.2763
+20.9103
+20.5507
+20.1972
+19.8499
+19.5085
+19.1729
+18.8432
+18.5191
+18.2005
+17.8875
+17.5799
+17.2775
+16.9803
+16.6883
+16.4012
+16.1192
+15.8419
+15.5694
+15.3017
+15.0385
+14.7798
+14.5256
+14.2758
+14.0302
+13.7889
+13.5518
+13.3187
+13.0896
+12.8645
+12.6432
+12.4258
+12.212
+12.002
+11.7956
+11.5927
+11.3933
+11.1974
+11.0048
+10.8155
+10.6295
+10.4467
+10.267
+10.0904
+9.91684
+9.74627
+9.57864
+9.4139
+9.25198
+9.09285
+8.93646
+8.78276
+8.6317
+8.48324
+8.33733
+8.19394
+8.05301
+7.9145
+7.77837
+7.64459
+7.51311
+7.38389
+7.25689
+7.13207
+7.0094
+6.88885
+6.77036
+6.65392
+6.53947
+6.427
+6.31646
+6.20782
+6.10105
+5.99611
+5.89298
+5.79163
+5.69201
+5.59411
+5.4979
+5.40334
+5.3104
+5.21907
+5.1293
+5.04108