(#5)Fix tests

tests/CMakeLists.txt

@@ -21,10 +21,8 @@ foreach(file_path ${test_list})
   get_filename_component(barename ${file_path} NAME)
   string(REPLACE ".cpp" "" new_name ${barename})
   add_executable(${new_name} ${file_path})
-  add_test(
-    NAME ${new_name}
-    COMMAND ${new_name}
-    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)
+  gtest_discover_tests(${new_name}
+                       WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)
 endforeach()
 
 foreach(file_path ${test_py_list})
@@ -41,11 +39,3 @@ foreach(file_path ${test_py_list})
                PYTHONPATH=$<TARGET_FILE_DIR:pyopstop>:$ENV{PYTHONPATH})
 
 endforeach()
-
-add_executable(gtest_l2_constraints
-               ${PROJECT_SOURCE_DIR}/tests/jerk_l2_constraints.cpp)
-gtest_discover_tests(gtest_l2_constraints)
-add_executable(gtest_minimum_time_bounded_jerk_l2
-               ${PROJECT_SOURCE_DIR}/tests/minimum_time_bounded_jerk_l2.cpp)
-gtest_discover_tests(gtest_minimum_time_bounded_jerk_l2
-                     WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests)

tests/acceleration_constraints.cpp

@@ -2,6 +2,7 @@
 #include <gsplines/Collocation/GaussLobattoPointsWeights.hpp>
 #include <gsplines/Functions/ElementalFunctions.hpp>
 #include <gsplines/Optimization/ipopt_solver.hpp>
+#include <gtest/gtest.h>
 #include <ifopt/ipopt_solver.h>
 #include <ifopt/problem.h>
 #include <opstop/acceleration_constraints.hpp>
@@ -43,17 +44,15 @@ Eigen::VectorXd pol_coeff(6);
 void compare_assert(Eigen::VectorXd &_m_nom, Eigen::VectorXd &_m_test) {
 
   if (_m_nom.array().abs().maxCoeff() < 1.0e-9) {
-    assert((_m_nom - _m_test).array().abs().maxCoeff() < 1.0e-9);
+    EXPECT_LE((_m_nom - _m_test).array().abs().maxCoeff(), 1.0e-9);
   } else {
     double err = (_m_nom - _m_test).array().abs().maxCoeff() /
                  _m_nom.array().abs().maxCoeff();
 
-    if (err > 1.0e-9) {
-      throw std::logic_error("Large error");
-    }
+    EXPECT_LE(err, 1.0e-9);
   }
 }
-int main() {
+TEST(AccelerationConstraints, valueAndDiff) {
   feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
 
   std::vector<double> tb(7, 10.0);
@@ -112,13 +111,13 @@ int main() {
   double err_inf_norm = err_mat.array().abs().maxCoeff();
 
   if (jac_nom_inf_norm < 1.0e-6) {
-    if (err_inf_norm > 5.0e-7)
-      return 1;
+    EXPECT_LE(err_inf_norm, 5.0e-7);
   } else {
-
-    if (err_inf_norm / jac_nom_inf_norm > 5.0e-7)
-      return 1;
+    EXPECT_LE(err_inf_norm / jac_nom_inf_norm, 5.0e-7);
   }
-
-  return 0;
+}
+int main(int argc, char **argv) {
+  feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
 }

tests/jerk_constraints.cpp

@@ -2,6 +2,7 @@
 #include <gsplines/Collocation/GaussLobattoPointsWeights.hpp>
 #include <gsplines/Functions/ElementalFunctions.hpp>
 #include <gsplines/Optimization/ipopt_solver.hpp>
+#include <gtest/gtest.h>
 #include <ifopt/ipopt_solver.h>
 #include <ifopt/problem.h>
 #include <opstop/jerk_constraints.hpp>
@@ -44,18 +45,21 @@ Eigen::VectorXd pol_coeff(6);
 
 void compare_assert(Eigen::VectorXd &_m_nom, Eigen::VectorXd &_m_test) {
 
-  std::cout << "\n-- norm of the differenc--\n";
-  std::cout << (_m_nom - _m_test).norm() << "\n----\n";
   if (_m_nom.array().abs().maxCoeff() < 1.0e-9) {
-    assert((_m_nom - _m_test).array().abs().maxCoeff() < 1.0e-9);
+    EXPECT_LE((_m_nom - _m_test).array().abs().maxCoeff(), 1.0e-9)
+        << "\n-- norm of the differenc--\n"
+        << (_m_nom - _m_test).norm() << "\n----\n";
+    ;
   } else {
     double err = (_m_nom - _m_test).array().abs().maxCoeff() /
                  _m_nom.array().abs().maxCoeff();
 
-    assert(err < 1.0e-9);
+    EXPECT_LE(err, 1.0e-9) << "\n-- norm of the differenc--\n"
+                           << (_m_nom - _m_test).norm() << "\n----\n";
+    ;
   }
 }
-int main() {
+TEST(JerkConstraints, value) {
   feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
 
   std::vector<double> tb(7, 10.0);
@@ -114,20 +118,25 @@ int main() {
     jac_nom.col(1) += diff_coeff(uici) * cnstrt.__GetValues(x);
   }
 
-  std::cout << "\n ----- \n" << jac_test.col(0) << "\n ----- \n";
-
   Eigen::MatrixXd err_mat = (jac_nom - jac_test).array().abs().matrix();
 
   double jac_nom_inf_norm = jac_nom.array().abs().maxCoeff();
 
   double err_inf_norm = err_mat.array().abs().maxCoeff();
 
   if (jac_nom_inf_norm < 1.0e-6) {
-    assert(err_inf_norm < 5.0e-7);
+    EXPECT_LE(err_inf_norm, 5.0e-7) << "\n ----- \n"
+                                    << jac_test.col(0) << "\n ----- \n";
+    ;
   } else {
-
-    assert(err_inf_norm / jac_nom_inf_norm < 5.0e-2);
+    EXPECT_LE(err_inf_norm / jac_nom_inf_norm, 5.0e-7)
+        << "\n ----- \n"
+        << jac_test.col(0) << "\n ----- \n";
+    ;
   }
-
-  return 0;
+}
+int main(int argc, char **argv) {
+  feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
 }

tests/jerk_l2_constraints.cpp

@@ -10,7 +10,6 @@
 #include <opstop/differ.hpp>
 
 #include <fenv.h>
-#include <iostream>
 
 #include <opstop/parametrization.hpp>
 
@@ -46,9 +45,9 @@ Eigen::VectorXd pol_coeff(6);
 void compare_assert(const Eigen::MatrixXd &_m_nom,
                     const Eigen::MatrixXd &_m_test) {
 
-  std::cout << "\n-- norm of the differenc--\n Nominal: \n";
-  std::cout << _m_nom << "\n----\n Test: \n";
-  std::cout << _m_test << "\n----\n";
+  // std::cout << "\n-- norm of the differenc--\n Nominal: \n";
+  // std::cout << _m_nom << "\n----\n Test: \n";
+  // std::cout << _m_test << "\n----\n";
   if (_m_nom.array().abs().maxCoeff() < 1.0e-9) {
     EXPECT_LT((_m_nom - _m_test).array().abs().maxCoeff(), 1.0e-9);
   } else {

tests/minimum_time_bounded_acceleration.cpp

@@ -1,13 +1,15 @@
 #include <chrono> // for high_resolution_clock
 #include <ctime>
+#include <fenv.h>
 #include <gsplines/Basis/BasisLegendre.hpp>
 #include <gsplines/Optimization/ipopt_solver.hpp>
+#include <gtest/gtest.h>
 #include <iostream>
 #include <opstop/ipopt_problem.hpp>
 #include <pinocchio/parsers/urdf.hpp>
 
 using namespace opstop;
-int main() {
+TEST(MINIMUN_TIME_BOUNDED_ACCELERATION, Computation) {
   std::size_t number_of_wp = 10;
   std::size_t codom_dim = 7;
   std::size_t nglp = 10;
@@ -46,37 +48,70 @@ int main() {
   Eigen::MatrixXd diffeo_diff_2_hist = diffeo_diff_2(time_span);
 
   Eigen::VectorXd vec(1);
-  vec(0) = ti;
 
+  /// -----------------------------------------
+  /// -----------------------------------------
+  /// Test of the diffeo
+  /// -----------------------------------------
+  /// -----------------------------------------
+  /// -----------------------------------------
+  /// Test stopping parametrization at the beginning
+  /// of the stopping procedure
+  /// --------------------------------------
+  vec(0) = ti;
+  ///
+  /// Here we check that the diffeo evaluated at ti is equal to ti, this is
+  /// because the the diffeo evaluated at ti will give us the values of the
+  /// trajectory at ti so q \circ s(t_i) = q(t_i)
   double ti_test = diffeo(vec)(0, 0);
+  EXPECT_NEAR(ti_test, ti, 1.0e-7);
+
+  /// Here we check that the derivative of the diffeo is one.
+  /// This is because then continuity condition between the original
+  /// and the stoping trajectory.
+  /// trajectory at ti so {d / d t {q \circ s } }(t_i) = \dot q(t_i)
   double ve = diffeo_diff_1(vec)(0, 0);
-  double ac = diffeo_diff_2(vec)(0, 0);
+  EXPECT_NEAR(ve, 1.0, 1.0e-7);
 
-  diffeo.print();
-  assert(std::abs(ti_test - ti) < 1.0e-9);
-  assert(std::abs(ve - 1.0) < 1.0e-9);
-  assert(std::abs(ac) < 1.0e-9);
+  /// Here we check that the 2nd derivative of the diffeo is one.
+  /// This is because then continuity condition between the original
+  /// and the stoping trajectory.
+  /// trajectory at ti so {d^2 / d t^2 {q \circ s } }(t_i) = \ddot q(t_i)
+  double ac = diffeo_diff_2(vec)(0, 0);
+  EXPECT_NEAR(ac, 0.0, 1.0e-7);
+  /// -----------------------------------------
+  /// Test stopping parametrization at the end
+  /// of the stopping procedure
+  /// --------------------------------------
+  vec(0) = T;
+  ti_test = diffeo(vec)(0, 0);
+  /// Here we check that the topping condition in velocity
+  /// {d / d t s }(T) = 0.0
+  ve = diffeo_diff_1(vec)(0, 0);
+  EXPECT_NEAR(ve, 0.0, 1.0e-7);
+  /// Here we check that the topping condition in acceleration
+  ac = diffeo_diff_2(vec)(0, 0);
+  EXPECT_NEAR(ac, 0.0, 1.0e-7);
 
+  /// -----------------------------------------
+  /// -----------------------------------------
+  /// Test of the stopping trajectory
+  /// -----------------------------------------
+  /// -----------------------------------------
   gsplines::functions::FunctionExpression stop_trj = trj.compose(diffeo);
   gsplines::functions::FunctionExpression stop_trj_diff_1 = stop_trj.derivate();
   gsplines::functions::FunctionExpression stop_trj_diff_2 =
       stop_trj.derivate(2);
 
   vec(0) = T;
-  ti_test = diffeo(vec)(0, 0);
-  ve = diffeo_diff_1(vec)(0, 0);
-  ac = diffeo_diff_2(vec)(0, 0);
-
-  assert(std::abs(ve) < 1.0e-9);
-  assert(std::abs(ac) < 1.0e-9);
-
-  double vel_value = stop_trj_diff_1(vec).row(0).norm();
-  double acc_value = stop_trj_diff_2(vec).row(0).norm();
-
-  assert(vel_value < 1.0e-9);
-  assert(acc_value < 1.0e-9);
-
-  double mean_time = 0.0;
+  // Test that the stopping trajectory actually stops
+  ve = stop_trj_diff_1(vec).row(0).norm();
+  EXPECT_NEAR(ve, 0.0, 1.0e-7);
+  // Test that the stopping trajectory actually stops with zero acceleration
+  ac = stop_trj_diff_2(vec).row(0).norm();
+  EXPECT_NEAR(ac, 0.0, 1.0e-7);
+
+  double mean_time_milliseconds = 0.0;
   std::size_t number_of_tests = 1;
   double time_elapsed_ms = 0;
   for (std::size_t _ = 0; _ < number_of_tests; _++) {
@@ -103,11 +138,19 @@ int main() {
     std::clock_t t_cpu_2 = std::clock();
     time_elapsed_ms = 1000.0 * (t_cpu_2 - t_cpu_1) / CLOCKS_PER_SEC;
     std::chrono::duration<double, std::milli> ms_double = t2 - t1;
-    mean_time += ms_double.count();
+    mean_time_milliseconds += ms_double.count();
   }
-  mean_time /= number_of_tests;
-  assert(mean_time < 100.0);
-  printf("Mean optimization time is %+14.7e ms\n", mean_time);
+
+  mean_time_milliseconds /= number_of_tests;
+#ifdef NDEBUG
+  EXPECT_LE(mean_time_milliseconds, 50.0);
+#endif
+  printf("Mean optimization time is %+14.7e ms\n", mean_time_milliseconds);
   printf("Mean optimization time is %+14.7e ms CPU\n", time_elapsed_ms);
-  return 0;
+}
+
+int main(int argc, char **argv) {
+  feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
 }

tests/minimum_time_bounded_acceleration.py

@@ -1,21 +1,11 @@
 """
 Example of how to get a curve that stops in minium time with accerelraion bouds
 """
-import gsplines
 import pathlib
-import sys
 import unittest
 import numpy as np
 import matplotlib.pyplot as plt
-from .tools import debug_on
-
-# Loads URDF model into pinocchio
-from pinocchio import buildModelFromUrdf
-# Stores the forward kinematics of the joints into the data argument
-from pinocchio import forwardKinematics
-# Updates the positions of the frames given the joints positions in data
-from pinocchio import updateFramePlacements
-from pinocchio import randomConfiguration
+
 try:
     import opstop
 except ImportError:
@@ -56,7 +46,8 @@ def show_piecewisefunction(_q, _up_to_deriv=3, _dt=0.1, _title=''):
         hspace=0.15)
 
 
-def show_compare_piecewisefunction(_q, _p, _up_to_deriv=3, _dt=0.02, _title=''):
+def show_compare_piecewisefunction(_q, _p,
+                                   _up_to_deriv=3, _dt=0.02, _title=''):
     dim = _q.get_codom_dim()
     fig, ax = plt.subplots(_up_to_deriv + 1, dim)
     if dim == 1:
@@ -125,7 +116,7 @@ def test(self):
 
         stop_ti = 0.7*exec_time
 
-        diffeo = opstop.minimum_time_bouded_acceleration(
+        diffeo = opstop.minimum_time_bounded_acceleration(
             trj, stop_ti, 1.5, str(model_file), 8)
 
         stop_trj = trj.compose(diffeo)