diff --git a/benchmark/lqr.cpp b/benchmark/lqr.cpp
index 43b4d995ce174e45a9fe92b942ebc4654c43922b..19d9f6104f733999f02872b2936b57aace639cc6 100644
--- a/benchmark/lqr.cpp
+++ b/benchmark/lqr.cpp
@@ -40,17 +40,52 @@ int main() {
   }
 
   // Solving the optimal control problem
-  std::clock_t c_start, c_end;
+  struct timespec start, finish;
+  double elapsed;
   Eigen::ArrayXd duration(T);
   for (unsigned int i = 0; i < T; ++i) {
-    c_start = std::clock();
+    clock_gettime(CLOCK_MONOTONIC, &start);
     ddp.solve(xs, us, MAXITER);
-    c_end = std::clock();
-    duration[i] = 1e3 * (double)(c_end - c_start) / CLOCKS_PER_SEC;
+    clock_gettime(CLOCK_MONOTONIC, &finish);
+    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
+    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    duration[i] = elapsed / 1000.;
   }
 
   double avrg_duration = duration.sum() / T;
   double min_duration = duration.minCoeff();
   double max_duration = duration.maxCoeff();
-  std::cout << "CPU time [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl;
+  std::cout << "Wall time [mu]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl;
+
+  // Running calc
+  for (unsigned int i = 0; i < T; ++i) {
+    clock_gettime(CLOCK_MONOTONIC, &start);
+    problem.calc(xs, us);
+    clock_gettime(CLOCK_MONOTONIC, &finish);
+    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
+    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    duration[i] = elapsed / 1000.;
+  }
+
+  avrg_duration = duration.sum() / T;
+  min_duration = duration.minCoeff();
+  max_duration = duration.maxCoeff();
+  std::cout << "Wall time calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+            << std::endl;
+
+  // Running calcDiff
+  for (unsigned int i = 0; i < T; ++i) {
+    clock_gettime(CLOCK_MONOTONIC, &start);
+    problem.calcDiff(xs, us);
+    clock_gettime(CLOCK_MONOTONIC, &finish);
+    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
+    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    duration[i] = elapsed / 1000.;
+  }
+
+  avrg_duration = duration.sum() / T;
+  min_duration = duration.minCoeff();
+  max_duration = duration.maxCoeff();
+  std::cout << "Wall time calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+            << std::endl;
 }
\ No newline at end of file
diff --git a/benchmark/unicycle.cpp b/benchmark/unicycle.cpp
index 1790494f8b3138592299edb8b5d41fbe22310774..92a726560e29b55110a93b8d98ea513d86d88117 100644
--- a/benchmark/unicycle.cpp
+++ b/benchmark/unicycle.cpp
@@ -39,9 +39,10 @@ int main() {
     cbs.push_back(new CallbackVerbose());
     ddp.setCallbacks(cbs);
   }
+
+  // Solving the optimal control problem
   struct timespec start, finish;
   double elapsed;
-  // Solving the optimal control problem
   Eigen::ArrayXd duration(T);
   for (unsigned int i = 0; i < T; ++i) {
     clock_gettime(CLOCK_MONOTONIC, &start);
@@ -49,28 +50,44 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &finish);
     elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
     elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
-
-    duration[i] = elapsed;  // in us
+    duration[i] = elapsed / 1000.;
   }
 
   double avrg_duration = duration.sum() / T;
   double min_duration = duration.minCoeff();
   double max_duration = duration.maxCoeff();
-  std::cout << "Wall time solve [us]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+  std::cout << "Wall time solve [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+            << std::endl;
+
+  // Running calc
+  for (unsigned int i = 0; i < T; ++i) {
+    clock_gettime(CLOCK_MONOTONIC, &start);
+    problem.calc(xs, us);
+    clock_gettime(CLOCK_MONOTONIC, &finish);
+    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
+    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    duration[i] = elapsed / 1000.;
+  }
+
+  avrg_duration = duration.sum() / T;
+  min_duration = duration.minCoeff();
+  max_duration = duration.maxCoeff();
+  std::cout << "Wall time calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
             << std::endl;
 
+  // Running calcDiff
   for (unsigned int i = 0; i < T; ++i) {
     clock_gettime(CLOCK_MONOTONIC, &start);
     problem.calcDiff(xs, us);
     clock_gettime(CLOCK_MONOTONIC, &finish);
     elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
     elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
-    duration[i] = elapsed;  // in us
+    duration[i] = elapsed / 1000.;
   }
 
   avrg_duration = duration.sum() / T;
   min_duration = duration.minCoeff();
   max_duration = duration.maxCoeff();
-  std::cout << "Wall time calcDiff [us]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+  std::cout << "Wall time calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
             << std::endl;
 }
diff --git a/src/core/optctrl/shooting.cpp b/src/core/optctrl/shooting.cpp
index 04ef09e48efbd933d2fdda3d1372e04ca541fde3..b3f185433e3f8ba32bbdba0ec290e15127341bf9 100644
--- a/src/core/optctrl/shooting.cpp
+++ b/src/core/optctrl/shooting.cpp
@@ -10,7 +10,7 @@
 #include <iostream>
 #ifdef WITH_MULTITHREADING
 #include <omp.h>
-#define NUM_THREADS NTHREADS
+#define NUM_THREADS WITH_NTHREADS
 #endif  // WITH_MULTITHREADING
 
 namespace crocoddyl {