diff --git a/.github/workflows/cmake-multi-platform.yml b/.github/workflows/cmake-multi-platform.yml
index c7a47a5..f36b7e0 100644
--- a/.github/workflows/cmake-multi-platform.yml
+++ b/.github/workflows/cmake-multi-platform.yml
@@ -15,44 +15,25 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-latest, windows-latest]
-        build_type: [Release]
-        c_compiler: [gcc, clang, cl]
         include:
-          - os: windows-latest
-            c_compiler: cl
-            cpp_compiler: cl
           - os: ubuntu-latest
-            c_compiler: gcc
-            cpp_compiler: g++
+            preset: ci-gcc
           - os: ubuntu-latest
-            c_compiler: clang
-            cpp_compiler: clang++
-        exclude:
-          - os: windows-latest
-            c_compiler: gcc
+            preset: ci-clang
           - os: windows-latest
-            c_compiler: clang
-          - os: ubuntu-latest
-            c_compiler: cl
+            preset: ci
 
     steps:
       - uses: actions/checkout@v4
 
       - name: Configure CMake
-        run: >
-          cmake -B ${{ github.workspace }}/build
-          -DCMAKE_CXX_COMPILER=${{ matrix.cpp_compiler }}
-          -DCMAKE_C_COMPILER=${{ matrix.c_compiler }}
-          -DCMAKE_BUILD_TYPE=${{ matrix.build_type }}
-          -S ${{ github.workspace }}
+        run: cmake --preset ${{ matrix.preset }}
 
       - name: Build
-        run: cmake --build ${{ github.workspace }}/build --config ${{ matrix.build_type }}
+        run: cmake --build --preset ${{ matrix.preset }}
 
       - name: Test
-        working-directory: ${{ github.workspace }}/build
-        run: ctest --build-config ${{ matrix.build_type }} --output-on-failure
+        run: ctest --preset ${{ matrix.preset }}
 
   # -------------------------------------------------------------------------
   # Install + consumer smoke test
diff --git a/CMakePresets.json b/CMakePresets.json
index 3fe81b6..37ca323 100644
--- a/CMakePresets.json
+++ b/CMakePresets.json
@@ -1,16 +1,19 @@
 {
   "version": 6,
   "configurePresets": [
+    {
+      "name": "base",
+      "hidden": true,
+      "cacheVariables": {
+        "BUILD_SHARED_LIBS": "OFF"
+      }
+    },
     {
       "name": "dev",
+      "inherits": "base",
       "displayName": "Developer build (Linux/macOS)",
       "description": "RelWithDebInfo, native tuning, compile-commands for tooling",
       "binaryDir": "${sourceDir}/build/${presetName}",
-      "condition": {
-        "type": "notEquals",
-        "lhs": "${hostSystemName}",
-        "rhs": "Windows"
-      },
       "cacheVariables": {
         "CMAKE_BUILD_TYPE": "RelWithDebInfo",
         "CMAKE_CXX_FLAGS": "-march=native",
@@ -19,6 +22,7 @@
     },
     {
       "name": "dev-msvc",
+      "inherits": "base",
       "displayName": "Developer build (Windows/MSVC)",
       "description": "RelWithDebInfo with AVX2, compile-commands for tooling",
       "binaryDir": "${sourceDir}/build/${presetName}",
@@ -35,13 +39,34 @@
     },
     {
       "name": "ci",
-      "displayName": "CI build",
+      "inherits": "base",
+      "displayName": "CI build (MSVC/default)",
       "description": "Release build without machine-specific tuning",
       "binaryDir": "${sourceDir}/build/${presetName}",
       "cacheVariables": {
         "CMAKE_BUILD_TYPE": "Release",
         "CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
       }
+    },
+    {
+      "name": "ci-gcc",
+      "inherits": "ci",
+      "displayName": "CI build (GCC)",
+      "binaryDir": "${sourceDir}/build/${presetName}",
+      "cacheVariables": {
+        "CMAKE_C_COMPILER": "gcc",
+        "CMAKE_CXX_COMPILER": "g++"
+      }
+    },
+    {
+      "name": "ci-clang",
+      "inherits": "ci",
+      "displayName": "CI build (Clang)",
+      "binaryDir": "${sourceDir}/build/${presetName}",
+      "cacheVariables": {
+        "CMAKE_C_COMPILER": "clang",
+        "CMAKE_CXX_COMPILER": "clang++"
+      }
     }
   ],
   "buildPresets": [
@@ -58,6 +83,42 @@
       "name": "ci",
       "configurePreset": "ci",
       "configuration": "Release"
+    },
+    {
+      "name": "ci-gcc",
+      "configurePreset": "ci-gcc",
+      "configuration": "Release"
+    },
+    {
+      "name": "ci-clang",
+      "configurePreset": "ci-clang",
+      "configuration": "Release"
+    }
+  ],
+  "testPresets": [
+    {
+      "name": "ci",
+      "configurePreset": "ci",
+      "configuration": "Release",
+      "output": {
+        "outputOnFailure": true
+      }
+    },
+    {
+      "name": "ci-gcc",
+      "configurePreset": "ci-gcc",
+      "configuration": "Release",
+      "output": {
+        "outputOnFailure": true
+      }
+    },
+    {
+      "name": "ci-clang",
+      "configurePreset": "ci-clang",
+      "configuration": "Release",
+      "output": {
+        "outputOnFailure": true
+      }
     }
   ]
-}
+}
\ No newline at end of file
diff --git a/blast/blast b/blast/blast
index 8c7e7f2..a4828b9 100644
--- a/blast/blast
+++ b/blast/blast
@@ -74,3 +74,7 @@ using i64 = int64_t;
 #include "blast_manipulator.hpp"
 #include "blast_optimization.hpp"
 #include "blast_trajectory.hpp"
+
+#include "utilities/file_io.hpp"
+#include "utilities/is_close.hpp"
+#include "utilities/print.hpp"
diff --git a/blast/blast_utilities.hpp b/blast/blast_utilities.hpp
index 30eec23..aa03b55 100644
--- a/blast/blast_utilities.hpp
+++ b/blast/blast_utilities.hpp
@@ -22,144 +22,11 @@
 #include "json.hpp"
 
 namespace blast {
-
-inline blast_fn void print(Vec3 v) {
-  printf("[% 0.4f, % 0.4f, % 0.4f]\n", v.x, v.y, v.z);
-}
-
-inline blast_fn void print(Mat3 m) {
-  printf("\n[% 0.4f, % 0.4f, % 0.4f]\n[% 0.4f, % 0.4f, % 0.4f]\n[% 0.4f, % 0.4f, % 0.4f]\n",
-         m(0, 0), m(0, 1), m(0, 2), m(1, 0), m(1, 1), m(1, 2), m(2, 0), m(2, 1), m(2, 2));
-}
-
-inline blast_fn void print(const Array& a) {
-  using namespace std;
-  if (a.size == 0)
-    return;
-  printf("[");
-  for (u32 i = 0; i < a.size - 1; i++)
-    printf("% 0.4f, ", a[i]);
-  printf("% 0.4f]\n", a[a.size - 1]);
-}
-
-inline blast_fn void print(const Matrix& m) {
-  if (m.size == 0)
-    return;
-  for (u32 i = 0; i < m.rows; i++) {
-    printf("[");
-    for (u32 j = 0; j < m.cols - 1; j++)
-      printf("% 0.4f, ", m(i, j));
-    printf("% 0.4f]\n", m(i, m.cols - 1));
-  }
-}
-
-inline blast_fn void print(const double* data, unsigned size) {
-  printf("[");
-  for (u32 i = 0; i < size - 1; i++)
-    printf("% 0.4f, ", data[i]);
-  printf("% 0.4f]\n", data[size - 1]);
-}
-
-inline blast_fn void print(const float* data, unsigned size) {
-  printf("[");
-  for (u32 i = 0; i < size - 1; i++)
-    printf("% 0.4f, ", data[i]);
-  printf("% 0.4f]\n", data[size - 1]);
-}
-
-inline blast_fn void print(const double* data, unsigned rows, unsigned cols) {
-  for (u32 i = 0; i < rows; i++) {
-    printf("[");
-    for (u32 j = 0; j < cols - 1; j++)
-      printf("% 0.4f, ", data[i + rows * j]);
-    printf("% 0.4f]\n", data[i + rows * (cols - 1)]);
-  }
-}
-
-inline blast_fn void print(const float* data, unsigned rows, unsigned cols) {
-  for (u32 i = 0; i < rows; i++) {
-    printf("[");
-    for (u32 j = 0; j < cols - 1; j++)
-      printf("% 0.4f, ", data[i + rows * j]);
-    printf("% 0.4f]\n", data[i + rows * (cols - 1)]);
-  }
-}
-
-inline host_fn void print_to_csv(const Matrix& m, const std::string& filename) {
-  std::ofstream file;
-  file.open(filename);
-  for (u32 i = 0; i < m.rows; i++) {
-    for (u32 j = 0; j < m.cols - 1; j++)
-      file << m(i, j) << ",";
-    file << m(i, m.cols - 1) << std::endl;
-  }
-  file.close();
-}
-
-inline host_fn void print_to_csv(const Trajectory& traj, const std::string& filename) {
-  std::ofstream file;
-  file.open(filename);
-
-  // print header
-  {
-    file << "t";
-    for (u32 i = 0; i < traj.pos.rows; i++)
-      file << ",p[" << i << "]";
-    for (u32 i = 0; i < traj.vel.rows; i++)
-      file << ",v[" << i << "]";
-    for (u32 i = 0; i < traj.acc.rows; i++)
-      file << ",a[" << i << "]";
-    file << std::endl;
-  }
-
-  // print data
-  for (u32 i = 0; i < traj.t.size; i++) {
-    file << traj.t[i];
-    for (u32 j = 0; j < traj.pos.rows; j++)
-      file << "," << traj.pos(j, i);
-    for (u32 j = 0; j < traj.vel.rows; j++)
-      file << "," << traj.vel(j, i);
-    for (u32 j = 0; j < traj.acc.rows; j++)
-      file << "," << traj.acc(j, i);
-    file << std::endl;
-  }
-  file.close();
-}
-
-inline host_fn void print_to_csv(const std::vector<Trajectory>& traj, const std::string& filename) {
-  std::ofstream file;
-  file.open(filename);
-
-  real start_time = 0.0;
-
-  file << traj[0].t[0];
-  for (u32 j = 0; j < traj[0].pos.rows; j++)
-    file << "," << traj[0].pos(j, 0);
-  for (u32 j = 0; j < traj[0].vel.rows; j++)
-    file << "," << traj[0].vel(j, 0);
-  for (u32 j = 0; j < traj[0].acc.rows; j++)
-    file << "," << traj[0].acc(j, 0);
-  file << std::endl;
-
-  for (int task_id = 0; task_id < traj.size(); task_id++) {
-    // int points_more = (int) std::ceil(res[task_id].x.back() * 1000.0) + 1;
-
-
-    for (u32 i = 1; i < traj[task_id].t.size; i++) {
-      file << traj[task_id].t[i] + start_time;
-      for (u32 j = 0; j < traj[task_id].pos.rows; j++)
-        file << "," << traj[task_id].pos(j, i);
-      for (u32 j = 0; j < traj[task_id].vel.rows; j++)
-        file << "," << traj[task_id].vel(j, i);
-      for (u32 j = 0; j < traj[task_id].acc.rows; j++)
-        file << "," << traj[task_id].acc(j, i);
-      file << std::endl;
-    }
-
-    start_time += traj[task_id].t[traj[task_id].t.size - 1];
-  }
-  file.close();
-}
+struct ConstraintSelection;
+struct Objective;
+struct Guess;
+struct Optimization;
+struct Result;
 
 inline host_fn int64_t get_tick_us() {
 #if defined(_MSC_VER)
@@ -174,189 +41,84 @@ inline host_fn int64_t get_tick_us() {
 #endif
 }
 
-inline host_fn blast::Matrix read_csv_matrix(const std::string& filename, const char* csv_sep = ",") {
-  std::ifstream file(filename);
-  std::cout << "Reading from file: " << filename << std::endl;
-  if (!file.is_open()) {
-    Assert(false);
-    std::cerr << "ERROR: File is unavailable" << std::endl;
-    return blast::Matrix(0, 0);
-  }
-
-  std::string line;
-  u32         num_rows = 0, num_cols = 0;
-
-  std::getline(file, line); // discard first line
-  // Determine number of rows and columns
-  if (std::getline(file, line)) {
-    num_cols = std::count(line.begin(), line.end(), *csv_sep) + 1;
-    num_rows++;
-  }
-  while (std::getline(file, line)) {
-    num_rows++;
-  }
-
-  // Rewind file to read again
-  file.clear();
-  file.seekg(0);
-
-  // Create matrix with detected dimensions
-  blast::Matrix pos(num_rows, num_cols);
-
-  std::getline(file, line); // discard first line
-  // Read the data into the matrix
-  for (u32 i = 0; i < num_rows; i++) {
-    std::getline(file, line);
-    std::istringstream iss(line);
-    for (u32 j = 0; j < num_cols; j++) {
-      std::string value;
-      if (std::getline(iss, value, *csv_sep)) {
-        pos(i, j) = std::stod(value);
-      }
-    }
-  }
-  return pos;
-}
+inline blast_fn void print(Vec3 v);
 
-inline host_fn blast::Matrix read_csv_matrix_no_header(const std::string& filename, const char* csv_sep = ",") {
-  std::ifstream file(filename);
-  std::cout << "Reading from file: " << filename << std::endl;
-  if (!file.is_open()) {
-    Assert(false);
-    std::cerr << "ERROR: File is unavailable" << std::endl;
-    return blast::Matrix(0, 0);
-  }
-
-  std::string line;
-  u32         num_rows = 0, num_cols = 0;
-
-  // Determine number of rows and columns
-  if (std::getline(file, line)) {
-    num_cols = std::count(line.begin(), line.end(), *csv_sep) + 1;
-    num_rows++;
-  }
-  while (std::getline(file, line)) {
-    num_rows++;
-  }
-
-  // Rewind file to read again
-  file.clear();
-  file.seekg(0);
-
-  // Create matrix with detected dimensions
-  blast::Matrix pos(num_rows, num_cols);
-
-  // Read the data into the matrix
-  for (u32 i = 0; i < num_rows; i++) {
-    std::getline(file, line);
-    std::istringstream iss(line);
-    for (u32 j = 0; j < num_cols; j++) {
-      std::string value;
-      if (std::getline(iss, value, *csv_sep)) {
-        pos(i, j) = std::stod(value);
-      }
-    }
-  }
-
-  return pos;
-}
+inline blast_fn void print(Mat3 m);
 
-inline host_fn Trajectory read_csv_trajectory_no_header(const std::string& filename, const char* csv_sep = ",") {
-  std::ifstream file(filename);
-  std::cout << "Reading from file: " << filename << std::endl;
-  if (!file.is_open()) {
-    Assert(false);
-    std::cerr << "ERROR: File is unavailable" << std::endl;
-    return {0, 0};
-  }
-
-  std::string line;
-  u32         num_rows = 0, num_cols = 0;
-
-  // Determine number of rows and columns
-  if (std::getline(file, line)) {
-    num_cols = std::count(line.begin(), line.end(), *csv_sep) + 1;
-    num_rows++;
-  }
-  while (std::getline(file, line)) {
-    num_rows++;
-  }
-
-  // Rewind file to read again
-  file.clear();
-  file.seekg(0);
-
-  u32        num_joints = (num_cols - 1) / 3;
-  Trajectory trajectory(num_rows, num_joints);
-
-  // Read the data into the matrix
-  for (u32 i = 0; i < num_rows; i++) {
-    std::getline(file, line);
-    std::istringstream iss(line);
-    u32                col = 0;
-
-    // Gets time
-    {
-      std::string value;
-      if (std::getline(iss, value, *csv_sep))
-        trajectory.t[i] = std::stod(value);
-    }
-
-    // Gets position
-    for (col = 0; col < num_joints; col++) {
-      std::string value;
-      if (std::getline(iss, value, *csv_sep)) {
-        trajectory.pos(col, i) = std::stod(value);
-      }
-    }
-
-    // Gets velocity
-    for (col = 0; col < num_joints; col++) {
-      std::string value;
-      if (std::getline(iss, value, *csv_sep)) {
-        trajectory.vel(col, i) = std::stod(value);
-      }
-    }
-
-    // Gets acceleration
-    for (col = 0; col < num_joints; col++) {
-      std::string value;
-      if (std::getline(iss, value, *csv_sep)) {
-        trajectory.acc(col, i) = std::stod(value);
-      }
-    }
-  }
-
-  return trajectory;
-}
+inline blast_fn void print(const Array& a);
 
-// note: CUDA stuff, only enabled if compiling for Nvidia GPUs
-#ifdef __NVCC__
-// print the properties of all connected GPUs
-inline host_fn void print_device_properties() {
-  int nDevices;
-  cudaGetDeviceCount(&nDevices);
-  for (int i = 0; i < nDevices; i++) {
-    cudaDeviceProp prop;
-    cudaGetDeviceProperties(&prop, i);
-    printf("Device Number: ............................... %d\n", i);
-    printf("  Device name: ............................... %s\n", prop.name);
-    printf("  Multiprocessors: ........................... %d\n", prop.multiProcessorCount);
-    printf("  Threads per multiprocessor: ................ %d\n", prop.maxThreadsPerMultiProcessor);
-    printf("  Threads per block: ......................... %d\n", prop.maxThreadsPerBlock);
-    printf("  Async Engine Count: ........................ %d\n", prop.asyncEngineCount);
-    printf("  Registers per block: ....................... %d\n", prop.regsPerBlock);
-    printf("  Registers per multiprocessor: .............. %d\n", prop.regsPerMultiprocessor);
-    printf("  Shared memory per block (KB): .............. %f\n", prop.sharedMemPerBlock / 1024.0);
-    printf("  Shared memory per multiprocessor (KB): ..... %f\n", prop.sharedMemPerMultiprocessor / 1024.0);
-    printf("  Concurrent Kernels: ........................ %d\n", prop.concurrentKernels);
-    printf("  Clock Rate (KHz): .......................... %d\n", prop.clockRate);
-    printf("  Memory Clock Rate (KHz): ................... %d\n", prop.memoryClockRate);
-    printf("  Memory Bus Width (bits): ................... %d\n", prop.memoryBusWidth);
-    printf("  Peak Memory Bandwidth (GB/s): .............. %f\n", 2.0 * prop.memoryClockRate * (prop.memoryBusWidth / 8) / 1.0e6);
-    printf("  Compute capabilities : ..................... %d,%d\n", prop.major, prop.minor);
-  }
-}
-#endif
+inline blast_fn void print(const Matrix& m);
+
+inline blast_fn void print(const double* data, unsigned size);
+
+inline blast_fn void print(const float* data, unsigned size);
+
+inline blast_fn void print(const double* data, unsigned rows, unsigned cols);
+
+inline blast_fn void print(const float* data, unsigned rows, unsigned cols);
+
+inline host_fn void print_to_csv(const Matrix& m, const std::string& filename);
+
+inline host_fn void print_to_csv(const Trajectory& traj, const std::string& filename);
+
+inline host_fn void print_to_csv(const std::vector<Trajectory>& traj, const std::string& filename);
+
+inline host_fn blast::Matrix read_csv_matrix(const std::string& filename, const char* csv_sep);
+
+inline host_fn blast::Matrix read_csv_matrix_no_header(const std::string& filename, const char* csv_sep);
+
+inline host_fn Trajectory read_csv_trajectory_no_header(const std::string& filename, const char* csv_sep);
+
+template<typename T>
+host_fn bool is_close(const T type1, const T type2, real eps = 1e-5);
+
+template<typename T, std::size_t N>
+host_fn bool is_close(const std::array<T, N>& a1, const std::array<T, N>& a2, real eps = 1e-5);
+
+template<typename T>
+host_fn bool is_close(const ObjMatrix<T>& a1, const ObjMatrix<T>& a2, real eps = 1e-5);
+
+template<typename T>
+host_fn bool is_close(const std::vector<T>& a1, const std::vector<T>& a2, real eps = 1e-5);
+
+// note: Does not use eps = 1e-5 but necessary for consistency for usability with templates
+inline blast_fn bool is_close(const u8 a1, const u8& a2, real eps = 1e-5);
+
+// note: Does not use eps = 1e-5 but necessary for consistency for usability with templates
+inline blast_fn bool is_close(u32 a1, u32 a2, real eps = 1e-5);
+
+inline host_fn bool is_close(real r1, real r2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Box& box1, const Box& box2, real eps = 1e-5);
+
+inline host_fn bool is_close(const DynamicBox& box1, const DynamicBox& box2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Sphere& sph1, const Sphere& sph2, real eps = 1e-5);
+
+inline host_fn bool is_close(const DynamicSphere& sph1, const DynamicSphere& sph2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Capsule& capsule1, const Capsule& capsule2, real eps = 1e-5);
+
+inline host_fn bool is_close(const DynamicCapsule& capsule1, const DynamicCapsule& capsule2, real eps = 1e-5);
+
+inline host_fn bool is_close(const World& world1, const World& world2, real eps = 1e-5);
+
+inline host_fn bool is_close(const CollisionModelCapsule& capsule1, const CollisionModelCapsule& capsule2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Manipulator& manip1, const Manipulator& manip2, real eps = 1e-5);
+
+inline host_fn bool is_close(const ManipulatorTempData& manip_data1, const ManipulatorTempData& manip_data2, const u32 n_joints, const u32 n_caps, real eps = 1e-5);
+
+inline host_fn bool is_close(const Bspline& spline1, const Bspline& spline2, real eps = 1e-5);
+
+inline host_fn bool is_close(const ConstraintSelection& constraints1, const ConstraintSelection& constraints2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Objective& objective1, const Objective& objective2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Guess& guess1, const Guess& guess2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Optimization& opt1, const Optimization& opt2, real eps = 1e-5);
+
+inline host_fn bool is_close(const Result& result1, Result& result2, real eps = 1e-5);
 
 } // namespace blast
diff --git a/blast/blast_world.hpp b/blast/blast_world.hpp
index a7eb6ab..bf67f9b 100644
--- a/blast/blast_world.hpp
+++ b/blast/blast_world.hpp
@@ -125,3 +125,5 @@ inline blast_fn Vec3 get_point(const Array& x, const Matrix& capsule_list);
 #include "world/dynamicsphere.hpp"
 
 #include "world/CoDO.hpp"
+
+#include "world/scenes.hpp"
diff --git a/blast/manipulator/Kinova_Gen3.hpp b/blast/manipulator/Kinova_Gen3.hpp
index 7883619..f2b5854 100644
--- a/blast/manipulator/Kinova_Gen3.hpp
+++ b/blast/manipulator/Kinova_Gen3.hpp
@@ -86,6 +86,9 @@ inline Manipulator make_Kinova_Gen3() {
   collisions.collision_matrix.resize(3, 3);
   collisions.collision_matrix(2, 0) = 1;
 
+  // mirrored just for consistency, technically not used
+  collisions.collision_matrix(0, 2) = 1;
+
   // Collision model
   CollisionModelCapsule model_caps;
 
@@ -115,4 +118,8 @@ inline Manipulator make_Kinova_Gen3() {
   return gen3;
 }
 
+inline host_fn Array get_gen3_home() {
+  return wrap2pi(deg2rad(Array({0, 15, 180, -130, 0, 55, 90})));
+}
+
 } // namespace blast
diff --git a/blast/manipulator/UR5e.hpp b/blast/manipulator/UR5e.hpp
index 95fb729..38244b6 100644
--- a/blast/manipulator/UR5e.hpp
+++ b/blast/manipulator/UR5e.hpp
@@ -22,8 +22,8 @@ inline Manipulator make_UR5e() {
           Vec3{0, 0, 0},
           {-0.425f, 0, 0},
           {-0.3922f, 0, 0.1333f},
-          {0, -0.0997f, -0},
-          {0, 0.0996f, -0},
+          {0, -0.0997f, 0},
+          {0, 0.0996f, 0},
           {0, 0, 0}}; // vector to next joint
   kinematics.joint_axes = {
           Vec3{0, 0, 1},
@@ -32,12 +32,14 @@ inline Manipulator make_UR5e() {
           {0, 0, 1},
           {0, 0, 1},
           {0, 0, 1}}; // direction vectors of joint
-  kinematics.static_rotations[0]  = {-1, 0, 0, -0, -1, 0, 0, -0, 1};
-  kinematics.static_rotations[1]  = {1, 0, 0, -0, -0, 1, 0, -1, -0};
-  kinematics.static_rotations[2]  = {1, 0, 0, -0, 1, 0, 0, -0, 1};
-  kinematics.static_rotations[3]  = {1, 0, 0, -0, 1, 0, 0, -0, 1};
-  kinematics.static_rotations[4]  = {1, 0, 0, -0, -0, 1, 0, -1, -0};
-  kinematics.static_rotations[5]  = {1, -0, 0, 0, -0, -1, 0, 1, -0};
+  kinematics.base_position        = {0.0, 0.0, 0.0};
+  kinematics.base_rotation        = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+  kinematics.static_rotations[0]  = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+  kinematics.static_rotations[1]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
+  kinematics.static_rotations[2]  = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+  kinematics.static_rotations[3]  = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+  kinematics.static_rotations[4]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
+  kinematics.static_rotations[5]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
   kinematics.first_joint_position = {0, 0, 0.162500f};
 
   // dynamic properties
diff --git a/blast/manipulator/manipulator.hpp b/blast/manipulator/manipulator.hpp
index 88fd663..40f292a 100644
--- a/blast/manipulator/manipulator.hpp
+++ b/blast/manipulator/manipulator.hpp
@@ -316,14 +316,6 @@ inline Array get_internal_collisions(const Manipulator& manip, const Manipulator
   return distances;
 }
 
-// inline host_fn Array Manipulator::get_tool_pose() const {
-//   Mat3 tmp             = _rotations_mult.back();
-//   Vec3 current_rpy     = rotation2rpy(tmp);
-//   Vec3 current_cartpos = _p_j.back();
-//   return {current_cartpos.x, current_cartpos.y, current_cartpos.z,
-//           current_rpy.x, current_rpy.y, current_rpy.z};
-// }
-
 inline host_fn void Manipulator::add_tool(const Tool& tool) {
   tool_offset         = tool.tool_offset;
   tool_rotation       = tool.tool_rotation;
@@ -332,37 +324,37 @@ inline host_fn void Manipulator::add_tool(const Tool& tool) {
   tool_cog_offset     = tool.cog_offset;
   has_tool            = true;
 
-  joint_offsets.back() += tool_rotation * tool.tool_offset;
-  static_rotations.back() *= tool_rotation;
-  real m_new    = link_masses.back() + tool.mass;
-  Vec3 av_new   = (link_masses.back() * cog_offsets.back() + tool.mass * tool.cog_offset) / m_new;
-  Vec3 delta_av = av_new - cog_offsets.back();
+  joint_offsets[n_joints - 1] += tool_rotation * tool.tool_offset;
+  static_rotations[n_joints - 1] *= tool_rotation;
+  real m_new    = link_masses[n_joints - 1] + tool.mass;
+  Vec3 av_new   = (link_masses[n_joints - 1] * cog_offsets[n_joints - 1] + tool.mass * tool.cog_offset) / m_new;
+  Vec3 delta_av = av_new - cog_offsets[n_joints - 1];
 
-  inertia_tensors.back()(0, 0) += link_masses.back() * delta_av.x * delta_av.x + tool.mass * tool.cog_offset.x * tool.cog_offset.x;
-  inertia_tensors.back()(1, 1) += link_masses.back() * delta_av.y * delta_av.y + tool.mass * tool.cog_offset.y * tool.cog_offset.y;
-  inertia_tensors.back()(2, 2) += link_masses.back() * delta_av.z * delta_av.z + tool.mass * tool.cog_offset.z * tool.cog_offset.z;
-  inertia_tensors.back() += tool.inertia_tensor;
+  inertia_tensors[n_joints - 1](0, 0) += link_masses[n_joints - 1] * delta_av.x * delta_av.x + tool.mass * tool.cog_offset.x * tool.cog_offset.x;
+  inertia_tensors[n_joints - 1](1, 1) += link_masses[n_joints - 1] * delta_av.y * delta_av.y + tool.mass * tool.cog_offset.y * tool.cog_offset.y;
+  inertia_tensors[n_joints - 1](2, 2) += link_masses[n_joints - 1] * delta_av.z * delta_av.z + tool.mass * tool.cog_offset.z * tool.cog_offset.z;
+  inertia_tensors[n_joints - 1] += tool.inertia_tensor;
 
-  link_masses.back()         = m_new;
-  cog_offsets.back()         = av_new;
-  cog_from_next_joint.back() = {-joint_offsets.back() + cog_offsets.back()};
+  link_masses[n_joints - 1]         = m_new;
+  cog_offsets[n_joints - 1]         = av_new;
+  cog_from_next_joint[n_joints - 1] = {-joint_offsets[n_joints - 1] + cog_offsets[n_joints - 1]};
 }
 
 inline host_fn void Manipulator::set_payload(real m_payload, Vec3 cg_payload, Mat3 I_payload) {
   Vec3 av_payload = cg_payload;
-  real m_new      = link_masses.back() + m_payload;
-  Vec3 av_new     = (link_masses.back() * cog_offsets.back() + m_payload * av_payload) / m_new;
-  Vec3 delta_av   = av_new - cog_offsets.back();
+  real m_new      = link_masses[n_joints - 1] + m_payload;
+  Vec3 av_new     = (link_masses[n_joints - 1] * cog_offsets[n_joints - 1] + m_payload * av_payload) / m_new;
+  Vec3 delta_av   = av_new - cog_offsets[n_joints - 1];
   Vec3 av_to_mass = av_payload - av_new;
 
-  inertia_tensors.back()(0, 0) += link_masses.back() * delta_av.x * delta_av.x + m_payload * av_to_mass.x * av_to_mass.x;
-  inertia_tensors.back()(1, 1) += link_masses.back() * delta_av.y * delta_av.y + m_payload * av_to_mass.y * av_to_mass.y;
-  inertia_tensors.back()(2, 2) += link_masses.back() * delta_av.z * delta_av.z + m_payload * av_to_mass.z * av_to_mass.z;
-  inertia_tensors.back() += I_payload;
+  inertia_tensors[n_joints - 1](0, 0) += link_masses[n_joints - 1] * delta_av.x * delta_av.x + m_payload * av_to_mass.x * av_to_mass.x;
+  inertia_tensors[n_joints - 1](1, 1) += link_masses[n_joints - 1] * delta_av.y * delta_av.y + m_payload * av_to_mass.y * av_to_mass.y;
+  inertia_tensors[n_joints - 1](2, 2) += link_masses[n_joints - 1] * delta_av.z * delta_av.z + m_payload * av_to_mass.z * av_to_mass.z;
+  inertia_tensors[n_joints - 1] += I_payload;
 
-  link_masses.back()         = m_new;
-  cog_offsets.back()         = av_new;
-  cog_from_next_joint.back() = {-joint_offsets.back() + cog_offsets.back()};
+  link_masses[n_joints - 1]         = m_new;
+  cog_offsets[n_joints - 1]         = av_new;
+  cog_from_next_joint[n_joints - 1] = {-joint_offsets[n_joints - 1] + cog_offsets[n_joints - 1]};
 }
 
 inline host_fn real clamped_root(real slope, real h0, real h1) {
diff --git a/blast/math/misc.hpp b/blast/math/misc.hpp
index 596a559..a78ed5f 100644
--- a/blast/math/misc.hpp
+++ b/blast/math/misc.hpp
@@ -160,6 +160,18 @@ inline host_fn real get_random() {
   static thread_local std::uniform_real_distribution<blast::real> dis(-1, 1);
   return dis(e2);
 }
+
+// todo: make thread safe?
+inline host_fn u32 random_int(u32 min, u32 max) {
+  // Create a static random number generator (seeded once)
+  static std::random_device rd;
+  static std::mt19937       gen(rd());
+
+  // Create a uniform distribution for integers in the given range
+  std::uniform_int_distribution<u32> dist(min, max);
+
+  return dist(gen);
+}
 #endif
 
 } // namespace blast
diff --git a/blast/optimization/constraints.hpp b/blast/optimization/constraints.hpp
index 5187d57..9ff9fe0 100644
--- a/blast/optimization/constraints.hpp
+++ b/blast/optimization/constraints.hpp
@@ -34,30 +34,6 @@ inline blast_fn real abs_constraint(const real& value, const real& value_max) {
   return std::abs(value) / value_max - 1.0;
 }
 
-// Derivative variants — return {constraint_value, d_constraint/d_value}.
-// When is_grad==false the gradient coefficient is 0 (unused).
-template<bool is_grad>
-inline blast_fn std::pair<real, real> abs_constraint_dev(const real& value, const real& value_max) {
-  const real constraint = std::abs(value) / value_max - 1.0f;
-  if constexpr (is_grad) {
-    return {constraint, (value >= 0.0f ? 1.0f : -1.0f) / value_max};
-  } else {
-    return {constraint, 0.0f};
-  }
-}
-
-template<bool is_grad>
-inline blast_fn std::pair<real, real> bound_constraint_dev(const real& value, const real& value_min, const real& value_max) {
-  const real center     = (value_max + value_min) / 2.0f;
-  const real range      = value_max - value_min;
-  const real constraint = 2.0f * std::abs(value - center) / range - 1.0f;
-  if constexpr (is_grad) {
-    return {constraint, 2.0f * (value >= center ? 1.0f : -1.0f) / range};
-  } else {
-    return {constraint, 0.0f};
-  }
-}
-
 inline blast_fn Matrix get_J_tool(const Optimization* opt, const ManipulatorTempData& temp) {
   std::vector<Vec3> r_tool(opt->manip.n_joints);
   r_tool[opt->manip.n_joints - 1] = opt->manip.joint_offsets[opt->manip.n_joints - 1];
@@ -1419,7 +1395,7 @@ blast_fn void compute_constraints_with_analytical_dynamics(double* result, Array
 
   // Lambda to process common bound constraint operations
   auto process_bound = [&](real value, real bound_max) {
-    auto [constraint, gradient_coeff] = abs_constraint_dev<is_grad>(value, bound_max);
+    auto [constraint, gradient_coeff] = abs_constraint_analytical<is_grad>(value, bound_max);
     *moving_result++                  = constraint;
     gradient_coeffs[grad_idx++]       = gradient_coeff;
   };
@@ -1466,7 +1442,7 @@ blast_fn void compute_constraints_with_analytical_dynamics(double* result, Array
           ZoneScopedN("Position");
 #endif
           for (int j = 0; j < joints; j++) {
-            auto [constraint, gradient_coeff] = bound_constraint_dev<is_grad>(opt->bspline.traj.pos(j, i), opt->manip.position_min[j], opt->manip.position_max[j]);
+            auto [constraint, gradient_coeff] = bound_constraint_analytical<is_grad>(opt->bspline.traj.pos(j, i), opt->manip.position_min[j], opt->manip.position_max[j]);
             *moving_result++                  = constraint;
             gradient_coeffs[grad_idx++]       = gradient_coeff;
           }
diff --git a/blast/optimization/optimization.hpp b/blast/optimization/optimization.hpp
index e8426ea..e9499b8 100644
--- a/blast/optimization/optimization.hpp
+++ b/blast/optimization/optimization.hpp
@@ -1,6 +1,5 @@
 #pragma once
 
-
 #include <blast>
 #include <utility>
 #ifdef BLAST_USE_NATIVE_SQP
@@ -45,9 +44,8 @@ inline Optimization::Optimization(const Manipulator& new_manip, const Task& new_
     task(new_task.to_matrix()),
     custom_data(nullptr) {
   // Default values
-  method               = OptimizationMethod::with_segments;
-  guess.type           = Guess::random;
-  guess.n_random_shots = 100;
+  method     = OptimizationMethod::with_segments;
+  guess.type = Guess::random;
 
   constraints.position     = true;
   constraints.velocity     = true;
diff --git a/blast/utilities/file_io.hpp b/blast/utilities/file_io.hpp
new file mode 100644
index 0000000..fe2483a
--- /dev/null
+++ b/blast/utilities/file_io.hpp
@@ -0,0 +1,237 @@
+#pragma once
+#include <blast>
+
+namespace blast {
+inline host_fn void print_to_csv(const Matrix& m, const std::string& filename) {
+  std::ofstream file;
+  file.open(filename);
+  for (u32 i = 0; i < m.rows; i++) {
+    for (u32 j = 0; j < m.cols - 1; j++)
+      file << m(i, j) << ",";
+    file << m(i, m.cols - 1) << std::endl;
+  }
+  file.close();
+}
+
+inline host_fn void print_to_csv(const Trajectory& traj, const std::string& filename) {
+  std::ofstream file;
+  file.open(filename);
+
+  // print header
+  {
+    file << "t";
+    for (u32 i = 0; i < traj.pos.rows; i++)
+      file << ",p[" << i << "]";
+    for (u32 i = 0; i < traj.vel.rows; i++)
+      file << ",v[" << i << "]";
+    for (u32 i = 0; i < traj.acc.rows; i++)
+      file << ",a[" << i << "]";
+    file << std::endl;
+  }
+
+  // print data
+  for (u32 i = 0; i < traj.t.size; i++) {
+    file << traj.t[i];
+    for (u32 j = 0; j < traj.pos.rows; j++)
+      file << "," << traj.pos(j, i);
+    for (u32 j = 0; j < traj.vel.rows; j++)
+      file << "," << traj.vel(j, i);
+    for (u32 j = 0; j < traj.acc.rows; j++)
+      file << "," << traj.acc(j, i);
+    file << std::endl;
+  }
+  file.close();
+}
+
+inline host_fn void print_to_csv(const std::vector<Trajectory>& traj, const std::string& filename) {
+  std::ofstream file;
+  file.open(filename);
+
+  real start_time = 0.0;
+
+  file << traj[0].t[0];
+  for (u32 j = 0; j < traj[0].pos.rows; j++)
+    file << "," << traj[0].pos(j, 0);
+  for (u32 j = 0; j < traj[0].vel.rows; j++)
+    file << "," << traj[0].vel(j, 0);
+  for (u32 j = 0; j < traj[0].acc.rows; j++)
+    file << "," << traj[0].acc(j, 0);
+  file << std::endl;
+
+  for (int task_id = 0; task_id < traj.size(); task_id++) {
+    // int points_more = (int) std::ceil(res[task_id].x.back() * 1000.0) + 1;
+
+
+    for (u32 i = 1; i < traj[task_id].t.size; i++) {
+      file << traj[task_id].t[i] + start_time;
+      for (u32 j = 0; j < traj[task_id].pos.rows; j++)
+        file << "," << traj[task_id].pos(j, i);
+      for (u32 j = 0; j < traj[task_id].vel.rows; j++)
+        file << "," << traj[task_id].vel(j, i);
+      for (u32 j = 0; j < traj[task_id].acc.rows; j++)
+        file << "," << traj[task_id].acc(j, i);
+      file << std::endl;
+    }
+
+    start_time += traj[task_id].t[traj[task_id].t.size - 1];
+  }
+  file.close();
+}
+
+inline host_fn blast::Matrix read_csv_matrix(const std::string& filename, const char* csv_sep = ",") {
+  std::ifstream file(filename);
+  std::cout << "Reading from file: " << filename << std::endl;
+  if (!file.is_open()) {
+    Assert(false);
+    std::cerr << "ERROR: File is unavailable" << std::endl;
+    return blast::Matrix(0, 0);
+  }
+
+  std::string line;
+  u32         num_rows = 0, num_cols = 0;
+
+  std::getline(file, line); // discard first line
+  // Determine number of rows and columns
+  if (std::getline(file, line)) {
+    num_cols = std::count(line.begin(), line.end(), *csv_sep) + 1;
+    num_rows++;
+  }
+  while (std::getline(file, line)) {
+    num_rows++;
+  }
+
+  // Rewind file to read again
+  file.clear();
+  file.seekg(0);
+
+  // Create matrix with detected dimensions
+  blast::Matrix pos(num_rows, num_cols);
+
+  std::getline(file, line); // discard first line
+  // Read the data into the matrix
+  for (u32 i = 0; i < num_rows; i++) {
+    std::getline(file, line);
+    std::istringstream iss(line);
+    for (u32 j = 0; j < num_cols; j++) {
+      std::string value;
+      if (std::getline(iss, value, *csv_sep)) {
+        pos(i, j) = std::stod(value);
+      }
+    }
+  }
+  return pos;
+}
+
+inline host_fn blast::Matrix read_csv_matrix_no_header(const std::string& filename, const char* csv_sep = ",") {
+  std::ifstream file(filename);
+  std::cout << "Reading from file: " << filename << std::endl;
+  if (!file.is_open()) {
+    Assert(false);
+    std::cerr << "ERROR: File is unavailable" << std::endl;
+    return blast::Matrix(0, 0);
+  }
+
+  std::string line;
+  u32         num_rows = 0, num_cols = 0;
+
+  // Determine number of rows and columns
+  if (std::getline(file, line)) {
+    num_cols = std::count(line.begin(), line.end(), *csv_sep) + 1;
+    num_rows++;
+  }
+  while (std::getline(file, line)) {
+    num_rows++;
+  }
+
+  // Rewind file to read again
+  file.clear();
+  file.seekg(0);
+
+  // Create matrix with detected dimensions
+  blast::Matrix pos(num_rows, num_cols);
+
+  // Read the data into the matrix
+  for (u32 i = 0; i < num_rows; i++) {
+    std::getline(file, line);
+    std::istringstream iss(line);
+    for (u32 j = 0; j < num_cols; j++) {
+      std::string value;
+      if (std::getline(iss, value, *csv_sep)) {
+        pos(i, j) = std::stod(value);
+      }
+    }
+  }
+
+  return pos;
+}
+
+inline host_fn Trajectory read_csv_trajectory_no_header(const std::string& filename, const char* csv_sep = ",") {
+  std::ifstream file(filename);
+  std::cout << "Reading from file: " << filename << std::endl;
+  if (!file.is_open()) {
+    Assert(false);
+    std::cerr << "ERROR: File is unavailable" << std::endl;
+    return {0, 0};
+  }
+
+  std::string line;
+  u32         num_rows = 0, num_cols = 0;
+
+  // Determine number of rows and columns
+  if (std::getline(file, line)) {
+    num_cols = std::count(line.begin(), line.end(), *csv_sep) + 1;
+    num_rows++;
+  }
+  while (std::getline(file, line)) {
+    num_rows++;
+  }
+
+  // Rewind file to read again
+  file.clear();
+  file.seekg(0);
+
+  u32        num_joints = (num_cols - 1) / 3;
+  Trajectory trajectory(num_rows, num_joints);
+
+  // Read the data into the matrix
+  for (u32 i = 0; i < num_rows; i++) {
+    std::getline(file, line);
+    std::istringstream iss(line);
+    u32                col = 0;
+
+    // Gets time
+    {
+      std::string value;
+      if (std::getline(iss, value, *csv_sep))
+        trajectory.t[i] = std::stod(value);
+    }
+
+    // Gets position
+    for (col = 0; col < num_joints; col++) {
+      std::string value;
+      if (std::getline(iss, value, *csv_sep)) {
+        trajectory.pos(col, i) = std::stod(value);
+      }
+    }
+
+    // Gets velocity
+    for (col = 0; col < num_joints; col++) {
+      std::string value;
+      if (std::getline(iss, value, *csv_sep)) {
+        trajectory.vel(col, i) = std::stod(value);
+      }
+    }
+
+    // Gets acceleration
+    for (col = 0; col < num_joints; col++) {
+      std::string value;
+      if (std::getline(iss, value, *csv_sep)) {
+        trajectory.acc(col, i) = std::stod(value);
+      }
+    }
+  }
+
+  return trajectory;
+}
+
+} // namespace blast
diff --git a/tests/test_helper/test_functions.hpp b/blast/utilities/is_close.hpp
similarity index 87%
rename from tests/test_helper/test_functions.hpp
rename to blast/utilities/is_close.hpp
index e8fceac..6473e3f 100644
--- a/tests/test_helper/test_functions.hpp
+++ b/blast/utilities/is_close.hpp
@@ -4,7 +4,7 @@
 
 namespace blast {
 template<typename T>
-host_fn bool is_close(const T type1, const T type2, real eps = 1e-5) {
+host_fn bool is_close(const T type1, const T type2, real eps) {
   if (type1 != type2)
     return false;
   if (!is_close(type1, type2, eps))
@@ -14,7 +14,7 @@ host_fn bool is_close(const T type1, const T type2, real eps = 1e-5) {
 }
 
 template<typename T, std::size_t N>
-host_fn bool is_close(const std::array<T, N>& a1, const std::array<T, N>& a2, real eps = 1e-5) {
+host_fn bool is_close(const std::array<T, N>& a1, const std::array<T, N>& a2, real eps) {
   for (u32 i = 0; i < N; i++)
     if (!is_close(a1[i], a2[i], eps))
       return false;
@@ -23,7 +23,7 @@ host_fn bool is_close(const std::array<T, N>& a1, const std::array<T, N>& a2, re
 }
 
 template<typename T>
-host_fn bool is_close(const ObjMatrix<T>& a1, const ObjMatrix<T>& a2, real eps = 1e-5) {
+host_fn bool is_close(const ObjMatrix<T>& a1, const ObjMatrix<T>& a2, real eps) {
   if (a1.size != a2.size) {
     Assert(false);
     return false;
@@ -32,7 +32,7 @@ host_fn bool is_close(const ObjMatrix<T>& a1, const ObjMatrix<T>& a2, real eps =
 }
 
 template<typename T>
-host_fn bool is_close(const std::vector<T>& a1, const std::vector<T>& a2, real eps = 1e-5) {
+host_fn bool is_close(const std::vector<T>& a1, const std::vector<T>& a2, real eps) {
   if (a1.size() != a2.size()) {
     return false;
   }
@@ -44,38 +44,17 @@ host_fn bool is_close(const std::vector<T>& a1, const std::vector<T>& a2, real e
   return true;
 }
 
-// todo: remove?
-// template<typename T>
-// host_fn bool is_close(const ObjDblArray<T>& a1, const ObjDblArray<T>& a2, real eps) {
-//   if (a1.rows.size != a2.rows.size) {
-//     Assert(false);
-//     return false;
-//   }
-//   for (u32 i = 0; i < a1.cols; i++) {
-//     if (a1.rows[i] != a2.rows[i]) {
-//       Assert(false);
-//       return false;
-//     }
-//     for (u32 j = 0; j < a2.rows[i]; j++) {
-//       if (!is_close(a1(j, i), a2(j, i), eps))
-//         return false;
-//     }
-//   }
-//
-//   return true;
-// }
-
 // note: Does not use eps but necessary for consistency for usability with templates
-inline blast_fn bool is_close(const u8 a1, const u8& a2, real eps = 1e-5) {
+inline blast_fn bool is_close(const u8 a1, const u8& a2, real eps) {
   return (a1 == a2);
 }
 
 // note: Does not use eps but necessary for consistency for usability with templates
-inline blast_fn bool is_close(u32 a1, u32 a2, real eps = 1e-5) {
+inline blast_fn bool is_close(u32 a1, u32 a2, real eps) {
   return (a1 == a2);
 }
 
-inline host_fn bool is_close(real r1, real r2, real eps = 1e-5) {
+inline host_fn bool is_close(real r1, real r2, real eps) {
   return (r1 == INF_REAL && r2 == INF_REAL) || (r1 == -INF_REAL && r2 == -INF_REAL)
                  ? true
                  : (r1 == INF_REAL || r2 == INF_REAL || r1 == -INF_REAL || r2 == -INF_REAL
@@ -83,7 +62,7 @@ inline host_fn bool is_close(real r1, real r2, real eps = 1e-5) {
                             : (std::abs(r1 - r2) < eps));
 }
 
-inline host_fn bool is_close(const Box& box1, const Box& box2, real eps = 1e-5) {
+inline host_fn bool is_close(const Box& box1, const Box& box2, real eps) {
   if (!is_close(box1.center, box2.center, eps))
     return false;
   if (!is_close(box1.extents, box2.extents, eps))
@@ -94,7 +73,7 @@ inline host_fn bool is_close(const Box& box1, const Box& box2, real eps = 1e-5)
   return true;
 }
 
-inline host_fn bool is_close(const DynamicBox& box1, const DynamicBox& box2, real eps = 1e-5) {
+inline host_fn bool is_close(const DynamicBox& box1, const DynamicBox& box2, real eps) {
   if (!is_close(box1.n_points, box2.n_points))
     return false;
   if (!is_close(box1.start_time, box2.start_time, eps))
@@ -107,7 +86,7 @@ inline host_fn bool is_close(const DynamicBox& box1, const DynamicBox& box2, rea
   return true;
 }
 
-inline host_fn bool is_close(const Sphere& sph1, const Sphere& sph2, real eps = 1e-5) {
+inline host_fn bool is_close(const Sphere& sph1, const Sphere& sph2, real eps) {
   if (!is_close(sph1.center, sph2.center, eps))
     return false;
   if (!is_close(sph1.radius, sph2.radius, eps))
@@ -116,7 +95,7 @@ inline host_fn bool is_close(const Sphere& sph1, const Sphere& sph2, real eps =
   return true;
 }
 
-inline host_fn bool is_close(const DynamicSphere& sph1, const DynamicSphere& sph2, real eps = 1e-5) {
+inline host_fn bool is_close(const DynamicSphere& sph1, const DynamicSphere& sph2, real eps) {
   if (!is_close(sph1.n_points, sph2.n_points))
     return false;
   if (!is_close(sph1.start_time, sph2.start_time, eps))
@@ -129,7 +108,7 @@ inline host_fn bool is_close(const DynamicSphere& sph1, const DynamicSphere& sph
   return true;
 }
 
-inline host_fn bool is_close(const Capsule& capsule1, const Capsule& capsule2, real eps = 1e-5) {
+inline host_fn bool is_close(const Capsule& capsule1, const Capsule& capsule2, real eps) {
   if (!((is_close(capsule1.p1, capsule2.p1, eps) && is_close(capsule1.p2, capsule2.p2, eps)) ||
         (is_close(capsule1.p1, capsule2.p2, eps) && is_close(capsule1.p2, capsule2.p1, eps))))
     return false;
@@ -139,7 +118,7 @@ inline host_fn bool is_close(const Capsule& capsule1, const Capsule& capsule2, r
   return true;
 }
 
-inline host_fn bool is_close(const DynamicCapsule& capsule1, const DynamicCapsule& capsule2, real eps = 1e-5) {
+inline host_fn bool is_close(const DynamicCapsule& capsule1, const DynamicCapsule& capsule2, real eps) {
   if (!is_close(capsule1.n_points, capsule2.n_points))
     return false;
   if (!is_close(capsule1.start_time, capsule2.start_time, eps))
@@ -152,7 +131,7 @@ inline host_fn bool is_close(const DynamicCapsule& capsule1, const DynamicCapsul
   return true;
 }
 
-inline host_fn bool is_close(const World& world1, const World& world2, real eps = 1e-5) {
+inline host_fn bool is_close(const World& world1, const World& world2, real eps) {
   if (!is_close(world1.boxes, world2.boxes, eps))
     return false;
   if (!is_close(world1.capsules, world2.capsules, eps))
@@ -173,7 +152,7 @@ inline host_fn bool is_close(const World& world1, const World& world2, real eps
 
 // todo: operator==
 inline host_fn bool is_close(const CollisionModelCapsule& capsule1, const CollisionModelCapsule& capsule2,
-                             real eps = 1e-5) {
+                             real eps) {
   if (!((is_close(capsule1.p1, capsule2.p1, eps) && is_close(capsule1.p2, capsule2.p2, eps)) ||
         (is_close(capsule1.p1, capsule2.p2, eps) && is_close(capsule1.p2, capsule2.p1, eps))))
     return false;
@@ -183,7 +162,7 @@ inline host_fn bool is_close(const CollisionModelCapsule& capsule1, const Collis
 }
 
 // todo: operator==
-inline host_fn bool is_close(const Manipulator& manip1, const Manipulator& manip2, real eps = 1e-5) {
+inline host_fn bool is_close(const Manipulator& manip1, const Manipulator& manip2, real eps) {
   if (manip1.n_joints != manip2.n_joints)
     return false;
 
@@ -267,7 +246,7 @@ inline host_fn bool is_close(const Manipulator& manip1, const Manipulator& manip
 }
 
 inline host_fn bool is_close(const ManipulatorTempData& manip_data1, const ManipulatorTempData& manip_data2,
-                             const u32 n_joints, const u32 n_caps, real eps = 1e-5) {
+                             const u32 n_joints, const u32 n_caps, real eps) {
   for (int joint = 0; joint < n_joints; joint++) {
     // internal variables
     if (!is_close(manip_data1.efforts[joint], manip_data2.efforts[joint], eps))
@@ -293,7 +272,7 @@ inline host_fn bool is_close(const ManipulatorTempData& manip_data1, const Manip
 
 // todo: operator==
 // todo: why only test these parameters?
-inline host_fn bool is_close(const Bspline& spline1, const Bspline& spline2, real eps = 1e-5) {
+inline host_fn bool is_close(const Bspline& spline1, const Bspline& spline2, real eps) {
   if (!is_close(spline1.n_joints, spline2.n_joints, eps))
     return false;
   if (!is_close(spline1.degree, spline2.degree, eps))
@@ -308,7 +287,7 @@ inline host_fn bool is_close(const Bspline& spline1, const Bspline& spline2, rea
 
 // todo: operator==
 inline host_fn bool is_close(const ConstraintSelection& constraints1, const ConstraintSelection& constraints2,
-                             real eps = 1e-5) {
+                             real eps) {
   if (constraints1.position != constraints2.position)
     return false;
   if (constraints1.velocity != constraints2.velocity)
@@ -338,7 +317,7 @@ inline host_fn bool is_close(const ConstraintSelection& constraints1, const Cons
 }
 
 // todo: operator==
-inline host_fn bool is_close(const Objective& objective1, const Objective& objective2, real eps = 1e-5) {
+inline host_fn bool is_close(const Objective& objective1, const Objective& objective2, real eps) {
   if (!is_close(objective1.time_weight, objective2.time_weight, eps))
     return false;
   if (!is_close(objective1.k_extra_objectives, objective2.k_extra_objectives, eps))
@@ -348,7 +327,7 @@ inline host_fn bool is_close(const Objective& objective1, const Objective& objec
 }
 
 // todo: operator==
-inline host_fn bool is_close(const Guess& guess1, const Guess& guess2, real eps = 1e-5) {
+inline host_fn bool is_close(const Guess& guess1, const Guess& guess2, real eps) {
   if (guess1.type != guess2.type)
     return false;
   switch (guess1.type) {
@@ -368,7 +347,7 @@ inline host_fn bool is_close(const Guess& guess1, const Guess& guess2, real eps
 }
 
 // todo: operator==
-inline host_fn bool is_close(const Optimization& opt1, const Optimization& opt2, real eps = 1e-5) {
+inline host_fn bool is_close(const Optimization& opt1, const Optimization& opt2, real eps) {
   if (!is_close(opt1.bspline, opt2.bspline, eps))
     return false;
   if (!is_close(opt1.constraints, opt2.constraints, eps))
@@ -388,12 +367,7 @@ inline host_fn bool is_close(const Optimization& opt1, const Optimization& opt2,
 }
 
 // todo: operator==
-inline host_fn bool is_close(const nlopt_result& result1, const nlopt_result& result2, real eps = 1e-5) {
-  return (result1 == result2);
-}
-
-// todo: operator==
-inline host_fn bool is_close(const Result& result1, Result& result2, real eps = 1e-5) {
+inline host_fn bool is_close(const Result& result1, Result& result2, real eps) {
   if (result1.success != result2.success)
     return false;
   if (result1.success_false != result2.success_false)
@@ -410,15 +384,5 @@ inline host_fn bool is_close(const Result& result1, Result& result2, real eps =
   return true;
 }
 
-// todo: make thread safe?
-inline host_fn u32 random_int(u32 min, u32 max) {
-  // Create a static random number generator (seeded once)
-  static std::random_device rd;
-  static std::mt19937       gen(rd());
-
-  // Create a uniform distribution for integers in the given range
-  std::uniform_int_distribution<u32> dist(min, max);
 
-  return dist(gen);
-}
 } // namespace blast
diff --git a/blast/utilities/print.hpp b/blast/utilities/print.hpp
new file mode 100644
index 0000000..c2794b5
--- /dev/null
+++ b/blast/utilities/print.hpp
@@ -0,0 +1,95 @@
+#pragma once
+#include <blast>
+
+namespace blast {
+inline blast_fn void print(Vec3 v) {
+  printf("[% 0.4f, % 0.4f, % 0.4f]\n", v.x, v.y, v.z);
+}
+
+inline blast_fn void print(Mat3 m) {
+  printf("\n[% 0.4f, % 0.4f, % 0.4f]\n[% 0.4f, % 0.4f, % 0.4f]\n[% 0.4f, % 0.4f, % 0.4f]\n",
+         m(0, 0), m(0, 1), m(0, 2), m(1, 0), m(1, 1), m(1, 2), m(2, 0), m(2, 1), m(2, 2));
+}
+
+inline blast_fn void print(const Array& a) {
+  using namespace std;
+  if (a.size == 0)
+    return;
+  printf("[");
+  for (u32 i = 0; i < a.size - 1; i++)
+    printf("% 0.4f, ", a[i]);
+  printf("% 0.4f]\n", a[a.size - 1]);
+}
+
+inline blast_fn void print(const Matrix& m) {
+  if (m.size == 0)
+    return;
+  for (u32 i = 0; i < m.rows; i++) {
+    printf("[");
+    for (u32 j = 0; j < m.cols - 1; j++)
+      printf("% 0.4f, ", m(i, j));
+    printf("% 0.4f]\n", m(i, m.cols - 1));
+  }
+}
+
+inline blast_fn void print(const double* data, unsigned size) {
+  printf("[");
+  for (u32 i = 0; i < size - 1; i++)
+    printf("% 0.4f, ", data[i]);
+  printf("% 0.4f]\n", data[size - 1]);
+}
+
+inline blast_fn void print(const float* data, unsigned size) {
+  printf("[");
+  for (u32 i = 0; i < size - 1; i++)
+    printf("% 0.4f, ", data[i]);
+  printf("% 0.4f]\n", data[size - 1]);
+}
+
+inline blast_fn void print(const double* data, unsigned rows, unsigned cols) {
+  for (u32 i = 0; i < rows; i++) {
+    printf("[");
+    for (u32 j = 0; j < cols - 1; j++)
+      printf("% 0.4f, ", data[i + rows * j]);
+    printf("% 0.4f]\n", data[i + rows * (cols - 1)]);
+  }
+}
+
+inline blast_fn void print(const float* data, unsigned rows, unsigned cols) {
+  for (u32 i = 0; i < rows; i++) {
+    printf("[");
+    for (u32 j = 0; j < cols - 1; j++)
+      printf("% 0.4f, ", data[i + rows * j]);
+    printf("% 0.4f]\n", data[i + rows * (cols - 1)]);
+  }
+}
+
+// note: CUDA stuff, only enabled if compiling for Nvidia GPUs
+#ifdef __NVCC__
+// print the properties of all connected GPUs
+inline host_fn void print_device_properties() {
+  int nDevices;
+  cudaGetDeviceCount(&nDevices);
+  for (int i = 0; i < nDevices; i++) {
+    cudaDeviceProp prop;
+    cudaGetDeviceProperties(&prop, i);
+    printf("Device Number: ............................... %d\n", i);
+    printf("  Device name: ............................... %s\n", prop.name);
+    printf("  Multiprocessors: ........................... %d\n", prop.multiProcessorCount);
+    printf("  Threads per multiprocessor: ................ %d\n", prop.maxThreadsPerMultiProcessor);
+    printf("  Threads per block: ......................... %d\n", prop.maxThreadsPerBlock);
+    printf("  Async Engine Count: ........................ %d\n", prop.asyncEngineCount);
+    printf("  Registers per block: ....................... %d\n", prop.regsPerBlock);
+    printf("  Registers per multiprocessor: .............. %d\n", prop.regsPerMultiprocessor);
+    printf("  Shared memory per block (KB): .............. %f\n", prop.sharedMemPerBlock / 1024.0);
+    printf("  Shared memory per multiprocessor (KB): ..... %f\n", prop.sharedMemPerMultiprocessor / 1024.0);
+    printf("  Concurrent Kernels: ........................ %d\n", prop.concurrentKernels);
+    printf("  Clock Rate (KHz): .......................... %d\n", prop.clockRate);
+    printf("  Memory Clock Rate (KHz): ................... %d\n", prop.memoryClockRate);
+    printf("  Memory Bus Width (bits): ................... %d\n", prop.memoryBusWidth);
+    printf("  Peak Memory Bandwidth (GB/s): .............. %f\n", 2.0 * prop.memoryClockRate * (prop.memoryBusWidth / 8) / 1.0e6);
+    printf("  Compute capabilities : ..................... %d,%d\n", prop.major, prop.minor);
+  }
+}
+#endif
+} // namespace blast
diff --git a/tests/test_helper/scenes.hpp b/blast/world/scenes.hpp
similarity index 68%
rename from tests/test_helper/scenes.hpp
rename to blast/world/scenes.hpp
index 8478445..224234c 100644
--- a/tests/test_helper/scenes.hpp
+++ b/blast/world/scenes.hpp
@@ -1,10 +1,11 @@
 #pragma once
 #include <blast>
 
+namespace blast {
 // Extracted scenes from MotionBenchMaker (github : https://github.com/KavrakiLab/motion_bench_maker)
 
-inline host_fn blast::World get_bookshelf_small() {
-  blast::World bookshelf_small;
+inline host_fn World get_bookshelf_small() {
+  World bookshelf_small;
   bookshelf_small.add_capsule({0.9, 0.0, 1.01}, {0.9, 0.0, 1.15}, 0.03);
   bookshelf_small.add_capsule({0.7, 0.0, 1.01}, {0.7, 0.0, 1.15}, 0.03);
   bookshelf_small.add_capsule({0.5, 0.0, 1.01}, {0.5, 0.0, 1.15}, 0.03);
@@ -15,8 +16,8 @@ inline host_fn blast::World get_bookshelf_small() {
   return bookshelf_small;
 }
 
-inline host_fn blast::World get_bookshelf_tall() {
-  blast::World bookshelf_tall;
+inline host_fn World get_bookshelf_tall() {
+  World bookshelf_tall;
   bookshelf_tall.add_capsule({0.9, 0.0, 1.31}, {0.9, 0.0, 1.45}, 0.03);
   bookshelf_tall.add_capsule({0.7, 0.0, 1.31}, {0.7, 0.0, 1.45}, 0.03);
   bookshelf_tall.add_capsule({0.5, 0.0, 1.31}, {0.5, 0.0, 1.45}, 0.03);
@@ -35,8 +36,8 @@ inline host_fn blast::World get_bookshelf_tall() {
   return bookshelf_tall;
 }
 
-inline host_fn blast::World get_bookshelf_thin() {
-  blast::World bookshelf_thin;
+inline host_fn World get_bookshelf_thin() {
+  World bookshelf_thin;
   bookshelf_thin.add_capsule({1.1, 0.25, 0.37}, {1.1, 0.25, 0.51}, 0.03);
   bookshelf_thin.add_capsule({0.8, 0.25, 0.37}, {0.8, 0.25, 0.51}, 0.03);
   bookshelf_thin.add_capsule({1.1, -0.25, 0.62}, {1.1, -0.25, 0.76}, 0.03);
@@ -61,8 +62,8 @@ inline host_fn blast::World get_bookshelf_thin() {
   return bookshelf_thin;
 }
 
-inline host_fn blast::World get_scene_box() {
-  blast::World box;
+inline host_fn World get_scene_box() {
+  World box;
   box.add_capsule({0.8, 0.0, 0.48}, {0.8, 0.0, 0.62}, 0.03);
   box.add_box({0.8, 0, 0.44}, {0.35, 0.35, 0.02}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
   box.add_box({0.8, -0.35, 0.8}, {0.35, 0.02, 0.35}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
@@ -73,8 +74,8 @@ inline host_fn blast::World get_scene_box() {
   return box;
 }
 
-inline host_fn blast::World get_scene_cage() {
-  blast::World cage;
+inline host_fn World get_scene_cage() {
+  World cage;
   cage.add_box({0.8, 0, 0.52}, {0.035, 0.035, 0.035}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
   cage.add_box({0.8, 0, 0.44}, {0.35, 0.35, 0.02}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
   cage.add_box({0.8, -0.35, 0.8}, {0.35, 0.02, 0.35}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
@@ -86,8 +87,8 @@ inline host_fn blast::World get_scene_cage() {
   return cage;
 }
 
-inline host_fn blast::World get_scene_table() {
-  blast::World table;
+inline host_fn World get_scene_table() {
+  World table;
   table.add_capsule({0.85, 0.0, 0.74}, {0.85, 0.0, 0.86}, 0.03);
   table.add_box({0.75, 0.4, 0.85}, {0.125, 0.125, 0.125}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
   table.add_box({1.5, 0.85, 0.35}, {0.025, 0.025, 0.35}, {-1, 0, 0, 0, -1, 0, 0, 0, 1});
@@ -103,8 +104,8 @@ inline host_fn blast::World get_scene_table() {
   return table;
 }
 
-inline host_fn blast::World get_kitchen_no_doors() {
-  blast::World world;
+inline host_fn World get_kitchen_no_doors() {
+  World world;
   world.add_box({0.427, -1.0847, 1.1974}, {0.36029, 0.06, 0.36242}, {1, 0, 0, 0, 1, 0, 0, 0, 1});
   world.add_box({0.427, 0.48791, 1.1974}, {0.36029, 0.06, 0.36242}, {1, 0, 0, 0, 1, 0, 0, 0, 1});
   world.add_box({0.427, -0.29838, 1.5298}, {0.36029, 0.84629, 0.03}, {1, 0, 0, 0, 1, 0, 0, 0, 1});
@@ -118,3 +119,44 @@ inline host_fn blast::World get_kitchen_no_doors() {
   world.add_box({0.35698, -0.80106, -0.47336}, {0.41378, 0.38356, 0.03}, {1, 0, 0, 0, 1, 0, 0, 0, 1});
   return world;
 }
+
+inline host_fn World get_demo2_world() {
+  World result;
+  result.add_box({0.7, 0, 0.381}, {1.0, 0.75, 0.381}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1});       // table 76 cm high
+  result.add_box({0.67, -0.1475, 0.96}, {0.35, 0.025, 0.2}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // vertical plate (no coll)
+  // result.add_box({0.7, 0, -0.55}, {1.0, 0.75, 0.4}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1});                 // table 76 cm high
+  // result.add_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // vertical plate (no coll)
+  return result;
+}
+
+inline host_fn World get_lab_world() {
+  World world;
+  // Lab environment
+  // add_box({0.6415, 0.0237, -0.53815}, {2.0, 2.0, 0.381}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); //table
+  world.add_box({0.7, 0, -0.55}, {1.0, 0.75, 0.4}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1});                 // table
+  world.add_box({-0.5654, -0.8145, 0.3248}, {0.381, 0.635, 0.381}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // computer and screens next to link6
+  world.add_box({0.4506, -1.3479, 0.3248}, {0.635, 0.381, 0.381}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // computer and screens next to gen3lite
+  world.add_box({0.0, 0.0, -0.4091}, {0.1459, 0.2018, 0.4091}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));     // link6 base
+  world.add_box({0.6665, 1.1286, 0.0}, {0.508, 0.3175, 1.0457}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));    // UR5
+  world.add_box({0, -0.75, 1.0}, {0.10, 0.15, 2.0}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));                // cables
+
+  // DEMO 1 bin 1
+  // world.add_box({0.4114, -0.3784, -0.04465}, {0.2125, 0.155, 0.0125}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // bottom
+  // world.add_box({0.2114, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // front
+  // world.add_box({0.6114, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // back
+  // world.add_box({0.4114, -0.2359, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // left
+  // world.add_box({0.4114, -0.5209, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // right
+
+  // // DEMO 1 bin 2
+  // world.add_box({0.8716, -0.3784, -0.04465}, {0.2125, 0.155, 0.0125}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // bottom
+  // world.add_box({0.6716, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // front
+  // world.add_box({1.0716, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // back
+  // world.add_box({0.8716, -0.2359, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // left
+  // world.add_box({0.8716, -0.5209, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // right
+
+  // DEMO 1 obstacle
+  world.add_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // vertical plate (no coll)
+
+  return world;
+}
+} // namespace blast
diff --git a/examples/example_02_trajectory_optimization.cpp b/examples/example_02_trajectory_optimization.cpp
index 6838157..c4377a2 100644
--- a/examples/example_02_trajectory_optimization.cpp
+++ b/examples/example_02_trajectory_optimization.cpp
@@ -75,7 +75,7 @@ int main() {
   // Step 6 — Run the optimizer.
   // -----------------------------------------------------------------------
   std::cout << "Running trajectory optimization...\n";
-  opt.method    = OptimizationMethod::with_segments;
+  opt.method    = OptimizationMethod::with_segments; // default method
   Result result = optimize(&opt);
 
   // -----------------------------------------------------------------------
diff --git a/examples/extras/print_bsplines_and_basis_functions.cpp b/examples/extras/print_bsplines_and_basis_functions.cpp
index 086edd6..056aa7e 100644
--- a/examples/extras/print_bsplines_and_basis_functions.cpp
+++ b/examples/extras/print_bsplines_and_basis_functions.cpp
@@ -13,209 +13,73 @@ int main() {
 
   Bspline spline(n_ctrl, points, p, joints);
 
-  int   task_id = 32;
   Task  task(joints);
-  Array x(spline.x_len(task.to_matrix())); // note: only works this way if task has no NaN values later on
-  if (task_id == 4) {
-    // Task 5
-    task.start.position = {1.94822, 0.473555, -0.0255247, -0.448375, 0.370356, -3.12883}; // 0
-    task.goal.position  = {-2.6558, -0.6703, 0.0599, -2.5311, -2.0486, -0.0127};          // 4
-    x                   = {
-            1.816791803469461,
-            1.4301664715707456,
-            0.9172624105388413,
-            0.41032842200174535,
-            -0.10014186426223334,
-            -0.6072139070598492,
-            -1.1180937604578087,
-            -1.6233561850740788,
-            -2.1380409785102317,
-            -2.5256741506232903,
-            0.3522706258988898,
-            0.11485758324975837,
-            0.1513619678668751,
-            -0.4501328230326604,
-            -0.8716995321655857,
-            -1.4736987481610542,
-            -1.7383052796106664,
-            -1.6529412101871985,
-            -1.1657158809571135,
-            -0.8252186539404436,
-            0.11015774963815754,
-            0.4934744880143819,
-            0.9896118720044864,
-            1.5413480175664709,
-            1.723735247858803,
-            1.5441156012532713,
-            1.2777877451399433,
-            0.9363731163532629,
-            0.31981075462132186,
-            0.04443879091545862,
-            -0.30755372430531075,
-            0.07956983539185727,
-            0.31098984562456283,
-            0.04095114805417483,
-            0.051197213327372045,
-            -0.4877527445523626,
-            -0.9967932820100162,
-            -1.4859348293064292,
-            -2.0146136126331977,
-            -2.4012208131583628,
-            0.2230622109760441,
-            -0.12496581895874909,
-            -0.7286093998588461,
-            -0.6462530499776119,
-            -0.9612078022878092,
-            -1.5230938307839277,
-            -1.5411562497372144,
-            -1.9133048552466936,
-            -2.0775699613657967,
-            -1.7840892035818243,
-            -3.0079971238431544,
-            -2.613777197577913,
-            -2.083017645280127,
-            -1.66573182183796,
-            -1.1201470214785312,
-            -0.6816244117747569,
-            -0.30088061253535264,
-            0.0027806409526269066,
-            -0.010280799630171871,
-            -0.1607918583161298,
-            1.782};
-  }
+  Array x(spline.x_len(task.to_matrix()));                                            // note: only works this way if task has no NaN values later on
 
-  if (task_id == 5) {
-    task.start.position = {1.94822, 0.473555, -0.0255247, -0.448375, 0.370356, -3.12883}; // 0
-    task.goal.position  = {3.12394, -0.553571, -0.509807, 1.06314, 1.52902, -3.10058};
-    x                   = {1.898439507269065,
-                           1.7674519136369835,
-                           1.7663759982701652,
-                           1.8615514535374722,
-                           1.9499240237594562,
-                           2.0697879872566567,
-                           2.3667905398002103,
-                           2.661078307838885,
-                           2.961701341027816,
-                           3.0767052858666517,
-                           0.4196460854663349,
-                           0.3171434166477383,
-                           0.03382173629170898,
-                           -0.23545689599356182,
-                           -0.5067363018301098,
-                           -0.6967318842799585,
-                           -0.7529169003757722,
-                           -0.6955135208068889,
-                           -0.5480915881958456,
-                           -0.5921715786145296,
-                           -0.08842547061039863,
-                           -0.17507961812267364,
-                           -0.11748160361339838,
-                           0.00668630554716525,
-                           0.11024153067852874,
-                           0.04319851632932237,
-                           -0.060431595877210476,
-                           -0.10718754657763621,
-                           -0.3952402157090322,
-                           -0.4786937117787419,
-                           -0.40238492266638937,
-                           -0.2846893080193645,
-                           0.015965172275822268,
-                           0.3033998518684378,
-                           0.6165993367415804,
-                           0.8025563405604532,
-                           0.8858759003390767,
-                           0.8104950875493071,
-                           0.9027513432714294,
-                           1.0192882637410092,
-                           0.3074205117987128,
-                           0.21791462763980574,
-                           0.26924322899577613,
-                           0.39857844919789487,
-                           0.6888423443677986,
-                           0.9792848458129146,
-                           1.2858009945830673,
-                           1.5193726303865358,
-                           1.6159898320825847,
-                           1.6063061063742987,
-                           -3.1034276874139337,
-                           -3.0031002885435054,
-                           -2.742306463555956,
-                           -2.6320426067628273,
-                           -2.6351399807428804,
-                           -2.6401127101972515,
-                           -2.663389944142752,
-                           -2.709535573817573,
-                           -2.9419150091219017,
-                           -3.0636811679214864,
-                           1.034};
-  }
+  task.start.position = {2.04549, 0.675615, 0.137326, -0.814844, 0.449157, -3.08683}; // 4
+  task.goal.position  = {2.5825, 0.0700, -0.3892, 0.3196, 0.9927, -3.17328};          // 0
+  x                   = {2.012367432705342,
+                         1.9183101026572604,
+                         1.7497289098956235,
+                         1.7056521658986308,
+                         1.7223101992224683,
+                         1.823821209145915,
+                         2.029475755747117,
+                         2.2359775337758414,
+                         2.4616270467628607,
+                         2.5472728000018465,
+                         0.7179444714026678,
+                         0.6747835328459124,
+                         0.5905813724701455,
+                         0.4732084367682011,
+                         0.3877419627152058,
+                         0.2679800405036057,
+                         0.20628662643489376,
+                         0.26175257983114314,
+                         0.10244647176615736,
+                         0.13085517853642165,
+                         0.14688471958586236,
+                         0.07543553241423792,
+                         -0.060942179699366515,
+                         -0.22408329710890498,
+                         -0.4691395740014129,
+                         -0.6189306230897347,
+                         -0.6778822187674548,
+                         -0.6447207604978721,
+                         -0.5187297319175941,
+                         -0.41538574895597546,
+                         -0.7804945384768247,
+                         -0.6930394133807527,
+                         -0.4766589490896912,
+                         -0.208616172275154,
+                         0.03676937270637994,
+                         0.285355337026231,
+                         0.4269786278073888,
+                         0.48795758655073995,
+                         0.4505959960586352,
+                         0.3575488334455236,
+                         0.43912758699716037,
+                         0.38082781706673297,
+                         0.18780296837181842,
+                         0.10228164417466619,
+                         0.10092979189901205,
+                         0.1966984370981407,
+                         0.37117795848532414,
+                         0.6580317779330959,
+                         0.8703471092778267,
+                         0.9605537399961706,
+                         -3.0677587708249896,
+                         -3.0310307373237357,
+                         -2.8258541900851637,
+                         -2.820323386173435,
+                         -2.710678680133922,
+                         -2.7566085960780518,
+                         -2.803503611759136,
+                         -2.9844253539900607,
+                         -3.1311235621554494,
+                         -3.163875191630662,
+                         0.893};
 
-  if (task_id == 32) {
-    // Task 35
-    task.start.position = {2.04549, 0.675615, 0.137326, -0.814844, 0.449157, -3.08683}; // 4
-    task.goal.position  = {2.5825, 0.0700, -0.3892, 0.3196, 0.9927, -3.17328};          // 0
-    x                   = {2.012367432705342,
-                           1.9183101026572604,
-                           1.7497289098956235,
-                           1.7056521658986308,
-                           1.7223101992224683,
-                           1.823821209145915,
-                           2.029475755747117,
-                           2.2359775337758414,
-                           2.4616270467628607,
-                           2.5472728000018465,
-                           0.7179444714026678,
-                           0.6747835328459124,
-                           0.5905813724701455,
-                           0.4732084367682011,
-                           0.3877419627152058,
-                           0.2679800405036057,
-                           0.20628662643489376,
-                           0.26175257983114314,
-                           0.10244647176615736,
-                           0.13085517853642165,
-                           0.14688471958586236,
-                           0.07543553241423792,
-                           -0.060942179699366515,
-                           -0.22408329710890498,
-                           -0.4691395740014129,
-                           -0.6189306230897347,
-                           -0.6778822187674548,
-                           -0.6447207604978721,
-                           -0.5187297319175941,
-                           -0.41538574895597546,
-                           -0.7804945384768247,
-                           -0.6930394133807527,
-                           -0.4766589490896912,
-                           -0.208616172275154,
-                           0.03676937270637994,
-                           0.285355337026231,
-                           0.4269786278073888,
-                           0.48795758655073995,
-                           0.4505959960586352,
-                           0.3575488334455236,
-                           0.43912758699716037,
-                           0.38082781706673297,
-                           0.18780296837181842,
-                           0.10228164417466619,
-                           0.10092979189901205,
-                           0.1966984370981407,
-                           0.37117795848532414,
-                           0.6580317779330959,
-                           0.8703471092778267,
-                           0.9605537399961706,
-                           -3.0677587708249896,
-                           -3.0310307373237357,
-                           -2.8258541900851637,
-                           -2.820323386173435,
-                           -2.710678680133922,
-                           -2.7566085960780518,
-                           -2.803503611759136,
-                           -2.9844253539900607,
-                           -3.1311235621554494,
-                           -3.163875191630662,
-                           0.893};
-  }
 
   print_to_csv(transpose(spline.basis_p), "../../../examples/bsplines_task32/basis_function_p.csv");
   print_to_csv(transpose(spline.basis_v), "../../../examples/bsplines_task32/basis_function_v.csv");
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index f0019ab..2789b0b 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,8 +1,8 @@
 include(FetchContent)
 FetchContent_Declare(
-    catch
-    GIT_REPOSITORY https://github.com/catchorg/Catch2.git
-    GIT_TAG v2.13.10
+        catch
+        GIT_REPOSITORY https://github.com/catchorg/Catch2.git
+        GIT_TAG v2.13.10
 )
 FetchContent_MakeAvailable(catch)
 
@@ -11,98 +11,86 @@ include(ParseAndAddCatchTests)
 
 # Usage: add_blast_target(target_name source_file [TEST | BENCH])
 function(add_blast_target target_name source_file type)
-  add_executable(${target_name} ${source_file})
-  target_compile_features(${target_name} PRIVATE cxx_std_17)
-
-  # PRIVATE: machine-specific tuning for dev targets only, never leaks to
-  # downstream consumers of the blast library.
-  if(MSVC)
-    target_compile_options(${target_name} PRIVATE /arch:AVX2)
-  else()
-    target_compile_options(${target_name} PRIVATE -march=native)
-  endif()
-
-  # tests/ is the include root for test_helper/, etc.
-  target_include_directories(${target_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
-
-  # Base dependencies
-  target_link_libraries(${target_name} PRIVATE Catch2::Catch2 blast)
-
-  # Conditional Tracy logic
-  if (ENABLE_TRACY)
-    target_link_libraries(${target_name} PRIVATE TracyClient)
-    target_compile_definitions(${target_name} PRIVATE TRACY_ENABLE TRACY_NO_EXIT)
-  endif ()
-
-  # If marked as a TEST, register it with CTest
-  if ("${type}" STREQUAL "TEST")
-    add_test(NAME ${target_name}_test COMMAND ${target_name})
-  endif ()
+    add_executable(${target_name} ${source_file})
+    target_compile_features(${target_name} PRIVATE cxx_std_17)
+
+    # PRIVATE: machine-specific tuning for dev targets only, never leaks to
+    # downstream consumers of the blast library.
+    if (MSVC)
+        target_compile_options(${target_name} PRIVATE /arch:AVX2)
+    else ()
+        target_compile_options(${target_name} PRIVATE -march=native)
+    endif ()
+
+    # tests/ is the include root for test_helper/, etc.
+    target_include_directories(${target_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+
+    # Base dependencies
+    target_link_libraries(${target_name} PRIVATE Catch2::Catch2 blast)
+
+    # Conditional Tracy logic
+    if (ENABLE_TRACY)
+        target_link_libraries(${target_name} PRIVATE TracyClient)
+        target_compile_definitions(${target_name} PRIVATE TRACY_ENABLE TRACY_NO_EXIT)
+    endif ()
+
+    # If marked as a TEST, register it with CTest
+    if ("${type}" STREQUAL "TEST")
+        add_test(NAME ${target_name}_test COMMAND ${target_name})
+    endif ()
 endfunction()
 
 # note: Add new benchmark files as follows :
 # NAME_OF_BENCHMARK BENCHMARK_TARGET_LOCATION
 # List of benchmarks with their source files
 set(BENCHMARKS
-    # Acceleration
-    #  bench_gradients_acceleration acceleration/bench_gradients_acceleration.cpp
-    # bench_generic benchmarks/bench_generic.cpp
-    # bench_test_manip benchmarks/bench_test_manip.cpp
-    #    bench_manipulator benchmarks/bench_manipulator.cpp
-    #    bench_accelerations acceleration/bench_accelerations.cpp
-    #    bench_inplace acceleration/bench_inplace.cpp
-    #    bench_paper_gradients benchmarks/bench_paper_gradients.cpp
-    #    bench_constraints benchmarks/bench_constraints.cpp
-    #    bench_functions benchmarks/bench_functions.cpp
-
-    bench_optimization_methods benchmarks/bench_optimization_methods.cpp
+        #
 )
 
 # note: Add new tests files as follows :
 # NAME_OF_TEST TEST_TARGET_LOCATION
 # List of tests with their source files
 set(TESTS
-    #    test_link6 test_manipulator/test_Link6.cpp
-    # Acceleration
-    # test_gradients_acceleration acceleration/test_gradients_acceleration.cpp
-    # test_constraints_acceleration acceleration/test_constraints_acceleration.cpp
-    #    test_accelerations acceleration/test_accelerations.cpp
-    #    test_inplace acceleration/test_inplace.cpp
-
-    # Container
-    #    test_container_objmatrix container/test_objmatrix.cpp
-    # test_container_objdblarray container/test_objdblarray.cpp
-    # Manipulator
-    #    test_manipulator_functions manipulators/test_functions.cpp
-    # test_generic_gen3 manipulators/test_generic_gen3.cpp
-    # test_generic_link6 manipulators/test_generic_link6.cpp
-    # test_generic_crx25 manipulators/test_generic_crx25.cpp
-    # test_generic_ur5e manipulators/test_generic_ur5e.cpp
-    # Optimization
-    # test_optimization optimization/test_optimization.cpp
-    # test_optimization_guess optimization/test_initial_guess.cpp
-    # test_optimization_objective optimization/test_objective.cpp
-    # test_optimization_constraints optimization/test_constraints.cpp
-    # test_optimization_tasks optimization/test_task.cpp
-    # test_optimization_hierarchical optimization/test_hierarchical_optimization.cpp
-    # Math
-    test_math_vec3 math/test_vec3.cpp
-    # World
-    #    test_primitives world/test_primitives.cpp
-    # Trajectory
-    test_bspline trajectory/test_bsplines.cpp
+
+        # Container
+        test_objmatrix container/test_objmatrix.cpp
+
+        # Manipulator
+        test_generic_gen3 manipulators/test_generic_gen3.cpp
+        test_generic_ur5e manipulators/test_generic_ur5e.cpp
+        test_manipulator_constructor manipulators/test_manipulator_constructor.cpp
+
+        # Optimization
+        test_constraints optimization/test_constraints.cpp
+        test_task optimization/test_task.cpp
+        test_optimization_basic optimization/test_optimization_basic.cpp
+        test_optimization_task_violation optimization/test_optimization_task_violation.cpp
+
+        # Math
+        test_math_matrix math/test_matrix.cpp
+        test_math_mat3 math/test_mat3.cpp
+        test_math_array math/test_array.cpp
+        test_misc math/test_misc.cpp
+        test_math_vec3 math/test_vec3.cpp
+
+        # World
+        test_primitives world/test_primitives.cpp
+
+        # Trajectory
+        test_bspline trajectory/test_bsplines.cpp
+        test_polynomial trajectory/test_polynomial.cpp
 )
 
 # Loop through and add benchmarks
 while (BENCHMARKS)
-  list(POP_FRONT BENCHMARKS target_name)
-  list(POP_FRONT BENCHMARKS source_file)
-  add_blast_target(${target_name} ${source_file} "BENCH")
+    list(POP_FRONT BENCHMARKS target_name)
+    list(POP_FRONT BENCHMARKS source_file)
+    add_blast_target(${target_name} ${source_file} "BENCH")
 endwhile ()
 
 # Loop through and add tests
 while (TESTS)
-  list(POP_FRONT TESTS target_name)
-  list(POP_FRONT TESTS source_file)
-  add_blast_target(${target_name} ${source_file} "TEST")
+    list(POP_FRONT TESTS target_name)
+    list(POP_FRONT TESTS source_file)
+    add_blast_target(${target_name} ${source_file} "TEST")
 endwhile ()
\ No newline at end of file
diff --git a/tests/acceleration/basis_a.csv b/tests/acceleration/basis_a.csv
deleted file mode 100644
index 14ca666..0000000
--- a/tests/acceleration/basis_a.csv
+++ /dev/null
@@ -1,16 +0,0 @@
-2420,2040.47,1702.85,1404.69,1143.54,916.946,722.455,557.617,419.977,307.083,216.484,145.727,92.3593,53.9287,27.983,12.0696,3.73628,0.53064,0.000307085,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
--3630,-2968.51,-2385.46,-1876.09,-1435.66,-1059.4,-742.566,-480.412,-268.183,-101.128,25.5045,116.466,176.509,210.384,222.842,218.636,202.516,179.234,153.541,129.575,108.242,89.3893,72.8635,58.5114,46.1796,35.7151,26.9644,19.7743,13.9915,9.46275,6.0348,3.55436,1.86814,0.822887,0.26532,0.0421664,0.000153543,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-1210,867.78,574.671,327.41,122.733,-42.6247,-171.927,-268.439,-335.423,-376.145,-393.868,-391.856,-373.374,-341.686,-300.056,-251.748,-200.026,-148.154,-99.3967,-56.5575,-20.0165,10.6403,35.8273,55.9588,71.4494,82.7133,90.165,94.2188,95.2893,93.7908,90.1377,84.7444,78.0253,70.3948,62.2674,54.0574,46.1792,39.0053,32.6153,26.9644,22.007,17.6979,13.9915,10.8425,8.20536,6.0348,4.28535,2.9116,1.86814,1.10955,0.590413,0.26532,0.0888552,0.0156044,0.000153543,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,59.3784,104.53,136.633,156.865,166.404,166.427,158.114,142.642,121.188,94.9309,65.0484,32.7185,-0.880947,-34.572,-67.1767,-97.5172,-124.415,-146.694,-163.378,-174.574,-180.736,-182.323,-179.791,-173.598,-164.199,-152.053,-137.616,-121.345,-103.697,-85.1292,-66.0985,-47.0619,-28.4764,-10.7988,5.51368,20.0042,32.2718,42.3056,50.2612,56.2947,60.5622,63.2198,64.4237,64.3299,63.0945,60.8737,57.8236,54.1003,49.8599,45.2584,40.4521,35.5971,30.8493,26.3651,22.2887,18.6553,15.4399,12.617,10.161,8.0464,6.24763,4.73914,3.4954,2.49086,1.69997,1.09718,0.656963,0.35376,0.162028,0.0562219,0.0107959,0.000204723,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.885396,3.38604,7.26862,12.2998,18.2463,24.8748,31.952,39.2445,46.519,53.5423,60.081,65.9018,70.7713,74.4564,76.7236,77.3396,76.0711,72.6848,67.0114,59.2181,49.5824,38.3819,25.8943,12.3971,-1.83217,-16.5158,-31.3762,-46.1359,-60.5171,-74.2424,-87.0341,-98.6147,-108.706,-117.032,-123.313,-127.273,-128.681,-127.63,-124.354,-119.086,-112.059,-103.506,-93.6616,-82.7581,-71.0292,-58.7081,-46.0281,-33.2225,-20.5247,-8.16802,3.61433,14.589,24.5226,33.1819,40.3554,46.0367,50.3322,53.3489,55.1943,55.9756,55.8001,54.7751,53.0079,50.6057,47.6759,44.3258,40.6627,36.7938,32.8264,28.8678,25.0254,21.4063,18.1125,15.1744,12.5725,10.2865,8.29584,6.58016,5.11902,3.89196,2.87856,2.05837,1.41097,0.915904,0.55275,0.301067,0.140419,0.0503694,0.0104818,0.00031988,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.00340607,0.0272485,0.0919638,0.217988,0.425758,0.73571,1.16828,1.74391,2.48302,3.40607,4.53347,5.88568,7.48312,9.34624,11.4955,13.9512,16.734,19.8642,23.3468,27.1063,31.0404,35.047,39.0239,42.8688,46.4797,49.7542,52.5904,54.8859,56.5385,57.4462,57.5067,56.6178,54.6774,51.5833,47.2332,41.5251,34.3846,25.9329,16.3738,5.91185,-5.24867,-16.9034,-28.8479,-40.8779,-52.789,-64.3768,-75.437,-85.7652,-95.1571,-103.408,-110.314,-115.671,-119.274,-120.918,-120.426,-117.866,-113.441,-107.355,-99.8122,-91.0172,-81.1744,-70.4881,-59.1628,-47.4027,-35.4122,-23.3957,-11.5576,-0.10217,10.7662,20.843,29.9241,37.805,44.2946,49.3727,53.1392,55.6963,57.1461,57.5907,57.1325,55.8735,53.916,51.3621,48.314,44.8739,41.144,37.2264,33.2234,29.2371,25.3697,21.7234,18.397,15.4273,12.7957,10.4818,8.46513,6.72531,5.24187,3.99438,2.96239,2.12549,1.46322,0.955152,0.580851,0.319879,0.151799,0.0561758,0.0125725,0.000552753,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.00255903,0.0236991,0.0838542,0.203461,0.402955,0.702773,1.12335,1.68513,2.40854,3.31401,4.422,5.75293,7.32723,9.16535,11.2877,13.7148,16.467,19.5647,23.0173,26.7558,30.6783,34.6825,38.6662,42.5274,46.1638,49.4731,52.3533,54.7022,56.4175,57.3972,57.5389,56.7405,54.9,51.9149,47.6833,42.1028,35.091,26.75,17.2832,6.89488,-4.21058,-15.8288,-27.7554,-39.7861,-51.7165,-63.3421,-74.4587,-84.8619,-94.3473,-102.711,-109.747,-115.253,-119.024,-120.855,-120.56,-118.179,-113.915,-107.971,-100.553,-91.8631,-82.1071,-71.4891,-60.2134,-48.4844,-36.5064,-24.4838,-12.621,-1.12238,9.80776,19.965,29.1451,37.1436,43.7643,48.9668,52.8483,55.5111,57.0573,57.5892,57.2088,56.0184,54.1201,51.6162,48.6088,45.2002,41.4924,37.5876,33.5882,29.5961,25.7137,22.043,18.6844,15.683,13.0215,10.6795,8.63672,6.87257,5.36667,4.09857,3.04784,2.19405,1.51675,0.995506,0.609888,0.339458,0.163778,0.0624122,0.0149243,0.000877752,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.00186553,0.0204722,0.0762359,0.189593,0.38098,0.670833,1.07959,1.62768,2.33555,3.22364,4.31237,5.62218,7.17352,8.98681,11.0825,13.481,16.2028,19.2683,22.69,26.4068,30.3168,34.3178,38.3076,42.1841,45.8452,49.1885,52.1119,54.5133,56.2905,57.3412,57.5633,56.8547,55.113,52.2363,48.1222,42.6685,35.7864,27.5579,18.185,7.87198,-3.17673,-14.7568,-26.6638,-38.6935,-50.6414,-62.3032,-73.4745,-83.951,-93.5283,-102.002,-109.168,-114.821,-118.758,-120.774,-120.676,-118.477,-114.375,-108.576,-101.283,-92.7002,-83.0327,-72.4846,-61.2603,-49.564,-37.6002,-25.5733,-13.6875,-2.1473,8.84298,19.079,28.3563,36.4706,43.2224,48.55,52.5474,55.3167,56.9602,57.5801,57.2784,56.1574,54.3193,51.8662,48.9004,45.5239,41.8391,37.9481,33.953,29.956,26.0594,22.3653,18.9749,15.9415,13.2499,10.8798,8.81061,7.02197,5.49344,4.20457,3.13492,2.26406,1.57156,1.03698,0.639878,0.35982,0.176371,0.0690938,0.0175524,0.00131023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.00131022,0.0175523,0.0690937,0.176371,0.35982,0.639877,1.03698,1.57156,2.26406,3.13492,4.20456,5.49343,7.02197,8.8106,10.8798,13.2499,15.9415,18.9749,22.3653,26.0594,29.956,33.953,37.9481,41.8391,45.5239,48.9004,51.8662,54.3193,56.1574,57.2784,57.5801,56.9602,55.3167,52.5474,48.55,43.2224,36.4706,28.3563,19.079,8.843,-2.14727,-13.6875,-25.5732,-37.6002,-49.564,-61.2602,-72.4846,-83.0327,-92.7002,-101.283,-108.576,-114.375,-118.477,-120.676,-120.774,-118.758,-114.821,-109.168,-102.002,-93.5283,-83.9511,-73.4746,-62.3032,-50.6414,-38.6935,-26.6639,-14.7568,-3.17676,7.87196,18.185,27.5579,35.7864,42.6685,48.1221,52.2363,55.113,56.8547,57.5633,57.3412,56.2905,54.5133,52.1119,49.1885,45.8452,42.1842,38.3076,34.3178,30.3168,26.4068,22.6901,19.2683,16.2028,13.481,11.0825,8.98682,7.17352,5.62218,4.31237,3.22364,2.33556,1.62769,1.07959,0.670834,0.380981,0.189593,0.0762361,0.0204723,0.00186554,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.000877743,0.0149242,0.0624121,0.163778,0.339457,0.609888,0.995505,1.51675,2.19405,3.04784,4.09857,5.36667,6.87257,8.63671,10.6795,13.0215,15.683,18.6844,22.043,25.7137,29.5961,33.5881,37.5876,41.4924,45.2002,48.6088,51.6162,54.1201,56.0184,57.2088,57.5892,57.0573,55.5111,52.8484,48.9668,43.7644,37.1436,29.1451,19.9651,9.80779,-1.12236,-12.621,-24.4838,-36.5064,-48.4843,-60.2134,-71.4891,-82.1071,-91.8631,-100.553,-107.971,-113.915,-118.179,-120.56,-120.855,-119.024,-115.253,-109.747,-102.711,-94.3474,-84.8619,-74.4588,-63.3421,-51.7165,-39.7861,-27.7555,-15.8288,-4.2106,6.89486,17.2832,26.75,35.091,42.1028,47.6833,51.9149,54.8999,56.7405,57.5389,57.3972,56.4175,54.7022,52.3533,49.4731,46.1638,42.5274,38.6663,34.6825,30.6783,26.7558,23.0173,19.5647,16.467,13.7148,11.2877,9.16536,7.32723,5.75293,4.422,3.31402,2.40854,1.68513,1.12335,0.702773,0.402955,0.203461,0.0838544,0.0236992,0.00255904,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.000552747,0.0125725,0.0561757,0.151799,0.319878,0.58085,0.955151,1.46322,2.12549,2.96239,3.99437,5.24187,6.72531,8.46513,10.4818,12.7957,15.4273,18.397,21.7234,25.3697,29.2371,33.2234,37.2264,41.144,44.8739,48.314,51.3621,53.916,55.8735,57.1325,57.5907,57.1461,55.6963,53.1392,49.3727,44.2946,37.805,29.9241,20.8431,10.7662,-0.102146,-11.5576,-23.3957,-35.4122,-47.4026,-59.1628,-70.4881,-81.1744,-91.0172,-99.8121,-107.355,-113.441,-117.866,-120.426,-120.918,-119.274,-115.671,-110.314,-103.408,-95.1571,-85.7653,-75.4371,-64.3769,-52.789,-40.8779,-28.8479,-16.9034,-5.24869,5.91183,16.3738,25.9329,34.3846,41.5251,47.2332,51.5833,54.6774,56.6178,57.5067,57.4462,56.5385,54.8859,52.5904,49.7542,46.4797,42.8688,39.0239,35.047,31.0405,27.1063,23.3469,19.8642,16.734,13.9512,11.4955,9.34625,7.48313,5.88568,4.53348,3.40607,2.48302,1.74391,1.16828,0.735711,0.425759,0.217989,0.091964,0.0272486,0.00340609,3.29766e-20
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.000319876,0.0104817,0.0503693,0.140419,0.301067,0.552749,0.915903,1.41096,2.05837,2.87856,3.89196,5.11901,6.58016,8.29583,10.2865,12.5725,15.1744,18.1125,21.4063,25.0254,28.8678,32.8264,36.7937,40.6627,44.3258,47.6759,50.6057,53.0078,54.7751,55.8001,55.9756,55.1943,53.3489,50.3322,46.0367,40.3554,33.1819,24.5226,14.589,3.61435,-8.16799,-20.5247,-33.2225,-46.0281,-58.7081,-71.0292,-82.7581,-93.6615,-103.506,-112.059,-119.086,-124.354,-127.63,-128.681,-127.273,-123.313,-117.032,-108.706,-98.6147,-87.0342,-74.2425,-60.5172,-46.1359,-31.3763,-16.5158,-1.8322,12.397,25.8943,38.3819,49.5824,59.2181,67.0114,72.6848,76.0711,77.3396,76.7236,74.4564,70.7714,65.9018,60.081,53.5423,46.519,39.2445,31.952,24.8748,18.2463,12.2998,7.26863,3.38605,0.8854,4.19862e-12
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.00020472,0.0107959,0.0562217,0.162028,0.35376,0.656963,1.09718,1.69996,2.49085,3.4954,4.73914,6.24762,8.0464,10.161,12.617,15.4399,18.6553,22.2887,26.3651,30.8493,35.5971,40.4521,45.2584,49.8599,54.1003,57.8236,60.8737,63.0945,64.3299,64.4237,63.2198,60.5622,56.2947,50.2612,42.3056,32.2718,20.0042,5.51371,-10.7988,-28.4763,-47.0619,-66.0985,-85.1292,-103.697,-121.345,-137.616,-152.053,-164.199,-173.598,-179.791,-182.323,-180.736,-174.574,-163.378,-146.694,-124.415,-97.5172,-67.1768,-34.5721,-0.881019,32.7184,65.0483,94.9308,121.188,142.642,158.114,166.427,166.404,156.865,136.633,104.53,59.3785,0.000142553
--0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,0.000153539,0.0156043,0.0888549,0.26532,0.590412,1.10955,1.86814,2.9116,4.28535,6.0348,8.20536,10.8424,13.9915,17.6979,22.007,26.9644,32.6153,39.0052,46.1792,54.0573,62.2674,70.3948,78.0253,84.7444,90.1377,93.7908,95.2893,94.2188,90.165,82.7133,71.4494,55.9589,35.8273,10.6404,-20.0164,-56.5574,-99.3966,-148.154,-200.026,-251.748,-300.056,-341.686,-373.374,-391.856,-393.868,-376.145,-335.423,-268.439,-171.928,-42.625,122.733,327.41,574.671,867.779,1210
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.000153537,0.0421662,0.265319,0.822886,1.86814,3.55435,6.03479,9.46274,13.9915,19.7742,26.9644,35.7151,46.1796,58.5113,72.8635,89.3893,108.242,129.575,153.541,179.234,202.516,218.636,222.842,210.384,176.509,116.467,25.5047,-101.128,-268.183,-480.412,-742.565,-1059.4,-1435.65,-1876.09,-2385.46,-2968.51,-3630
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.000307064,0.530637,3.73627,12.0696,27.9829,53.9287,92.3592,145.727,216.484,307.083,419.976,557.616,722.455,916.945,1143.54,1404.69,1702.85,2040.47,2420
diff --git a/tests/acceleration/basis_p.csv b/tests/acceleration/basis_p.csv
deleted file mode 100644
index 41d523c..0000000
--- a/tests/acceleration/basis_p.csv
+++ /dev/null
@@ -1,16 +0,0 @@
-1,0.75253,0.556674,0.403901,0.286677,0.198402,0.13335,0.0866032,0.0539945,0.0320431,0.0178931,0.0092514,0.00432622,0.00176472,0.000591286,0.000145589,2.06241e-05,7.97336e-07,3.20434e-12,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.233419,0.391712,0.489613,0.539994,0.553986,0.541102,0.509352,0.465365,0.414511,0.361019,0.308095,0.258047,0.212402,0.172024,0.137238,0.107948,0.0837565,0.0640862,0.0482974,0.0357861,0.0260134,0.0185028,0.0128367,0.00865244,0.0056382,0.00352943,0.00210485,0.00118257,0.00061624,0.000291179,0.000120501,4.12481e-05,1.05185e-05,1.59467e-06,7.43588e-08,6.40869e-12,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.0137836,0.0495836,0.0999917,0.158757,0.220704,0.281651,0.338326,0.388284,0.429827,0.46192,0.484109,0.496437,0.499365,0.493685,0.480443,0.46085,0.436207,0.407816,0.376902,0.344547,0.311674,0.279058,0.247336,0.217018,0.188495,0.162052,0.137879,0.11608,0.0966805,0.0796452,0.0648825,0.052257,0.0415998,0.0327191,0.0254107,0.019468,0.0146929,0.0109045,0.00794122,0.0056603,0.00393645,0.00266078,0.0017396,0.00109328,0.000655152,0.000370295,0.000194442,9.28082e-05,3.89476e-05,1.36091e-05,3.58801e-06,5.79484e-07,3.19141e-08,1.44196e-11,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.000265533,0.00200054,0.00634768,0.0141201,0.0258311,0.0417241,0.0618021,0.0858578,0.113503,0.144198,0.177283,0.212005,0.247551,0.283074,0.317726,0.350687,0.381193,0.408567,0.432248,0.451816,0.466985,0.477601,0.483621,0.485108,0.482219,0.475189,0.464324,0.449988,0.432591,0.412578,0.390415,0.366584,0.341563,0.315821,0.289803,0.263921,0.23854,0.213968,0.190461,0.168218,0.147394,0.128095,0.11039,0.0943085,0.0798494,0.0669814,0.0556489,0.0457751,0.0372664,0.0300159,0.0239079,0.0188213,0.0146338,0.0112259,0.00848458,0.00630705,0.00460147,0.00328662,0.00229113,0.00155295,0.00101862,0.00064267,0.000386945,0.00021999,0.000116383,5.60988e-05,2.38629e-05,8.50491e-06,2.31458e-06,3.9658e-07,2.53467e-08,3.41797e-11,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,1.89589e-06,2.95487e-05,0.000145614,0.000447642,0.00106219,0.00213893,0.00384474,0.00635786,0.00986194,0.0145402,0.0205694,0.0281144,0.0373214,0.0483131,0.0611819,0.0759847,0.0927365,0.111405,0.131904,0.15409,0.177768,0.202695,0.228588,0.255133,0.28199,0.308799,0.335191,0.360791,0.385226,0.408135,0.429171,0.448012,0.464366,0.477978,0.488639,0.496192,0.500536,0.501635,0.499517,0.494262,0.486004,0.47492,0.461224,0.445166,0.42702,0.407081,0.38566,0.363078,0.339656,0.315715,0.291567,0.267508,0.243816,0.22074,0.198499,0.177274,0.157208,0.138411,0.120958,0.104897,0.0902476,0.0770053,0.0651445,0.054621,0.0453742,0.0373305,0.0304054,0.0245067,0.0195369,0.0153961,0.0119844,0.00920521,0.00696722,0.00518736,0.00379139,0.00271356,0.0018961,0.00128871,0.00084802,0.000537085,0.000324878,0.000185767,9.89972e-05,4.81776e-05,2.0764e-05,7.54283e-06,2.11579e-06,3.83154e-07,2.79998e-08,8.34465e-11,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,4.30048e-09,1.37615e-07,1.04502e-06,4.40369e-06,1.3439e-05,3.34405e-05,7.22781e-05,0.000140918,0.000253939,0.000430048,0.000692596,0.0010701,0.00159674,0.0023129,0.00326568,0.00450938,0.00610606,0.00812605,0.0106484,0.0137609,0.0175582,0.0221395,0.0276058,0.0340572,0.0415906,0.0502971,0.0602589,0.0715477,0.0842211,0.0983206,0.113869,0.130867,0.149293,0.169098,0.190202,0.212496,0.235836,0.260041,0.284898,0.310168,0.335585,0.360868,0.385724,0.409852,0.432947,0.45471,0.474849,0.493084,0.509155,0.522826,0.533888,0.542168,0.547531,0.549886,0.549193,0.545462,0.538759,0.529195,0.516924,0.502134,0.485048,0.465914,0.445002,0.422596,0.398994,0.374498,0.349411,0.324031,0.298647,0.273533,0.248944,0.225108,0.202223,0.180454,0.159929,0.140744,0.122962,0.106621,0.091733,0.0782858,0.0662479,0.0555703,0.0461887,0.0380262,0.0309963,0.025005,0.0199535,0.015741,0.012267,0.00943418,0.00715056,0.00533225,0.00390422,0.00279997,0.00196103,0.00133644,0.00088221,0.000560846,0.0003408,0.000195971,0.000105183,5.16674e-05,2.25529e-05,8.34457e-06,2.40922e-06,4.59562e-07,3.7914e-08,2.07642e-10,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2.67026e-09,1.09056e-07,8.9599e-07,3.9255e-06,1.22609e-05,3.09828e-05,6.77051e-05,0.000133091,0.00024137,0.000410852,0.000664446,0.00103017,0.00154168,0.00223878,0.00316791,0.00438272,0.00594454,0.00792291,0.0103961,0.0134511,0.0171821,0.021688,0.0270696,0.0334274,0.0408586,0.0494548,0.0592995,0.070465,0.0830105,0.0969791,0.112395,0.129263,0.14756,0.167242,0.188232,0.210424,0.233675,0.25781,0.282618,0.30786,0.333275,0.358583,0.38349,0.407696,0.430898,0.452794,0.473092,0.491512,0.50779,0.521688,0.532995,0.541535,0.547167,0.549798,0.549382,0.545925,0.539489,0.530179,0.518146,0.503577,0.48669,0.467731,0.446969,0.424687,0.401182,0.376754,0.351708,0.326343,0.300948,0.275799,0.251152,0.227238,0.20426,0.182384,0.161741,0.14243,0.124519,0.108047,0.0930266,0.0794494,0.0672852,0.0564864,0.04699,0.0387201,0.031591,0.0255093,0.0203765,0.0160919,0.0125549,0.00966769,0.00733774,0.00548036,0.00401972,0.00288857,0.00202773,0.00138557,0.000917494,0.000585441,0.000357341,0.00020662,0.000111674,5.53564e-05,2.44631e-05,9.21308e-06,2.73435e-06,5.47702e-07,5.04566e-08,4.48795e-10,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.57676e-09,8.54482e-08,7.64472e-07,3.48979e-06,1.11669e-05,2.86717e-05,6.33665e-05,0.000125616,0.000229304,0.000392349,0.000637218,0.000991448,0.00148816,0.00216656,0.0030725,0.00425892,0.00578645,0.00772385,0.0101486,0.0131469,0.0168124,0.0212437,0.0265416,0.0328065,0.0401362,0.048623,0.0583509,0.0693937,0.0818117,0.0956497,0.110934,0.12767,0.145839,0.165398,0.186273,0.20836,0.231522,0.255585,0.280341,0.305554,0.330965,0.356294,0.38125,0.405533,0.428838,0.450866,0.471321,0.489922,0.506405,0.520529,0.532079,0.540877,0.546778,0.549684,0.549546,0.546364,0.540195,0.531141,0.519348,0.505001,0.488315,0.469534,0.448924,0.426768,0.403361,0.379005,0.354003,0.328654,0.30325,0.278068,0.253365,0.229376,0.206305,0.184323,0.163564,0.144129,0.126089,0.109484,0.0943322,0.0806247,0.0683338,0.0574133,0.0478014,0.0394234,0.0321944,0.0260215,0.0208066,0.0164491,0.0128482,0.0099058,0.00752881,0.00563174,0.00413794,0.0029794,0.00209623,0.00143613,0.000953899,0.000610891,0.000374518,0.000217726,0.000118481,5.92533e-05,2.65007e-05,1.01524e-05,3.09367e-06,6.48875e-07,6.61185e-08,8.75e-10,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8.74987e-10,6.61181e-08,6.48872e-07,3.09366e-06,1.01524e-05,2.65006e-05,5.92532e-05,0.000118481,0.000217726,0.000374517,0.00061089,0.000953898,0.00143613,0.00209623,0.0029794,0.00413794,0.00563174,0.00752881,0.00990579,0.0128482,0.016449,0.0208066,0.0260215,0.0321944,0.0394234,0.0478014,0.0574132,0.0683338,0.0806247,0.0943322,0.109484,0.126089,0.144129,0.163564,0.184323,0.206305,0.229376,0.253365,0.278068,0.30325,0.328654,0.354003,0.379005,0.403361,0.426768,0.448924,0.469534,0.488314,0.505001,0.519348,0.531141,0.540195,0.546364,0.549546,0.549684,0.546778,0.540877,0.532079,0.520529,0.506405,0.489922,0.471321,0.450866,0.428838,0.405533,0.38125,0.356294,0.330965,0.305554,0.280341,0.255585,0.231522,0.20836,0.186273,0.165398,0.145839,0.12767,0.110934,0.0956497,0.0818118,0.0693937,0.0583509,0.048623,0.0401362,0.0328065,0.0265416,0.0212437,0.0168124,0.0131469,0.0101486,0.00772385,0.00578645,0.00425892,0.0030725,0.00216656,0.00148816,0.000991449,0.000637219,0.000392349,0.000229304,0.000125616,6.33666e-05,2.86717e-05,1.11669e-05,3.4898e-06,7.64475e-07,8.54487e-08,1.57678e-09,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.48788e-10,5.04562e-08,5.477e-07,2.73434e-06,9.21306e-06,2.44631e-05,5.53563e-05,0.000111673,0.000206619,0.000357341,0.00058544,0.000917494,0.00138557,0.00202773,0.00288857,0.00401972,0.00548036,0.00733773,0.00966769,0.0125549,0.0160919,0.0203765,0.0255093,0.031591,0.0387201,0.0469899,0.0564864,0.0672852,0.0794494,0.0930266,0.108047,0.124519,0.14243,0.161741,0.182384,0.20426,0.227238,0.251152,0.275798,0.300947,0.326343,0.351708,0.376754,0.401181,0.424687,0.446969,0.467731,0.48669,0.503577,0.518146,0.530179,0.539489,0.545925,0.549382,0.549798,0.547167,0.541535,0.532995,0.521688,0.50779,0.491512,0.473092,0.452794,0.430898,0.407696,0.38349,0.358583,0.333275,0.30786,0.282618,0.25781,0.233675,0.210424,0.188232,0.167242,0.147561,0.129263,0.112395,0.0969792,0.0830105,0.070465,0.0592995,0.0494549,0.0408586,0.0334274,0.0270696,0.021688,0.0171821,0.0134511,0.0103961,0.00792291,0.00594454,0.00438272,0.00316791,0.00223878,0.00154168,0.00103017,0.000664446,0.000410853,0.000241371,0.000133091,6.77052e-05,3.09828e-05,1.22609e-05,3.92552e-06,8.95993e-07,1.09057e-07,2.67029e-09,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2.07637e-10,3.79137e-08,4.5956e-07,2.40922e-06,8.34455e-06,2.25529e-05,5.16673e-05,0.000105183,0.000195971,0.0003408,0.000560845,0.00088221,0.00133644,0.00196103,0.00279997,0.00390422,0.00533225,0.00715056,0.00943418,0.012267,0.015741,0.0199535,0.025005,0.0309963,0.0380262,0.0461887,0.0555703,0.0662479,0.0782857,0.091733,0.106621,0.122962,0.140744,0.159929,0.180454,0.202223,0.225108,0.248944,0.273533,0.298647,0.324031,0.349411,0.374498,0.398994,0.422596,0.445002,0.465914,0.485048,0.502134,0.516923,0.529195,0.538759,0.545462,0.549193,0.549886,0.547531,0.542168,0.533888,0.522826,0.509155,0.493084,0.474849,0.45471,0.432947,0.409852,0.385724,0.360868,0.335585,0.310168,0.284899,0.260041,0.235836,0.212496,0.190202,0.169098,0.149294,0.130867,0.113869,0.0983206,0.0842211,0.0715477,0.060259,0.0502971,0.0415906,0.0340572,0.0276058,0.0221395,0.0175582,0.0137609,0.0106484,0.00812605,0.00610607,0.00450938,0.00326568,0.0023129,0.00159674,0.0010701,0.000692597,0.000430048,0.000253939,0.000140918,7.22783e-05,3.34406e-05,1.3439e-05,4.4037e-06,1.04502e-06,1.37616e-07,4.30052e-09,1.89135e-37
--0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,8.34445e-11,2.79996e-08,3.83152e-07,2.11578e-06,7.54281e-06,2.07639e-05,4.81775e-05,9.89971e-05,0.000185767,0.000324878,0.000537084,0.00084802,0.00128871,0.0018961,0.00271356,0.00379138,0.00518736,0.00696722,0.0092052,0.0119844,0.0153961,0.0195369,0.0245067,0.0304054,0.0373305,0.0453742,0.054621,0.0651445,0.0770052,0.0902476,0.104897,0.120958,0.138411,0.157208,0.177274,0.198499,0.22074,0.243816,0.267508,0.291567,0.315715,0.339656,0.363078,0.38566,0.407081,0.42702,0.445166,0.461224,0.47492,0.486004,0.494262,0.499517,0.501635,0.500536,0.496192,0.48864,0.477978,0.464366,0.448012,0.429171,0.408135,0.385227,0.360791,0.335191,0.308799,0.28199,0.255133,0.228588,0.202695,0.177768,0.15409,0.131904,0.111405,0.0927366,0.0759848,0.061182,0.0483131,0.0373214,0.0281144,0.0205695,0.0145402,0.00986195,0.00635787,0.00384475,0.00213893,0.00106219,0.000447643,0.000145614,2.95488e-05,1.89591e-06,4.01348e-29
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3.41787e-11,2.53465e-08,3.96578e-07,2.31457e-06,8.50489e-06,2.38629e-05,5.60987e-05,0.000116383,0.00021999,0.000386945,0.000642669,0.00101862,0.00155295,0.00229113,0.00328661,0.00460147,0.00630704,0.00848457,0.0112259,0.0146338,0.0188213,0.0239079,0.0300159,0.0372664,0.0457751,0.0556489,0.0669814,0.0798493,0.0943085,0.11039,0.128095,0.147394,0.168218,0.19046,0.213968,0.238539,0.263921,0.289803,0.315821,0.341563,0.366583,0.390415,0.412577,0.432591,0.449988,0.464324,0.475189,0.482219,0.485108,0.483621,0.477601,0.466985,0.451816,0.432248,0.408567,0.381193,0.350687,0.317727,0.283074,0.247551,0.212005,0.177283,0.144198,0.113503,0.0858579,0.0618022,0.0417241,0.0258311,0.0141201,0.00634769,0.00200055,0.000265534,2.72534e-21
--0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,1.4419e-11,3.19138e-08,5.79481e-07,3.588e-06,1.36091e-05,3.89476e-05,9.28081e-05,0.000194442,0.000370295,0.000655151,0.00109328,0.0017396,0.00266078,0.00393645,0.00566029,0.00794122,0.0109045,0.0146929,0.019468,0.0254107,0.0327191,0.0415997,0.0522569,0.0648825,0.0796452,0.0966804,0.116079,0.137879,0.162052,0.188495,0.217018,0.247336,0.279058,0.311674,0.344547,0.376902,0.407816,0.436207,0.46085,0.480443,0.493685,0.499365,0.496437,0.484109,0.46192,0.429827,0.388284,0.338326,0.281651,0.220705,0.158757,0.0999918,0.0495837,0.0137837,6.93987e-14
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6.40831e-12,7.43581e-08,1.59466e-06,1.05185e-05,4.1248e-05,0.0001205,0.000291178,0.00061624,0.00118257,0.00210485,0.00352943,0.00563819,0.00865243,0.0128367,0.0185028,0.0260134,0.0357861,0.0482974,0.0640862,0.0837565,0.107948,0.137238,0.172024,0.212402,0.258047,0.308095,0.361019,0.414511,0.465365,0.509352,0.541102,0.553986,0.539994,0.489613,0.391712,0.233419,5.89061e-07
--0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,3.20397e-12,7.97328e-07,2.06239e-05,0.000145589,0.000591284,0.00176471,0.00432621,0.00925139,0.017893,0.0320431,0.0539944,0.0866031,0.13335,0.198402,0.286677,0.403901,0.556674,0.75253,0.999999
diff --git a/tests/acceleration/basis_v.csv b/tests/acceleration/basis_v.csv
deleted file mode 100644
index c94a685..0000000
--- a/tests/acceleration/basis_v.csv
+++ /dev/null
@@ -1,16 +0,0 @@
--55,-43.8109,-34.4226,-26.6307,-20.2431,-15.08,-10.9738,-7.76944,-5.32405,-3.50712,-2.20044,-1.29816,-0.706724,-0.344907,-0.143814,-0.0468669,-0.00981436,-0.000727236,-3.50715e-08,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0
-55,38.4547,25.0343,14.3567,6.06363,-0.179461,-4.68311,-7.73402,-9.595,-10.505,-10.6791,-10.3084,-9.56035,-8.57829,-7.48181,-6.36659,-5.30442,-4.34325,-3.50712,-2.7969,-2.20044,-1.70489,-1.29816,-0.968952,-0.706724,-0.501708,-0.344907,-0.228095,-0.143814,-0.0853766,-0.0468669,-0.0231382,-0.00981436,-0.00328929,-0.000727236,-6.26043e-05,-3.50715e-08,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,5.1993,8.80367,11.0517,12.1656,12.351,11.7976,10.6781,9.14914,7.35097,5.40731,3.42555,1.49665,-0.304835,-1.92073,-3.30929,-4.44514,-5.31934,-5.93934,-6.32846,-6.51831,-6.53949,-6.42053,-6.18789,-5.86591,-5.47688,-5.041,-4.57639,-4.09908,-3.62301,-3.16007,-2.72002,-2.31058,-1.93737,-1.60393,-1.31171,-1.06009,-0.846391,-0.666759,-0.517361,-0.394598,-0.295099,-0.215721,-0.153548,-0.105894,-0.0703003,-0.0445371,-0.0266021,-0.0147215,-0.00734969,-0.00316919,-0.00109085,-0.000253698,-2.49499e-05,-5.26072e-08,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.155396,0.572937,1.18409,1.92625,2.74271,3.58271,4.40138,5.1598,5.82495,6.36972,6.77295,7.01938,7.09966,7.01037,6.75402,6.33902,5.77971,5.09635,4.31488,3.46355,2.5688,1.65477,0.743308,-0.146042,-0.996024,-1.79168,-2.52036,-3.17168,-3.7376,-4.21232,-4.59239,-4.87662,-5.06613,-5.16434,-5.17695,-5.11197,-4.97966,-4.79138,-4.55796,-4.28946,-3.99514,-3.68349,-3.3622,-3.03819,-2.71758,-2.40572,-2.10718,-1.82571,-1.56432,-1.32521,-1.1098,-0.918738,-0.751865,-0.608253,-0.486197,-0.383503,-0.298007,-0.227671,-0.170588,-0.124978,-0.0891906,-0.0617019,-0.0411176,-0.0261716,-0.015726,-0.00877145,-0.00442677,-0.0019393,-0.000684678,-0.000166945,-1.84946e-05,-9.35239e-08,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0.00149936,0.0116041,0.0378447,0.0865795,0.162994,0.271102,0.413744,0.592588,0.80813,1.05969,1.34543,1.66232,2.00617,2.37161,2.7521,3.13993,3.52622,3.90092,4.25286,4.57085,4.84493,5.06654,5.22852,5.3251,5.35189,5.30592,5.18561,4.99075,4.72256,4.38364,3.97797,3.51095,2.98936,2.42138,1.81659,1.18594,0.541763,-0.103215,-0.737224,-1.34967,-1.93112,-2.47333,-2.96922,-3.41288,-3.79957,-4.12575,-4.389,-4.58813,-4.72308,-4.79498,-4.80613,-4.76001,-4.66125,-4.51568,-4.33027,-4.1126,-3.86991,-3.60889,-3.3357,-3.05596,-2.77474,-2.49658,-2.22548,-1.9649,-1.71776,-1.48645,-1.2728,-1.07812,-0.90318,-0.748195,-0.612856,-0.49631,-0.397168,-0.313678,-0.244098,-0.186792,-0.140222,-0.102956,-0.0736633,-0.0511164,-0.0341903,-0.0218626,-0.0132137,-0.00742682,-0.00378769,-0.00168482,-0.000609402,-0.000155319,-1.91536e-05,-1.82664e-07,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,4.27898e-06,6.84636e-05,0.000346597,0.00109542,0.00267436,0.00554555,0.0102738,0.0175267,0.0280744,0.0427898,0.0626485,0.0887288,0.122212,0.164381,0.216623,0.280427,0.357384,0.44919,0.557623,0.684295,0.830341,0.996381,1.18252,1.38836,1.61297,1.85493,2.11228,2.38257,2.66282,2.94955,3.23875,3.52591,3.80601,4.0735,4.32233,4.54593,4.73725,4.88931,4.99603,5.05236,5.05428,4.99878,4.88391,4.70871,4.47327,4.17871,3.82715,3.42177,2.96676,2.46733,1.92973,1.36124,0.770152,0.165792,-0.441508,-1.04106,-1.62297,-2.17839,-2.69948,-3.17943,-3.61247,-3.99384,-4.31982,-4.58771,-4.79584,-4.94357,-5.03127,-5.06037,-5.03328,-4.95349,-4.82547,-4.65475,-4.44787,-4.21195,-3.95385,-3.67991,-3.39595,-3.10726,-2.81866,-2.53441,-2.25829,-1.99354,-1.74291,-1.50863,-1.2924,-1.09543,-0.918411,-0.761503,-0.624372,-0.506161,-0.405507,-0.320667,-0.249893,-0.191535,-0.14405,-0.105995,-0.0760299,-0.0529177,-0.0355244,-0.0228182,-0.0138702,-0.00785415,-0.00404659,-0.00182663,-0.000676127,-0.00017964,-2.44099e-05,-3.78772e-07,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2.9226e-06,5.6839e-05,0.000306457,0.000999178,0.0024851,0.00521702,0.00975042,0.0167435,0.0269571,0.0412549,0.060603,0.0860705,0.118829,0.160153,0.211419,0.274108,0.349801,0.440184,0.547033,0.671992,0.816243,0.980453,1.16478,1.36885,1.59181,1.83226,2.08831,2.35754,2.63702,2.92332,3.21247,3.50002,3.78098,4.04986,4.30065,4.52683,4.72138,4.87728,4.98834,5.04943,5.05644,5.00626,4.89684,4.72713,4.49714,4.20788,3.86139,3.46075,3.01005,2.51441,1.98,1.41399,0.824583,0.221017,-0.386462,-0.987141,-1.57104,-2.1292,-2.65371,-3.13765,-3.57517,-3.96141,-4.29255,-4.56581,-4.77941,-4.93263,-5.02575,-5.06009,-5.03798,-4.96281,-4.83897,-4.67187,-4.46798,-4.23441,-3.97806,-3.70531,-3.42203,-3.13357,-2.84478,-2.55997,-2.28296,-2.01706,-1.76505,-1.5292,-1.31128,-1.11252,-0.933672,-0.774942,-0.63604,-0.516158,-0.413977,-0.327773,-0.25579,-0.196369,-0.147957,-0.109101,-0.0784529,-0.0547662,-0.0368972,-0.0238048,-0.0145508,-0.00829967,-0.00431853,-0.0019772,-0.000748186,-0.00020671,-3.06804e-05,-7.01722e-07,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.91751e-06,4.67615e-05,0.000269908,0.000909423,0.00230607,0.0049033,0.00924728,0.0159868,0.0258735,0.0397616,0.058608,0.0834723,0.115517,0.156007,0.20631,0.267896,0.342339,0.431314,0.536593,0.659849,0.802311,0.964693,1.14719,1.3495,1.57079,1.80973,2.06445,2.33259,2.61127,2.89711,3.18618,3.47407,3.75585,4.02607,4.27877,4.50747,4.70519,4.86487,4.98024,5.04606,5.05812,5.01324,4.90927,4.74506,4.52052,4.23658,3.89518,3.49931,3.05296,2.56117,2.03,1.46654,0.878897,0.27621,-0.33136,-0.933084,-1.51889,-2.07974,-2.60761,-3.0955,-3.53745,-3.92852,-4.2648,-4.54341,-4.76249,-4.9212,-5.01974,-5.05934,-5.04224,-4.97173,-4.8521,-4.68869,-4.48785,-4.25669,-4.00214,-3.73063,-3.44808,-3.15988,-2.87093,-2.58559,-2.30773,-2.0407,-1.78732,-1.54993,-1.33031,-1.12978,-0.949099,-0.788544,-0.647865,-0.526302,-0.422579,-0.334997,-0.26179,-0.201293,-0.151942,-0.112275,-0.0809335,-0.0566627,-0.0383093,-0.024823,-0.0152562,-0.00876388,-0.00460396,-0.00213689,-0.000825858,-0.000236731,-3.80877e-05,-1.19711e-06,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.19709e-06,3.80875e-05,0.00023673,0.000825856,0.00213689,0.00460396,0.00876387,0.0152561,0.024823,0.0383093,0.0566627,0.0809334,0.112275,0.151942,0.201293,0.26179,0.334997,0.422579,0.526301,0.647865,0.788544,0.949099,1.12978,1.33031,1.54992,1.78732,2.0407,2.30773,2.58559,2.87093,3.15988,3.44808,3.73063,4.00214,4.25669,4.48785,4.68869,4.8521,4.97173,5.04224,5.05934,5.01974,4.9212,4.76249,4.54341,4.26481,3.92852,3.53745,3.0955,2.60761,2.07974,1.51889,0.933086,0.331361,-0.276209,-0.878896,-1.46654,-2.03,-2.56117,-3.05296,-3.49931,-3.89518,-4.23658,-4.52052,-4.74506,-4.90927,-5.01324,-5.05812,-5.04606,-4.98024,-4.86487,-4.70519,-4.50747,-4.27877,-4.02607,-3.75585,-3.47407,-3.18618,-2.89711,-2.61128,-2.33259,-2.06445,-1.80973,-1.5708,-1.3495,-1.1472,-0.964693,-0.802311,-0.659849,-0.536593,-0.431314,-0.342339,-0.267896,-0.20631,-0.156007,-0.115517,-0.0834724,-0.058608,-0.0397617,-0.0258736,-0.0159869,-0.00924729,-0.00490331,-0.00230607,-0.000909425,-0.000269909,-4.67618e-05,-1.91753e-06,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7.01712e-07,3.06802e-05,0.00020671,0.000748185,0.0019772,0.00431853,0.00829966,0.0145508,0.0238047,0.0368971,0.0547662,0.0784529,0.109101,0.147957,0.196369,0.25579,0.327773,0.413977,0.516158,0.636039,0.774941,0.933671,1.11252,1.31128,1.5292,1.76505,2.01706,2.28296,2.55997,2.84478,3.13357,3.42203,3.70531,3.97806,4.23441,4.46798,4.67187,4.83897,4.96281,5.03798,5.06009,5.02575,4.93263,4.77941,4.56581,4.29255,3.96141,3.57517,3.13766,2.65371,2.1292,1.57104,0.987142,0.386464,-0.221016,-0.824582,-1.41399,-1.98,-2.51441,-3.01005,-3.46075,-3.86139,-4.20788,-4.49714,-4.72713,-4.89684,-5.00626,-5.05644,-5.04943,-4.98834,-4.87728,-4.72138,-4.52683,-4.30065,-4.04986,-3.78098,-3.50002,-3.21247,-2.92332,-2.63702,-2.35754,-2.08831,-1.83226,-1.59181,-1.36885,-1.16478,-0.980454,-0.816244,-0.671993,-0.547033,-0.440184,-0.349801,-0.274108,-0.211419,-0.160153,-0.118829,-0.0860706,-0.060603,-0.0412549,-0.0269571,-0.0167435,-0.00975043,-0.00521703,-0.00248511,-0.00099918,-0.000306457,-5.68392e-05,-2.92262e-06,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3.78766e-07,2.44098e-05,0.000179639,0.000676125,0.00182662,0.00404658,0.00785414,0.0138702,0.0228181,0.0355243,0.0529177,0.0760298,0.105995,0.14405,0.191535,0.249893,0.320667,0.405507,0.506161,0.624371,0.761503,0.91841,1.09543,1.2924,1.50863,1.74291,1.99354,2.25828,2.53441,2.81866,3.10726,3.39595,3.67991,3.95385,4.21195,4.44787,4.65475,4.82547,4.95349,5.03328,5.06037,5.03127,4.94357,4.79584,4.58771,4.31982,3.99384,3.61247,3.17943,2.69948,2.17839,1.62297,1.04106,0.441509,-0.165791,-0.770151,-1.36124,-1.92973,-2.46733,-2.96676,-3.42177,-3.82715,-4.17871,-4.47327,-4.70871,-4.88391,-4.99878,-5.05428,-5.05236,-4.99603,-4.88931,-4.73725,-4.54593,-4.32233,-4.0735,-3.80601,-3.52591,-3.23875,-2.94955,-2.66282,-2.38257,-2.11228,-1.85493,-1.61297,-1.38836,-1.18252,-0.996381,-0.830341,-0.684295,-0.557623,-0.44919,-0.357385,-0.280427,-0.216623,-0.164381,-0.122212,-0.0887289,-0.0626485,-0.0427898,-0.0280744,-0.0175267,-0.0102738,-0.00554556,-0.00267436,-0.00109542,-0.000346598,-6.84639e-05,-4.27901e-06,-8.82966e-29
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.82661e-07,1.91535e-05,0.000155319,0.000609401,0.00168482,0.00378768,0.00742681,0.0132137,0.0218625,0.0341902,0.0511164,0.0736633,0.102956,0.140222,0.186792,0.244098,0.313677,0.397168,0.49631,0.612856,0.748195,0.903179,1.07812,1.2728,1.48645,1.71776,1.9649,2.22548,2.49658,2.77474,3.05596,3.3357,3.60889,3.86991,4.1126,4.33027,4.51568,4.66125,4.76001,4.80613,4.79498,4.72308,4.58813,4.389,4.12575,3.79957,3.41288,2.96922,2.47333,1.93112,1.34967,0.737225,0.103216,-0.541761,-1.18594,-1.81658,-2.42138,-2.98936,-3.51095,-3.97797,-4.38364,-4.72256,-4.99075,-5.18561,-5.30592,-5.35189,-5.3251,-5.22852,-5.06654,-4.84493,-4.57085,-4.25286,-3.90092,-3.52623,-3.13993,-2.7521,-2.37161,-2.00617,-1.66232,-1.34543,-1.0597,-0.808131,-0.592588,-0.413744,-0.271102,-0.162994,-0.0865796,-0.0378448,-0.0116041,-0.00149937,-1.49894e-20
--0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,9.35217e-08,1.84945e-05,0.000166944,0.000684676,0.00193929,0.00442677,0.00877144,0.015726,0.0261716,0.0411176,0.0617019,0.0891906,0.124978,0.170588,0.227671,0.298007,0.383503,0.486197,0.608253,0.751865,0.918738,1.1098,1.32521,1.56432,1.82571,2.10717,2.40572,2.71758,3.03819,3.3622,3.68349,3.99514,4.28946,4.55796,4.79138,4.97966,5.11197,5.17695,5.16434,5.06613,4.87662,4.59239,4.21232,3.7376,3.17168,2.52036,1.79168,0.996026,0.146044,-0.743306,-1.65477,-2.5688,-3.46355,-4.31488,-5.09635,-5.77971,-6.33902,-6.75402,-7.01037,-7.09966,-7.01938,-6.77295,-6.36972,-5.82495,-5.1598,-4.40138,-3.58271,-2.74271,-1.92625,-1.18409,-0.572938,-0.155397,-7.63386e-13
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.26056e-08,2.49498e-05,0.000253697,0.00109085,0.00316918,0.00734967,0.0147215,0.0266021,0.0445371,0.0703003,0.105894,0.153548,0.215721,0.295099,0.394598,0.517361,0.666759,0.846391,1.06008,1.31171,1.60393,1.93737,2.31058,2.72002,3.16007,3.62301,4.09908,4.57639,5.041,5.47688,5.86591,6.18788,6.42053,6.53949,6.51831,6.32846,5.93934,5.31934,4.44514,3.30929,1.92074,0.304838,-1.49664,-3.42555,-5.40731,-7.35096,-9.14914,-10.6781,-11.7976,-12.351,-12.1656,-11.0517,-8.80367,-5.19931,-1.29593e-05
--0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,-0,3.50698e-08,6.26039e-05,0.000727234,0.00328929,0.00981434,0.0231382,0.0468668,0.0853765,0.143814,0.228095,0.344907,0.501707,0.706723,0.968952,1.29816,1.70489,2.20044,2.7969,3.50712,4.34325,5.30442,6.36658,7.48181,8.57829,9.56035,10.3084,10.6791,10.505,9.59501,7.73403,4.68312,0.179472,-6.06362,-14.3567,-25.0343,-38.4546,-55
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3.50682e-08,0.000727231,0.00981432,0.0468668,0.143813,0.344907,0.706723,1.29816,2.20044,3.50711,5.32405,7.76943,10.9738,15.08,20.2431,26.6307,34.4226,43.8108,55
diff --git a/tests/acceleration/bench_accelerations.cpp b/tests/acceleration/bench_accelerations.cpp
deleted file mode 100644
index 9039ff5..0000000
--- a/tests/acceleration/bench_accelerations.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#define BLAST_TRACE_LEVEL 3
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include "tracy/Tracy.hpp"
-#include "tracy/TracyC.h"
-
-#include <fcntl.h> // _O_WRONLY
-#include <io.h>    // _open, _dup, _dup2, _close
-
-struct IOSilencer {
-  int             saved_stdout_fd = -1;
-  int             null_fd         = -1;
-  std::streambuf* old_cout        = nullptr;
-  std::streambuf* old_cerr        = nullptr;
-  std::ofstream   null_stream;
-
-  IOSilencer() {
-    // Flush buffers
-    std::cout.flush();
-    std::cerr.flush();
-    fflush(stdout);
-
-    // Redirect std::cout and std::cerr to /dev/null
-    null_stream.open("NUL"); // Windows null device
-    old_cout = std::cout.rdbuf();
-    old_cerr = std::cerr.rdbuf();
-    std::cout.rdbuf(null_stream.rdbuf());
-    std::cerr.rdbuf(null_stream.rdbuf());
-
-    // Redirect printf (C stdout)
-    saved_stdout_fd = _dup(_fileno(stdout));
-    null_fd         = _open("NUL", _O_WRONLY);
-    _dup2(null_fd, _fileno(stdout));
-  }
-
-  ~IOSilencer() {
-    std::cout.rdbuf(old_cout);
-    std::cerr.rdbuf(old_cerr);
-    fflush(stdout);
-
-    _dup2(saved_stdout_fd, _fileno(stdout));
-    _close(null_fd);
-    _close(saved_stdout_fd);
-  }
-};
-
-namespace blast {
-
-TEST_CASE("Benchmark accelerations", "[accelerations]") {
-
-  ManipulatorTempData manip_data;
-
-  // Optimization opt(get_generic_gen3(), get_gen3_task());
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 100, 5, 6);
-  opt.bspline = bspline;
-
-  opt.guess.type      = Guess::custom;
-  opt.guess.initial_x = Array(opt.bspline.x_len(opt.task), 2.0);
-  // opt.guess.n_random_shots = 100;
-
-  opt.constraints.position            = true;
-  opt.constraints.velocity            = true;
-  opt.constraints.acceleration        = true;
-  opt.constraints.torque              = true;
-  opt.constraints.tool_speed          = false;
-  opt.constraints.self_collisions     = false;
-  opt.constraints.external_collisions = false;
-
-  opt.max_tries         = 1;
-  opt.success_tolerance = 0.01;
-
-  Result result(&opt);
-  Result result_dev(&opt);
-  Result result_dev_new(&opt);
-
-  BENCHMARK_ADVANCED("optimize")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      IOSilencer _;
-      result = optimize(&opt, OptimizationMethod::baseline);
-      return result;
-    });
-  };
-
-  BENCHMARK_ADVANCED("optimize with_analytical_pva")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      IOSilencer _;
-      result_dev = optimize(&opt, OptimizationMethod::with_analytical_pva);
-      return result_dev;
-    });
-  };
-
-  // BENCHMARK_ADVANCED("optimize with_analytical_dynamics")(Catch::Benchmark::Chronometer meter) {
-  //   meter.measure([&] {
-  //     IOSilencer _;
-  //     result_dev_new = optimize(&opt, OptimizationMethod::with_analytical_dynamics);
-  //     return result_dev_new;
-  //   });
-  // };
-
-  CHECK(std::abs(result.x.back() - result_dev.x.back()) < 1e-5);
-  // CHECK(std::abs(result.x.back() - result_dev_new.x.back()) < 1e-5);
-}
-
-} // namespace blast
diff --git a/tests/acceleration/bench_gradients_acceleration.cpp b/tests/acceleration/bench_gradients_acceleration.cpp
deleted file mode 100644
index a0874bb..0000000
--- a/tests/acceleration/bench_gradients_acceleration.cpp
+++ /dev/null
@@ -1,161 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-
-#include "blast"
-#include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
-using namespace blast;
-
-struct BenchCase {
-  u32 nctrl;
-  u32 npoints;
-  u32 p;
-};
-
-TEST_CASE("benchmark phase 1 gradient acceleration  (P,V,A,Tau)", "[Acceleration]") {
-  using namespace blast;
-
-  std::string input_file_initial_guess = "../../../examples/acceleration/files/link6_initial_guess.csv";
-
-  // blast::begin_profile();
-  // --- Define Manipulator ---
-  auto link6 = get_generic_Link6();
-
-  // --- Define Tasks ---
-  u32                task_size = 7;
-  std::vector<Array> positions(task_size + 1);
-  positions[0] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};                   // zero
-  positions[1] = deg2rad({-40.4, -26.9, 83.6, 1.5, 20.0, -42.2});  // w1 + 10
-  positions[2] = deg2rad({51.9, -13.6, 107.9, 3.6, 33.1, 51.2});   // wb1
-  positions[3] = deg2rad({-33.9, -35.6, 70.9, -0.7, 16.9, -31.8}); // w3 + 10
-  positions[4] = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});    // wb3
-  positions[5] = deg2rad({-20.0, -19.3, 95.9, -3.0, 23.5, -17.6}); // w4 + 10
-  positions[6] = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});    // wb4
-  positions[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};                   // zero
-
-  std::vector<Matrix> tasks;
-  for (u32 i = 0; i < positions.size() - 1; i++) {
-    Matrix tmp(6, 6);
-
-    for (u32 j = 0; j < link6.joints; j++) {
-      tmp(j, 0) = positions[i][j];
-      tmp(j, 3) = positions[i + 1][j];
-    }
-    tasks.push_back(tmp);
-  }
-
-  // --- Define Guess ---
-  Guess guess;
-  // std::vector<Guess> guess_predefined;
-
-  // auto xlen = bspline.xlen(tasks[0]);
-  // guess_predefined = read_array_from_csv(input_file_initial_guess, xlen);
-  // Assert(!guess_predefined.empty());
-
-
-  // -- Define Constraints ---
-  Constraints<Manipulator> constraints;
-  constraints.show_info = false;
-
-  constraints.position     = true;
-  constraints.velocity     = true;
-  constraints.acceleration = true;
-  constraints.torque       = true;
-
-  constraints.tool_speed              = false;
-  constraints.self_collisions         = false;
-  constraints.external_collisions     = false;
-  constraints.n_collision_constraints = 100;
-  constraints.n_collision_skip        = 2;
-
-  // --- Define Objective ---
-  Objective<Manipulator> objective;
-  objective.time_weight = 1;
-
-  u32               bench_counter = 0;
-  std::vector<real> traj_time(task_size);
-  std::vector<real> traj_time_acc(task_size);
-  std::vector<real> comp_time(task_size);
-  std::vector<real> comp_time_acc(task_size);
-
-  std::vector<BenchCase> cases;
-  const u32              nctrl[]   = {12, 16, 20};
-  const u32              npoints[] = {100, 200, 256};
-  const u32              p[]       = {3, 5};
-  for (const auto& p_val: p) {
-    for (const auto& n_ctrl: nctrl) {
-      for (const auto& n_points: npoints) {
-        cases.push_back({n_ctrl, n_points, p_val});
-      }
-    }
-  }
-
-  for (u32 c = 0; c < cases.size(); c++) {
-
-    // --- Define Bspline ---
-    Bspline bspline(cases[c].nctrl, cases[c].npoints, cases[c].p, 6);
-
-    for (u32 t = 0; t < tasks.size(); t++) {
-      // get underlying buffer
-      std::streambuf* orig_buf = cout.rdbuf();
-
-      // Base Line Optimization
-      Optimization<Manipulator> opt(link6, tasks[t]);
-
-      opt.set_bspline(bspline);
-      opt.set_guess(guess);
-      opt.set_constraints(constraints);
-      opt.set_objective(objective);
-
-      bench_counter = 0;
-      // set null
-      cout.rdbuf(NULL);
-      BENCHMARK("Optimize") {
-        auto results = optimize(&opt, OptimizationMethod::baseline);
-        traj_time[t] += results.x[results.x.size - 1];
-        comp_time[t] += results.compute_time;
-        bench_counter++;
-      };
-      traj_time[t] = traj_time[t] / bench_counter;
-      comp_time[t] = comp_time[t] / bench_counter;
-      // restore buffer
-      cout.rdbuf(orig_buf);
-
-      // Acceleration 1 Optimization
-      Optimization<Manipulator> opt_acc(link6, tasks[t]);
-      opt_acc.set_bspline(bspline);
-      opt_acc.set_guess(guess);
-      opt_acc.set_constraints(constraints);
-      opt_acc.set_objective(objective);
-
-      bench_counter = 0;
-      // set null
-      cout.rdbuf(NULL);
-      BENCHMARK("Optimize Acceleration") {
-        auto result_acc = optimize_acc(&opt_acc);
-        traj_time_acc[t] += result_acc.x[result_acc.x.size - 1];
-        comp_time_acc[t] += result_acc.compute_time;
-        bench_counter++;
-      };
-      traj_time_acc[t] = traj_time_acc[t] / bench_counter;
-      comp_time_acc[t] = comp_time_acc[t] / bench_counter;
-      // restore buffer
-      cout.rdbuf(orig_buf);
-    }
-
-    std::cout << std::endl;
-    std::cout << "Benchmark Results for n_ctrl = " << bspline.n_ctrl << ", n_points = " << bspline.n_points << " & p = " << bspline.degree << std::endl;
-    for (u32 i = 0; i < traj_time.size(); i++) {
-      auto acceleration = (comp_time[i] - comp_time_acc[i]) / comp_time[i] * 100;
-      auto quality      = 100 - (traj_time[i] - traj_time_acc[i]) / traj_time[i] * 100;
-
-      std::cout << "Traj t:        " << i << std::endl;
-      std::cout << "Acceleration:  " << acceleration << "%" << std::endl;
-      std::cout << "Quality:       " << quality << "%" << std::endl;
-      std::cout << "Traj time:     " << traj_time[i] << " Comp time:     " << comp_time[i] << std::endl;
-      std::cout << "Traj time acc: " << traj_time_acc[i] << " Comp time acc: " << comp_time_acc[i] << std::endl;
-    }
-  }
-}
diff --git a/tests/acceleration/bench_inplace.cpp b/tests/acceleration/bench_inplace.cpp
deleted file mode 100644
index 127f9f0..0000000
--- a/tests/acceleration/bench_inplace.cpp
+++ /dev/null
@@ -1,216 +0,0 @@
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#define BLAST_TRACE_LEVEL 3
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include "tracy/Tracy.hpp"
-#include "tracy/TracyC.h"
-
-#include <fcntl.h> // _O_WRONLY
-#include <io.h>    // _open, _dup, _dup2, _close
-
-struct IOSilencer {
-  int             saved_stdout_fd = -1;
-  int             null_fd         = -1;
-  std::streambuf* old_cout        = nullptr;
-  std::streambuf* old_cerr        = nullptr;
-  std::ofstream   null_stream;
-
-  IOSilencer() {
-    // Flush buffers
-    std::cout.flush();
-    std::cerr.flush();
-    fflush(stdout);
-
-    // Redirect std::cout and std::cerr to /dev/null
-    null_stream.open("NUL"); // Windows null device
-    old_cout = std::cout.rdbuf();
-    old_cerr = std::cerr.rdbuf();
-    std::cout.rdbuf(null_stream.rdbuf());
-    std::cerr.rdbuf(null_stream.rdbuf());
-
-    // Redirect printf (C stdout)
-    saved_stdout_fd = _dup(_fileno(stdout));
-    null_fd         = _open("NUL", _O_WRONLY);
-    _dup2(null_fd, _fileno(stdout));
-  }
-
-  ~IOSilencer() {
-    std::cout.rdbuf(old_cout);
-    std::cerr.rdbuf(old_cerr);
-    fflush(stdout);
-
-    _dup2(saved_stdout_fd, _fileno(stdout));
-    _close(null_fd);
-    _close(saved_stdout_fd);
-  }
-};
-
-namespace blast {
-
-TEST_CASE("compute_constraints vs constraints_with_segments", "[acceleration]") {
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 110, 5, 6);
-  opt.bspline = bspline;
-
-  auto x_len    = opt.bspline.x_len(opt.task);
-  auto n_joints = opt.manip.n_joints;
-
-  opt.guess.type = Guess::random;
-
-
-  opt.constraints.position            = true;
-  opt.constraints.velocity            = true;
-  opt.constraints.acceleration        = true;
-  opt.constraints.torque              = true;
-  opt.constraints.tool_speed          = false;
-  opt.constraints.self_collisions     = false;
-  opt.constraints.external_collisions = false;
-
-  opt.max_tries         = 1;
-  opt.success_tolerance = 0.01;
-
-  real eps = 1e-5;
-
-  initialize_optimization(&opt);
-  n_con(&opt);
-
-  auto n_constraints = opt.constraints.n_constraints;
-
-  auto x = init_guess(&opt);
-  bspline.compute_trajectory(x, opt.task);
-
-  BENCHMARK_ADVANCED("Original nlopt_constraints function")
-  (Catch::Benchmark::Chronometer meter) {
-    Array result(n_constraints);
-    Array grad(x_len * n_constraints);
-    meter.measure([&] {
-      nlopt_constraints(n_constraints, result.data, x_len, x.data, grad.data, &opt);
-      return result;
-    });
-  };
-
-  BENCHMARK_ADVANCED("New nlopt_constraints_with_segments function")
-  (Catch::Benchmark::Chronometer meter) {
-    auto  n_constraints_with_segments = (n_joints * 4) * ((int) opt.bspline.n_ctrl - (int) opt.bspline.degree);
-    Array result(n_constraints_with_segments);
-    Array grad(x_len * n_constraints_with_segments);
-    meter.measure([&] {
-      nlopt_constraints_with_segments(n_constraints_with_segments, result.data, x_len, x.data, grad.data, &opt);
-      return result;
-    });
-  };
-};
-
-TEST_CASE("Optimization with compute_constraints vs constraints_and_gradients_with_segments", "[acceleration]") {
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 110, 5, 6);
-  opt.bspline = bspline;
-
-  auto x_len    = opt.bspline.x_len(opt.task);
-  auto n_joints = opt.manip.n_joints;
-
-  // opt.guess.type = Guess::random;
-
-  opt.constraints.position            = true;
-  opt.constraints.velocity            = true;
-  opt.constraints.acceleration        = true;
-  opt.constraints.torque              = true;
-  opt.constraints.tool_speed          = false;
-  opt.constraints.self_collisions     = false;
-  opt.constraints.external_collisions = false;
-
-  opt.max_tries         = 1;
-  opt.success_tolerance = 0.01;
-
-  // initialize_optimization(&opt);
-  // n_con(&opt);
-  // note: use the same initial guess
-  opt.guess.type      = Guess::custom;
-  opt.guess.initial_x = Array(x_len, 2.0);
-  // note: this only works with 16 n_ctrl
-  // opt.guess.initial_x   = Array{0.152355, -0.597958, -0.756348, 0.558375, -0.946557, -0.10268, 0.901938, -0.145201, -0.436917, -0.827993,
-  //                      -0.210473, -0.0664338, 0.370776, -0.29955, -0.139142, 0.757138, -0.157328, 0.909626, -0.214084, 0.745555,
-  //                      -0.0416009, 0.375275, 0.126153, 0.402337, 0.723458, -0.070362, -0.444889, 0.784517, 0.655188, 0.216073,
-  //                      0.889643, -0.969162, -0.0279569, 0.496376, -0.891977, 0.0247388, -0.332795, -0.239096, -0.418835, 0.922935,
-  //                      0.124817, 0.34831, -0.00905602, -0.157159, 0.492551, 0.0325292, -0.299891, 0.828649, -0.909505, -0.294613,
-  //                      -0.768063, -0.489629, 0.0918699, -0.603901, -0.391773, -0.919239, 0.472415, 0.591886, -0.494069, 0.0488416,
-  //                      4.81184};
-
-  Array x_orig = opt.guess.initial_x;
-
-  Result result(&opt);
-  Result result_with_segments(&opt);
-
-  // result_with_segments = optimize(&opt);
-
-  // cout << "Compute time:        " << result_with_segments.compute_time << endl;
-  // cout << "Function evaluations:" << result_with_segments.num_eval << endl;
-  // cout << "Trajectory time:     " << result_with_segments.x[result_with_segments.x.size - 1] << endl;
-  // cout << "Success:             " << result_with_segments.success << endl;
-  // cout << "False success:       " << result_with_segments.success_false << endl;
-  // cout << "Number of tries:     " << result_with_segments.num_tries << endl;
-  // cout << "Stopping criteria:   " << result_with_segments.nlopt_exit_criteria << endl;
-
-  // CHECK(result_with_segments.x.size > 0);
-
-  // nlopt_result sqp(blast::Array& x, blast::Optimization& opt, nlopt_stopping* stop);
-  BENCHMARK_ADVANCED("OG")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      IOSilencer _;
-      result = optimize(&opt, OptimizationMethod::baseline);
-      return result;
-    });
-  };
-
-  // opt.guess.type = Guess::custom;
-  opt.guess.initial_x = x_orig;
-
-  BENCHMARK_ADVANCED("New OG")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      IOSilencer _;
-      result_with_segments = optimize(&opt);
-      return result;
-    });
-  };
-  cout << endl;
-  cout << "=============== Optimization Parameters ===============" << endl;
-  cout << " n_ctrl : " << opt.bspline.n_ctrl << endl;
-  cout << " n_points : " << opt.bspline.n_points << endl;
-  cout << endl;
-  cout << "================== Result of last OG ==================" << endl;
-  cout << "Compute time:        " << result.compute_time << endl;
-  cout << "Function evaluations:" << result.num_eval << endl;
-  cout << "Trajectory time:     " << result.x[result.x.size - 1] << endl;
-  cout << "Success:             " << result.success << endl;
-  cout << "False success:       " << result.success_false << endl;
-  cout << "Number of tries:     " << result.num_tries << endl;
-  cout << "Stopping criteria:   " << result.nlopt_exit_criteria << endl;
-
-  cout << endl;
-  cout << "============= Result of last with_segment =============" << endl;
-  cout << "Compute time:        " << result_with_segments.compute_time << endl;
-  cout << "Function evaluations:" << result_with_segments.num_eval << endl;
-  cout << "Trajectory time:     " << result_with_segments.x[result_with_segments.x.size - 1] << endl;
-  cout << "Success:             " << result_with_segments.success << endl;
-  cout << "False success:       " << result_with_segments.success_false << endl;
-  cout << "Number of tries:     " << result_with_segments.num_tries << endl;
-  cout << "Stopping criteria:   " << result_with_segments.nlopt_exit_criteria << endl;
-
-
-  CHECK(std::abs(result.x.back() - result_with_segments.x.back()) < 1e-5);
-};
-
-} // namespace blast
diff --git a/tests/acceleration/link6_initial_guess.csv b/tests/acceleration/link6_initial_guess.csv
deleted file mode 100644
index 1ffd303..0000000
--- a/tests/acceleration/link6_initial_guess.csv
+++ /dev/null
@@ -1,13 +0,0 @@
-0.520965,0.258472,0.353055,-0.479498,0.129781,-0.852219,-0.0341322,0.562547,0.722244,0.092291,-0.0588835,-0.00196774,-0.517492,-0.472568,-0.25479,0.628049,-0.948133,0.358083,-0.394205,0.66981,0.253117,0.377355,-0.673516,-0.430921,-0.13931,0.551757,0.292389,-0.645706,0.974206,0.92809,0.510702,-0.385604,-0.398394,-0.233628,0.728358,-0.792466,-0.143766,-0.980695,-0.841097,0.403066,-0.264548,0.618282,0.170691,-0.0625544,-0.500578,0.753689,0.314454,0.79467,-0.27201,0.624692,0.232615,0.862111,0.901236,-0.00691529,-0.816532,0.176593,0.976859,0.126807,-0.160727,0.327277,4.52033
--0.322957,0.494893,-0.394444,-0.526774,-0.999386,0.469154,0.251039,0.581827,-0.937002,-0.237725,-0.345,0.411473,-0.573926,-0.304383,0.543799,0.289972,-0.116742,-0.722357,0.226391,0.338805,0.599832,0.321683,-0.0793167,0.629161,-0.485238,0.125874,-0.575462,0.578127,0.230505,0.262837,0.242295,0.243727,0.333438,-0.238489,0.438601,0.825494,0.154115,0.436012,-0.836427,-0.726538,0.147075,0.673927,-0.175729,0.460718,-0.796815,-0.15328,0.394426,-0.610332,0.646642,-0.633024,-0.700264,0.460006,0.933387,0.0823035,0.384415,-0.347608,-0.517365,-0.983551,-0.675529,-0.563094,4.35
-0.42875,-0.887812,-0.371282,-0.0274917,0.490703,-0.66468,-0.343902,-0.520898,0.930896,0.489652,-0.365102,-0.733215,-0.133073,-0.978865,-0.638515,-0.879184,-0.887443,-0.900226,-0.670546,-0.334131,0.376741,0.811148,-0.667417,-0.956197,0.126404,0.731607,0.806414,0.954959,-0.794363,0.125197,0.665748,0.505053,-0.659447,0.435736,-0.557805,0.974181,-0.669661,-0.0221464,0.343606,0.45031,0.0742528,0.768744,0.343459,-0.915387,0.413454,0.638794,-0.983559,-0.957862,0.187533,0.652465,0.168632,0.692215,0.768627,0.570359,-0.648847,0.750367,-0.296888,0.518309,-0.0434154,0.129424,4.9525
-0.152355,-0.597958,-0.756348,0.558375,-0.946557,-0.10268,0.901938,-0.145201,-0.436917,-0.827993,-0.210473,-0.0664338,0.370776,-0.29955,-0.139142,0.757138,-0.157328,0.909626,-0.214084,0.745555,-0.0416009,0.375275,0.126153,0.402337,0.723458,-0.070362,-0.444889,0.784517,0.655188,0.216073,0.889643,-0.969162,-0.0279569,0.496376,-0.891977,0.0247388,-0.332795,-0.239096,-0.418835,0.922935,0.124817,0.34831,-0.00905602,-0.157159,0.492551,0.0325292,-0.299891,0.828649,-0.909505,-0.294613,-0.768063,-0.489629,0.0918699,-0.603901,-0.391773,-0.919239,0.472415,0.591886,-0.494069,0.0488416,4.81184
--0.705123,0.430079,0.592632,-0.231201,-0.980294,0.489665,0.884675,0.30794,-0.821478,-0.55112,-0.568708,0.201885,0.211323,0.943103,-0.396686,0.180336,0.574745,0.683153,-0.0472159,-0.514735,0.259958,0.193906,-0.331232,0.861172,0.365647,-0.327575,-0.163685,-0.0942069,-0.388764,0.543858,0.837717,-0.290369,0.853476,0.76,-0.935924,0.00656872,-0.496119,0.736197,0.630257,-0.454549,-0.231908,0.827507,-0.953683,0.0960177,0.922729,0.0408372,-0.487053,-0.341823,0.318078,0.755618,0.0177072,0.959472,-0.375499,-0.534804,0.318079,-0.116296,0.000778529,0.577933,-0.62405,-0.993203,5.09817
-0.441144,-0.344259,-0.218319,0.785685,-0.432943,-0.894713,0.970185,-0.579886,0.654717,-0.81531,0.378464,0.259737,0.535515,-0.105977,0.350296,0.0513789,0.851923,0.611471,0.876702,0.477735,0.84055,0.316317,0.970379,0.922536,-0.375314,-0.514059,-0.90619,0.104206,0.578632,0.216013,0.484626,0.997461,0.0708144,0.604829,-0.123263,0.757253,-0.759755,-0.407946,-0.338477,-0.266929,0.121277,0.515909,0.74369,-0.988798,-0.332917,-0.0378804,0.65366,0.196389,-0.48901,0.38025,0.787443,-0.92237,0.584872,-0.736803,-0.245882,-0.461087,0.564595,-0.637341,-0.650151,0.408478,4.88295
-0.461934,0.416563,0.759797,0.883314,-0.551144,0.06367,0.846719,0.849024,-0.934095,-0.677953,-0.928724,-0.774815,-0.4249,-0.403751,0.0234722,-0.149695,0.135661,0.501791,0.284144,0.176617,0.892421,-0.981815,-0.321736,-0.159931,-0.577257,-0.676006,-0.282954,0.0296403,0.545578,-0.145398,0.884892,0.205937,-0.653268,0.389787,-0.916189,-0.13221,0.660109,0.266415,0.90696,-0.479171,-0.736901,-0.265289,0.252006,-0.601526,-0.172575,-0.956541,-0.294268,0.589953,-0.578863,0.374748,0.708428,-0.856035,0.437489,0.729699,0.608779,0.761617,-0.490753,0.321853,-0.169191,-0.372199,4.7645
-0.110816,-0.680806,-0.506191,0.384226,0.277994,-0.561718,-0.453835,0.692472,0.773287,-0.210047,-0.23769,0.22661,0.760994,0.52769,-0.574774,0.806149,0.949169,0.49577,-0.456424,0.862651,0.604636,0.36188,0.632104,0.696239,-0.59854,-0.675195,-0.494403,0.109994,0.108378,0.412812,-0.180719,0.772824,-0.602801,0.26776,0.627365,0.0906862,-0.929546,0.537274,-0.935786,0.690892,-0.743691,-0.912439,0.5237,-0.487869,-0.707965,0.171759,-0.465367,0.521727,-0.419507,0.758097,0.834168,-0.689636,0.475491,-0.954855,0.126278,-0.441254,-0.851771,0.0525906,-0.822495,-0.0698428,4.69848
--0.13308,0.764478,-0.316978,0.0900733,0.729565,-0.211314,0.377133,-0.0738052,0.795185,-0.962169,-0.728975,-0.352073,-0.0970928,-0.59424,-0.906304,0.819647,0.509195,0.0821788,-0.61926,0.884999,0.374375,0.681736,-0.81485,0.538115,-0.439309,0.0407956,-0.4536,0.0993263,0.853433,0.558214,-0.555945,0.154766,-0.955163,0.111509,-0.9201,-0.498825,-0.245263,-0.783614,-0.93775,0.898931,0.798689,0.0133889,-0.816083,-0.117214,-0.605874,0.991518,-0.991487,-0.0418258,0.432414,0.871649,-0.0847334,0.774002,0.901636,0.436838,0.160996,0.41317,-0.424878,-0.215561,-0.13097,0.973977,4.88573
--0.389829,0.995115,-0.911855,-0.140816,0.805252,-0.0449156,-0.455307,0.927629,-0.684028,0.00404764,0.700396,0.92993,0.965023,-0.166918,0.607259,0.313,0.0362969,-0.626462,0.326739,0.360886,0.827726,-0.91424,0.127536,-0.96053,-0.072841,-0.346406,0.270619,-0.959506,-0.729662,0.781363,0.00354696,-0.608973,0.73658,-0.160813,0.249944,-0.0327569,-0.444414,0.332633,-0.85443,-0.0743272,-0.323251,-0.950109,0.404635,-0.112177,0.980866,0.893286,0.0488869,-0.52359,-0.297255,-0.157782,0.193092,0.834074,0.935295,0.734051,0.2113,-0.75416,-0.188389,0.276441,0.0753209,-0.943587,4.95207
--0.294063,0.15152,0.81439,0.293338,-0.327832,-0.510057,-0.372365,0.559136,-0.0472755,-0.3352,-0.830152,0.381336,0.322457,0.346809,-0.11046,-0.864772,0.316278,0.392142,-0.250097,0.180551,0.923916,-0.168378,0.76603,-0.872913,0.790998,-0.823941,-0.208043,-0.153408,0.741896,0.293525,-0.239328,-0.0145848,-0.763236,0.657055,0.271266,-0.114699,-0.986472,-0.711608,-0.330149,-0.42131,-0.110895,-0.545975,-0.500277,-0.634017,0.466197,-0.344158,-0.249135,-0.595451,-0.561941,-0.938055,-0.889135,-0.181065,-0.416551,0.234326,0.324663,-0.571463,-0.338472,-0.273767,0.140561,-0.554213,4.91382
--0.386051,-0.61461,0.364816,0.956527,0.0223092,-0.935387,0.694861,-0.23125,0.821111,-0.113046,0.858836,-0.899095,-0.120211,-0.335518,-0.522938,-0.0932843,-0.495172,0.817418,0.587667,-0.375358,-0.166389,0.64778,-0.67282,0.141167,-0.882286,-0.331092,-0.977467,0.408928,0.959399,0.381655,0.768418,0.325231,0.889859,-0.0110819,-0.893443,0.285845,-0.514382,0.795294,-0.0274769,-0.350973,0.105699,0.0249596,-0.951003,0.470288,0.0541411,-0.367121,-0.378976,0.360538,0.38001,-0.525894,0.86405,-0.852519,0.548804,0.907365,-0.219511,0.161662,-0.0214803,0.522304,-0.468518,0.478572,4.99391
--0.206097,0.234871,0.557573,0.21214,-0.524197,-0.635708,0.595647,-0.563299,0.210018,0.785316,0.499231,0.00699991,0.717831,-0.287876,0.181288,0.650762,-0.363863,-0.327473,-0.771212,-0.919833,0.886497,-0.386509,0.590446,0.851038,0.872316,-0.50391,0.223072,0.653468,-0.688834,-0.705991,0.344056,0.999774,-0.776248,-0.303832,0.259117,0.341955,-0.00395953,-0.106715,0.218965,0.541155,0.247033,0.959367,-0.177021,0.478178,0.759755,0.637176,-0.484309,0.45421,-0.418053,0.385094,0.308476,-0.114677,-0.8979,0.642805,0.175833,0.071765,-0.586686,-0.648394,-0.552792,0.88556,4.88095
diff --git a/tests/acceleration/test_accelerations.cpp b/tests/acceleration/test_accelerations.cpp
deleted file mode 100644
index 386e78b..0000000
--- a/tests/acceleration/test_accelerations.cpp
+++ /dev/null
@@ -1,365 +0,0 @@
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#define BLAST_TRACE_LEVEL 3
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include "tracy/Tracy.hpp"
-#include "tracy/TracyC.h"
-
-#include <fcntl.h> // _O_WRONLY
-#include <io.h>    // _open, _dup, _dup2, _close
-
-struct IOSilencer {
-  int             saved_stdout_fd = -1;
-  int             null_fd         = -1;
-  std::streambuf* old_cout        = nullptr;
-  std::streambuf* old_cerr        = nullptr;
-  std::ofstream   null_stream;
-
-  IOSilencer() {
-    // Flush buffers
-    std::cout.flush();
-    std::cerr.flush();
-    fflush(stdout);
-
-    // Redirect std::cout and std::cerr to /dev/null
-    null_stream.open("NUL"); // Windows null device
-    old_cout = std::cout.rdbuf();
-    old_cerr = std::cerr.rdbuf();
-    std::cout.rdbuf(null_stream.rdbuf());
-    std::cerr.rdbuf(null_stream.rdbuf());
-
-    // Redirect printf (C stdout)
-    saved_stdout_fd = _dup(_fileno(stdout));
-    null_fd         = _open("NUL", _O_WRONLY);
-    _dup2(null_fd, _fileno(stdout));
-  }
-
-  ~IOSilencer() {
-    std::cout.rdbuf(old_cout);
-    std::cerr.rdbuf(old_cerr);
-    fflush(stdout);
-
-    _dup2(saved_stdout_fd, _fileno(stdout));
-    _close(null_fd);
-    _close(saved_stdout_fd);
-  }
-};
-
-namespace blast {
-
-TEST_CASE("Test gradient", "[accelerations]") {
-
-  ManipulatorTempData manip_data;
-
-  // Optimization opt(get_generic_gen3(), get_gen3_task());
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 100, 5, 6);
-  opt.bspline = bspline;
-
-  opt.guess.type      = Guess::custom;
-  opt.guess.initial_x = Array{0.152355, -0.597958, -0.756348, 0.558375, -0.946557, -0.10268, 0.901938, -0.145201, -0.436917, -0.827993,
-                              -0.210473, -0.0664338, 0.370776, -0.29955, -0.139142, 0.757138, -0.157328, 0.909626, -0.214084, 0.745555,
-                              -0.0416009, 0.375275, 0.126153, 0.402337, 0.723458, -0.070362, -0.444889, 0.784517, 0.655188, 0.216073,
-                              0.889643, -0.969162, -0.0279569, 0.496376, -0.891977, 0.0247388, -0.332795, -0.239096, -0.418835, 0.922935,
-                              0.124817, 0.34831, -0.00905602, -0.157159, 0.492551, 0.0325292, -0.299891, 0.828649, -0.909505, -0.294613,
-                              -0.768063, -0.489629, 0.0918699, -0.603901, -0.391773, -0.919239, 0.472415, 0.591886, -0.494069, 0.0488416,
-                              4.81184};
-  //   opt.guess.initial_x   = Array(opt.bspline.x_len(opt.task), 2.0);
-  // opt.guess.n_random_shots = 100;
-
-  opt.constraints.show_info = true;
-
-  opt.constraints.position     = true;
-  opt.constraints.velocity     = true;
-  opt.constraints.acceleration = true;
-  opt.constraints.torque       = true;
-
-  opt.constraints.tool_speed          = false;
-  opt.constraints.self_collisions     = false;
-  opt.constraints.external_collisions = false;
-
-  opt.max_tries         = 1;
-  opt.success_tolerance = 0.01;
-
-
-  auto results = optimize(&opt, OptimizationMethod::baseline);
-
-  Optimization opt_acc(get_generic_Link6(), get_link6_task());
-  opt_acc.set_bspline(bspline);
-  opt_acc.guess.type      = Guess::custom;
-  opt_acc.guess.initial_x = Array{0.152355, -0.597958, -0.756348, 0.558375, -0.946557, -0.10268, 0.901938, -0.145201, -0.436917, -0.827993,
-                                  -0.210473, -0.0664338, 0.370776, -0.29955, -0.139142, 0.757138, -0.157328, 0.909626, -0.214084, 0.745555,
-                                  -0.0416009, 0.375275, 0.126153, 0.402337, 0.723458, -0.070362, -0.444889, 0.784517, 0.655188, 0.216073,
-                                  0.889643, -0.969162, -0.0279569, 0.496376, -0.891977, 0.0247388, -0.332795, -0.239096, -0.418835, 0.922935,
-                                  0.124817, 0.34831, -0.00905602, -0.157159, 0.492551, 0.0325292, -0.299891, 0.828649, -0.909505, -0.294613,
-                                  -0.768063, -0.489629, 0.0918699, -0.603901, -0.391773, -0.919239, 0.472415, 0.591886, -0.494069, 0.0488416,
-                                  4.81184};
-
-  opt_acc.constraints.show_info = true;
-
-  opt_acc.constraints.position     = true;
-  opt_acc.constraints.velocity     = true;
-  opt_acc.constraints.acceleration = true;
-  opt_acc.constraints.torque       = true;
-
-  opt_acc.constraints.tool_speed          = false;
-  opt_acc.constraints.self_collisions     = false;
-  opt_acc.constraints.external_collisions = false;
-
-  opt_acc.max_tries         = 1;
-  opt_acc.success_tolerance = 0.01;
-
-  auto result_acc = optimize(&opt_acc, OptimizationMethod::with_analytical_pva);
-
-  // ------------- Constraint function -------------------------
-  auto joints = opt_acc.manip.n_joints;
-
-  Array pos_plus(joints);
-  Array vel_plus(joints);
-  Array acc_plus(joints);
-  Array torque_constraint(joints);
-  Array torque_constraint_plus(joints);
-
-  Matrix gradient_torque_coeffs(opt_acc.manip.n_joints, 3 * opt_acc.manip.n_joints * opt_acc.bspline.n_points);
-
-  constexpr real eps = 1e-5;
-
-  initialize_optimization(&opt_acc);
-  n_con(&opt_acc);
-
-  auto x = opt.guess.initial_x;
-
-  opt_acc.bspline.compute_trajectory(x, opt_acc.task);
-
-  u32 i = 1;
-
-  auto pos = opt_acc.bspline.traj.pos.col(i);
-
-  if (opt_acc.constraints.torque) {
-#if BLAST_TRACE_LEVEL >= 3
-    ZoneScopedN("Dynamics");
-#endif
-    auto vel = opt_acc.bspline.traj.vel.col(i);
-    auto acc = opt_acc.bspline.traj.acc.col(i);
-    dynamics(opt_acc.manip, manip_data, vel, acc);
-    for (int j = 0; j < joints; j++) {
-      torque_constraint[j] = abs_constraint(1.01 * manip_data.efforts[j], opt_acc.manip.torque_max[j]);
-    }
-
-    // grad_coeffs pos
-    for (u32 j = 0; j < joints; j++) {
-      pos[j] += eps;
-      forward_kinematics(opt_acc.manip, manip_data, pos);
-      dynamics(opt_acc.manip, manip_data, vel, acc);
-      pos[j] -= eps;
-
-      for (u32 k = 0; k < joints; k++) {
-        torque_constraint_plus[k]                     = abs_constraint(1.01 * manip_data.efforts[k], opt_acc.manip.torque_max[k]);
-        gradient_torque_coeffs(k, j + 3 * joints * i) = (torque_constraint_plus[k] - torque_constraint[k]) / eps;
-      }
-    }
-
-    forward_kinematics(opt_acc.manip, manip_data, pos);
-    // grad_coeffs vel
-    for (u32 j = 0; j < joints; j++) {
-      vel[j] += eps;
-      dynamics(opt_acc.manip, manip_data, vel, acc);
-      vel[j] -= eps;
-
-      for (u32 k = 0; k < joints; k++) {
-        torque_constraint_plus[k]                              = abs_constraint(1.01 * manip_data.efforts[k], opt_acc.manip.torque_max[k]);
-        gradient_torque_coeffs(k, j + joints + 3 * joints * i) = (torque_constraint_plus[k] - torque_constraint[k]) / eps;
-      }
-    }
-
-    // grad_coeffs acc
-    for (u32 j = 0; j < joints; j++) {
-      acc[j] += eps;
-      dynamics(opt_acc.manip, manip_data, vel, acc);
-      acc[j] -= eps;
-
-      for (u32 k = 0; k < joints; k++) {
-        torque_constraint_plus[k]                                  = abs_constraint(1.01 * manip_data.efforts[k], opt_acc.manip.torque_max[k]);
-        gradient_torque_coeffs(k, j + 2 * joints + 3 * joints * i) = (torque_constraint_plus[k] - torque_constraint[k]) / eps;
-      }
-    }
-  }
-
-  const auto xlen = opt_acc.bspline.x_len(opt_acc.task);
-
-  u32   n_con_lb = 0;
-  Array x_plus(xlen);
-
-  u32 x_per_joint = (xlen - 1) / opt_acc.manip.n_joints; // = nctrl - 6 (skip first and last 3)
-  u32 joint       = 0;
-
-  u32 grad_idx       = 0;
-  u32 constraint_idx = 0;
-  u32 x_idx          = 0;
-
-  u32 n_torque         = 0;
-  u32 n_tool_speed     = 0;
-  u32 n_self_collision = 0;
-
-  u32   n_con = opt_acc.constraints.n_constraints;
-  Array grad(xlen * n_con);
-
-  memcpy(x_plus.data, x.data, xlen * sizeof(real)); // todo: is this necessary ? done once, and x_plus is modified to original at the end of each iteration
-
-  for (u32 j = 0; j < xlen - 1; j++) {              // last one is T todo: maybe change to 2 for loops (joint & x_per_joint)
-    // vector x is stored as (ctrl points for joint 0, ctrl points for joint 1, ...)
-    joint = j / x_per_joint > joint ? joint + 1 : joint; // increase joint by 1 everytime we reach its ctrl points
-    x_idx = j - joint * x_per_joint + 3;                 // todo: fix for NaN in PVA of task
-
-    x_plus[j] += eps;
-
-    // Array xv_plus;
-    // xv_plus.alias(x_plus.data, xlen);
-    opt_acc.bspline.compute_trajectory(x_plus, opt_acc.task);
-
-    n_con_lb = ncon_lb_acc(&opt_acc, x_idx); // find the amount of constraints before the current point
-
-    Array external_collisions(opt_acc.bspline.upper_bounds[x_idx] - opt_acc.bspline.lower_bounds[x_idx]);
-    // todo: create alias matrix that points to grad
-    // todo: can we change the order in which we store the gradients ?
-    grad_idx       = n_con_lb * xlen + j;                                                              // gradients are stored column-wise xlen * npoints
-    constraint_idx = n_con_lb;
-    for (u32 i = opt_acc.bspline.lower_bounds[x_idx]; i <= opt_acc.bspline.upper_bounds[x_idx]; i++) { // lb & ub are inclusive
-      grad_idx += joint * xlen;
-      constraint_idx += joint;
-      n_torque         = 0;
-      n_tool_speed     = 0;
-      n_self_collision = 0;
-      if (opt_acc.constraints.torque) { // todo: add analytical gradients for torque
-        n_torque = opt_acc.manip.n_joints;
-
-        for (u32 k = 0; k < opt_acc.manip.n_joints; k++) {
-          grad[grad_idx] = opt_acc.bspline.basis_p(x_idx, i) * gradient_torque_coeffs(k, joint + 3 * opt_acc.manip.n_joints * i) +
-                           opt_acc.bspline.basis_v(x_idx, i) / opt_acc.bspline.traj.t.back() * gradient_torque_coeffs(k, joint + opt_acc.manip.n_joints + 3 * opt_acc.manip.n_joints * i) +
-                           opt_acc.bspline.basis_a(x_idx, i) / (opt_acc.bspline.traj.t.back() * opt_acc.bspline.traj.t.back()) * gradient_torque_coeffs(k, joint + 2 * opt_acc.manip.n_joints + 3 * opt_acc.manip.n_joints * i);
-          grad_idx += xlen;
-          constraint_idx++; // note: eventhough this gradient is not done analytically, we still need to increase the constraint index
-        }
-      }
-    }
-  }
-
-  CHECK(grad_idx == 100);
-};
-
-TEST_CASE("Test gradient accuracy", "[accelerations]") {
-
-  ManipulatorTempData manip_data;
-
-  // Optimization opt(get_generic_gen3(), get_gen3_task());
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 100, 5, 6);
-  opt.bspline = bspline;
-
-  opt.guess.type      = Guess::custom;
-  opt.guess.initial_x = Array{0.152355, -0.597958, -0.756348, 0.558375, -0.946557, -0.10268, 0.901938, -0.145201, -0.436917, -0.827993,
-                              -0.210473, -0.0664338, 0.370776, -0.29955, -0.139142, 0.757138, -0.157328, 0.909626, -0.214084, 0.745555,
-                              -0.0416009, 0.375275, 0.126153, 0.402337, 0.723458, -0.070362, -0.444889, 0.784517, 0.655188, 0.216073,
-                              0.889643, -0.969162, -0.0279569, 0.496376, -0.891977, 0.0247388, -0.332795, -0.239096, -0.418835, 0.922935,
-                              0.124817, 0.34831, -0.00905602, -0.157159, 0.492551, 0.0325292, -0.299891, 0.828649, -0.909505, -0.294613,
-                              -0.768063, -0.489629, 0.0918699, -0.603901, -0.391773, -0.919239, 0.472415, 0.591886, -0.494069, 0.0488416,
-                              4.81184};
-  //   opt.guess.initial_x   = Array(opt.bspline.x_len(opt.task), 2.0);
-  // opt.guess.n_random_shots = 100;
-
-  opt.constraints.show_info = true;
-
-  opt.constraints.position     = true;
-  opt.constraints.velocity     = true;
-  opt.constraints.acceleration = true;
-  opt.constraints.torque       = true;
-
-  opt.constraints.tool_speed          = false;
-  opt.constraints.self_collisions     = false;
-  opt.constraints.external_collisions = false;
-
-  opt.max_tries         = 1;
-  opt.success_tolerance = 0.01;
-
-
-  auto results = optimize(&opt, OptimizationMethod::baseline);
-
-  Optimization opt_acc(get_generic_Link6(), get_link6_task());
-  opt_acc.set_bspline(bspline);
-  opt_acc.guess.type      = Guess::custom;
-  opt_acc.guess.initial_x = Array{0.152355, -0.597958, -0.756348, 0.558375, -0.946557, -0.10268, 0.901938, -0.145201, -0.436917, -0.827993,
-                                  -0.210473, -0.0664338, 0.370776, -0.29955, -0.139142, 0.757138, -0.157328, 0.909626, -0.214084, 0.745555,
-                                  -0.0416009, 0.375275, 0.126153, 0.402337, 0.723458, -0.070362, -0.444889, 0.784517, 0.655188, 0.216073,
-                                  0.889643, -0.969162, -0.0279569, 0.496376, -0.891977, 0.0247388, -0.332795, -0.239096, -0.418835, 0.922935,
-                                  0.124817, 0.34831, -0.00905602, -0.157159, 0.492551, 0.0325292, -0.299891, 0.828649, -0.909505, -0.294613,
-                                  -0.768063, -0.489629, 0.0918699, -0.603901, -0.391773, -0.919239, 0.472415, 0.591886, -0.494069, 0.0488416,
-                                  4.81184};
-
-  opt_acc.constraints.show_info = true;
-
-  opt_acc.constraints.position     = true;
-  opt_acc.constraints.velocity     = true;
-  opt_acc.constraints.acceleration = true;
-  opt_acc.constraints.torque       = true;
-
-  opt_acc.constraints.tool_speed          = false;
-  opt_acc.constraints.self_collisions     = false;
-  opt_acc.constraints.external_collisions = false;
-
-  opt_acc.max_tries         = 1;
-  opt_acc.success_tolerance = 0.01;
-
-  auto result_acc = optimize(&opt_acc, OptimizationMethod::with_analytical_pva);
-
-  //   Optimization opt_acc_2(get_generic_Link6(), get_link6_task());
-  //   opt_acc_2.set_bspline(bspline);
-  //   opt_acc_2.guess.type = Guess::custom;
-  //   opt_acc_2.guess.initial_x   = Array{0.152355, -0.597958, -0.756348, 0.558375, -0.946557, -0.10268, 0.901938, -0.145201, -0.436917, -0.827993,
-  //                              -0.210473, -0.0664338, 0.370776, -0.29955, -0.139142, 0.757138, -0.157328, 0.909626, -0.214084, 0.745555,
-  //                              -0.0416009, 0.375275, 0.126153, 0.402337, 0.723458, -0.070362, -0.444889, 0.784517, 0.655188, 0.216073,
-  //                              0.889643, -0.969162, -0.0279569, 0.496376, -0.891977, 0.0247388, -0.332795, -0.239096, -0.418835, 0.922935,
-  //                              0.124817, 0.34831, -0.00905602, -0.157159, 0.492551, 0.0325292, -0.299891, 0.828649, -0.909505, -0.294613,
-  //                              -0.768063, -0.489629, 0.0918699, -0.603901, -0.391773, -0.919239, 0.472415, 0.591886, -0.494069, 0.0488416,
-  //                              4.81184};
-
-  //   opt_acc_2.constraints.show_info = true;
-
-  //   opt_acc_2.constraints.position     = true;
-  //   opt_acc_2.constraints.velocity     = true;
-  //   opt_acc_2.constraints.acceleration = true;
-  //   opt_acc_2.constraints.torque       = true;
-
-  //   opt_acc_2.constraints.tool_speed           = false;
-  //   opt_acc_2.constraints.self_collisions     = false;
-  //   opt_acc_2.constraints.external_collisions = false;
-
-  //   opt_acc_2.max_tries         = 1;
-  //   opt_acc_2.success_tolerance = 0.01;
-
-  //   auto result_acc_2 = optimize(&opt_acc_2, OptimizationMethod::with_analytical_dynamics);
-
-  CHECK(std::abs(results.x.back() - result_acc.x.back()) < 1e-5);
-
-  u32 i = 0;
-  for (u32 j = 0; j < results.opt->constraints.grad_list[i].rows; j++) {
-    for (u32 k = 0; k < results.opt->constraints.grad_list[i].cols; k++) {
-      CHECK(std::abs(results.opt->constraints.grad_list[i](j, k) - result_acc.opt->constraints.grad_list[i](j, k)) < 0.01);
-      CHECK(std::abs(results.opt->constraints.constr_list[i][k] - result_acc.opt->constraints.constr_list[i][k]) < 0.01);
-      //   CHECK(std::abs(results.opt->constraints.grad_list[i](j, k) - result_acc_2.opt->constraints.grad_list[i](j, k)) < 0.01);
-      //   CHECK(std::abs(results.opt->constraints.constr_list[i][k] - result_acc_2.opt->constraints.constr_list[i][k]) < 0.01);
-    }
-  }
-}
-
-} // namespace blast
diff --git a/tests/acceleration/test_constraints_acceleration.cpp b/tests/acceleration/test_constraints_acceleration.cpp
deleted file mode 100644
index b411738..0000000
--- a/tests/acceleration/test_constraints_acceleration.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-
-#include "blast"
-#include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
-using namespace blast;
-
-TEST_CASE("Test bound constraints", "[Accelerated]") {
-}
diff --git a/tests/acceleration/test_gradients_acceleration.cpp b/tests/acceleration/test_gradients_acceleration.cpp
deleted file mode 100644
index 364072b..0000000
--- a/tests/acceleration/test_gradients_acceleration.cpp
+++ /dev/null
@@ -1,332 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-
-#include "blast"
-#include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
-using namespace blast;
-
-std::vector<blast::Guess> read_array_from_csv(std::string file_path, blast::u32 xlen) {
-  std::ifstream file(file_path);
-  if (!file.is_open()) {
-    std::cerr << "Error opening file" << std::endl;
-    return {};
-  }
-  std::vector<blast::Guess> initial_guess_list;
-  std::string               line;
-
-  // Read each row from the CSV file
-  while (std::getline(file, line)) {
-    std::stringstream ss(line);
-    std::string       token;
-    blast::Array      arr(xlen);
-
-    blast::u32 idx = 0;
-    // Split the row using comma as the delimiter
-    while (std::getline(ss, token, ',')) {
-      try {
-        // Convert token to double and add it to the Array's values
-        arr[idx] = std::stod(token);
-      } catch (const std::invalid_argument& e) {
-        std::cerr << "Invalid number found: " << token << std::endl;
-      }
-      idx++;
-    }
-    Guess guess_array(arr);
-    initial_guess_list.push_back(guess_array);
-  }
-  file.close();
-  return initial_guess_list;
-}
-
-TEST_CASE("test gradient upper and lower bounds nctrl = 16 npoints = 200", "[Acceleration]") {
-
-  // find lb and ub for x, given p = 5
-  u32 nctrl   = 16;
-  u32 npoints = 200;
-  u32 joints  = 6;
-
-  Bspline bspline(nctrl, npoints, 5, joints);
-
-  // print_to_csv(bspline.basis_p, "../../../tests/acceleration/basis_p.csv");
-  // print_to_csv(bspline.basis_v, "../../../tests/acceleration/basis_v.csv");
-  // print_to_csv(bspline.basis_a, "../../../tests/acceleration/basis_a.csv");
-
-  for (u32 i = 0; i < nctrl; i++) {
-    // we do not test the first and last points
-    auto lb = bspline.lower_bounds[i];
-    auto ub = bspline.upper_bounds[i];
-    for (u32 k = lb; k <= ub; k++) {
-      if (bspline.basis_p(i, k) == 0.0)
-        std::cout << "i: " << i << " k: " << k << " basis_p: " << bspline.basis_p(i, k) << std::endl;
-      CHECK(bspline.basis_p(i, k) != 0.0);
-      CHECK(bspline.basis_v(i, k) != 0.0);
-      CHECK(bspline.basis_a(i, k) != 0.0);
-    }
-    for (u32 k = 1; k < lb; k++) {
-      if (bspline.basis_a(i, k) != 0.0)
-        std::cout << "i: " << i << " k: " << k << " basis_a: " << bspline.basis_a(i, k) << std::endl;
-      CHECK(is_close(bspline.basis_p(i, k), 0.0));
-      CHECK(is_close(bspline.basis_v(i, k), 0.0));
-      CHECK(is_close(bspline.basis_a(i, k), 0.0));
-    }
-    for (u32 k = ub + 1; k < npoints - 1; k++) {
-      if (bspline.basis_p(i, k) != 0.0)
-        std::cout << "i: " << i << " k: " << k << " basis_p: " << bspline.basis_p(i, k) << std::endl;
-      CHECK(is_close(bspline.basis_p(i, k), 0.0));
-      CHECK(is_close(bspline.basis_v(i, k), 0.0));
-      CHECK(is_close(bspline.basis_a(i, k), 0.0));
-    }
-  }
-}
-
-TEST_CASE("test gradient upper and lower bounds random nctrl & npoints", "[Acceleration]") {
-  u32 n_tests = 1000;
-
-  for (u32 t = 0; t < n_tests; t++) {
-
-    // find lb and ub for x, given p = 5
-    u32 nctrl   = 10 + (int) std::abs(10 * get_random());
-    u32 npoints = 200 + (int) std::abs(300 * get_random());
-    u32 joints  = 6;
-
-    Bspline bspline(nctrl, npoints, 5, joints);
-
-    for (u32 i = 0; i < nctrl; i++) {
-      auto lb = bspline.lower_bounds[i];
-      auto ub = bspline.upper_bounds[i];
-      for (u32 k = lb; k <= ub; k++) {
-        // todo: make sure value is max/min
-        bool b_p = bspline.basis_p(i, k) != 0.0 ? true : bspline.basis_p(i - 1, k) != 0.0 && bspline.basis_p(i + 1, k) != 0.0 ? true
-                                                                                                                              : false; // remove outliers
-        bool b_v = bspline.basis_v(i, k) != 0.0 ? true : bspline.basis_v(i - 1, k) != 0.0 && bspline.basis_v(i + 1, k) != 0.0 ? true
-                                                                                                                              : false; // remove outliers
-        bool b_a = bspline.basis_a(i, k) != 0.0 ? true : bspline.basis_a(i - 1, k) != 0.0 && bspline.basis_a(i + 1, k) != 0.0 ? true
-                                                                                                                              : false; // remove outliers
-        CHECK(b_p);
-        CHECK(b_v);
-        CHECK(b_a);
-      }
-      for (u32 k = 1; k < lb; k++) {
-        if (bspline.basis_p(i, k) != 0.0 || bspline.basis_v(i, k) != 0.0 || bspline.basis_a(i, k) != 0.0)
-          std::cout << "i: " << i << " k: " << k << " basis_p: " << bspline.basis_p(i, k) << " basis_v: " << bspline.basis_v(i, k) << " basis_a: " << bspline.basis_a(i, k) << std::endl;
-        CHECK(is_close(bspline.basis_p(i, k), 0.0));
-        CHECK(is_close(bspline.basis_v(i, k), 0.0));
-        CHECK(is_close(bspline.basis_a(i, k), 0.0));
-      }
-      for (u32 k = ub + 1; k < npoints - 1; k++) {
-        if (bspline.basis_p(i, k) != 0.0 || bspline.basis_v(i, k) != 0.0 || bspline.basis_a(i, k) != 0.0)
-          std::cout << "i: " << i << " k: " << k << " basis_p: " << bspline.basis_p(i, k) << " basis_v: " << bspline.basis_v(i, k) << " basis_a: " << bspline.basis_a(i, k) << std::endl;
-        CHECK(is_close(bspline.basis_p(i, k), 0.0));
-        CHECK(is_close(bspline.basis_v(i, k), 0.0));
-        CHECK(is_close(bspline.basis_a(i, k), 0.0));
-      }
-    }
-  }
-}
-
-TEST_CASE("test gradient points with upper & lower bounds", "[Acceleration]") {
-  using namespace blast;
-
-  std::string input_file_initial_guess = "../../../examples/acceleration/files/link6_initial_guess.csv";
-
-  // blast::begin_profile();
-  // --- Define Manipulator ---
-  auto link6 = get_generic_Link6();
-
-  // --- Define Tasks ---
-  std::vector<Array> positions(14);
-  positions[0]  = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};                   // zero
-  positions[1]  = deg2rad({-40.4, -26.9, 83.6, 1.5, 20.0, -42.2});  // w1 + 10
-  positions[2]  = deg2rad({-40.2, -34.9, 96.4, 1.4, 41.7, -41.5});  // w1
-  positions[3]  = deg2rad({-40.4, -26.9, 83.6, 1.5, 20.0, -42.2});  // w1 + 10
-  positions[4]  = deg2rad({51.9, -13.6, 107.9, 3.6, 33.1, 51.2});   // wb1
-  positions[5]  = deg2rad({-33.9, -35.6, 70.9, -0.7, 16.9, -31.8}); // w3 + 10
-  positions[6]  = deg2rad({-34.8, -42.5, 81.6, 0.0, 33.3, -35.1});  // w3
-  positions[7]  = deg2rad({-33.9, -35.6, 70.9, -0.7, 16.9, -31.8}); // w3 + 10
-  positions[8]  = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});    // wb3
-  positions[9]  = deg2rad({-20.0, -19.3, 95.9, -3.0, 23.5, -17.6}); // w4 + 10
-  positions[10] = deg2rad({-20.5, -30.9, 105.2, 1.3, 43.3, -22.1}); // w4
-  positions[11] = deg2rad({-20.0, -19.3, 95.9, -3.0, 23.5, -17.6}); // w4 + 10
-  positions[12] = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});    // wb4
-  positions[13] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};                   // zero
-
-  std::vector<Matrix> tasks;
-  for (u32 i = 0; i < positions.size() - 1; i++) {
-    Matrix tmp(6, 6);
-
-    for (u32 j = 0; j < link6.joints; j++) {
-      tmp(j, 0) = positions[i][j];
-      tmp(j, 3) = positions[i + 1][j];
-    }
-    tasks.push_back(tmp);
-  }
-
-  // --- Define Bspline ---
-  Bspline bspline(16, 200, 5, 6);
-
-  // --- Define Guess ---
-  std::vector<Guess> guess_predefined;
-
-
-  auto xlen        = bspline.xlen(tasks[0]);
-  guess_predefined = read_array_from_csv(input_file_initial_guess, xlen);
-  Assert(!guess_predefined.empty());
-
-  // -- Define Constraints ---
-  Constraints<GenericManipulator> constraints;
-  constraints.show_info = true;
-
-  constraints.position     = true;
-  constraints.velocity     = true;
-  constraints.acceleration = true;
-  constraints.torque       = true;
-
-  constraints.tool_speed              = false;
-  constraints.self_collisions         = false;
-  constraints.external_collisions     = false;
-  constraints.n_collision_constraints = 100;
-  constraints.n_collision_skip        = 2;
-
-  // --- Define Objective ---
-  Objective<GenericManipulator> objective;
-  objective.time_weight = 1;
-
-  real start_time = 0.0;
-  for (u32 t = 0; t < tasks.size(); t++) {
-    Optimization<GenericManipulator> opt(link6, tasks[t]);
-
-    opt.set_bspline(bspline);
-    opt.set_guess(guess_predefined[t]);
-    opt.set_constraints(constraints);
-    opt.set_objective(objective);
-    auto results = optimize(&opt, OptimizationMethod::baseline);
-
-    Optimization<GenericManipulator> opt_acc(link6, tasks[t]);
-    opt_acc.set_bspline(bspline);
-    opt_acc.set_guess(guess_predefined[t]);
-    opt_acc.set_constraints(constraints);
-    opt_acc.set_objective(objective);
-
-    auto result_acc = optimize_acc(&opt_acc);
-
-    // CHECK(results.opt.constraints.grad_list.size() == result_acc.opt.constraints.grad_list.size());
-    // CHECK(results.opt.constraints.constr_list.size() == result_acc.opt.constraints.constr_list.size());
-    // CHECK(results.opt.constraints.x_list.size() == result_acc.opt.constraints.x_list.size());
-    CHECK(is_close(results.x, result_acc.x));
-    // for (u32 i = 0; i <results.opt.constraints.grad_list.size(); i++) {
-    //     // u32 i = 10;
-    //     for (u32 j = 0; j < results.opt.constraints.grad_list[i].rows; j++) {
-    //         for (u32 k = 1; k < results.opt.constraints.grad_list[i].cols - 1; k++) { // skip first and last point
-    //             CHECK(is_close(results.opt.constraints.grad_list[i](j, k), result_acc.opt.constraints.grad_list[i](j, k)));
-    //             CHECK(is_close(results.opt.constraints.constr_list[i][k], result_acc.opt.constraints.constr_list[i][k]));
-    //         }
-    //     }
-    // }
-  }
-}
-
-TEST_CASE("test gradient acceleration accuracy", "[Acceleration]") {
-  using namespace blast;
-
-  std::string input_file_initial_guess = "../../../examples/acceleration/files/link6_initial_guess.csv";
-
-  // blast::begin_profile();
-  // --- Define Manipulator ---
-  auto link6 = get_generic_Link6();
-
-  // --- Define Tasks ---
-  std::vector<Array> positions(14);
-  positions[0]  = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};                   // zero
-  positions[1]  = deg2rad({-40.4, -26.9, 83.6, 1.5, 20.0, -42.2});  // w1 + 10
-  positions[2]  = deg2rad({-40.2, -34.9, 96.4, 1.4, 41.7, -41.5});  // w1
-  positions[3]  = deg2rad({-40.4, -26.9, 83.6, 1.5, 20.0, -42.2});  // w1 + 10
-  positions[4]  = deg2rad({51.9, -13.6, 107.9, 3.6, 33.1, 51.2});   // wb1
-  positions[5]  = deg2rad({-33.9, -35.6, 70.9, -0.7, 16.9, -31.8}); // w3 + 10
-  positions[6]  = deg2rad({-34.8, -42.5, 81.6, 0.0, 33.3, -35.1});  // w3
-  positions[7]  = deg2rad({-33.9, -35.6, 70.9, -0.7, 16.9, -31.8}); // w3 + 10
-  positions[8]  = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});    // wb3
-  positions[9]  = deg2rad({-20.0, -19.3, 95.9, -3.0, 23.5, -17.6}); // w4 + 10
-  positions[10] = deg2rad({-20.5, -30.9, 105.2, 1.3, 43.3, -22.1}); // w4
-  positions[11] = deg2rad({-20.0, -19.3, 95.9, -3.0, 23.5, -17.6}); // w4 + 10
-  positions[12] = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});    // wb4
-  positions[13] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};                   // zero
-
-  std::vector<Matrix> tasks;
-  for (u32 i = 0; i < positions.size() - 1; i++) {
-    Matrix tmp(6, 6);
-
-    for (u32 j = 0; j < link6.joints; j++) {
-      tmp(j, 0) = positions[i][j];
-      tmp(j, 3) = positions[i + 1][j];
-    }
-    tasks.push_back(tmp);
-  }
-
-  // --- Define Bspline ---
-  Bspline bspline(16, 200, 5, 6);
-
-  // --- Define Guess ---
-  std::vector<Guess> guess_predefined;
-
-
-  auto xlen        = bspline.xlen(tasks[0]);
-  guess_predefined = read_array_from_csv(input_file_initial_guess, xlen);
-  Assert(!guess_predefined.empty());
-
-  // -- Define Constraints ---
-  Constraints<GenericManipulator> constraints;
-  constraints.show_info = true;
-
-  constraints.position     = true;
-  constraints.velocity     = true;
-  constraints.acceleration = true;
-  constraints.torque       = true;
-
-  constraints.tool_speed              = false;
-  constraints.self_collisions         = false;
-  constraints.external_collisions     = false;
-  constraints.n_collision_constraints = 100;
-  constraints.n_collision_skip        = 2;
-
-  // --- Define Objective ---
-  Objective<GenericManipulator> objective;
-  objective.time_weight = 1;
-
-  real start_time = 0.0;
-  for (u32 t = 0; t < tasks.size(); t++) {
-    Optimization<GenericManipulator> opt(link6, tasks[t]);
-
-    opt.set_bspline(bspline);
-    opt.set_guess(guess_predefined[t]);
-    opt.set_constraints(constraints);
-    opt.set_objective(objective);
-    auto results = optimize(&opt, OptimizationMethod::baseline);
-
-    Optimization<GenericManipulator> opt_acc(link6, tasks[t]);
-    opt_acc.set_bspline(bspline);
-    opt_acc.set_guess(guess_predefined[t]);
-    opt_acc.set_constraints(constraints);
-    opt_acc.set_objective(objective);
-
-    auto result_acc = optimize(&opt_acc, OptimizationMethod::with_analytical_pva);
-
-    // CHECK(results.opt.constraints.grad_list.size() == result_acc.opt.constraints.grad_list.size());
-    // CHECK(results.opt.constraints.constr_list.size() == result_acc.opt.constraints.constr_list.size());
-    // CHECK(results.opt.constraints.x_list.size() == result_acc.opt.constraints.x_list.size());
-    // CHECK(is_close(results.x, result_acc.x));
-    // for (u32 i = 0; i <results.opt.constraints.grad_list.size(); i++) {
-    u32 i = 0;
-    for (u32 j = 0; j < results.opt.constraints.grad_list[i].rows; j++) {
-      for (u32 k = 0; k < results.opt.constraints.grad_list[i].cols; k++) {
-        // if (!is_close(results.opt.constraints.grad_list[i](j, k), result_acc.opt.constraints.grad_list[i](j, k), 0.01))
-        //     std::cout << "i: " << i << " j: " << j << " k: " << k << " grad: " << results.opt.constraints.grad_list[i](j, k) << " grad_acc: " << result_acc.opt.constraints.grad_list[i](j, k) << std::endl;
-        CHECK(is_close(results.opt.constraints.grad_list[i](j, k), result_acc.opt.constraints.grad_list[i](j, k), 0.01));
-        CHECK(is_close(results.opt.constraints.constr_list[i][k], result_acc.opt.constraints.constr_list[i][k], 0.01));
-      }
-    }
-    // }
-  }
-}
diff --git a/tests/acceleration/test_inplace.cpp b/tests/acceleration/test_inplace.cpp
deleted file mode 100644
index 6bf7041..0000000
--- a/tests/acceleration/test_inplace.cpp
+++ /dev/null
@@ -1,525 +0,0 @@
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#define BLAST_TRACE_LEVEL 3
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include "tracy/Tracy.hpp"
-#include "tracy/TracyC.h"
-
-namespace blast {
-
-
-TEST_CASE("Replace INF_REAL with huge value", "[acceleration]") {
-  int n_iterations = 10000;
-
-  // both pmax and pmin are +/-INF_REAL
-  real pmax = INF_REAL;
-  real pmin = -INF_REAL;
-
-  pmax = 1e300;
-  pmin = -1e300;
-
-  real c          = (pmax + pmin) / 2;
-  real b          = (pmax - pmin) / 2;
-  real one_over_b = 1 / b;
-
-  Array p(n_iterations);
-  p = fill_random(p, 360);
-
-  for (int i = 0; i < n_iterations; i++) {
-    real q_prime    = p[i] - c;
-    real constraint = (std::abs(q_prime) - b) / b;
-    real gradient   = sign(q_prime) * 1 / b;
-
-    CHECK(constraint == -1.0);
-    CHECK(std::abs(gradient) <= 1e-300);
-  }
-};
-
-TEST_CASE("Analytic dC/dX per segments tests", "[acceleration]") {
-  int n_iterations = 1000;
-
-  for (int iter = 0; iter < n_iterations; iter++) {
-
-    Optimization opt(get_generic_Link6(), get_link6_task());
-
-    opt.world = get_lab_world();
-
-    Bspline bspline(16, 110, 5, 6);
-    opt.bspline = bspline;
-
-    auto x_len    = opt.bspline.x_len(opt.task);
-    auto n_joints = opt.manip.n_joints;
-
-    opt.guess.type = Guess::random;
-
-
-    opt.constraints.position            = true;
-    opt.constraints.velocity            = true;
-    opt.constraints.acceleration        = true;
-    opt.constraints.torque              = true;
-    opt.constraints.tool_speed          = false;
-    opt.constraints.self_collisions     = false;
-    opt.constraints.external_collisions = false;
-
-    opt.max_tries         = 1;
-    opt.success_tolerance = 0.01;
-
-    real eps = 1e-5;
-
-    initialize_optimization(&opt);
-    n_con(&opt);
-
-    auto x = init_guess(&opt);
-    bspline.compute_trajectory(x, opt.task);
-
-    Array result(opt.constraints.n_constraints);
-    compute_constraints(result.data, x, &opt);
-
-    Matrix grad(x_len, opt.constraints.n_constraints);
-
-    Array r_plus(opt.constraints.n_constraints);
-
-    Array x_plus(x_len);
-    x_plus = x;
-
-    int result_idx = 0;
-    for (u32 j = 0; j < x_len; j++) {
-      constexpr real eps = 1e-5;
-      // todo: only copy value that changed last j
-      x_plus[j] += eps;
-
-      Array xv_plus;
-      xv_plus.alias(x_plus.data, x_len);
-
-      // compute constraints
-      compute_constraints(r_plus.data, x_plus, &opt);
-
-      for (u32 i = 0; i < opt.constraints.n_constraints; i++)
-        grad(j, i) = (r_plus[i] - result[i]) / eps;
-
-      x_plus[j] = x[j];
-    }
-
-    // New implementation
-
-    const int n_segments                = (int) opt.bspline.n_ctrl - (int) opt.bspline.degree;
-    const int n_points_per_segment      = (int) opt.bspline.n_points / n_segments; // todo: check if fine?
-    const int n_ctrl                    = (int) opt.bspline.n_ctrl;
-    const int n_constraints_per_segment = (n_joints * 4);                          //  todo: remove hard-coded 4
-    // Assert(constraints.size == n_segments * n_constraints_per_segment);
-
-    Array  constraints(n_segments * n_constraints_per_segment);
-    Matrix grad_analytic(x_len, n_segments * n_constraints_per_segment);
-
-    // basis: n_ctrl x n_points
-    // Assert(constraints.is_alias);
-    if (grad_analytic.size) {
-      // Assert(grad_analytic.is_alias);
-      Assert(grad_analytic.rows == x.size);
-      Assert(grad_analytic.cols == constraints.size);
-    }
-
-    // limits
-    Array       pmax    = opt.manip.position_max;
-    Array       pmin    = opt.manip.position_min;
-    const Array vmax    = opt.manip.velocity_max;
-    const Array amax    = opt.manip.acceleration_max;
-    const Array tau_max = opt.manip.torque_max;
-
-    ManipulatorTempData manip_data;
-    std::vector<u8>     max_pos_indices(n_joints);
-    std::vector<u8>     max_vel_indices(n_joints);
-    std::vector<u8>     max_acc_indices(n_joints);
-    std::vector<u8>     max_tor_indices(n_joints);
-
-
-    opt.bspline.compute_trajectory(x, opt.task);
-
-
-    for (int segment = 0; segment < n_segments; segment++) {
-      Array max_pos_constraints(n_joints, -INF_REAL); // note:
-      Array max_vel_constraints(n_joints, -INF_REAL); // note:
-      Array max_acc_constraints(n_joints, -INF_REAL); // note:
-      Array max_tor_constraints(n_joints, -INF_REAL); // note:
-#if BLAST_TRACE_LEVEL >= 2
-      ZoneScopedN("All Segment Constraints");
-#endif
-      const int first_affected_control_point = std::max(3, segment);
-      const int last_affected_control_point  = std::min((n_ctrl - 1) - 3, segment + (int) opt.bspline.degree);
-      const int n_affected_control_points    = last_affected_control_point - first_affected_control_point + 1; // note: added +1 the bounds are inclusive are inclusive
-      Assert(n_affected_control_points >= 3);
-      Assert(n_affected_control_points <= 6);
-
-      // Find the start_point_for_segment, so tha twe can access the right memory, and also iterate the points of the given segment
-      const int start_point_for_segment = segment * n_points_per_segment; // note:
-
-      // Matrices are always the size of n_ctrl * n_points_per_segment. They "slide" with the segment & do not deal with the rest
-      Matrix bp(&opt.bspline.basis_p(0, start_point_for_segment), n_ctrl, n_points_per_segment); // note:
-      Matrix bv(&opt.bspline.basis_v(0, start_point_for_segment), n_ctrl, n_points_per_segment); // note:
-      Matrix ba(&opt.bspline.basis_a(0, start_point_for_segment), n_ctrl, n_points_per_segment); // note:
-
-
-      for (int point_in_segment = 0; point_in_segment < n_points_per_segment; point_in_segment++) { // note:
-        auto p = opt.bspline.traj.pos.col(start_point_for_segment + point_in_segment);              // note:
-        auto v = opt.bspline.traj.vel.col(start_point_for_segment + point_in_segment);              // note:
-        auto a = opt.bspline.traj.acc.col(start_point_for_segment + point_in_segment);              // note:
-
-        forward_kinematics(opt.manip, manip_data, p);
-        dynamics(opt.manip, manip_data, v, a);
-
-        for (int j = 0; j < n_joints; j++) {
-          {
-            // todo (andre): do this when initializing the optimization, not here
-            // todo: document the current behaviour in the API
-            //        (doesn't currently work if one is inf and the other is not)
-            if (pmax[j] == INF_REAL)  // note: replace INF_REAL with huge value
-              pmax[j] = 1e300;
-            if (pmin[j] == -INF_REAL) // note: replace -INF_REAL with huge negative value
-              pmin[j] = -1e300;
-          }
-          // position
-          if (auto c = bound_constraint(1.01 * p[j], pmin[j], pmax[j]); // note:
-              c > max_pos_constraints[j]) {
-            max_pos_constraints[j] = c;
-            max_pos_indices[j]     = point_in_segment; // note:
-          }
-          // velocity
-          if (auto c = std::abs(1.01 * v[j]) / vmax[j] - 1.0; // note:
-              c > max_vel_constraints[j]) {
-            max_vel_constraints[j] = c;
-            max_vel_indices[j]     = point_in_segment; // note:
-          }
-          // acceleration
-          if (auto c = std::abs(1.01 * a[j]) / amax[j] - 1.0; // note:
-              c > max_acc_constraints[j]) {
-            max_acc_constraints[j] = c;
-            max_acc_indices[j]     = point_in_segment; // note:
-          }
-          // torque
-          if (auto c = std::abs(1.01 * manip_data.efforts[j]) / tau_max[j] - 1.0; // note:
-              c > max_tor_constraints[j]) {
-            max_tor_constraints[j] = c;
-            max_tor_indices[j]     = point_in_segment; // note:
-          }
-        }
-      }
-
-      // at this point we have max constraints for pos, vel, acc, tor, for this segment
-
-      // fill in the constraints for the current segment
-      // [p1, p2,..., v1, v2,..., a1, a2,..., t1, t2,...]
-      auto fill_idx = segment * n_constraints_per_segment;
-      std::copy_n(max_pos_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; // note (andre): we can use the comma operator because we don't need the result of copy_n()
-      std::copy_n(max_vel_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints;
-      std::copy_n(max_acc_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints;
-      std::copy_n(max_tor_constraints.data, n_joints, &constraints[fill_idx]);
-
-
-      // The gradient should be a (x_len)x(n_constraints) matrix that looks like this:
-      // [dp1/dx1, dp2/dx1, ..., dv1/dx1, dv2/dx1, ..., da1/dx1, da2/dx1, ..., dt1/dx1, dt2/dx1]
-      // [dp1/dx2, dp2/dx2, ..., dv1/dx2, dv2/dx2, ..., da1/dx2, da2/dx2, ..., dt1/dx2, dt2/dx2]
-      // [dp1/dx3, dp2/dx3, ..., dv1/dx3, dv2/dx3, ..., da1/dx3, da2/dx3, ..., dt1/dx3, dt2/dx3]
-      // [dp1/dx4, dp2/dx4, ..., dv1/dx4, dv2/dx4, ..., da1/dx4, da2/dx4, ..., dt1/dx4, dt2/dx4]
-      // [dp1/dx5, dp2/dx5, ..., dv1/dx5, dv2/dx5, ..., da1/dx5, da2/dx5, ..., dt1/dx5, dt2/dx5]
-      // [dp1/dx6, dp2/dx6, ..., dv1/dx6, dv2/dx6, ..., da1/dx6, da2/dx6, ..., dt1/dx6, dt2/dx6]
-      // [.....................]
-      // [.....................]
-      // [.....................]
-      // [.....................]
-      // [dp1/dT=0,dp2/dT=0,..., dv1/dT , dv1/dT , ..., da1/dT , da2/dT , ..., dt1/dT , dt2/dT ]
-      // *** per segment ***
-      // where x is the optimization vector
-      if (grad_analytic.size) {
-        // Matrix (alias) in which we can insert the gradient for the current segment
-        Matrix grad_segment(&grad_analytic(0, segment * n_constraints_per_segment), x_len, n_constraints_per_segment);
-        Assert(grad_segment.is_alias);
-
-        int con = 0;
-
-        // positions
-        for (int joint = 0; joint < n_joints; joint++) {
-
-          // Array of the column where to put the gradient for the current constraint
-          Array fill_column = grad_segment.col(con);
-          Assert(con == joint); // note:
-          Assert(fill_column.size == x_len);
-          Assert(fill_column.is_alias);
-
-          // Which values in 'x' affect the current joint's position
-          auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!!
-          // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector
-          x_idx += first_affected_control_point - 3;
-
-          // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x)
-          Array bp_to_use(&bp(first_affected_control_point, max_pos_indices[joint]), n_affected_control_points);
-          Assert(bp_to_use.is_alias);
-
-          real coeff = 2.0 * std::abs(opt.bspline.traj.pos(joint, start_point_for_segment + max_pos_indices[joint])) / (pmax[joint] - pmin[joint]); // note:
-
-          for (int i = 0; i < n_affected_control_points; i++) {
-            fill_column[x_idx++] = bp_to_use[i] * coeff;
-          }
-
-          // note: dp/dT == 0
-          con++;
-        }
-
-        // velocities
-        auto one_over_T = 1 / opt.bspline.traj.t.back();
-        for (int joint = 0; joint < n_joints; joint++) {
-          // Array of the column where to put the gradient for the current constraint
-          Array fill_column = grad_segment.col(con);
-          Assert(con == n_joints + joint); // note:
-          Assert(fill_column.size == x_len);
-          Assert(fill_column.is_alias);
-
-          // Which values in 'x' affect the current joint's position
-          auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!!
-          // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector
-          x_idx += first_affected_control_point - 3;
-
-          Array bv_to_use(&bv(first_affected_control_point, max_vel_indices[joint]), n_affected_control_points);
-          Assert(bv_to_use.is_alias);
-
-          real coeff = sign(opt.bspline.traj.vel(joint, start_point_for_segment + max_vel_indices[joint])) / vmax[joint] * one_over_T; // note:
-
-          for (int i = 0; i < n_affected_control_points; i++) {
-            fill_column[x_idx++] = bv_to_use[i] * coeff;
-          }
-
-          // dvj/dT = - (Cv + 1) / T
-          fill_column.back() = -(max_vel_constraints[joint] + 1) * one_over_T;
-
-          con++;
-        }
-
-        // accelerations
-        auto one_over_T2 = one_over_T * one_over_T;
-        for (int joint = 0; joint < n_joints; joint++) {
-          // Array of the column where to put the gradient for the current constraint
-          Array fill_column = grad_segment.col(con);
-          Assert(con == 2 * n_joints + joint); // note:
-          Assert(fill_column.size == x_len);
-          Assert(fill_column.is_alias);
-
-          // Which values in 'x' affect the current joint's position
-          auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!!
-          // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector
-          x_idx += first_affected_control_point - 3;
-
-          Array ba_to_use(&ba(first_affected_control_point, max_acc_indices[joint]), n_affected_control_points);
-          Assert(ba_to_use.is_alias);
-
-          real coeff = sign(opt.bspline.traj.acc(joint, start_point_for_segment + max_acc_indices[joint])) / amax[joint] * one_over_T2; // note:
-
-          for (int i = 0; i < n_affected_control_points; i++) {
-            fill_column[x_idx++] = ba_to_use[i] * coeff;
-          }
-
-          // daj/dT = -2 * (Ca + 1) / T
-          fill_column.back() = -2 * (max_acc_constraints[joint] + 1) * one_over_T;
-
-          con++;
-        }
-
-        // torque
-        // [dt0/dp0, dt1/dp0, ..., dt4/dp0, dt5/dp0]
-        // [dt0/dp1, dt1/dp1, ..., dt4/dp1, dt5/dp1]
-        // [dt0/dp2, dt1/dp2, ..., dt4/dp2, dt5/dp2]
-        // [dt0/dp3, dt1/dp3, ..., dt4/dp3, dt5/dp3]
-        // [.
-        // [.
-        // [.
-        for (int joint = 0; joint < n_joints; joint++) {
-          Array fill_column = grad_segment.col(con);
-          Assert(con == 3 * n_joints + joint); // note:
-          Assert(fill_column.size == x_len);
-          Assert(fill_column.is_alias);
-
-          constexpr real eps            = 1e-5;
-          const auto     point          = max_tor_indices[joint];
-          auto           p              = opt.bspline.traj.pos.col(start_point_for_segment + point); // note:
-          auto           v              = opt.bspline.traj.vel.col(start_point_for_segment + point); // note:
-          auto           a              = opt.bspline.traj.acc.col(start_point_for_segment + point); // note:
-          auto           old_constraint = max_tor_constraints[joint];                                // note:
-          auto           tau_max_now    = tau_max[joint];                                            // note:
-
-          auto p_plus = p;
-          auto v_plus = v;
-          auto a_plus = a;
-
-          Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points);
-          Array bv_to_use(&bv(first_affected_control_point, point), n_affected_control_points);
-          Array ba_to_use(&ba(first_affected_control_point, point), n_affected_control_points);
-
-          auto x_idx      = first_affected_control_point - 3;
-          auto x_idx_skip = n_ctrl - 6;
-
-          fill_column.back() = 0;
-          for (int j = 0; j < n_joints; j++) {
-
-            // partial derivative of torque constraints w.r.t. position
-            // finite difference on position
-            p_plus[j] += eps;
-            // compute the derivative of constraint(joint) w.r.t. theta(j). (remember, joint != j)
-            forward_kinematics(opt.manip, manip_data, p_plus);
-            dynamics(opt.manip, manip_data, v_plus, a_plus);
-            const real new_constraint_p = std::abs(1.01 * manip_data.efforts[joint]) / tau_max_now - 1; // note:
-            const real dtau_dp          = (new_constraint_p - old_constraint) / eps;
-            // reset finite difference
-            p_plus[j] = p[j];
-
-            // note: reset forward kinematics because 'v' and 'a' don't change it
-            forward_kinematics(opt.manip, manip_data, p); // todo: precompute once
-
-            // partial derivative of torque constraints w.r.t. velocity
-            // finite difference on velocity
-            v_plus[j] += eps;
-            dynamics(opt.manip, manip_data, v_plus, a_plus);
-            const real new_constraint_v = std::abs(1.01 * manip_data.efforts[joint]) / tau_max_now - 1; // note:
-            const real dtau_dv          = (new_constraint_v - old_constraint) / eps;
-            // reset finite difference
-            v_plus[j] = v[j];
-
-            // partial derivative of torque constraints w.r.t. acceleration
-            // finite difference on acceleration
-            a_plus[j] += eps;
-            dynamics(opt.manip, manip_data, v_plus, a_plus);
-            const real new_constraint_a = std::abs(1.01 * manip_data.efforts[joint]) / tau_max_now - 1; // note:
-            const real dtau_da          = (new_constraint_a - old_constraint) / eps;
-            // reset finite difference
-            a_plus[j] = a[j];
-
-            // insert into the gradient
-            for (int i = 0; i < n_affected_control_points; i++) {
-              fill_column[x_idx + i] = bp_to_use[i] * dtau_dp +
-                                       bv_to_use[i] * dtau_dv * one_over_T +
-                                       ba_to_use[i] * dtau_da * one_over_T2;
-            }
-            x_idx += x_idx_skip;
-            // gradient w.r.t. T note: fix
-            fill_column.back() += dtau_dv * (-v[j] * one_over_T) + dtau_da * (-2 * a[j] * one_over_T); // note:
-          }
-          con++;                                                                                       // note: moved out of the inner j loop (only changes at the end of all j torques per joint)
-        }
-
-        // Tests for grad of segment compared to the old compute_constraint function
-        // position
-        for (int joint = 0; joint < n_joints; joint++) {
-          CHECK(is_close(grad_segment.col(joint), grad.col(n_constraints_per_segment * (start_point_for_segment + max_pos_indices[joint]) + joint), 0.02));
-          CHECK(std::abs(max_pos_constraints[joint] - result[n_constraints_per_segment * (start_point_for_segment + max_pos_indices[joint]) + joint]) < eps);
-        }
-        // velocity
-        for (int joint = 0; joint < n_joints; joint++) {
-          bool condition_1 = is_close(grad_segment.col(n_joints + joint), grad.col(n_constraints_per_segment * (start_point_for_segment + max_vel_indices[joint]) + n_joints + joint), 0.02);
-          bool condition_2 = std::abs(opt.bspline.traj.vel(joint, start_point_for_segment + max_vel_indices[joint])) < eps;
-          CHECK((condition_1 || condition_2));
-
-          CHECK(std::abs(max_vel_constraints[joint] - result[n_constraints_per_segment * (start_point_for_segment + max_vel_indices[joint]) + n_joints + joint]) < eps);
-        }
-        // acceleration
-        for (int joint = 0; joint < n_joints; joint++) {
-          bool condition_1 = is_close(grad_segment.col(2 * n_joints + joint), grad.col(n_constraints_per_segment * (start_point_for_segment + max_acc_indices[joint]) + 2 * n_joints + joint), 0.2);
-          bool condition_2 = std::abs(opt.bspline.traj.acc(joint, start_point_for_segment + max_acc_indices[joint])) < eps;
-
-          CHECK((condition_1 || condition_2));
-          CHECK(std::abs(max_acc_constraints[joint] - result[n_constraints_per_segment * (start_point_for_segment + max_acc_indices[joint]) + 2 * n_joints + joint]) < eps);
-        }
-        // torque
-        for (int joint = 0; joint < n_joints; joint++) {
-          CHECK(is_close(grad_segment.col(3 * n_joints + joint), grad.col(n_constraints_per_segment * (start_point_for_segment + max_tor_indices[joint]) + 3 * n_joints + joint), 0.02));
-          CHECK(std::abs(max_tor_constraints[joint] - result[n_constraints_per_segment * (start_point_for_segment + max_tor_indices[joint]) + 3 * n_joints + joint]) < eps);
-        }
-      }
-    }
-  }
-};
-
-TEST_CASE("Analytic dv/dT & da/dT tests", "[acceleration]") {
-  int n_iterations = 1000;
-
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 100, 5, 6);
-  opt.bspline = bspline;
-
-  opt.guess.type      = Guess::custom;
-  opt.guess.initial_x = Array(opt.bspline.x_len(opt.task), 2.0);
-  // opt.guess.n_random_shots = 100;
-
-  opt.constraints.position            = true;
-  opt.constraints.velocity            = true;
-  opt.constraints.acceleration        = true;
-  opt.constraints.torque              = true;
-  opt.constraints.tool_speed          = false;
-  opt.constraints.self_collisions     = false;
-  opt.constraints.external_collisions = false;
-
-  opt.max_tries         = 1;
-  opt.success_tolerance = 0.01;
-
-  real eps = 1e-5;
-
-  auto xlen = opt.bspline.x_len(opt.task);
-
-  for (int iter = 0; iter < n_iterations; iter++) {
-
-    initialize_optimization(&opt);
-    n_con(&opt);
-
-    auto x = init_guess(&opt);
-    bspline.compute_trajectory(x, opt.task);
-
-    Array result(opt.constraints.n_constraints);
-    compute_constraints(result.data, x, &opt);
-
-    Array x_plus(xlen);
-    x_plus = x;
-    Array grad(opt.constraints.n_constraints);
-    Array grad_analytic(opt.constraints.n_constraints);
-
-    // last point T
-    u32 j = xlen - 1;
-    // memcpy(x_plus.data, x, xlen * sizeof(real)); todo: check if necessary
-    x_plus[j] += eps;
-
-    // compute constraints
-    Array r_plus_T(opt.constraints.n_constraints);
-    compute_constraints(r_plus_T.data, x_plus, &opt);
-
-    for (u32 i = 0; i < opt.constraints.n_constraints; i++) {
-      grad[i] = (r_plus_T[i] - result[i]) / eps;
-    }
-
-    int fill_idx = 24;
-    for (int point = 1; point < opt.bspline.n_points; point++) {
-      // Position is all zeros
-      fill_idx += opt.manip.n_joints;
-      // Velocity
-      for (int joint = 0; joint < opt.manip.n_joints; joint++) {
-        grad_analytic[fill_idx] = -(result[fill_idx] + 1) / x[xlen - 1];
-        CHECK(std::abs(grad[fill_idx] - grad_analytic[fill_idx]) < 1e-4);
-        fill_idx++;
-      }
-      // Acceleration
-      for (int joint = 0; joint < opt.manip.n_joints; joint++) {
-        grad_analytic[fill_idx] = -2.0 * (result[fill_idx] + 1) / x[xlen - 1];
-        CHECK(std::abs(grad[fill_idx] - grad_analytic[fill_idx]) < 1e-4);
-        fill_idx++;
-      }
-
-      for (int joint = 0; joint < opt.manip.n_joints; joint++) {
-        fill_idx++;
-      }
-    }
-  }
-};
-
-} // namespace blast
diff --git a/tests/benchmarks/bench_constraints.cpp b/tests/benchmarks/bench_constraints.cpp
deleted file mode 100644
index f0e5917..0000000
--- a/tests/benchmarks/bench_constraints.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-namespace blast {
-
-
-// Testing by filling result instead of returning. No significant difference
-inline void bound_constraint(real* result, const real& value, const real& lower_bound, const real& higher_bound) {
-  real denom             = higher_bound - lower_bound;
-  denom                  = denom == 0 ? 1 : 0; // todo: what??
-  real constraint_lower  = (lower_bound == -INF_REAL) ? -1.0 : -(value - lower_bound) / denom;
-  real constraint_higher = (higher_bound == INF_REAL) ? -1.0 : (value - higher_bound) / denom;
-  result[0]              = constraint_higher >= constraint_lower ? constraint_higher : constraint_lower;
-}
-
-// No significant difference
-// note : bounds constraint function changed since testing. It is now more robust and cannot be directly compared.
-TEST_CASE("Bound constraint function benchmark", "[constraint]") {
-  auto opt   = get_generic_link6_opt();
-  auto manip = opt.manip;
-
-  Array result(manip.joints);
-
-  Array pos = get_Link6_singularity();
-
-  Array expected_result = {-1.0, -1.0, -1.0, -1.0, -1.0, -1.0};
-
-  BENCHMARK_ADVANCED("Written position function")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      for (int j = 0; j < manip.joints; j++) {
-        real denom = manip.position_max[j] - manip.position_min[j];
-        denom      = denom == 0 ? 1 : 0;
-        // denom = manip.position_min[j] == 0 ? 1 : manip.position_min[j];
-        real constraint_lower = (manip.position_min[j] == -INF_REAL) ? -1.0 : (pos[j] - manip.position_min[j]) / denom;
-        // denom = manip.position_max[j] == 0 ? 1 : manip.position_max[j];
-        real constraint_higher = (manip.position_max[j] == INF_REAL) ? -1.0 : (pos[j] - manip.position_max[j]) / denom;
-        result[j]              = constraint_higher >= constraint_lower ? constraint_higher : constraint_lower;
-      }
-      return result;
-    });
-    CHECK(is_close(result, expected_result));
-  };
-
-
-  BENCHMARK_ADVANCED("Bounds constraint position function with pointer")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      for (int j = 0; j < manip.joints; j++) {
-        bound_constraint(&(result[j]), pos[j], manip.position_min[j], manip.position_max[j]);
-        // result[j] bound_constraint(pos[j], manip.position_min[j], manip.position_max[j]);
-      }
-      return result;
-    });
-    CHECK(is_close(result, expected_result));
-  };
-
-  BENCHMARK_ADVANCED("Bounds constraint position function")
-  (Catch::Benchmark::Chronometer meter) {
-    meter.measure([&] {
-      for (int j = 0; j < manip.joints; j++) {
-        // bound_constraint(result.data[j], pos[j], manip.position_min[j], manip.position_max[j]);
-        result[j] = bound_constraint(pos[j], manip.position_min[j], manip.position_max[j]);
-      }
-      return result;
-    });
-    CHECK(is_close(result, expected_result));
-  };
-}
-
-} // namespace blast
diff --git a/tests/benchmarks/bench_functions.cpp b/tests/benchmarks/bench_functions.cpp
deleted file mode 100644
index 583f70c..0000000
--- a/tests/benchmarks/bench_functions.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-
-#include <blast>
-#include "catch2/catch.hpp"
-#include "test_helper/test_helper.hpp"
-
-namespace blast {
-
-// No significant difference
-TEST_CASE("Benchmark of .at() vs [] for std::vector", "[general]") {
-  std::vector<Capsule> capsules(250);
-  Capsule              caps;
-  caps.p1 = {1, 1, 1};
-  caps.p2 = {2, 2, 2};
-  caps.r  = 2.0;
-  for (auto& capsule: capsules) {
-    capsule = caps;
-  }
-
-  real result = 0;
-  BENCHMARK(".at() performance") {
-    result = 0;
-    for (int i = 0; i < capsules.size(); i++) {
-      result += capsules.at(i).r;
-    }
-    return result;
-  };
-  real at_result = result;
-
-  BENCHMARK("[] performance") {
-    result = 0;
-    for (int i = 0; i < capsules.size(); i++) {
-      result += capsules[i].r;
-    }
-    return result;
-  };
-
-  BENCHMARK("range based for loop performance") {
-    result = 0;
-    for (auto& capsule: capsules) {
-      result += capsule.radius;
-    }
-    return result;
-  };
-  CHECK(at_result == Approx(result));
-}
-} // namespace blast
diff --git a/tests/benchmarks/bench_generic.cpp b/tests/benchmarks/bench_generic.cpp
deleted file mode 100644
index e69de29..0000000
diff --git a/tests/benchmarks/bench_manipulator.cpp b/tests/benchmarks/bench_manipulator.cpp
deleted file mode 100644
index 732a511..0000000
--- a/tests/benchmarks/bench_manipulator.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#define BLAST_TRACE_LEVEL 3
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include "tracy/Tracy.hpp"
-#include "tracy/TracyC.h"
-
-
-namespace blast {
-
-TEST_CASE("Manipulator benchmarks", "[manipulator]") {
-
-  ManipulatorTempData manip_data;
-  BENCHMARK_ADVANCED("Kinematics & Dynamics on a Gen3")
-  (Catch::Benchmark::Chronometer meter) {
-
-    const u32 n_points = 256;
-    const u32 n_joints = 7;
-    const u32 p        = 5;
-    const u32 ncontrol = 24;
-    auto      manip    = get_generic_gen3();
-    Bspline   bspline(ncontrol, n_points, p, n_joints);
-    Matrix    efforts(n_joints, n_points);
-
-    // random task
-    real   amp = 10;
-    Matrix task(n_joints, 6);
-    for (u32 i = 0; i < task.rows; i++) {
-      for (u32 j = 0; j < task.cols; j++) {
-        task(i, j) = amp * get_random();
-      }
-    }
-
-
-    // random optimization vector
-    Array x(bspline.x_len(task));
-    for (u32 i = 0; i < x.size; i++)
-      x[i] = amp * get_random();
-    x[x.size - 1] = 3.f;
-    bspline.compute_trajectory(x, task);
-
-    meter.measure([&] {
-      for (int i = 0; i < n_points; ++i) {
-#if BLAST_TRACE_LEVEL >= 3
-        ZoneScoped;
-#endif
-        auto q   = bspline.traj.pos.col(i);
-        auto qd  = bspline.traj.vel.col(i);
-        auto qdd = bspline.traj.acc.col(i);
-
-        forward_kinematics(manip, manip_data, q);
-        dynamics(manip, manip_data, qd, qdd);
-#if BLAST_TRACE_LEVEL >= 3
-        FrameMark;
-#endif
-      }
-      return manip_data.efforts;
-    });
-  };
-}
-
-} // namespace blast
diff --git a/tests/benchmarks/bench_optimization_methods.cpp b/tests/benchmarks/bench_optimization_methods.cpp
deleted file mode 100644
index 0aacff3..0000000
--- a/tests/benchmarks/bench_optimization_methods.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-#define BLAST_USE_NATIVE_SQP
-
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-#include <iomanip>
-
-using blast::get_generic_link6_opt;
-using blast::Optimization;
-using blast::OptimizationMethod;
-using blast::optimize;
-using blast::real;
-using blast::Result;
-
-struct MethodStats {
-  real success_rate         = 0;
-  real mean_compute_time_ms = 0;
-  real mean_traj_time_s     = 0;
-  real mean_max_constraint  = 0;
-};
-
-inline MethodStats run_trials(OptimizationMethod method, int n_trials) {
-  using namespace blast;
-  int  n_success            = 0;
-  real total_compute_time   = 0;
-  real total_traj_time      = 0;
-  real total_max_constraint = 0;
-
-
-  // Optimization opt(get_generic_gen3(), get_gen3_task());
-  Optimization opt(get_generic_Link6(), get_link6_task());
-
-  opt.world = get_lab_world();
-
-  Bspline bspline(16, 110, 5, 6);
-  opt.bspline = bspline;
-
-  opt.constraints.position            = true;
-  opt.constraints.velocity            = true;
-  opt.constraints.acceleration        = true;
-  opt.constraints.torque              = true;
-  opt.constraints.tool_speed          = true;
-  opt.constraints.self_collisions     = true;
-  opt.constraints.external_collisions = true;
-
-  opt.constraints.n_collision_constraints = 1;
-  opt.max_tries                           = 1;
-  opt.success_tolerance                   = 0.01;
-
-  opt.guess.type = Guess::custom;
-
-  auto   t1 = get_tick_us();
-  Result result(&opt);
-
-  for (int i = 0; i < n_trials; i++) {
-    opt.guess.initial_x = guess_random(opt.bspline, opt.task);
-
-    opt.method = method;
-    result     = optimize(&opt);
-
-    if (result.success) {
-      n_success++;
-      total_traj_time += result.x.back();
-      total_compute_time += result.compute_time;
-      total_max_constraint = std::max(total_max_constraint, result.max_constraint_value);
-    }
-  }
-
-  MethodStats s;
-  s.success_rate         = 100.0 * (real) n_success / n_trials;
-  s.mean_compute_time_ms = total_compute_time / n_trials;
-  s.mean_traj_time_s     = n_success > 0 ? total_traj_time / n_success : 0;
-  s.mean_max_constraint  = total_max_constraint / n_trials;
-  return s;
-}
-
-int main() {
-  constexpr int n_trials = 10;
-
-  struct Entry {
-    OptimizationMethod method;
-    const char*        name;
-  };
-
-  constexpr std::array<Entry, 4> methods = {{
-          {OptimizationMethod::with_segments, "with_segments"},
-          {OptimizationMethod::baseline, "baseline"},
-          {OptimizationMethod::with_analytical_pva, "with_analytical_pva"},
-          {OptimizationMethod::with_analytical_dynamics, "with_analytical_dynamics"},
-  }};
-
-  std::cout << "\n--- Optimization Method Benchmark (" << n_trials << " trials each) ---\n";
-  std::cout << std::left
-            << std::setw(32) << "Method"
-            << std::setw(14) << "Success"
-            << std::setw(20) << "Compute time (ms)"
-            << std::setw(16) << "Traj time (s)"
-            << "Max constraint\n";
-  std::cout << std::string(92, '-') << "\n";
-
-  for (const auto& e: methods) {
-    auto s = run_trials(e.method, n_trials);
-    std::cout << std::left
-              << std::setw(32) << e.name
-              << std::setw(14) << s.success_rate
-              << std::setw(20) << s.mean_compute_time_ms
-              << std::setw(16) << s.mean_traj_time_s
-              << s.mean_max_constraint << std::endl;
-  }
-
-  return 0;
-}
diff --git a/tests/benchmarks/bench_paper_gradients.cpp b/tests/benchmarks/bench_paper_gradients.cpp
deleted file mode 100644
index f0753d8..0000000
--- a/tests/benchmarks/bench_paper_gradients.cpp
+++ /dev/null
@@ -1,1372 +0,0 @@
-//
-// Created by nikos on 2025-09-05.
-//
-#include <blast>
-#include "../tests/test_helper/test_helper.hpp"
-
-#define BLAST_TRACE_LEVEL 0
-// #define BLAST_USE_NATIVE_SQP
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include "tracy/Tracy.hpp"
-#include "tracy/TracyC.h"
-
-#include <fcntl.h> // _O_WRONLY
-#include <io.h>    // _open, _dup, _dup2, _close
-
-using namespace blast;
-
-struct IOSilencer {
-  int             saved_stdout_fd = -1;
-  int             null_fd         = -1;
-  std::streambuf* old_cout        = nullptr;
-  std::streambuf* old_cerr        = nullptr;
-  std::ofstream   null_stream;
-
-  IOSilencer() {
-    // Flush buffers
-    std::cout.flush();
-    std::cerr.flush();
-    fflush(stdout);
-
-    // Redirect std::cout and std::cerr to /dev/null
-    null_stream.open("NUL"); // Windows null device
-    old_cout = std::cout.rdbuf();
-    old_cerr = std::cerr.rdbuf();
-    std::cout.rdbuf(null_stream.rdbuf());
-    std::cerr.rdbuf(null_stream.rdbuf());
-
-    // Redirect printf (C stdout)
-    saved_stdout_fd = _dup(_fileno(stdout));
-    null_fd         = _open("NUL", _O_WRONLY);
-    _dup2(null_fd, _fileno(stdout));
-  }
-
-  ~IOSilencer() {
-    std::cout.rdbuf(old_cout);
-    std::cerr.rdbuf(old_cerr);
-    fflush(stdout);
-
-    _dup2(saved_stdout_fd, _fileno(stdout));
-    _close(null_fd);
-    _close(saved_stdout_fd);
-  }
-};
-
-struct Config {
-  int  n_ctrl   = 0;
-  int  n_points = 0;
-  int  task_idx = 0;
-  int  n_optim  = 0;
-  Task task;
-};
-
-// We create a config list that will be used for all benchmarks
-inline void fill_config_list(std::array<Config, 13>& config_list) {
-
-  std::array<std::tuple<int, int>, 1> bspline_config_list = {std::make_tuple(16, 110)}; // These give rounded number of n_points_per_segment
-  std::vector<Task>                   tasks               = get_Link6_demo1_tasks();    // 13 tasks
-
-  int config_idx = 0;
-  int task_id    = 0;
-
-  for (auto& task: tasks) {
-    for (auto& bspline_config: bspline_config_list) {
-      auto [n_ctrl, n_points]          = bspline_config;
-      config_list[config_idx].n_ctrl   = n_ctrl;
-      config_list[config_idx].n_points = n_points;
-      config_list[config_idx].task_idx = task_id;
-      config_list[config_idx].n_optim  = 1;
-      config_list[config_idx++].task   = task;
-    }
-    task_id++;
-  }
-}
-
-// ------------------------- Accelerated functions --------------------------------
-
-// acc 1
-inline u32 ncon_acc(const Optimization* opt, const int x_idx) {
-  const u32 n_points            = opt->bspline.upper_bounds[x_idx] + 1 - opt->bspline.lower_bounds[x_idx]; // + 1 since ub is inclusive
-  const u32 n_joints            = opt->manip.n_joints;
-  const u32 n_constraints_basic = n_points * n_joints;
-  u32       n_constraints       = 0;
-
-  if (opt->constraints.position)
-    n_constraints += n_constraints_basic;
-  if (opt->constraints.velocity)
-    n_constraints += n_constraints_basic;
-  if (opt->constraints.acceleration)
-    n_constraints += n_constraints_basic;
-  if (opt->constraints.torque)
-    n_constraints += n_constraints_basic;
-
-  if (opt->constraints.tool_speed)
-    n_constraints += n_points;
-
-  if (opt->constraints.self_collisions) {
-    n_constraints += n_points;
-  }
-  if (opt->constraints.external_collisions)
-    n_constraints += n_points * opt->manip._n_caps;
-
-  for (auto& n_con: opt->constraints.n_extra_constraints)
-    n_constraints += n_con;
-
-  return n_constraints;
-}
-
-struct ConstraintPerPoint {
-  Array  pos_constraint;
-  Array  vel_constraint;
-  Array  acc_constraint;
-  Matrix tor_constraint;
-  Array  tool_constraint;
-  Matrix collision_constraint;
-  Array  self_collision_constraint;
-
-  ConstraintPerPoint(int joints, int points, int n_capsules) {
-    pos_constraint.resize(points);
-    vel_constraint.resize(points);
-    acc_constraint.resize(points);
-    tor_constraint.resize(joints, points);
-    tool_constraint.resize(points);
-    self_collision_constraint.resize(points);
-    collision_constraint.resize(n_capsules, points);
-  }
-  ~ConstraintPerPoint() = default;
-};
-
-inline void compute_constraints_grad1(ConstraintPerPoint& constraints, const Array& x, const u32 x_idx, const u32 joint_idx, Optimization* opt) {
-
-  opt->bspline.compute_trajectory(x, opt->task);
-
-  auto n_joints = opt->manip.n_joints;
-
-  // ObjMatrix<Capsule> capsules(opt->manip._n_caps, (u32) (opt->bspline.upper_bounds[x_idx] - opt->bspline.lower_bounds[x_idx] + 1) / opt->constraints.n_collision_skip);
-  for (u32 i = opt->bspline.lower_bounds[x_idx]; i <= opt->bspline.upper_bounds[x_idx]; i++) {
-#if BLAST_TRACE_LEVEL >= 3
-    ZoneScopedN("ConstraintSinglePoint");
-#endif
-
-    // todo: reset frame???
-    // rotations_computed -> bool
-    // forward_kinematics_computed -> bool
-
-    ManipulatorTempData manip_data;
-
-
-    // This is calculated every loop
-    auto pos = opt->bspline.traj.pos.col(i);
-    auto vel = opt->bspline.traj.vel.col(i);
-    auto acc = opt->bspline.traj.acc.col(i);
-    Assert(pos.is_alias);
-
-    {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("Kinematics");
-#endif
-      forward_kinematics(opt->manip, manip_data, pos); // fills _Q, _Q_mult, and _p_j
-    }
-
-    {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("Capsules");
-#endif
-      // todo: this cleaner
-      // if (opt->constraints.external_collisions || opt->constraints.self_collisions) {
-      compute_capsules(opt->manip, manip_data);
-      // }
-    }
-
-    if (opt->constraints.position) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("Position");
-#endif
-      // for (int j = 0; j < opt->manip.n_joints; j++) {
-      constraints.pos_constraint[i - opt->bspline.lower_bounds[x_idx]] = bound_constraint(pos[joint_idx], opt->manip.position_min[joint_idx], opt->manip.position_max[joint_idx]);
-
-      // }
-    }
-
-    if (opt->constraints.velocity) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("Velocity");
-#endif
-      // for (int j = 0; j < opt->manip.n_joints; j++) {
-      constraints.vel_constraint[i - opt->bspline.lower_bounds[x_idx]] = std::abs(vel[joint_idx]) / opt->manip.velocity_max[joint_idx] - 1.0;
-      // }
-    }
-
-    if (opt->constraints.acceleration) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("Acceleration");
-#endif
-      // for (int j = 0; j < opt->manip.n_joints; j++) {
-      constraints.acc_constraint[i - opt->bspline.lower_bounds[x_idx]] = std::abs(acc[joint_idx]) / opt->manip.acceleration_max[joint_idx] - 1.0;
-
-      // }
-    }
-
-    if (opt->constraints.torque) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("Dynamics");
-#endif
-      dynamics(opt->manip, manip_data, vel, acc); // fills _efforts
-
-      for (int j = 0; j < opt->manip.n_joints; j++) {
-        constraints.tor_constraint(j, i - opt->bspline.lower_bounds[x_idx]) = std::abs(manip_data.efforts[j]) / opt->manip.torque_max[j] - 1.0;
-      }
-    }
-
-    if (opt->constraints.tool_speed) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("ToolSpeed");
-#endif
-      auto J_tool     = get_J_tool(opt, manip_data);
-      real tool_speed = norm(J_tool * opt->bspline.traj.vel.col(i));
-
-      constraints.tool_constraint[i - opt->bspline.lower_bounds[x_idx]] = bound_constraint(tool_speed, 0.0, opt->manip.tool_speed_max);
-    }
-
-    if (opt->constraints.self_collisions) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("SelfCollisions");
-#endif
-      auto tmp_coll = max(-get_internal_collisions(opt->manip, manip_data));
-      // for (u32 j = 0; j < tmp_coll.size; j++)
-      constraints.self_collision_constraint[i - opt->bspline.lower_bounds[x_idx]] = tmp_coll; //*std::abs(tmp_coll[j]);
-    }
-
-    if (opt->constraints.external_collisions) {
-#if BLAST_TRACE_LEVEL >= 3
-      ZoneScopedN("ExternalCollisions");
-#endif
-      // auto collisions = test_collisions_per_point(manip_data.capsule_list, &(opt->world));
-      Array max_col_constraints(opt->manip._n_caps, -INF_REAL);
-      for (int capsule_id = 0; capsule_id < opt->manip._n_caps; capsule_id++) {
-        real       dist_min = INF_REAL;
-        const auto capsule  = manip_data.capsule_list[capsule_id];
-
-        // check against boxes
-        int count = 0;
-        for (const auto& box: opt->world.boxes) {
-          if (const auto dist = distance(capsule, box);
-              dist < dist_min) {
-            dist_min = dist;
-          }
-          count++;
-        }
-
-        // check against capsules
-        count = 0;
-        for (const auto caps: opt->world.capsules) {
-          if (const auto dist = distance(capsule, caps);
-              dist < dist_min) {
-            dist_min = dist;
-          }
-          count++;
-        }
-
-        // check against spheres
-        count = 0;
-        for (const auto sphere: opt->world.spheres) {
-          if (const auto dist = distance(capsule, sphere);
-              dist < dist_min) {
-            dist_min = dist;
-          }
-          count++;
-        }
-
-        dist_min = -dist_min; // negative distance is positive constraint
-
-        // update worst position for the current capsule if necessary
-        if (dist_min > max_col_constraints[capsule_id]) {
-          max_col_constraints[capsule_id] = dist_min;
-        }
-      }
-      for (int k = 0; k < opt->manip._n_caps; k++)
-        constraints.collision_constraint(k, i - opt->bspline.lower_bounds[x_idx]) = max_col_constraints[k];
-    }
-  }
-}
-
-inline void nlopt_constraints_acc1(unsigned m, double* result, unsigned xlen, const double* x, double* grad, void* f_data) {
-#if BLAST_TRACE_LEVEL >= 1
-  ZoneScoped;
-#endif
-  auto* opt = (Optimization*) f_data;
-
-  Array xv;
-  xv.alias(x, xlen);
-  {
-
-#if BLAST_TRACE_LEVEL >= 3
-    ZoneScopedN("Constraints");
-#endif
-    compute_constraints(result, xv, opt);
-  }
-
-  // gradients calculation
-  if (grad) {
-#if BLAST_TRACE_LEVEL >= 3
-    ZoneScopedN("Grad");
-#endif
-    memset(grad, 0, m * xlen * sizeof(real)); // note: zeros grad, since grad originally starts with -6e+66 ...
-
-    constexpr real eps = 1e-5;
-
-    u32   n_con_lb = 0;
-    Array x_plus(xlen);
-
-    // lb & ub automatically calculated by bsplines
-    u32 x_per_joint = (xlen - 1) / opt->manip.n_joints; // = nctrl - 6 (skip first and last 3)
-    u32 joint       = 0;
-
-    u32 grad_idx = 0;
-    u32 x_idx    = 0;
-
-    auto n_joints   = opt->manip.n_joints;
-    auto n_capsules = opt->manip._n_caps;
-
-    memcpy(x_plus.data, x, xlen * sizeof(real)); // todo: is this necessary ? done once, and x_plus is modified to original at the end of each iteration
-
-    for (u32 j = 0; j < xlen - 1; j++) {
-      // last one is T todo: maybe change to 2 for loops (joint & x_per_joint)
-      // vector x is stored as (ctrl points for joint 0, ctrl points for joint 1, ...)
-      joint = j / x_per_joint > joint ? joint + 1 : joint; // increase joint by 1 everytime we reach its ctrl points
-      x_idx = j - joint * x_per_joint + 3;                 // todo: fix for NaN in PVA of task
-
-      x_plus[j] += eps;                                    // todo: add this is for extra constraints (tool, collisions)
-      opt->bspline.compute_trajectory(x_plus, opt->task);
-
-      auto n_con_per_point = ncon_acc(opt, x_idx) / (opt->bspline.upper_bounds[x_idx] + 1 - opt->bspline.lower_bounds[x_idx]);
-
-      auto               n_points = (opt->bspline.upper_bounds[x_idx] + 1 - opt->bspline.lower_bounds[x_idx]);
-      ConstraintPerPoint constraint(n_joints, n_points, n_capsules);
-      compute_constraints_grad1(constraint, x_plus, x_idx, joint, opt);
-
-      n_con_lb = ncon_lb_acc(opt, x_idx); // find the amount of constraints before the current point
-
-      grad_idx = n_con_lb * xlen + j;     // gradients are stored column-wise xlen * npoints
-      for (u32 i = opt->bspline.lower_bounds[x_idx]; i <= opt->bspline.upper_bounds[x_idx]; i++) {
-        // lb & ub are inclusive
-        grad_idx += joint * xlen;
-
-        // position
-        grad[grad_idx] = (constraint.pos_constraint[i - opt->bspline.lower_bounds[x_idx]] - result[i * n_con_per_point + joint]) / eps;
-        grad_idx += n_joints * xlen;
-
-        // velocity
-        grad[grad_idx] = (constraint.vel_constraint[i - opt->bspline.lower_bounds[x_idx]] - result[i * n_con_per_point + n_joints + joint]) / eps;
-        grad_idx += n_joints * xlen;
-
-        // acceleration
-        grad[grad_idx] = (constraint.acc_constraint[i - opt->bspline.lower_bounds[x_idx]] - result[i * n_con_per_point + 2 * n_joints + joint]) / eps;
-        grad_idx += (n_joints - joint) * xlen;
-
-        // torque
-        for (int k = 0; k < n_joints; k++) {
-          grad[grad_idx] = (constraint.tor_constraint(k, i - opt->bspline.lower_bounds[x_idx]) - result[i * n_con_per_point + 3 * n_joints + k]) / eps;
-          grad_idx += xlen;
-        }
-
-        // tool
-        grad[grad_idx] = (constraint.tool_constraint[i - opt->bspline.lower_bounds[x_idx]] - result[i * n_con_per_point + 4 * n_joints]) / eps;
-        grad_idx += xlen;
-
-        // self col
-        grad[grad_idx] = (constraint.self_collision_constraint[i - opt->bspline.lower_bounds[x_idx]] - result[i * n_con_per_point + 4 * n_joints + 1]) / eps;
-        grad_idx += xlen;
-
-        // col
-        for (int k = 0; k < opt->manip._n_caps; k++) {
-          grad[grad_idx] = (constraint.collision_constraint(k, i - opt->bspline.lower_bounds[x_idx]) - result[i * n_con_per_point + 4 * n_joints + 2 + k]) / eps;
-          grad_idx += xlen;
-        }
-      }
-
-      x_plus[j] = x[j];
-    }
-
-    {
-      // last point T
-      u32 j = xlen - 1;
-      memcpy(x_plus.data, x, xlen * sizeof(real));
-      x_plus[j] += eps;
-
-      Array xv_plus;
-      xv_plus.alias(x_plus.data, xlen);
-
-      // compute constraints
-      Array r_plus(m);
-      compute_constraints(r_plus.data, x_plus, opt);
-
-      for (u32 i = 0; i < m; i++)
-        grad[i * xlen + j] = (r_plus[i] - result[i]) / eps;
-    }
-  }
-}
-
-inline Result optimize_acc1(Optimization* opt, u32 output_steps_ms = 1 /*ms*/) {
-  auto   T1 = get_tick_us();
-  Result result(opt); // todo: this is expensive
-
-  // Initialization
-  // configure_internal_data(opt); // todo: Ensure we can remove
-  // initialize_optimization(opt);
-  n_con(opt);
-
-  // Initial validation
-  if (!validate_task(opt)) { // todo: support validate_task when there are no capsules...
-    print(opt->task);
-    return result;
-  }
-
-  const auto n = opt->bspline.x_len(opt->task);
-
-  Array con_tol(opt->constraints.n_constraints, opt->success_tolerance);
-  Array x_tol(n, 0.001);
-
-#ifdef BLAST_USE_NATIVE_SQP
-  nlopt_stopping stop;
-  stop.n          = n;
-  stop.minf_max   = -HUGE_VAL;
-  stop.ftol_rel   = 0;
-  stop.ftol_abs   = 0.001;
-  stop.xtol_rel   = 0;
-  stop.xtol_abs   = x_tol.data;
-  stop.x_weights  = nullptr;
-  stop.nevals_p   = 0;
-  stop.maxeval    = 1000;
-  stop.maxtime    = 30;
-  stop.start      = nlopt_seconds();
-  stop.force_stop = false;
-  stop.stop_msg   = nullptr;
-
-  Array ub(n, INF_REAL);
-  Array lb(n, -INF_REAL);
-  ub.back() = 30.0;
-  lb.back() = 0.01;
-
-  nlopt_constraint fc{};
-  fc.m      = opt->constraints.n_constraints;
-  fc.f      = nullptr;
-  fc.mf     = nlopt_constraints_acc1;
-  fc.pre    = nullptr;
-  fc.f_data = opt;
-  fc.tol    = con_tol.data;
-#else
-  nlopt_opt    o = nlopt_create(NLOPT_LD_SLSQP, n);
-  nlopt_result nlopt_res;
-  nlopt_res = nlopt_add_inequality_mconstraint(o, opt->constraints.n_constraints, nlopt_constraints_acc1, opt, con_tol.data);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_min_objective(o, objective_function, opt);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_lower_bound(o, (int) n - 1, 0.01);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_upper_bound(o, (int) n - 1, 60.0);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_ftol_abs(o, 0.0001);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_xtol_abs(o, x_tol.data);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_maxtime(o, 30);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_maxeval(o, 1000);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-#endif
-
-
-  auto start_guess   = opt->guess; // save for restoration after restarts if necessary
-  int  try_count     = 0;
-  bool is_valid_more = false;
-  bool is_valid      = false;
-  for (; try_count < opt->max_tries; try_count++) { // todo: add nlopt stop criteria to list, add max_time for full loop
-#if BLAST_TRACE_LEVEL >= 1
-    ZoneScopedN("Optimization");
-#endif
-
-    // initial guess
-    Array x;
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("Initial guess");
-#endif
-      x         = init_guess(opt);
-      result.x0 = x;
-    }
-
-    // launch optimization
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("NLopt optimization");
-#endif
-
-      double f = HUGE_VAL;
-      // note: can we initialize grad to 0 here
-#ifdef BLAST_USE_NATIVE_SQP
-      stop.nevals_p              = 0;
-      result.nlopt_exit_criteria = sqp(
-              opt->bspline.x_len(opt->task),
-              objective_function,
-              opt,
-              1,
-              &fc,
-              0,
-              nullptr,
-              lb.data,
-              ub.data,
-              x.data,
-              &f,
-              &stop);
-      result.num_eval = stop.nevals_p;
-
-#else
-      result.nlopt_exit_criteria = nlopt_optimize(o, x.data, &f);
-      result.num_eval            = nlopt_get_numevals(o);
-#endif
-    }
-
-    // validate solution
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("Solution validation");
-#endif
-      Array constraints_points(opt->constraints.n_constraints);
-      compute_constraints(constraints_points.data, x, opt);
-      auto max_con                = max(constraints_points);
-      result.max_constraint_idx   = argmax(constraints_points);
-      result.max_constraint_value = max_con;
-      is_valid                    = max_con < opt->success_tolerance * 2;
-    }
-
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("Solution validation (more points)");
-#endif
-      u64 steps_ms    = (u64) (std::ceil(x.back() * 1e3 / output_steps_ms));
-      x.back()        = (real) (std::ceil(x.back() * 1000.0 / output_steps_ms) * output_steps_ms) * 1e-3;
-      int points_more = (int) (steps_ms + 1);
-
-      Bspline bspline_val_more(opt->bspline.n_ctrl, points_more, opt->bspline.degree, opt->manip.n_joints); // todo: this is expensive
-      bspline_val_more.compute_trajectory(x, opt->task);
-      auto opt_val_more(*opt);
-      opt_val_more.set_bspline(bspline_val_more);
-      n_con(&opt_val_more);
-      Array constraints_more_points(opt_val_more.constraints.n_constraints);
-      compute_constraints(constraints_more_points.data, x, &opt_val_more);
-      auto max_con_more                       = max(constraints_more_points);
-      result.max_constraint_more_points_idx   = argmax(constraints_more_points);
-      result.max_constraint_more_points_value = max_con_more;
-      is_valid_more                           = max_con_more < opt->success_tolerance * 2;
-
-      result.x = x;
-
-      if (is_valid && is_valid_more) {
-        result.trajectory = bspline_val_more.traj;
-        // break;
-      } else if (opt->guess.type != Guess::random && try_count == 0) {
-        opt->guess.type = Guess::random;
-      }
-    }
-#if BLAST_TRACE_LEVEL >= 1
-    FrameMark;
-#endif
-  }
-
-  opt->guess = start_guess; // reset to original
-
-  auto time = (real) (get_tick_us() - T1) / 1000.0;
-
-  // Output results
-  result.success       = is_valid && is_valid_more;
-  result.success_false = is_valid && !is_valid_more;
-  result.compute_time  = time;
-  result.opt           = opt;
-  result.num_tries     = try_count;
-
-#ifndef BLAST_USE_NATIVE_SQP
-  nlopt_destroy(o);
-#endif
-
-  return result;
-}
-
-// acc 2
-template<bool is_grad> // note: n_collision_skip must be 1 for this to work !!!
-blast_fn void compute_constraints_acc2(double* result, Array& gradient_coeffs, Matrix& dtau_dp, Matrix& dtau_dv, Matrix& dtau_da, Matrix& dtool_dp, Matrix& dtool_dv, Matrix& dselfcol_dp, std::vector<Matrix>& dcol_dp, const Array& x, Optimization* opt) {
-#if BLAST_TRACE_LEVEL >= 2
-  ZoneScoped;
-#endif
-
-  enum class CollisionObjectType {
-    box,
-    sphere,
-    capsule,
-  };
-  struct CollisionEntities {
-    // object in the world
-    CollisionObjectType other_object_type = CollisionObjectType::box;
-    union {
-      Box     box{};
-      Sphere  sphere;
-      Capsule capsule;
-    };
-
-    // which point in time to look up the capsule
-    int point_in_segment = 0;
-  };
-
-  double* moving_result = result;
-  u32     grad_idx      = 0;
-
-  constexpr real eps = 1e-5;
-
-  auto joints     = opt->manip.n_joints;
-  auto n_capsules = opt->manip._n_caps;
-
-  std::array<CollisionEntities, MAX_CAPSULES> max_collision_entities{};
-
-  {
-#if BLAST_TRACE_LEVEL >= 3
-    ZoneScopedN("ComputeTrajectory");
-#endif
-    opt->bspline.compute_trajectory(x, opt->task);
-  }
-
-  // Lambda to process common bound constraint operations
-  auto process_bound = [&](real value, real bound_max) {
-    auto [constraint, gradient_coeff] = abs_constraint_dev<is_grad>(value, bound_max);
-    *moving_result++                  = constraint;
-    gradient_coeffs[grad_idx++]       = gradient_coeff;
-  };
-
-  for (u32 i = 0; i < opt->bspline.n_points; i++) {
-#if BLAST_TRACE_LEVEL >= 2
-    ZoneScopedN("ConstraintSinglePoint");
-#endif
-
-    ManipulatorTempData manip_data;
-    // Array               torque_constraint_plus(joints);
-
-    auto pos = opt->bspline.traj.pos.col(i);
-    auto vel = opt->bspline.traj.vel.col(i);
-    auto acc = opt->bspline.traj.acc.col(i);
-    Assert(pos.is_alias);
-
-    Array torque_constraint(joints);
-    real  tool_constraint;
-    real  self_collision_constraint;
-    Array max_col_constraints(n_capsules, -INF_REAL);
-
-    {
-#if BLAST_TRACE_LEVEL >= 2
-      ZoneScopedN("Constraints");
-#endif
-
-
-      {
-#if BLAST_TRACE_LEVEL >= 3
-        ZoneScopedN("Constraints per point");
-#endif
-
-
-        {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("Kinematics");
-#endif
-          forward_kinematics(opt->manip, manip_data, pos);
-        }
-
-        if (opt->constraints.position) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("Position");
-#endif
-          for (int j = 0; j < joints; j++) {
-            auto [constraint, gradient_coeff] = bound_constraint_dev<is_grad>(opt->bspline.traj.pos(j, i), opt->manip.position_min[j], opt->manip.position_max[j]);
-            *moving_result++                  = constraint;
-            gradient_coeffs[grad_idx++]       = gradient_coeff;
-          }
-        }
-
-        if (opt->constraints.velocity) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("Velocity");
-#endif
-          for (int j = 0; j < joints; j++) {
-            process_bound(opt->bspline.traj.vel(j, i), opt->manip.velocity_max[j]);
-          }
-        }
-
-        if (opt->constraints.acceleration) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("Acceleration");
-#endif
-          for (int j = 0; j < joints; j++) {
-            process_bound(opt->bspline.traj.acc(j, i), opt->manip.acceleration_max[j]);
-          }
-        }
-
-        compute_capsules(opt->manip, manip_data);
-
-        if (opt->constraints.torque) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("Dynamics");
-#endif
-          dynamics(opt->manip, manip_data, vel, acc);
-          for (int j = 0; j < joints; j++) {
-            torque_constraint[j] = abs_constraint(manip_data.efforts[j], opt->manip.torque_max[j]);
-            *moving_result++     = torque_constraint[j];
-          }
-        }
-
-        if (opt->constraints.tool_speed) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("ToolSpeed");
-#endif
-          auto J_tool      = get_J_tool(opt, manip_data);
-          real tool_speed  = norm(J_tool * vel);
-          tool_constraint  = bound_constraint(tool_speed, 0.0, opt->manip.tool_speed_max);
-          *moving_result++ = tool_constraint;
-        }
-
-        if (opt->constraints.self_collisions) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("SelfCollisions");
-#endif
-          self_collision_constraint = max(-get_internal_collisions(opt->manip, manip_data));
-          *moving_result++          = self_collision_constraint;
-        }
-
-        if (opt->constraints.external_collisions) {
-#if BLAST_TRACE_LEVEL >= 3
-          ZoneScopedN("ExternalCollisionsCalculate");
-#endif
-          // check every capsule with world
-          for (int capsule_id = 0; capsule_id < n_capsules; capsule_id++) {
-            real       dist_min = INF_REAL;
-            const auto capsule  = manip_data.capsule_list[capsule_id];
-
-            CollisionEntities collision_objects{};
-
-            // check against boxes
-            int count = 0;
-            for (const auto& box: opt->world.boxes) {
-              if (const auto dist = distance(capsule, box);
-                  dist < dist_min) {
-                dist_min                            = dist;
-                collision_objects.other_object_type = CollisionObjectType::box;
-                collision_objects.box               = box;
-                collision_objects.point_in_segment  = i;
-              }
-              count++;
-            }
-
-            // check against capsules
-            count = 0;
-            for (const auto caps: opt->world.capsules) {
-              if (const auto dist = distance(capsule, caps);
-                  dist < dist_min) {
-                dist_min                            = dist;
-                collision_objects.other_object_type = CollisionObjectType::capsule;
-                collision_objects.capsule           = capsule;
-                collision_objects.point_in_segment  = i;
-              }
-              count++;
-            }
-
-            // check against spheres
-            count = 0;
-            for (const auto sphere: opt->world.spheres) {
-              if (const auto dist = distance(capsule, sphere);
-                  dist < dist_min) {
-                dist_min                            = dist;
-                collision_objects.other_object_type = CollisionObjectType::sphere;
-                collision_objects.sphere            = sphere;
-                collision_objects.point_in_segment  = i;
-              }
-              count++;
-            }
-
-            dist_min = -dist_min; // negative distance is positive constraint
-
-            // update worst position for the current capsule if necessary
-            if (dist_min > max_col_constraints[capsule_id]) {
-              max_col_constraints[capsule_id]    = dist_min;
-              max_collision_entities[capsule_id] = collision_objects;
-            }
-          }
-          for (int k = 0; k < n_capsules; k++)
-            *moving_result++ = max_col_constraints[k];
-        }
-      }
-    }
-    if (is_grad) {
-#if BLAST_TRACE_LEVEL >= 2
-      ZoneScopedN("Grad");
-#endif
-      Array pos_plus(joints);
-      Array vel_plus(joints);
-      Array acc_plus(joints);
-      pos_plus = pos;
-      vel_plus = vel;
-      acc_plus = acc;
-
-      // grad_coeffs pos
-      for (u32 j = 0; j < joints; j++) {
-        pos_plus[j] += eps;
-        forward_kinematics(opt->manip, manip_data, pos_plus);
-
-        if (opt->constraints.torque) {
-          dynamics(opt->manip, manip_data, vel_plus, acc_plus);
-          for (u32 k = 0; k < joints; k++) {
-            auto torque_constraint_plus = abs_constraint(manip_data.efforts[k], opt->manip.torque_max[k]);
-            dtau_dp(k, j + joints * i)  = (torque_constraint_plus - torque_constraint[k]) / eps;
-          }
-        }
-
-        if (opt->constraints.tool_speed) {
-          auto J_tool_plus          = get_J_tool(opt, manip_data);
-          real tool_speed_plus      = norm(J_tool_plus * vel_plus);
-          auto tool_constraint_plus = bound_constraint(tool_speed_plus, 0.0, opt->manip.tool_speed_max);
-          dtool_dp(j, i)            = (tool_constraint_plus - tool_constraint) / eps;
-        }
-
-        compute_capsules(opt->manip, manip_data);
-        if (opt->constraints.self_collisions) {
-          auto self_collision_constraint_plus = max(-get_internal_collisions(opt->manip, manip_data));
-          dselfcol_dp(j, i)                   = (self_collision_constraint_plus - self_collision_constraint) / eps;
-        }
-
-        if (opt->constraints.external_collisions) {
-          for (int capsule_id = 0; capsule_id < n_capsules; capsule_id++) {
-            const auto  capsule = manip_data.capsule_list[capsule_id];
-            real        distance_plus;
-            const auto& objects = max_collision_entities[capsule_id];
-            switch (objects.other_object_type) {
-              case CollisionObjectType::box: {
-                distance_plus = distance(capsule, objects.box);
-                break;
-              }
-              case CollisionObjectType::capsule: {
-                distance_plus = distance(capsule, objects.capsule);
-                break;
-              }
-              case CollisionObjectType::sphere: {
-                distance_plus = distance(capsule, objects.sphere);
-                break;
-              }
-            }
-            distance_plus = -distance_plus;
-            // auto external_collisions_plus = -test_collisions_per_point(manip_data.capsule_list, &(opt->world));
-            dcol_dp[i].resize(n_capsules, joints);
-            dcol_dp[i](capsule_id, j) = (distance_plus - max_col_constraints[capsule_id]) / eps;
-          }
-        }
-
-        pos_plus[j] = pos[j];
-      }
-
-      forward_kinematics(opt->manip, manip_data, pos_plus);
-      // grad_coeffs vel
-      for (u32 j = 0; j < joints; j++) {
-        vel_plus[j] += eps;
-
-        if (opt->constraints.torque) {
-          dynamics(opt->manip, manip_data, vel_plus, acc_plus);
-          for (u32 k = 0; k < joints; k++) {
-            auto torque_constraint_plus = abs_constraint(manip_data.efforts[k], opt->manip.torque_max[k]);
-            dtau_dv(k, j + joints * i)  = (torque_constraint_plus - torque_constraint[k]) / eps;
-          }
-        }
-
-        if (opt->constraints.tool_speed) {
-          auto J_tool_plus          = get_J_tool(opt, manip_data);
-          real tool_speed_plus      = norm(J_tool_plus * vel_plus);
-          auto tool_constraint_plus = bound_constraint(tool_speed_plus, 0.0, opt->manip.tool_speed_max);
-          dtool_dv(j, i)            = (tool_constraint_plus - tool_constraint) / eps;
-        }
-
-        vel_plus[j] = vel[j];
-      }
-
-      // grad_coeffs acc
-      for (u32 j = 0; j < joints; j++) {
-        acc_plus[j] += eps;
-
-        if (opt->constraints.torque) {
-          dynamics(opt->manip, manip_data, vel_plus, acc_plus);
-          for (u32 k = 0; k < joints; k++) {
-            auto torque_constraint_plus = abs_constraint(manip_data.efforts[k], opt->manip.torque_max[k]);
-            dtau_da(k, j + joints * i)  = (torque_constraint_plus - torque_constraint[k]) / eps;
-          }
-        }
-
-        acc_plus[j] = acc[j];
-      }
-    }
-  }
-
-  {
-#if BLAST_TRACE_LEVEL >= 3
-    ZoneScopedN("CustomConstraints");
-#endif
-    for (u32 i = 0; i < opt->constraints.extra_constraints.size(); i++) {
-      auto extra_constraint = opt->constraints.extra_constraints[i];
-      extra_constraint(moving_result, opt);
-      moving_result += opt->constraints.n_extra_constraints[i];
-      grad_idx += opt->constraints.n_extra_constraints[i]; // todo: add analytical gradients
-    }
-  }
-}
-
-inline blast_fn void nlopt_constraints_acc2(unsigned m, double* result, unsigned xlen, const double* x, double* grad, void* f_data) {
-#if BLAST_TRACE_LEVEL >= 1
-  ZoneScoped;
-#endif
-  auto* opt = (Optimization*) f_data;
-
-  Array xv;
-  xv.alias(x, xlen);
-
-  int n_active_constraints = 0;
-  if (opt->constraints.position)
-    n_active_constraints++;
-  if (opt->constraints.velocity)
-    n_active_constraints++;
-  if (opt->constraints.acceleration)
-    n_active_constraints++;
-
-  Array               gradient_coeffs(opt->manip.n_joints * n_active_constraints * opt->bspline.n_points); // todo: check performance
-  Matrix              dtau_dp(opt->manip.n_joints, opt->manip.n_joints * opt->bspline.n_points);           // todo: check performance
-  Matrix              dtau_dv(opt->manip.n_joints, opt->manip.n_joints * opt->bspline.n_points);           // todo: check performance
-  Matrix              dtau_da(opt->manip.n_joints, opt->manip.n_joints * opt->bspline.n_points);           // todo: check performance
-  Matrix              dtool_dp(opt->manip.n_joints, opt->bspline.n_points);
-  Matrix              dtool_dv(opt->manip.n_joints, opt->bspline.n_points);
-  Matrix              dselfcol_dp(opt->manip.n_joints, opt->bspline.n_points);
-  std::vector<Matrix> dcol_dp; // (n_caps, n_joints)
-  dcol_dp.resize(opt->bspline.n_points);
-  // (opt->manip.n_joints, opt->bspline.n_points);
-  if (!grad) {
-
-    compute_constraints_acc2<false>(result, gradient_coeffs, dtau_dp, dtau_dv, dtau_da, dtool_dp, dtool_dv, dselfcol_dp, dcol_dp, xv, opt);
-  }
-  if (grad) {
-    compute_constraints_acc2<true>(result, gradient_coeffs, dtau_dp, dtau_dv, dtau_da, dtool_dp, dtool_dv, dselfcol_dp, dcol_dp, xv, opt);
-    {
-#if BLAST_TRACE_LEVEL >= 2
-      ZoneScopedN("Grad Fill");
-#endif
-      memset(grad, 0, m * xlen * sizeof(real)); // note: zeros grad, since grad originally starts with -6e+66 ...
-
-      constexpr real eps      = 1e-5;
-      u32            n_con_lb = 0;
-      Array          x_plus(xlen);
-
-      // lb & ub automatically calculated by bsplines
-      u32 x_per_joint = (xlen - 1) / opt->manip.n_joints; // = nctrl - 6 (skip first and last 3)
-      u32 joint       = 0;
-
-      u32 grad_idx       = 0;
-      u32 constraint_idx = 0;
-      u32 x_idx          = 0;
-
-      for (u32 j = 0; j < xlen - 1; j++) { // last one is T todo: maybe change to 2 for loops (joint & x_per_joint)
-        // vector x is stored as (ctrl points for joint 0, ctrl points for joint 1, ...)
-        joint = j / x_per_joint > joint ? joint + 1 : joint; // increase joint by 1 everytime we reach its ctrl points
-        x_idx = j - joint * x_per_joint + 3;                 // todo: fix for NaN in PVA of task
-
-        n_con_lb = ncon_lb_acc(opt, x_idx);                  // find the amount of constraints before the current point
-
-        // todo: create alias matrix that points to grad
-        // todo: can we change the order in which we store the gradients ?
-        grad_idx       = n_con_lb * xlen + j;                                                        // gradients are stored column-wise xlen * npoints
-        constraint_idx = opt->manip.n_joints * n_active_constraints * opt->bspline.lower_bounds[x_idx];
-        for (u32 i = opt->bspline.lower_bounds[x_idx]; i <= opt->bspline.upper_bounds[x_idx]; i++) { // lb & ub are inclusive
-          grad_idx += joint * xlen;
-          constraint_idx += joint;
-
-          if (opt->constraints.position) {
-            grad[grad_idx] = opt->bspline.basis_p(x_idx, i) * gradient_coeffs[constraint_idx];
-            grad_idx += opt->manip.n_joints * xlen; // increase index by the amount of joints * xlen
-            constraint_idx += opt->manip.n_joints;
-          }
-
-          if (opt->constraints.velocity) {          // todo: basis_v / t once before this loop
-            grad[grad_idx] = opt->bspline.basis_v(x_idx, i) / opt->bspline.traj.t.data[opt->bspline.traj.t.size - 1] * gradient_coeffs[constraint_idx];
-            grad_idx += opt->manip.n_joints * xlen; // increase index by the amount of joints * xlen
-            constraint_idx += opt->manip.n_joints;
-          }
-
-          if (opt->constraints.acceleration) {                // todo: basis_a / t / t once before this loop
-            grad[grad_idx] = opt->bspline.basis_a(x_idx, i) / (opt->bspline.traj.t.data[opt->bspline.traj.t.size - 1] * opt->bspline.traj.t.data[opt->bspline.traj.t.size - 1]) * gradient_coeffs[constraint_idx];
-            grad_idx += (opt->manip.n_joints - joint) * xlen; // increase index by the amount of (joints - current joint) * xlen
-            constraint_idx += (opt->manip.n_joints - joint);
-          }
-
-          if (opt->constraints.torque) {
-
-            for (u32 k = 0; k < opt->manip.n_joints; k++) {
-              grad[grad_idx] = opt->bspline.basis_p(x_idx, i) * dtau_dp(k, joint + opt->manip.n_joints * i) +
-                               opt->bspline.basis_v(x_idx, i) / opt->bspline.traj.t.back() * dtau_dv(k, joint + opt->manip.n_joints * i) +
-                               opt->bspline.basis_a(x_idx, i) / (opt->bspline.traj.t.back() * opt->bspline.traj.t.back()) * dtau_da(k, joint + opt->manip.n_joints * i);
-              grad_idx += xlen;
-              // constraint_idx++; // note: eventhough this gradient is not done analytically, we still need to increase the constraint index
-            }
-          }
-
-          if (opt->constraints.tool_speed) {
-            grad[grad_idx] = opt->bspline.basis_p(x_idx, i) * dtool_dp(joint, i) +
-                             opt->bspline.basis_v(x_idx, i) / opt->bspline.traj.t.back() * dtool_dv(joint, i);
-            grad_idx += xlen;
-            // constraint_idx++; // note: eventhough this gradient is not done analytically, we still need to increase the constraint index
-          }
-
-          if (opt->constraints.self_collisions) {
-            grad[grad_idx] = opt->bspline.basis_p(x_idx, i) * dselfcol_dp(joint, i);
-            grad_idx += xlen;
-            // constraint_idx++; // note: eventhough this gradient is not done analytically, we still need to increase the constraint index
-          }
-
-          if (opt->constraints.external_collisions) {
-            for (int k = 0; k < opt->manip._n_caps; k++) {
-              grad[grad_idx] = opt->bspline.basis_p(x_idx, i) * dcol_dp[i](k, joint);
-              grad_idx += xlen;
-            }
-            // constraint_idx++; // note: eventhough this gradient is not done analytically, we still need to increase the constraint index
-          }
-        }
-      }
-
-      {
-        // last point T
-        u32 j = xlen - 1;
-
-        auto   n_joints             = opt->manip.n_joints;
-        auto   constraint_per_point = m / opt->bspline.n_points;
-        auto   one_over_T           = 1 / opt->bspline.traj.t.back();
-        Matrix gradients;
-        gradients.alias(grad, xlen, m);
-        Array constraints;
-        constraints.alias(result, m);
-
-        for (int i = 1; i < opt->bspline.n_points; i++) {
-          int    constraint_in_point_idx = 0;
-          auto   vel                     = opt->bspline.traj.vel.col(i);
-          auto   acc                     = opt->bspline.traj.acc.col(i);
-          Matrix grad_point(&gradients(0, i * constraint_per_point), xlen, constraint_per_point);
-          Array  constraint_point(&constraints[i * constraint_per_point], constraint_per_point);
-
-          // dp/dT == 0
-          constraint_in_point_idx += (int) n_joints;
-          // dv/dT
-          for (int k = 0; k < n_joints; k++) {
-            grad_point.data[constraint_in_point_idx * xlen + j] = -(constraint_point[constraint_in_point_idx] + 1) * one_over_T;
-            constraint_in_point_idx++;
-          }
-          // da_dT
-          for (int k = 0; k < n_joints; k++) {
-            grad_point.data[constraint_in_point_idx * xlen + j] = -2 * (constraint_point[constraint_in_point_idx] + 1) * one_over_T;
-            constraint_in_point_idx++;
-          }
-          // dtau_dT
-          for (int k = 0; k < n_joints; k++) {
-            for (int l = 0; l < n_joints; l++) {
-              grad_point.data[constraint_in_point_idx * xlen + j] += dtau_dv(k, l + n_joints * i) * (-vel[l] * one_over_T) + dtau_da(k, l + n_joints * i) * (-2 * acc[l] * one_over_T);
-            }
-            constraint_in_point_idx++;
-          }
-          // dtool_dT
-          {
-            for (int k = 0; k < n_joints; k++) {
-              grad_point.data[constraint_in_point_idx * xlen + j] += dtool_dv(k, i) * (-vel[k] * one_over_T);
-            }
-            constraint_in_point_idx++; // unused, but added for uniformity
-          }
-        }
-      }
-
-      if (opt->constraints.show_info) { // when more info is needed per iteration
-        Matrix gradients(xlen, m);
-        Array  constraints(m);
-        for (u32 j = 0; j < xlen; j++) {
-          for (u32 i = 0; i < m; i++) {
-            gradients(j, i) = grad[i * xlen + j];
-            constraints[i]  = result[i];
-          }
-        }
-        opt->constraints.grad_list.push_back(gradients);
-        opt->constraints.constr_list.push_back(constraints);
-        opt->constraints.x_list.push_back(xv);
-      }
-    }
-  }
-}
-
-inline Result optimize_acc2(Optimization* opt, u32 output_steps_ms = 1 /*ms*/) {
-  auto   T1 = get_tick_us();
-  Result result(opt); // todo: this is expensive
-
-  // Initialization
-  // configure_internal_data(opt); // todo: Ensure we can remove
-  // initialize_optimization(opt);
-  n_con(opt);
-
-  // Initial validation
-  if (!validate_task(opt)) { // todo: support validate_task when there are no capsules...
-    print(opt->task);
-    return result;
-  }
-
-  const auto n = opt->bspline.x_len(opt->task);
-
-  Array con_tol(opt->constraints.n_constraints, opt->success_tolerance);
-  Array x_tol(n, 0.001);
-
-#ifdef BLAST_USE_NATIVE_SQP
-  nlopt_stopping stop;
-  stop.n          = n;
-  stop.minf_max   = -HUGE_VAL;
-  stop.ftol_rel   = 0;
-  stop.ftol_abs   = 0.001;
-  stop.xtol_rel   = 0;
-  stop.xtol_abs   = x_tol.data;
-  stop.x_weights  = nullptr;
-  stop.nevals_p   = 0;
-  stop.maxeval    = 100000;
-  stop.maxtime    = 30;
-  stop.start      = nlopt_seconds();
-  stop.force_stop = false;
-  stop.stop_msg   = nullptr;
-
-  Array ub(n, INF_REAL);
-  Array lb(n, -INF_REAL);
-  ub.back() = 30.0;
-  lb.back() = 0.01;
-
-  nlopt_constraint fc{};
-  fc.m      = opt->constraints.n_constraints;
-  fc.f      = nullptr;
-  fc.mf     = nlopt_constraints_acc2;
-  fc.pre    = nullptr;
-  fc.f_data = opt;
-  fc.tol    = con_tol.data;
-#else
-  nlopt_opt    o = nlopt_create(NLOPT_LD_SLSQP, n);
-  nlopt_result nlopt_res;
-  nlopt_res = nlopt_add_inequality_mconstraint(o, opt->constraints.n_constraints, nlopt_constraints_acc2, opt, con_tol.data);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_min_objective(o, objective_function, opt);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_lower_bound(o, (int) n - 1, 0.01);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_upper_bound(o, (int) n - 1, 60.0);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_ftol_abs(o, 0.0001);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_xtol_abs(o, x_tol.data);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_maxtime(o, 5000);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-  nlopt_res = nlopt_set_maxeval(o, 100000);
-  Assert(nlopt_res == NLOPT_SUCCESS);
-#endif
-
-
-  auto start_guess   = opt->guess; // save for restoration after restarts if necessary
-  int  try_count     = 0;
-  bool is_valid_more = false;
-  bool is_valid      = false;
-  for (; try_count < opt->max_tries; try_count++) { // todo: add nlopt stop criteria to list, add max_time for full loop
-#if BLAST_TRACE_LEVEL >= 1
-    ZoneScopedN("Optimization");
-#endif
-
-    // initial guess
-    Array x;
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("Initial guess");
-#endif
-      x         = init_guess(opt);
-      result.x0 = x;
-    }
-
-    // launch optimization
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("NLopt optimization");
-#endif
-
-      double f = HUGE_VAL;
-#ifdef BLAST_USE_NATIVE_SQP
-      stop.nevals_p              = 0;
-      result.nlopt_exit_criteria = sqp(
-              opt->bspline.x_len(opt->task),
-              objective_function,
-              opt,
-              1,
-              &fc,
-              0,
-              nullptr,
-              lb.data,
-              ub.data,
-              x.data,
-              &f,
-              &stop);
-      result.num_eval = stop.nevals_p;
-
-#else
-      result.nlopt_exit_criteria = nlopt_optimize(o, x.data, &f);
-      result.num_eval            = nlopt_get_numevals(o);
-#endif
-    }
-
-    // validate solution
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("Solution validation");
-#endif
-      Array constraints_points(opt->constraints.n_constraints);
-      compute_constraints(constraints_points.data, x, opt);
-      auto max_con                = max(constraints_points);
-      result.max_constraint_idx   = argmax(constraints_points);
-      result.max_constraint_value = max_con;
-      is_valid                    = max_con < opt->success_tolerance * 2;
-    }
-
-    {
-#if BLAST_TRACE_LEVEL >= 1
-      ZoneScopedN("Solution validation (more points)");
-#endif
-      u64 steps_ms    = (u64) (std::ceil(x.back() * 1e3 / output_steps_ms));
-      x.back()        = (real) (std::ceil(x.back() * 1000.0 / output_steps_ms) * output_steps_ms) * 1e-3;
-      int points_more = (int) (steps_ms + 1);
-
-      Bspline bspline_val_more(opt->bspline.n_ctrl, points_more, opt->bspline.degree, opt->manip.n_joints); // todo: this is expensive
-      bspline_val_more.compute_trajectory(x, opt->task);
-      auto opt_val_more(*opt);
-      opt_val_more.set_bspline(bspline_val_more);
-      n_con(&opt_val_more);
-      Array constraints_more_points(opt_val_more.constraints.n_constraints);
-      compute_constraints(constraints_more_points.data, x, &opt_val_more);
-      auto max_con_more                       = max(constraints_more_points);
-      result.max_constraint_more_points_idx   = argmax(constraints_more_points);
-      result.max_constraint_more_points_value = max_con_more;
-      is_valid_more                           = max_con_more < opt->success_tolerance * 2;
-
-      result.x = x;
-
-      if (is_valid && is_valid_more) {
-        result.trajectory = bspline_val_more.traj;
-        // break;
-      } else if (opt->guess.type != Guess::random && try_count == 0) {
-        opt->guess.type = Guess::random;
-      }
-    }
-#if BLAST_TRACE_LEVEL >= 1
-    FrameMark;
-#endif
-  }
-
-  opt->guess = start_guess; // reset to original
-
-  auto time = (real) (get_tick_us() - T1) / 1000.0;
-
-  // Output results
-  result.success       = is_valid && is_valid_more;
-  result.success_false = is_valid && !is_valid_more;
-  result.compute_time  = time;
-  result.opt           = opt;
-  result.num_tries     = try_count;
-
-#ifndef BLAST_USE_NATIVE_SQP
-  nlopt_destroy(o);
-#endif
-
-  return result;
-}
-
-TEST_CASE("Acceleration tests", "[Paper2]") {
-  std::array<Config, 13> config_list;
-  fill_config_list(config_list);
-
-  real eps = 0.01;
-
-  for (int config_id = 0; config_id < config_list.size(); config_id++) {
-    auto n_ctrl   = config_list[config_id].n_ctrl;
-    auto n_points = config_list[config_id].n_points;
-    auto task_idx = config_list[config_id].task_idx;
-    auto n_optim  = config_list[config_id].n_optim;
-    auto task     = config_list[config_id].task;
-    // Optimization opt(get_generic_gen3(), get_gen3_task());
-    Optimization opt(get_generic_Link6(), task);
-
-    opt.world = get_lab_world();
-
-    Bspline bspline(n_ctrl, n_points, 5, 6);
-    opt.bspline = bspline;
-
-    opt.constraints.position            = true;
-    opt.constraints.velocity            = true;
-    opt.constraints.acceleration        = true;
-    opt.constraints.torque              = true;
-    opt.constraints.tool_speed          = true;
-    opt.constraints.self_collisions     = true;
-    opt.constraints.external_collisions = true;
-
-    opt.constraints.n_collision_skip = 1;
-
-    opt.max_tries         = 1;
-    opt.success_tolerance = 0.01;
-
-    opt.guess.type      = Guess::custom;
-    opt.guess.initial_x = guess_random((opt.bspline), opt.task);
-
-    cout << "Config ID:                  " << config_id << endl;
-    cout << "Task id:                    " << task_idx << endl;
-    cout << "n_ctrl:                     " << n_ctrl << endl;
-    cout << "n_points:                   " << n_points << endl;
-
-    Result result(&opt);
-    Result result_acc1(&opt);
-    Result result_acc2(&opt);
-    Result result_acc3(&opt);
-    for (int iter = 0; iter < n_optim; iter++) {
-      {
-        {
-          IOSilencer _;
-          result = optimize(&opt, OptimizationMethod::baseline);
-        }
-
-        {
-          IOSilencer _;
-          result_acc1 = optimize_acc1(&opt);
-        }
-
-        {
-          IOSilencer _;
-          result_acc2 = optimize_acc2(&opt);
-        }
-
-        {
-          IOSilencer _;
-          result_acc3 = optimize(&opt);
-        }
-        CHECK(result_acc1.success == result.success);
-        CHECK(result_acc2.success == result.success);
-        CHECK(result_acc3.success == result.success);
-        CHECK(result_acc1.success_false == result.success_false);
-        CHECK(result_acc2.success_false == result.success_false);
-        CHECK(result_acc3.success_false == result.success_false);
-        CHECK(std::abs(result.x.back() - result_acc1.x.back()) < 0.01);
-        CHECK(std::abs(result.x.back() - result_acc2.x.back()) < 0.01);
-        CHECK(std::abs(result.x.back() - result_acc3.x.back()) < 0.01);
-      }
-    }
-  }
-};
diff --git a/tests/benchmarks/bench_test_manip.cpp b/tests/benchmarks/bench_test_manip.cpp
deleted file mode 100644
index 3a5be9b..0000000
--- a/tests/benchmarks/bench_test_manip.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include "catch2/catch.hpp"
-
-#include <blast>
-#include "test_helper/test_helper.hpp"
-
-namespace blast {
-
-TEST_CASE("Test manipulator benchmark", "[blast]") {
-  using namespace blast;
-
-  // --- Test characteristics ---
-
-  std::vector<real> gen_compute_time;
-  std::vector<real> gen_trajectory_time;
-
-  Array x0(37);
-  fill_random(x0, PI);
-  x0.back() = std::abs(x0.back()) * 5 + 0.1;
-
-  Array gen_x(37);
-  // BENCHMARK("Test manipulator benchmark") {
-  auto opt_gen = get_generic_link6_opt();
-  // opt_gen.guess.change_to_custom(x0);
-  auto t1         = get_tick_us();
-  auto result_gen = blast::optimize(&opt_gen, blast::OptimizationMethod::baseline);
-  auto t2         = get_tick_us();
-  // gen_compute_time.push_back(result_gen.compute_time);
-  gen_compute_time.push_back((t2 - t1) / 1000);
-  // gen_trajectory_time.push_back(result_gen.x[result_gen.x.size-1]);
-  gen_trajectory_time.push_back(result_gen.x[result_gen.x.size - 1]);
-  gen_x = result_gen.x;
-  // };
-
-  std::cout << "Success gen: " << result_gen.success << " / " << result_gen.success_false << std::endl;
-  std::cout << "Compute time gen: " << mean(gen_compute_time) << " ms" << std::endl;
-  std::cout << "Traj time gen: " << mean(gen_trajectory_time) << std::endl;
-
-  std::vector<real> hc_compute_time;
-  std::vector<real> hc_trajectory_time;
-  Array             hc_x(37);
-  // BENCHMARK("Hard-coded Link6 manipulator benchmark") {
-  auto opt_hc = get_hardcoded_link6_opt();
-  // opt_gen.guess.change_to_custom(x0);
-  t1             = get_tick_us();
-  auto result_hc = blast::optimize(&opt_hc, blast::OptimizationMethod::baseline);
-  t2             = get_tick_us();
-  // hc_compute_time.push_back(result_hc.compute_time);
-  hc_compute_time.push_back((t2 - t1) / 1000);
-  // hc_trajectory_time.push_back(result_hc.x[result_hc.x.size-1]);
-  hc_trajectory_time.push_back(result_hc.x[result_hc.x.size - 1]);
-  hc_x = result_hc.x;
-  // };
-
-  std::cout << "Success hc: " << result_hc.success << " / " << result_hc.success_false << std::endl;
-  std::cout << "Compute time hc: " << mean(hc_compute_time) << std::endl;
-  std::cout << "Traj time hc: " << mean(hc_trajectory_time) << std::endl;
-
-  std::cout << "Resulting optimization vector : " << std::endl;
-  print(gen_x);
-  print(hc_x);
-}
-} // namespace blast
diff --git a/tests/container/test_objmatrix.cpp b/tests/container/test_objmatrix.cpp
index 271dfcb..f0bf638 100644
--- a/tests/container/test_objmatrix.cpp
+++ b/tests/container/test_objmatrix.cpp
@@ -2,7 +2,8 @@
 #include <catch2/catch.hpp>
 
 #include <blast>
-#include "test_helper/test_functions.hpp"
+#include "blast_utilities.hpp"
+#include "manipulator/UR5e.hpp"
 
 TEST_CASE("ObjMatrix Copy Constructor", "[ObjMatrix]") {
   blast::ObjMatrix<int> mat1(2, 3);
@@ -105,26 +106,22 @@ TEST_CASE("ObjMatrix Capsules", "[ObjMatrix]") {
 
   blast::ObjMatrix<blast::Capsule> caps_matrix(7, 1);
 
-  blast::Link6 manip;
-  manip._efforts.resize(manip.joints);
-  manip._Q.resize(manip.joints);
-  manip._Q_mult.resize(manip.joints);
-  manip._p_j.resize(manip.joints + 1);
-  manip._capsule_list.resize(manip._n_caps);
+  blast::Manipulator manip = blast::make_UR5e();
 
-  blast::Array pos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-  blast::forward_kinematics(manip, pos);
+  blast::Array               pos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  blast::ManipulatorTempData data;
 
-  manip.compute_capsules();
+  blast::forward_kinematics(manip, data, pos);
+  blast::compute_capsules(manip, data);
 
   blast::World world;
-  blast::add_box({1.0, 0.0, 0.0}, {0.5, 0.5, 0.5}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world);
+  world.add_box({1.0, 0.0, 0.0}, {0.5, 0.5, 0.5}, {1, 0, 0, 0, 1, 0, 0, 0, 1});
 
-  for (blast::u32 i = 0; i < manip.joints; i++)
-    caps_matrix(i, 0) = manip._capsule_list[i];
+  for (blast::u32 i = 0; i < manip.n_joints; i++)
+    caps_matrix(i, 0) = data.capsule_list[i];
 
-  auto distance = blast::test_collisions(caps_matrix, &world, 1, 0, 0);
+  auto distance = blast::test_collisions(caps_matrix, &world, 7, 0, 0);
 
-  auto tmp_dist = 0.5 - manip._p_j[1].x - 0.065;
-  REQUIRE(blast::is_close(distance[0], (blast::real) tmp_dist));
+  auto tmp_dist = 0.5 - data.p_j[1].x - data.capsule_list[0].radius;
+  CHECK(blast::is_close(distance[0], (blast::real) tmp_dist));
 }
diff --git a/tests/manipulators/test_auto_manip.cpp b/tests/manipulators/test_auto_manip.cpp
index 766655e..91b78e5 100644
--- a/tests/manipulators/test_auto_manip.cpp
+++ b/tests/manipulators/test_auto_manip.cpp
@@ -2,9 +2,6 @@
 
 #include <blast>
 #include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
 
 TEST_CASE("get_generic_gen3_auto() test", "[Generic]") {
   auto auto_manip    = blast::get_generic_gen3_auto();
diff --git a/tests/manipulators/test_functions.cpp b/tests/manipulators/test_functions.cpp
deleted file mode 100644
index f081fc5..0000000
--- a/tests/manipulators/test_functions.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#include "catch2/catch.hpp"
-
-#include <blast>
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
-
-TEST_CASE("Conversions Q & rx, ry, rz", "[Manipulator]") {
-  using namespace blast;
-  auto manip = get_generic_Link6();
-  for (u32 i = 0; i < 100; i++) {
-
-    Array joint_pose(6);
-
-    fill_random(joint_pose, PI);
-
-    forward_kinematics(manip, joint_pose);
-    auto current_pose = manip.get_tool_pose();
-    auto rot_rxyz     = rpy2rotation({current_pose[3], current_pose[4], current_pose[5]});
-    auto rxyz_rot     = rotation2rpy(rot_rxyz);
-
-    CHECK(is_close(manip._rotations_mult[manip._rotations_mult.size() - 1], rot_rxyz, 0.001));
-    CHECK(is_close(rxyz_rot, {current_pose[3], current_pose[4], current_pose[5]}));
-  }
-}
-
-// todo: fix inverse_kinematics
-/*TEST_CASE("Inverse kinematics zero position test", "[Manipulator]") {
-using namespace blast;
-    auto manip = get_generic_Link6();
-    Array goal_joint_pos = {0, -0.349, 1.92, 0, 0.698, 0}; // home
-    // Array goal_joint_pos(6);
-    // fill_random(goal_joint_pos, PI);
-    Array joint_pos(6);
-
-    // for (int i = 0; i < joint_pos.size; i++) {
-    //     joint_pos[i] = goal_joint_pos[i] + 0.1;
-    // }
-
-    forward_kinematics(manip, goal_joint_pos);
-    auto goal_pose = manip.get_tool_pose();
-
-    auto joint_sol = inverse_kinematics(manip, goal_pose, joint_pos);
-    auto joint_sol_nlopt = inverse_kinematics_nlopt(manip, goal_pose, joint_pos);
-    forward_kinematics(manip, joint_sol);
-    auto actual_pose = manip.get_tool_pose();
-    Array position_diff(3);
-    Vec3 goal_rpy;
-    Vec3 actual_rpy;
-    for (int i = 0; i < 3; i++) {
-        position_diff[i] = (goal_pose[i] - actual_pose[i]);
-        goal_rpy[i] = goal_pose[i+3];
-        actual_rpy[i] = actual_pose[i+3];
-    }
-    auto goal_rotation = rpy2rotation(goal_rpy);
-    auto actual_rotation = rpy2rotation(actual_rpy);
-    auto rotation_diff = goal_rotation - actual_rotation;
-    CHECK(is_close(goal_pose, actual_pose));
-}
-
-TEST_CASE("Inverse kinematics randomized test", "[Manipulator]") {
-using namespace blast;
-    auto manip = get_generic_Link6();
-    Array goal_pose(6);
-    Array joint_pos(6);
-
-    goal_pose = {get_random(), get_random(), get_random(), PI*get_random(), PI*get_random(), PI*get_random()};
-    fill_random(joint_pos, PI);
-
-    auto joint_sol = inverse_kinematics(manip, goal_pose, joint_pos);
-    forward_kinematics(manip, joint_sol);
-    CHECK(is_close(goal_pose, manip.get_tool_pose()));
-}*/
diff --git a/tests/manipulators/test_generic_crx25.cpp b/tests/manipulators/test_generic_crx25.cpp
deleted file mode 100644
index 70b8314..0000000
--- a/tests/manipulators/test_generic_crx25.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#define CATCH_CONFIG_MAIN
-
-#include "blast_rush.h"
-#include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
-using namespace blast;
-
-
-TEST_CASE("Fanuc Crx25ia forward_kinematics() test", "[Generic]") {
-  int                n_tests = 1;
-  real               epsilon = 1e-6;
-  GenericManipulator manip   = get_generic_fanuc_crx25ia();
-
-  for (int i = 0; i < n_tests; i++) {
-    Array test_position(6);
-    fill_random(test_position, PI);
-    print(test_position);
-    std::cout << std::endl;
-    forward_kinematics(manip, test_position);
-    int stop = 0;
-  }
-}
diff --git a/tests/manipulators/test_generic_gen3.cpp b/tests/manipulators/test_generic_gen3.cpp
index 64a84e9..2a8f651 100644
--- a/tests/manipulators/test_generic_gen3.cpp
+++ b/tests/manipulators/test_generic_gen3.cpp
@@ -2,245 +2,136 @@
 #include "catch2/catch.hpp"
 
 #include <blast>
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
 
 using namespace blast;
 
-// GEN3 TESTS
-TEST_CASE("Gen3 forward_kinematics() test", "[Generic]") {
-  int  n_tests = 100;
-  real epsilon = 1e-6;
-  Gen3 expected_manip;
-  auto generic_manip           = get_generic_gen3();
-  expected_manip.base_position = generic_manip.base_position;
-  expected_manip.base_rotation = generic_manip.base_rotation;
-
-  // todo: remove?
-  // setup_manip(&expected_manip);
-  // setup_manip(&generic_manip);
-
-  for (int i = 0; i < n_tests; i++) {
-    Array test_position(7);
-    fill_random(test_position, PI);
-    forward_kinematics(generic_manip, test_position);
-    forward_kinematics(expected_manip, test_position);
-
-    CHECK(is_close(generic_manip._p_j, expected_manip._p_j, epsilon));
-    CHECK(is_close(generic_manip._rotations, expected_manip._rotations, epsilon));
-    CHECK(is_close(generic_manip._rotations_mult, expected_manip._rotations_mult, epsilon));
+// user guide: https://www.kinovarobotics.com/uploads/User-Guide-Gen3-R07.pdf
+struct Gen3 {
+  // basic manipulator properties
+  int joints = 7;
+
+  // All limits are from webapp
+  Array pmax    = {INF_REAL, 2.25, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad
+  Array pmin    = -pmax;                                                       // rad
+  Array vmax    = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f};    // rad/s
+  Array vmin    = -vmax;                                                       // rad/s
+  Array amax    = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL};
+  Array amin    = -amax;                                                       // rad/s^2
+  Array tau_max = {52, 52, 52, 52, 17, 17, 17};                                // Nm
+  Array tau_min = -tau_max;                                                    // Nm
+
+  // tcp & elbow speed limits
+  real tcp_max = 0.5; // from user guide
+
+  // kinematic properties
+  Vec3 p_base = {0, 0, 0};
+  Mat3 Q_base = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+
+  Vec3 p_j0 = {0, 0, 0.1564f};
+
+  Vec3 dv[7] = {
+          {0.0, 0.0054, -0.1284},
+          {0.0, -0.2104, -0.0064},
+          {0.0, -0.0064, -0.2104},
+          {0.0, -0.2084, -0.0064},
+          {0.0, 0.0, -0.1059},
+          {0.0, -0.1059, 0.0},
+          {0.0, 0.0, -0.0615 /*- 0.164*/} // todo: add gripper capsule
+  }; // vector to next joint
+
+  Vec3 ev[7] = {
+          {0, 0, 1},
+          {0, 0, 1},
+          {0, 0, 1},
+          {0, 0, 1},
+          {0, 0, 1},
+          {0, 0, 1},
+          {0, 0, 1}}; // direction vectors of joint
+
+  // dynamic properties
+  real m[7] = {
+          1.377f,
+          1.1636f,
+          1.1636f,
+          0.93f,
+          0.678f,
+          0.678f,
+          0.364f}; // link masses
+
+  Mat3 I[7] = {
+          {0.004570f, 0.000001f, 0.000002f, 0.000001f, 0.004831f, 0.000448f, 0.000002f, 0.000448f, 0.001409f},
+          {0.011088f, 0.000005f, 0.000000f, 0.000005f, 0.001072f, -0.000691f, 0.000000f, -0.000691f, 0.011255f},
+          {0.010932f, 0.000000f, -0.000007f, 0.000000f, 0.011127f, 0.000606f, -0.000007f, 0.000606f, 0.001043f},
+          {0.008147f, -0.000001f, 0.000000f, -0.000001f, 0.000631f, -0.000500f, 0.000000f, -0.000500f, 0.008316f},
+          {0.001596f, 0.000000f, 0.000000f, 0.000000f, 0.001607f, 0.000256f, 0.000000f, 0.000256f, 0.000399f},
+          {0.001641f, 0.000000f, 0.000000f, 0.000000f, 0.000410f, -0.000278f, 0.000000f, -0.000278f, 0.001641f},
+          {0.000214f, 0.000000f, 0.000001f, 0.000000f, 0.000223f, -0.000002f, 0.000001f, -0.000002f, 0.000240f}}; // Inertial tensors
+
+  Vec3 av[7] = {
+          {-0.000023f, -0.010364f, -0.073360f},
+          {-0.000044f, -0.099580f, -0.013278f},
+          {-0.000044f, -0.006641f, -0.117892f},
+          {-0.000018f, -0.075478f, -0.015006f},
+          {0.000001f, -0.009432f, -0.063883f},
+          {0.000001f, -0.045483f, -0.009650f},
+          {-0.000093f, 0.000132f, -0.022905f}}; // centers of mass
+
+  Vec3 sv[7] = {
+          {-dv[0] + av[0]},
+          {-dv[1] + av[1]},
+          {-dv[2] + av[2]},
+          {-dv[3] + av[3]},
+          {-dv[4] + av[4]},
+          {-dv[5] + av[5]},
+          {-dv[6] + av[6]}}; // centers of mass from next joint
+
+  Array _efforts;            // put the efforts temporarily when computing the constraints
+
+  // Internal variables
+  std::vector<Mat3>    Q_static               = {};                      // static rotation to next joint todo: remove this
+  std::vector<Mat3>    _rotations             = std::vector<Mat3>(7);    // put the rotation matrices temporarily when computing the constraints
+  std::vector<Mat3>    _rotations_mult        = std::vector<Mat3>(7);    // put the rotation matrices multiplications temporarily when computing the constraints
+  std::vector<Vec3>    _p_j                   = std::vector<Vec3>(8);    // put the joint coordinates temporarily when computing the constraints
+  std::vector<Capsule> _capsule_list          = std::vector<Capsule>(3); // put the capsules temporarily when computing the constraints
+  u32                  _n_caps                = 3;
+  u32                  _n_internal_collisions = 2;
+
+  inline void compute_rotation_matrices(const Array& joint_position) {
+    Array s(7);
+    Array c(7);
+    sincos(joint_position, s, c);
+
+    // note: these are stored column-wise
+    _rotations[0] = {c[0], -s[0], 0, -s[0], -c[0], 0, 0, 0, -1};
+    _rotations[1] = {c[1], 0, s[1], -s[1], 0, c[1], 0, -1, 0};
+    _rotations[2] = {c[2], 0, -s[2], -s[2], 0, -c[2], 0, 1, 0};
+    _rotations[3] = {c[3], 0, s[3], -s[3], 0, c[3], 0, -1, 0};
+    _rotations[4] = {c[4], 0, -s[4], -s[4], 0, -c[4], 0, 1, 0};
+    _rotations[5] = {c[5], 0, s[5], -s[5], 0, c[5], 0, -1, 0};
+    _rotations[6] = {c[6], 0, -s[6], -s[6], 0, -c[6], 0, 1, 0};
   }
-}
+};
 
-TEST_CASE("Gen3 dynamics() test", "[Generic]") {
+TEST_CASE("Gen3 compute_rotation_matrices() test", "[Generic]") {
   int  n_tests = 100;
   real epsilon = 1e-6;
   Gen3 expected_manip;
-  auto generic_manip           = get_generic_gen3();
-  expected_manip.base_position = generic_manip.base_position;
-  expected_manip.base_rotation = generic_manip.base_rotation;
+  auto generic_manip          = make_Kinova_Gen3();
+  generic_manip.base_position = {0.0, 0.0, 0.0};
+  generic_manip.base_rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 
-  // todo: remove?
-  // setup_manip(&expected_manip);
-  // setup_manip(&generic_manip);
-
-  for (int i = 0; i < n_tests; i++) {
-    Matrix random_task(7, 6);
-    fill_random(random_task, PI);
-    Trajectory test_trajectory = compute_5order_trajectory(2.0, random_task);
-
-    Matrix result_gen(7, test_trajectory.t.size);
-    Matrix result_hc(7, test_trajectory.t.size);
-    for (u32 p = 0; p < test_trajectory.t.size; p++) {
-      auto tmp_pos = test_trajectory.pos.col(p);
-      forward_kinematics(generic_manip, tmp_pos);
-      forward_kinematics(expected_manip, tmp_pos);
-
-      auto tmp_vel = test_trajectory.vel.col(p);
-      auto tmp_acc = test_trajectory.acc.col(p);
-      dynamics(generic_manip, tmp_vel, tmp_acc);
-      dynamics(expected_manip, tmp_vel, tmp_acc);
-
-      for (int j = 0; j < 7; j++) {
-        result_gen(j, p) = generic_manip._efforts[j];
-        result_hc(j, p)  = expected_manip._efforts[j];
-      }
-    }
-    CHECK(is_close(result_gen, result_hc));
-  }
-}
-
-// todo: collisions not working properly
-TEST_CASE("Gen3 compute_capsules() test", "[Generic]") {
-  int  n_tests = 100;
-  Gen3 expected_manip;
-  auto generic_manip           = get_generic_gen3();
-  expected_manip.base_position = generic_manip.base_position;
-  expected_manip.base_rotation = generic_manip.base_rotation;
+  ManipulatorTempData data_expected;
+  ManipulatorTempData data_generic;
 
   for (int i = 0; i < n_tests; i++) {
     Array test_position(7);
     fill_random(test_position, PI);
 
-    forward_kinematics(generic_manip, test_position);
-    forward_kinematics(expected_manip, test_position);
-
-    generic_manip.compute_capsules();
-    expected_manip.compute_capsules();
-    auto generic_internal       = generic_manip.get_internal_collisions();
-    auto expected_internal      = expected_manip.internal_collisions();
-    auto generic_internal_dist  = min(generic_internal);
-    auto expected_internal_dist = min(expected_internal);
-    // CHECK(is_close(generic_internal_dist, expected_internal_dist, epsilon));
-
-    // CHECK(is_close(generic_manip._capsule_list, expected_manip._capsule_list));
-
-    // auto gen_self_collision_distances = generic_manip.internal_collisions();
-    // auto expected_self_collision_distances = expected_manip.internal_collisions();
-
-    // CHECK(is_close(gen_self_collision_distances, expected_self_collision_distances));
-  }
-}
-
-TEST_CASE("Gen3 compute_constraints() test", "[Generic]") {
-  int  n_tests = 100;
-  Gen3 manip_hc;
-
-  auto         opt_gen       = get_generic_gen3_opt();
-  Optimization opt_hc        = get_gen3_gen3_opt();
-  opt_hc.manip.base_position = opt_gen.manip.base_position;
-  opt_hc.manip.base_rotation = opt_gen.manip.base_rotation;
-
-  opt_gen.constraints.self_collisions     = false;
-  opt_gen.constraints.external_collisions = false;
-  opt_hc.constraints.self_collisions      = false;
-  opt_hc.constraints.external_collisions  = false;
-
-  ncon(&opt_gen);
-  ncon(&opt_hc);
-
-  real x_len = opt_gen.bspline.x_len(opt_gen.task);
-
-  Array x_test(x_len);
-  Array result_gen(opt_gen.constraints.n_constraints);
-  Array result_hc(opt_hc.constraints.n_constraints);
-  for (int i = 0; i < n_tests; i++) {
-    fill_random(x_test, PI);
-
-    compute_constraints(result_gen.data, x_test, &opt_gen);
-    compute_constraints(result_hc.data, x_test, &opt_hc);
-
-    CHECK(is_close(result_gen, result_hc));
-  }
-}
-
-TEST_CASE("Gen3 nlopt_constraints() test", "[Generic]") {
-  int  n_tests = 100;
-  Gen3 manip_hc;
-
-  auto         opt_gen       = get_generic_gen3_opt();
-  Optimization opt_hc        = get_gen3_gen3_opt();
-  opt_hc.manip.base_position = opt_gen.manip.base_position;
-  opt_hc.manip.base_rotation = opt_gen.manip.base_rotation;
-
-  opt_gen.constraints.self_collisions     = false;
-  opt_gen.constraints.external_collisions = false;
-  opt_hc.constraints.self_collisions      = false;
-  opt_hc.constraints.external_collisions  = false;
-
-  ncon(&opt_gen);
-  ncon(&opt_hc);
-
-  real x_len = opt_gen.bspline.x_len(opt_gen.task);
-
-  Array x_test(x_len);
-  Array constraints_value_gen(opt_gen.constraints.n_constraints);
-  Array constraints_value_hc(opt_hc.constraints.n_constraints);
-  Array grad_gen(opt_gen.constraints.n_constraints * x_len);
-  Array grad_hc(opt_hc.constraints.n_constraints * x_len);
-  for (int i = 0; i < n_tests; i++) {
-    fill_random(x_test, PI);
-
-    nlopt_constraints(opt_gen.constraints.n_constraints, constraints_value_gen.data, (unsigned int) x_len, x_test.data, grad_gen.data, (void*) (&opt_gen));
-    nlopt_constraints(opt_hc.constraints.n_constraints, constraints_value_hc.data, x_len, x_test.data, grad_hc.data, &opt_hc);
-
-    CHECK(is_close(constraints_value_gen, constraints_value_hc));
-    CHECK(is_close(grad_gen, grad_hc));
-  }
-}
-
-TEST_CASE("Gen3 compute_objective() test", "[objective]") {
-  int  n_tests = 100;
-  Gen3 manip_hc;
-
-  auto         opt_gen       = get_generic_gen3_opt();
-  Optimization opt_hc        = get_gen3_gen3_opt();
-  opt_hc.manip.base_position = opt_gen.manip.base_position;
-  opt_hc.manip.base_rotation = opt_gen.manip.base_rotation;
-
-  for (int i = 0; i < n_tests; i++) {
-    Array x(37, 5 * (get_random() + 1));
-
-    double obj_fun_gen = compute_objective(x, &opt_gen);
-    double obj_fun_hc  = compute_objective(x, &opt_hc);
-
-    CHECK(is_close(obj_fun_gen, obj_fun_hc));
-  }
-}
-
-TEST_CASE("Gen3 objective_function() test", "[objective]") {
-  int  n_tests = 100;
-  Gen3 manip_hc;
-
-  auto         opt_gen       = get_generic_gen3_opt();
-  Optimization opt_hc        = get_gen3_gen3_opt();
-  opt_hc.manip.base_position = opt_gen.manip.base_position;
-  opt_hc.manip.base_rotation = opt_gen.manip.base_rotation;
-
-  real x_len = opt_gen.bspline.x_len(opt_gen.task);
-
-  Array x_test(x_len);
-  Array grad_gen(opt_gen.constraints.n_constraints * x_len);
-  Array grad_hc(opt_hc.constraints.n_constraints * x_len);
-  for (int i = 0; i < n_tests; i++) {
-    fill_random(x_test, PI);
-
-    double obj_fun_gen = objective_function(x_len, x_test.data, grad_gen.data, &opt_gen);
-    double obj_fun_hc  = objective_function(x_len, x_test.data, grad_hc.data, &opt_hc);
-
-    CHECK(is_close(obj_fun_gen, obj_fun_hc));
-    CHECK(is_close(grad_gen, grad_hc));
-  }
-}
+    forward_kinematics(generic_manip, data_generic, test_position);
+    expected_manip.compute_rotation_matrices(test_position);
 
-TEST_CASE("Gen3 optimize() test", "[Optimization][objective]") {
-  int n_tests = 10;
-
-  for (int i = 0; i < n_tests; i++) {
-    auto         opt_gen = get_generic_gen3_opt();
-    Optimization opt_hc  = get_gen3_gen3_opt();
-
-    opt_gen.constraints.self_collisions     = false;
-    opt_gen.constraints.external_collisions = false;
-    opt_hc.constraints.self_collisions      = false;
-    opt_hc.constraints.external_collisions  = false;
-
-    World world;
-    add_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world); // vertical plate (no coll)
-    // add_box({0.6415, 0.0237, -0.53815}, {2.0, 2.0, 0.381}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world); // table
-
-    opt_gen.set_world(world);
-    opt_hc.set_world(world);
-
-    auto result_gen = optimize(&opt_gen, OptimizationMethod::baseline);
-
-    opt_hc.guess.type      = Guess::custom;
-    opt_hc.guess.initial_x = (result_gen.x0);
-    auto result_hc         = optimize(&opt_hc, OptimizationMethod::baseline);
-
-    CHECK(is_close(result_gen.x0, result_hc.x0));
-    CHECK(is_close(result_gen.x, result_hc.x));
-    CHECK(result_gen.success == result_hc.success);
-    CHECK(result_gen.success_false == result_hc.success_false);
+    for (int j = 0; j < expected_manip._rotations.size(); j++) {
+      CHECK(is_close(data_generic.rotations[j], expected_manip._rotations[j], epsilon));
+    }
   }
 }
diff --git a/tests/manipulators/test_generic_link6.cpp b/tests/manipulators/test_generic_link6.cpp
deleted file mode 100644
index 271706b..0000000
--- a/tests/manipulators/test_generic_link6.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-#define CATCH_CONFIG_MAIN
-#include "catch2/catch.hpp"
-
-#include <blast>
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
-
-
-// From OG blast :
-blast::Matrix jacobian(blast::Link6 manip, const blast::Array& pos) {
-  blast::Array s(6);
-  blast::Array c(6);
-  blast::sincos(pos, s, c);
-  blast::Mat3 Q1 = {c[0], -s[0], 0, -s[0], -c[0], 0, 0, 0, -1}; // base -> 0
-  blast::Mat3 Q2 = {c[1], 0, -s[1], -s[1], 0, -c[1], 0, 1, 0};  // 0    -> 1
-  blast::Mat3 Q3 = {c[2], -s[2], 0, -s[2], -c[2], 0, 0, 0, -1}; // 1    -> 2
-  blast::Mat3 Q4 = {c[3], 0, -s[3], -s[3], 0, -c[3], 0, 1, 0};  // 2    -> 3
-  blast::Mat3 Q5 = {c[4], 0, -s[4], -s[4], 0, -c[4], 0, 1, 0};  // 3    -> 4
-  blast::Mat3 Q6 = {0, s[5], c[5], 0, c[5], -s[5], -1, 0, 0};   // 4    -> 5
-
-  // unit vectors in 1st reference
-  blast::Vec3 e[6];
-  auto        Q_tmp = Q1;
-  e[0]              = Q_tmp * manip.joint_axes[0];
-  e[1]              = (Q_tmp *= Q2) * manip.joint_axes[1];
-  e[2]              = (Q_tmp *= Q3) * manip.joint_axes[2];
-  e[3]              = (Q_tmp *= Q4) * manip.joint_axes[3];
-  e[4]              = (Q_tmp *= Q5) * manip.joint_axes[4];
-  e[5]              = (Q_tmp *= Q6) * manip.joint_axes[5];
-
-  blast::Vec3 r[6];
-  r[5] = manip.joint_offsets[5];
-  r[4] = manip.joint_offsets[4] + Q6 * r[5];
-  r[3] = manip.joint_offsets[3] + Q5 * r[4];
-  r[2] = manip.joint_offsets[2] + Q4 * r[3];
-  r[1] = manip.joint_offsets[1] + Q3 * r[2];
-  r[0] = manip.joint_offsets[0] + Q2 * r[1];
-
-  Q_tmp = Q1;
-  r[0]  = (Q_tmp) *r[0];
-  r[1]  = (Q_tmp *= Q2) * r[1];
-  r[2]  = (Q_tmp *= Q3) * r[2];
-  r[3]  = (Q_tmp *= Q4) * r[3];
-  r[4]  = (Q_tmp *= Q5) * r[4];
-  r[5]  = (Q_tmp *= Q6) * r[5];
-
-
-  auto cr0 = cross(e[0], r[0]);
-  auto cr1 = cross(e[1], r[1]);
-  auto cr2 = cross(e[2], r[2]);
-  auto cr3 = cross(e[3], r[3]);
-  auto cr4 = cross(e[4], r[4]);
-  auto cr5 = cross(e[5], r[5]);
-
-  // jacobian matrix
-  blast::Matrix J(6, 6);
-  J(3, 0) = e[0].x;
-  J(4, 0) = e[0].y;
-  J(5, 0) = e[0].z;
-  J(3, 1) = e[1].x;
-  J(4, 1) = e[1].y;
-  J(5, 1) = e[1].z;
-  J(3, 2) = e[2].x;
-  J(4, 2) = e[2].y;
-  J(5, 2) = e[2].z;
-  J(3, 3) = e[3].x;
-  J(4, 3) = e[3].y;
-  J(5, 3) = e[3].z;
-  J(3, 4) = e[4].x;
-  J(4, 4) = e[4].y;
-  J(5, 4) = e[4].z;
-  J(3, 5) = e[5].x;
-  J(4, 5) = e[5].y;
-  J(5, 5) = e[5].z;
-
-  J(0, 0) = cr0.x;
-  J(1, 0) = cr0.y;
-  J(2, 0) = cr0.z;
-  J(0, 1) = cr1.x;
-  J(1, 1) = cr1.y;
-  J(2, 1) = cr1.z;
-  J(0, 2) = cr2.x;
-  J(1, 2) = cr2.y;
-  J(2, 2) = cr2.z;
-  J(0, 3) = cr3.x;
-  J(1, 3) = cr3.y;
-  J(2, 3) = cr3.z;
-  J(0, 4) = cr4.x;
-  J(1, 4) = cr4.y;
-  J(2, 4) = cr4.z;
-  J(0, 5) = cr5.x;
-  J(1, 5) = cr5.y;
-  J(2, 5) = cr5.z;
-
-  return J;
-}
-
-// LINK6 TESTS
-
-TEST_CASE("Link6 jacobian() test", "[Generic]") {
-  int  n_tests       = 100;
-  auto generic_manip = blast::get_generic_Link6();
-
-  for (int i = 0; i < n_tests; i++) {
-    blast::Link6 expected_manip;
-    blast::Array test_position(6);
-    fill_random(test_position, blast::PI);
-    forward_kinematics(generic_manip, test_position);
-    blast::Matrix jac_gen = jacobian(generic_manip);
-    blast::Matrix jac_exp = jacobian(expected_manip, test_position);
-
-    CHECK(is_close(jac_gen, jac_exp));
-  }
-}
-
-TEST_CASE("Link6 dynamics() test", "[Generic]") {
-  int          n_tests = 100;
-  blast::real  epsilon = 1e-6;
-  blast::Link6 expected_manip;
-  auto         generic_manip = blast::get_generic_Link6();
-
-  for (int i = 0; i < n_tests; i++) {
-    blast::Matrix random_task(6, 6);
-    fill_random(random_task, blast::PI);
-    auto test_trajectory = compute_5order_trajectory(2.0, random_task);
-
-    blast::Matrix result_gen(6, test_trajectory.t.size);
-    blast::Matrix result_hc(6, test_trajectory.t.size);
-    for (blast::u32 p = 0; p < test_trajectory.t.size; p++) {
-      auto tmp_pos = test_trajectory.pos.col(p);
-      forward_kinematics(generic_manip, tmp_pos);
-      forward_kinematics(expected_manip, tmp_pos);
-
-      auto tmp_vel = test_trajectory.vel.col(p);
-      auto tmp_acc = test_trajectory.acc.col(p);
-      dynamics(generic_manip, tmp_vel, tmp_acc);
-      dynamics(expected_manip, tmp_vel, tmp_acc);
-
-      for (int j = 0; j < 6; j++) {
-        result_gen(j, p) = generic_manip._efforts[j];
-        result_hc(j, p)  = expected_manip._efforts[j];
-      }
-    }
-    CHECK(is_close(result_gen, result_hc));
-  }
-}
-
-TEST_CASE("Link6 forward_kinematics() test", "[Generic]") {
-  int  n_tests       = 100;
-  auto generic_manip = blast::get_generic_Link6();
-
-  for (int i = 0; i < n_tests; i++) {
-    blast::Link6 expected_manip;
-    blast::Array test_position(6);
-    // fill_random(test_position, PI);
-    forward_kinematics(generic_manip, test_position);
-    forward_kinematics(expected_manip, test_position);
-
-    CHECK(is_close(generic_manip._p_j, expected_manip._p_j));
-    CHECK(is_close(generic_manip._Q, expected_manip._Q));
-    CHECK(is_close(generic_manip._Q_mult, expected_manip._Q_mult));
-  }
-}
-
-TEST_CASE("Link6 compute_capsules() test", "[Generic]") {
-  int                n_tests = 100;
-  real               epsilon = 1e-6;
-  blast::Link6       expected_manip;
-  GenericManipulator generic_manip = blast::get_generic_Link6();
-
-  for (int i = 0; i < n_tests; i++) {
-    blast::Array test_position(6);
-    fill_random(test_position, blast::PI);
-
-    forward_kinematics(generic_manip, test_position);
-    forward_kinematics(expected_manip, test_position);
-
-    generic_manip.compute_capsules();
-    expected_manip.compute_capsules();
-
-    auto generic_internal_dist  = min(generic_manip.internal_collisions());
-    auto expected_internal_dist = min(expected_manip.internal_collisions());
-    CHECK(is_close(generic_internal_dist, expected_internal_dist, epsilon));
-  }
-}
-
-TEST_CASE("Link6 compute_constraints() test", "[Generic]") {
-  int          n_tests = 100;
-  blast::Link6 manip_hc;
-
-  auto                       opt_gen = get_generic_link6_opt();
-  Optimization<blast::Link6> opt_hc  = get_hardcoded_link6_opt();
-
-  ncon(&opt_gen);
-  ncon(&opt_hc);
-
-  real xlen = opt_gen.bspline.xlen(opt_gen.task);
-
-  blast::Array x_test(xlen);
-  blast::Array result_gen(opt_gen.constraints.n_constraints);
-  blast::Array result_hc(opt_hc.constraints.n_constraints);
-  for (int i = 0; i < n_tests; i++) {
-    fill_random(x_test, blast::PI);
-
-    compute_constraints(result_gen.data, x_test, &opt_gen);
-    compute_constraints(result_hc.data, x_test, &opt_hc);
-
-    CHECK(is_close(result_gen, result_hc));
-  }
-}
-
-TEST_CASE("Link6 nlopt_constraints() test", "[Generic]") {
-  int          n_tests = 100;
-  blast::Link6 manip_hc;
-
-  auto                       opt_gen = get_generic_link6_opt();
-  Optimization<blast::Link6> opt_hc  = get_hardcoded_link6_opt();
-
-  ncon(&opt_gen);
-  ncon(&opt_hc);
-
-  real xlen = opt_gen.bspline.xlen(opt_gen.task);
-
-  blast::Array x_test(xlen);
-  blast::Array constraints_value_gen(opt_gen.constraints.n_constraints);
-  blast::Array constraints_value_hc(opt_hc.constraints.n_constraints);
-  blast::Array grad_gen(opt_gen.constraints.n_constraints * xlen);
-  blast::Array grad_hc(opt_hc.constraints.n_constraints * xlen);
-  for (int i = 0; i < n_tests; i++) {
-    fill_random(x_test, blast::PI);
-
-    nlopt_constraints<GenericManipulator>((unsigned int) opt_gen.constraints.n_constraints, constraints_value_gen.data, (unsigned int) xlen, x_test.data, grad_gen.data, (void*) (&opt_gen));
-    nlopt_constraints<blast::Link6>(opt_hc.constraints.n_constraints, constraints_value_hc.data, xlen, x_test.data, grad_hc.data, &opt_hc);
-
-    CHECK(is_close(constraints_value_gen, constraints_value_hc));
-    CHECK(is_close(grad_gen, grad_hc));
-  }
-}
-
-TEST_CASE("Link6 compute_objective() test", "[objective]") {
-  int          n_tests = 100;
-  blast::Link6 manip_hc;
-
-  auto                       opt_gen = get_generic_link6_opt();
-  Optimization<blast::Link6> opt_hc  = get_hardcoded_link6_opt();
-
-  for (int i = 0; i < n_tests; i++) {
-    blast::Array x(37, 5 * (get_random() + 1));
-
-    double obj_fun_gen = compute_objective(x, &opt_gen);
-    double obj_fun_hc  = compute_objective(x, &opt_hc);
-
-    CHECK(is_close(obj_fun_gen, obj_fun_hc));
-  }
-}
-
-TEST_CASE("Link6 objective_function() test", "[objective]") {
-  int          n_tests = 100;
-  blast::Link6 manip_hc;
-
-  auto                       opt_gen = get_generic_link6_opt();
-  Optimization<blast::Link6> opt_hc  = get_hardcoded_link6_opt();
-
-  real xlen = opt_gen.bspline.xlen(opt_gen.task);
-
-  blast::Array x_test(xlen);
-  blast::Array grad_gen(opt_gen.constraints.n_constraints * xlen);
-  blast::Array grad_hc(opt_hc.constraints.n_constraints * xlen);
-  for (int i = 0; i < n_tests; i++) {
-    fill_random(x_test, blast::PI);
-
-    double obj_fun_gen = objective_function<GenericManipulator>(xlen, x_test.data, grad_gen.data, &opt_gen);
-    double obj_fun_hc  = objective_function<blast::Link6>(xlen, x_test.data, grad_hc.data, &opt_hc);
-
-    CHECK(is_close(obj_fun_gen, obj_fun_hc));
-    CHECK(is_close(grad_gen, grad_hc));
-  }
-}
-
-TEST_CASE("Link6 optimize() test", "[Optimization]") {
-  blast::Link6 manip_hc;
-  int          n_tests = 1;
-
-  auto                       opt_gen = get_generic_link6_opt();
-  Optimization<blast::Link6> opt_hc  = get_hardcoded_link6_opt();
-
-  opt_gen.constraints.self_collisions     = false;
-  opt_gen.constraints.external_collisions = false;
-  opt_hc.constraints.self_collisions      = false;
-  opt_hc.constraints.external_collisions  = false;
-
-  World world;
-  add_static_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world);  // vertical plate (no coll)
-  add_static_box({0.6415, 0.0237, -0.53815}, {2.0, 2.0, 0.381}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world); // table
-
-  opt_gen.set_world(world);
-  opt_hc.set_world(world);
-
-  std::vector<blast::Task> tasks = get_Link6_demo1_tasks();
-
-  for (int j = 0; j < n_tests; j++) {
-    for (int i = 0; i < tasks.size(); i++) {
-      opt_gen.set_task(tasks[i]);
-      opt_hc.set_task(tasks[i]);
-      auto result_gen = optimize(&opt_gen, OptimizationMethod::baseline);
-
-      opt_hc.guess.type      = GuessType::custom;
-      opt_hc.guess.initial_x = result_gen.x0;
-      auto result_hc         = optimize(&opt_hc, OptimizationMethod::baseline);
-
-      CHECK(is_close(result_gen.x0, result_hc.x0));
-      CHECK(is_close(result_gen.x, result_hc.x));
-      if (!is_close(result_gen.x, result_hc.x)) {
-        std::cout << "Task " << i << " was not the same x" << std::endl;
-        std::cout << "Exit criteria = " << result_gen.nlopt_exit_criteria << std::endl;
-        std::cout << "Exit criteria = " << result_hc.nlopt_exit_criteria << std::endl;
-        std::cout << "num eval = " << result_gen.num_eval << std::endl;
-        std::cout << "num eval = " << result_hc.num_eval << std::endl;
-        print(result_gen.x - result_hc.x);
-      }
-      CHECK(result_gen.success == result_hc.success);
-      CHECK(result_gen.success_false == result_hc.success_false);
-    }
-  }
-}
diff --git a/tests/manipulators/test_generic_ur5e.cpp b/tests/manipulators/test_generic_ur5e.cpp
index b03ef7c..b49dbe8 100644
--- a/tests/manipulators/test_generic_ur5e.cpp
+++ b/tests/manipulators/test_generic_ur5e.cpp
@@ -1,97 +1,102 @@
 #define CATCH_CONFIG_MAIN
-
-#include "../../examples/files/manipulators/get_generic_ur5e_auto.hpp"
-#include "blast_rush.h"
 #include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
+
+#include <blast>
 
 using namespace blast;
 
 TEST_CASE("UR5e forward_kinematics() test", "[Generic]") {
+  Manipulator generic_manip = make_UR5e();
 
-  GenericManipulator generic_manip = get_generic_ur5e_robot();
+  ManipulatorTempData data;
 
   //  @ home position
   Vec3  expected_tool_position = {0.0, -0.233, 1.08};
   Array test_position          = {0.0, -PI / 2, 0.0, -PI / 2, 0.0, 0.0};
-  forward_kinematics(generic_manip, test_position);
-  CHECK(is_close(generic_manip._p_j[generic_manip.joints], expected_tool_position, 1e-2));
+  forward_kinematics(generic_manip, data, test_position);
+  CHECK(is_close(data.p_j[generic_manip.n_joints], expected_tool_position, 1e-2));
 
   //  @ 2nd position
   Vec3  expected_tool_position_2 = {-0.1179, -0.436, 0.152};
   Array test_position_2          = {-1.54, -1.83, -2.28, -0.59, 1.60, 0.023};
-  forward_kinematics(generic_manip, test_position_2);
-  CHECK(is_close(generic_manip._p_j[generic_manip.joints], expected_tool_position_2, 1e-2));
+  forward_kinematics(generic_manip, data, test_position_2);
+
+  CHECK(is_close(data.p_j[generic_manip.n_joints], expected_tool_position_2, 1e-2));
+}
+
+TEST_CASE("UR5e jacobian() - dimensions are 6 x n_joints", "[Generic]") {
+  Manipulator         manip = make_UR5e();
+  ManipulatorTempData data;
+  Array               q = {0.0, -PI / 2, 0.0, -PI / 2, 0.0, 0.0};
+
+  forward_kinematics(manip, data, q);
+  Matrix J = jacobian(manip, data);
+
+  CHECK(J.rows == 6u);
+  CHECK(J.cols == manip.n_joints);
+}
+
+TEST_CASE("UR5e jacobian() - all entries are finite", "[Generic]") {
+  Manipulator         manip = make_UR5e();
+  ManipulatorTempData data;
+  Array               q = {0.0, -PI / 2, 0.0, -PI / 2, 0.0, 0.0};
+
+  forward_kinematics(manip, data, q);
+  Matrix J = jacobian(manip, data);
+
+  for (u32 r = 0; r < J.rows; r++)
+    for (u32 c = 0; c < J.cols; c++)
+      CHECK(std::isfinite(J(r, c)));
 }
 
-TEST_CASE("UR5e optimize() trajectory with obstacles", "[Generic]") {
-  using namespace blast;
-
-  GenericManipulator generic_ur5e = get_generic_ur5e_robot();
-
-  // --- Define Tasks ---
-  std::vector<Array> positions(2);
-  positions[0] = {2.0473, -0.3930, 1.2048, -2.3880, -1.6118, 1.8403}; // First touch point
-  positions[1] = {0.7529, -0.1918, 0.8131, -2.2195, -1.6113, 1.8410}; // Second touch point
-
-  Array top_screen_pos = {1.4610, -0.8622, 0.6568, -1.5820, -1.6106, 1.8282};
-
-  std::vector<Matrix> tasks;
-  for (u32 i = 0; i < positions.size() - 1; i++) {
-    Matrix tmp(6, 6);
-
-    for (u32 j = 0; j < generic_ur5e.joints; j++) {
-      tmp(j, 0) = positions[i][j];
-      tmp(j, 3) = positions[i + 1][j];
-    }
-    tasks.push_back(tmp);
-  }
-
-  // --- Define World ---
-  World world;
-  add_static_box({0.038, -0.676, 0.146}, {0.025, 0.35, 0.2}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world); // vertical plate
-  add_static_box({0.038, -0.676, -0.404}, {0.6, 0.38, 0.35}, {1, 0, 0, 0, 1, 0, 0, 0, 1}, &world); // table
-
-  // --- Define Bspline ---
-  Bspline bspline(16, 200, 5, generic_ur5e.joints);
-
-  // --- Define Guess ---
-  Guess guess; // using default guess
-  guess.n_random_shots = 1000;
-
-  // -- Define Constraints ---
-  Constraints<GenericManipulator> constraints;
-  constraints.position                = true;
-  constraints.velocity                = true;
-  constraints.acceleration            = false;
-  constraints.torque                  = true;
-  constraints.tool_speed              = true;
-  constraints.self_collisions         = true;
-  constraints.external_collisions     = true;
-  constraints.n_collision_constraints = 100;
-  constraints.n_collision_skip        = 2;
-
-  // --- Define Objective ---
-  Objective<GenericManipulator> objective;
-  objective.time_weight = 1;
-
-  real start_time = 0.0;
-
-  for (u32 t = 0; t < tasks.size(); t++) {
-    std::cout << "Task " << t + 1 << " of " << tasks.size() << std::endl;
-    std::cout << "Trajectory starts at time t = " << start_time << " s" << std::endl;
-    Optimization<GenericManipulator> opt(generic_ur5e, tasks[t]);
-    opt.set_world(world);
-    opt.set_bspline(bspline);
-    opt.set_guess(guess);
-    opt.set_constraints(constraints);
-    opt.set_objective(objective);
-
-    auto results = optimize(&opt, OptimizationMethod::baseline, 2);
-    start_time += results.x[results.x.size - 1];
-    std::cout << "Optimization done in " << results.compute_time / 1000.0 << std::endl;
-    CHECK(results.success == true);
-    CHECK(results.success_false == false);
-  }
+TEST_CASE("UR5e jacobian() - linear velocity rows are non-trivial at home position", "[Generic]") {
+  Manipulator         manip = make_UR5e();
+  ManipulatorTempData data;
+  Array               q = {0.0, -PI / 2, 0.0, -PI / 2, 0.0, 0.0};
+
+  forward_kinematics(manip, data, q);
+  Matrix J = jacobian(manip, data);
+
+  // linear velocity part (rows 0-2): at least one column must be nonzero
+  real col_norm_sum = 0.0;
+  for (u32 c = 0; c < J.cols; c++)
+    col_norm_sum += J(0, c) * J(0, c) + J(1, c) * J(1, c) + J(2, c) * J(2, c);
+  CHECK(col_norm_sum > 0.0);
+}
+
+TEST_CASE("UR5e dynamics() - efforts are finite at home position with zero motion", "[Generic]") {
+  Manipulator         manip = make_UR5e();
+  ManipulatorTempData data;
+  Array               q = {0.0, -PI / 2, 0.0, -PI / 2, 0.0, 0.0};
+  Array               vel(manip.n_joints, 0.0);
+  Array               acc(manip.n_joints, 0.0);
+
+  forward_kinematics(manip, data, q);
+  dynamics(manip, data, vel, acc);
+
+  for (u32 j = 0; j < manip.n_joints; j++)
+    CHECK(std::isfinite(data.efforts[j]));
+}
+
+TEST_CASE("UR5e dynamics() - efforts change with nonzero accelerations (Coriolis/centrifugal terms)", "[Generic]") {
+  Manipulator         manip = make_UR5e();
+  ManipulatorTempData data;
+  Array               q = {0.0, -PI / 2, 0.0, -PI / 2, 0.0, 0.0};
+  Array               vel(manip.n_joints, 0.0);
+  Array               acc_zero(manip.n_joints, 0.0);
+  Array               acc_nonzero = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5};
+
+  forward_kinematics(manip, data, q);
+
+  dynamics(manip, data, vel, acc_zero);
+  Array efforts_zero(manip.n_joints);
+  for (u32 j = 0; j < manip.n_joints; j++)
+    efforts_zero[j] = data.efforts[j];
+
+  dynamics(manip, data, vel, acc_nonzero);
+  Array efforts_nonzero(manip.n_joints);
+  for (u32 j = 0; j < manip.n_joints; j++)
+    efforts_nonzero[j] = data.efforts[j];
+
+  CHECK(!is_close(efforts_zero, efforts_nonzero, 1e-6));
 }
diff --git a/tests/manipulators/test_manipulator_constructor.cpp b/tests/manipulators/test_manipulator_constructor.cpp
new file mode 100644
index 0000000..019c1cf
--- /dev/null
+++ b/tests/manipulators/test_manipulator_constructor.cpp
@@ -0,0 +1,126 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+
+#include <blast>
+
+using namespace blast;
+
+static Manipulator make_1dof(real mass, Vec3 cog, Mat3 inertia) {
+  ManipulatorLimits lim;
+  lim.position_max = Array{PI};
+  lim.position_min = Array{-PI};
+  lim.velocity_max = Array{1.0f};
+
+  ManipulatorKinematics kin;
+  kin.joint_offsets[0]    = {0, 0, 0.5f};
+  kin.joint_axes[0]       = {0, 0, 1};
+  kin.static_rotations[0] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+
+  ManipulatorDynamics dyn;
+  dyn.link_masses[0]     = mass;
+  dyn.cog_offsets[0]     = cog;
+  dyn.inertia_tensors[0] = inertia;
+
+  return Manipulator{1, lim, kin, &dyn};
+}
+
+// ─── constructor ────────────────────────────────────────────────────────────
+
+TEST_CASE("Manipulator constructor - kinematics-only leaves dynamic fields zero-initialized", "[constructor]") {
+  ManipulatorLimits lim;
+  lim.position_max = Array{PI};
+  lim.position_min = Array{-PI};
+  lim.velocity_max = Array{1.0f};
+
+  ManipulatorKinematics kin;
+  kin.joint_offsets[0]    = {0, 0, 0.5f};
+  kin.joint_axes[0]       = {0, 0, 1};
+  kin.static_rotations[0] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+
+  Manipulator m{1, lim, kin}; // no dynamics, no capsules
+
+  CHECK(m.n_joints == 1u);
+  CHECK(m._n_caps == 0);
+  CHECK(m.link_masses[0] == 0.0f);
+  CHECK(m._n_internal_collisions == 0);
+}
+
+TEST_CASE("Manipulator constructor - n_joints matches joint_count for standard manipulators", "[constructor]") {
+  CHECK(make_Kinova_Gen3().n_joints == 7u);
+  CHECK(make_UR5e().n_joints == 6u);
+}
+
+// ─── set_capsules ────────────────────────────────────────────────────────────
+
+TEST_CASE("UR5e set_capsules() - capsule count is correct", "[constructor]") {
+  Manipulator ur5e = make_UR5e();
+  CHECK(ur5e._n_caps == 7);
+}
+
+TEST_CASE("UR5e set_capsules() - _n_internal_collisions counts lower-triangle pairs and base contacts", "[constructor]") {
+  Manipulator ur5e = make_UR5e();
+  // 8 lower-triangle collision-matrix pairs + 4 capsules that collide with the base sphere = 12
+  CHECK(ur5e._n_internal_collisions == 12);
+}
+
+// ─── set_payload ─────────────────────────────────────────────────────────────
+
+TEST_CASE("set_payload() - combined mass is sum of link and payload masses", "[payload]") {
+  Manipulator m = make_1dof(10.0f, {0, 0, 0.1f}, Mat3{});
+  m.set_payload(2.0f, {0, 0, 0.3f}, Mat3{});
+  CHECK(is_close(m.link_masses[0], 12.0f, 1e-5f));
+}
+
+TEST_CASE("set_payload() - combined CoG is mass-weighted average", "[payload]") {
+  Manipulator m = make_1dof(10.0f, {0, 0, 0.1f}, Mat3{});
+  m.set_payload(2.0f, {0, 0, 0.3f}, Mat3{});
+  // av_new_z = (10*0.1 + 2*0.3) / 12 = 1.6/12
+  CHECK(is_close(m.cog_offsets[0].z, 1.6f / 12.0f, 1e-5f));
+  CHECK(is_close(m.cog_offsets[0].x, 0.0f, 1e-5f));
+  CHECK(is_close(m.cog_offsets[0].y, 0.0f, 1e-5f));
+}
+
+TEST_CASE("set_payload() - inertia updated via parallel axis theorem", "[payload]") {
+  Manipulator m = make_1dof(10.0f, {0, 0, 0.1f}, Mat3{});
+  m.set_payload(2.0f, {0, 0, 0.3f}, Mat3{});
+  // delta_av_z = 1.6/12 - 0.1 = 1/30
+  // av_to_mass_z = 0.3 - 1.6/12 = 1/6
+  // I_zz = 10*(1/30)^2 + 2*(1/6)^2 = 1/90 + 1/18 = 1/15
+  CHECK(is_close(m.inertia_tensors[0](2, 2), 1.0f / 15.0f, 1e-5f));
+  CHECK(is_close(m.inertia_tensors[0](0, 0), 0.0f, 1e-5f));
+  CHECK(is_close(m.inertia_tensors[0](1, 1), 0.0f, 1e-5f));
+}
+
+// ─── add_tool ────────────────────────────────────────────────────────────────
+
+TEST_CASE("add_tool() - has_tool flag is set", "[tool]") {
+  Manipulator m = make_1dof(10.0f, {0, 0, 0.1f}, Mat3{});
+  CHECK(m.has_tool == false);
+  Tool tool;
+  tool.mass           = 1.0f;
+  tool.inertia_tensor = Mat3{};
+  m.add_tool(tool);
+  CHECK(m.has_tool == true);
+}
+
+TEST_CASE("add_tool() - combined mass is sum of link and tool masses", "[tool]") {
+  Manipulator m = make_1dof(10.0f, {0, 0, 0.1f}, Mat3{});
+  Tool        tool;
+  tool.mass           = 2.0f;
+  tool.cog_offset     = {0, 0, 0.3f};
+  tool.inertia_tensor = Mat3{};
+  m.add_tool(tool);
+  CHECK(is_close(m.link_masses[0], 12.0f, 1e-5f));
+}
+
+TEST_CASE("add_tool() - combined CoG is mass-weighted average", "[tool]") {
+  Manipulator m = make_1dof(10.0f, {0, 0, 0.1f}, Mat3{});
+  Tool        tool;
+  tool.mass           = 2.0f;
+  tool.cog_offset     = {0, 0, 0.3f};
+  tool.inertia_tensor = Mat3{};
+  m.add_tool(tool);
+  CHECK(is_close(m.cog_offsets[0].z, 1.6f / 12.0f, 1e-5f));
+  CHECK(is_close(m.cog_offsets[0].x, 0.0f, 1e-5f));
+  CHECK(is_close(m.cog_offsets[0].y, 0.0f, 1e-5f));
+}
diff --git a/tests/math/test_array.cpp b/tests/math/test_array.cpp
new file mode 100644
index 0000000..c3af841
--- /dev/null
+++ b/tests/math/test_array.cpp
@@ -0,0 +1,418 @@
+#define CATCH_CONFIG_MAIN
+
+#include <blast>
+#include "catch2/catch.hpp"
+
+// ---------------------------------------------------------------------------
+// Construction
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - default constructor produces empty array", "[Math][Array]") {
+  using namespace blast;
+  Array a;
+  CHECK(a.size == 0u);
+  CHECK(a.data == nullptr);
+  CHECK(a.is_alias == false);
+  CHECK(a.is_empty());
+}
+
+TEST_CASE("Array - size constructor allocates and zeroes elements", "[Math][Array]") {
+  using namespace blast;
+  Array a(4u);
+  CHECK(a.size == 4u);
+  CHECK(a.is_empty() == false);
+  for (u32 i = 0; i < a.size; i++)
+    CHECK(a[i] == 0.0);
+}
+
+TEST_CASE("Array - size+value constructor fills all elements", "[Math][Array]") {
+  using namespace blast;
+  Array a(3u, 7.0);
+  CHECK(a.size == 3u);
+  for (u32 i = 0; i < a.size; i++)
+    CHECK(a[i] == 7.0);
+}
+
+TEST_CASE("Array - initializer list constructor stores elements in order", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  CHECK(a.size == 3u);
+  CHECK(a[0] == 1.0);
+  CHECK(a[1] == 2.0);
+  CHECK(a[2] == 3.0);
+}
+
+TEST_CASE("Array - copy constructor produces a deep independent copy", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array b(a);
+  CHECK(b.size == 3u);
+  CHECK(b.is_alias == false);
+  CHECK(b[0] == 1.0);
+  b[0] = 99.0;
+  CHECK(a[0] == 1.0);
+}
+
+TEST_CASE("Array - move constructor transfers ownership", "[Math][Array]") {
+  using namespace blast;
+  Array a(3u, 1.0);
+  Array b(std::move(a));
+  CHECK(b.size == 3u);
+  CHECK(b[0] == 1.0);
+  CHECK(a.size == 0u);
+}
+
+// ---------------------------------------------------------------------------
+// Element access
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - operator[] provides mutable element access", "[Math][Array]") {
+  using namespace blast;
+  Array a(3u);
+  a[0] = 42.0;
+  CHECK(a[0] == 42.0);
+}
+
+TEST_CASE("Array - operator[] provides const element access", "[Math][Array]") {
+  using namespace blast;
+  const Array a{1.0, 2.0, 3.0};
+  CHECK(a[0] == 1.0);
+  CHECK(a[2] == 3.0);
+}
+
+TEST_CASE("Array - back() returns and allows modifying the last element", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  CHECK(a.back() == 3.0);
+  a.back() = 99.0;
+  CHECK(a[2] == 99.0);
+}
+
+TEST_CASE("Array - back() const returns the last element", "[Math][Array]") {
+  using namespace blast;
+  const Array a{1.0, 2.0, 5.0};
+  CHECK(a.back() == 5.0);
+}
+
+// ---------------------------------------------------------------------------
+// Size and state
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - is_empty() reflects whether the array has elements", "[Math][Array]") {
+  using namespace blast;
+  CHECK(Array().is_empty() == true);
+  CHECK(Array(3u).is_empty() == false);
+}
+
+// ---------------------------------------------------------------------------
+// Equality
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - operator== returns true when arrays are within epsilon", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array b{1.0 + 1e-6, 2.0 + 1e-6, 3.0 + 1e-6};
+  CHECK(a == b);
+}
+
+TEST_CASE("Array - operator== returns false when any element difference exceeds epsilon", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array b{1.0 + 2e-5, 2.0, 3.0};
+  CHECK_FALSE(a == b);
+}
+
+// ---------------------------------------------------------------------------
+// Arithmetic
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - unary minus negates all elements", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, -2.0, 3.0};
+  Array result = -a;
+  Array expected{-1.0, 2.0, -3.0};
+  CHECK(result == expected);
+}
+
+TEST_CASE("Array - operator+ adds element-wise", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array b{4.0, 5.0, 6.0};
+  Array expected{5.0, 7.0, 9.0};
+  CHECK(a + b == expected);
+}
+
+TEST_CASE("Array - operator- subtracts element-wise", "[Math][Array]") {
+  using namespace blast;
+  Array a{4.0, 5.0, 6.0};
+  Array b{1.0, 2.0, 3.0};
+  Array expected{3.0, 3.0, 3.0};
+  CHECK(a - b == expected);
+}
+
+TEST_CASE("Array - right scalar operator* scales all elements", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array expected{2.0, 4.0, 6.0};
+  CHECK(a * 2.0 == expected);
+}
+
+TEST_CASE("Array - left scalar operator* scales all elements", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array expected{2.0, 4.0, 6.0};
+  CHECK(2.0 * a == expected);
+}
+
+TEST_CASE("Array - operator/ divides all elements by scalar", "[Math][Array]") {
+  using namespace blast;
+  Array a{2.0, 4.0, 6.0};
+  Array expected{1.0, 2.0, 3.0};
+  CHECK(a / 2.0 == expected);
+}
+
+TEST_CASE("Array - operator*= scales in place", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  a *= 3.0;
+  Array expected{3.0, 6.0, 9.0};
+  CHECK(a == expected);
+}
+
+TEST_CASE("Array - operator/= divides in place", "[Math][Array]") {
+  using namespace blast;
+  Array a{4.0, 6.0, 8.0};
+  a /= 2.0;
+  Array expected{2.0, 3.0, 4.0};
+  CHECK(a == expected);
+}
+
+// ---------------------------------------------------------------------------
+// Dot product
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - dot() computes correct inner product", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  Array b{4.0, 5.0, 6.0};
+  CHECK(std::abs(dot(a, b) - 32.0) < 1e-9);
+}
+
+TEST_CASE("Array - dot(a, a) equals norm_sqr(a)", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 4.0};
+  CHECK(std::abs(dot(a, a) - norm_sqr(a)) < 1e-9);
+  CHECK(std::abs(dot(a, a) - 25.0) < 1e-9);
+}
+
+TEST_CASE("Array - dot() of orthogonal arrays is zero", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 0.0, 0.0};
+  Array b{0.0, 1.0, 0.0};
+  CHECK(std::abs(dot(a, b)) < 1e-9);
+}
+
+// ---------------------------------------------------------------------------
+// Norms
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - norm() computes the Euclidean norm", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 4.0};
+  CHECK(std::abs(norm(a) - 5.0) < 1e-9);
+}
+
+TEST_CASE("Array - norm_sqr() computes the squared Euclidean norm", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 4.0};
+  CHECK(std::abs(norm_sqr(a) - 25.0) < 1e-9);
+}
+
+TEST_CASE("Array - norm_1() computes the L1 norm", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, -2.0, 3.0};
+  CHECK(std::abs(norm_1(a) - 6.0) < 1e-9);
+}
+
+TEST_CASE("Array - norm_inf() computes the L-infinity norm", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, -5.0, 3.0};
+  CHECK(std::abs(norm_inf(a) - 5.0) < 1e-9);
+}
+
+// ---------------------------------------------------------------------------
+// Stats
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - sum() computes the sum of all elements", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  CHECK(std::abs(sum(a) - 6.0) < 1e-9);
+}
+
+TEST_CASE("Array - mean() computes the arithmetic mean", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  CHECK(std::abs(mean(a) - 2.0) < 1e-9);
+}
+
+TEST_CASE("Array - min() returns the smallest element", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 1.0, 2.0};
+  CHECK(std::abs(min(a) - 1.0) < 1e-9);
+}
+
+TEST_CASE("Array - max() returns the largest element", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 1.0, 2.0};
+  CHECK(std::abs(max(a) - 3.0) < 1e-9);
+}
+
+TEST_CASE("Array - argmin() returns the index of the smallest element", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 1.0, 2.0};
+  CHECK(argmin(a) == 1u);
+}
+
+TEST_CASE("Array - argmax() returns the index of the largest element", "[Math][Array]") {
+  using namespace blast;
+  Array a{3.0, 1.0, 2.0};
+  CHECK(argmax(a) == 0u);
+}
+
+// ---------------------------------------------------------------------------
+// Element-wise operations
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - abs() computes absolute value of each element", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, -2.0, 3.0};
+  Array expected{1.0, 2.0, 3.0};
+  CHECK(abs(a) == expected);
+}
+
+TEST_CASE("Array - abs_inplace() modifies array in place", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, -2.0, -3.0};
+  abs_inplace(a);
+  Array expected{1.0, 2.0, 3.0};
+  CHECK(a == expected);
+}
+
+TEST_CASE("Array - sqr() squares each element", "[Math][Array]") {
+  using namespace blast;
+  Array a{2.0, 3.0};
+  Array expected{4.0, 9.0};
+  CHECK(sqr(a) == expected);
+}
+
+TEST_CASE("Array - sqr_inplace() squares each element in place", "[Math][Array]") {
+  using namespace blast;
+  Array a{2.0, 3.0, 4.0};
+  sqr_inplace(a);
+  Array expected{4.0, 9.0, 16.0};
+  CHECK(a == expected);
+}
+
+TEST_CASE("Array - clamp() clamps elements to [min, max]", "[Math][Array]") {
+  using namespace blast;
+  Array a{-1.0, 0.5, 2.0};
+  Array result = clamp(a, 0.0, 1.0);
+  Array expected{0.0, 0.5, 1.0};
+  CHECK(result == expected);
+}
+
+// ---------------------------------------------------------------------------
+// Alias
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - pointer constructor creates alias without ownership", "[Math][Array]") {
+  using namespace blast;
+  real  buf[3] = {1.0, 2.0, 3.0};
+  Array a(buf, 3u);
+  CHECK(a.is_alias == true);
+  CHECK(a.size == 3u);
+  CHECK(a[0] == 1.0);
+}
+
+TEST_CASE("Array - writing through alias modifies the original buffer", "[Math][Array]") {
+  using namespace blast;
+  real  buf[3] = {1.0, 2.0, 3.0};
+  Array a(buf, 3u);
+  a[0] = 99.0;
+  CHECK(buf[0] == 99.0);
+}
+
+// ---------------------------------------------------------------------------
+// Resize
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - resize() increases size and preserves existing elements", "[Math][Array]") {
+  using namespace blast;
+  Array a(3u, 1.0);
+  a.resize(5u);
+  CHECK(a.size == 5u);
+  CHECK(a[0] == 1.0);
+  CHECK(a[2] == 1.0);
+  CHECK(a[3] == 0.0);
+  CHECK(a[4] == 0.0);
+}
+
+// ---------------------------------------------------------------------------
+// Utility
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Array - zero() sets all elements to 0.0", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0, 3.0};
+  zero(a);
+  CHECK(is_small(a));
+}
+
+TEST_CASE("Array - zero() returns a reference to the same object", "[Math][Array]") {
+  using namespace blast;
+  Array  a{1.0, 2.0, 3.0};
+  Array& ref = zero(a);
+  CHECK(&ref == &a);
+}
+
+TEST_CASE("Array - constant() sets all elements to the given value", "[Math][Array]") {
+  using namespace blast;
+  Array a(3u);
+  constant(a, 4.0);
+  for (u32 i = 0; i < a.size; i++)
+    CHECK(a[i] == 4.0);
+}
+
+TEST_CASE("Array - constant() returns a reference to the same object", "[Math][Array]") {
+  using namespace blast;
+  Array  a(3u);
+  Array& ref = constant(a, 2.0);
+  CHECK(&ref == &a);
+}
+
+TEST_CASE("Array - is_small() returns true when all elements are within epsilon", "[Math][Array]") {
+  using namespace blast;
+  Array a{1e-6, 1e-7, 1e-8};
+  CHECK(is_small(a));
+}
+
+TEST_CASE("Array - is_small() returns false when any element exceeds epsilon", "[Math][Array]") {
+  using namespace blast;
+  CHECK_FALSE(is_small(Array{2e-5, 0.0, 0.0}));
+  CHECK_FALSE(is_small(Array{0.0, 2e-5, 0.0}));
+}
+
+TEST_CASE("Array - is_close() returns true when arrays are within epsilon", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0};
+  Array b{1.0 + 1e-6, 2.0 + 1e-6};
+  CHECK(is_close(a, b));
+}
+
+TEST_CASE("Array - is_close() returns false when any element difference exceeds epsilon", "[Math][Array]") {
+  using namespace blast;
+  Array a{1.0, 2.0};
+  Array b{1.0 + 2e-5, 2.0};
+  CHECK_FALSE(is_close(a, b));
+}
diff --git a/tests/math/test_mat3.cpp b/tests/math/test_mat3.cpp
new file mode 100644
index 0000000..1f55794
--- /dev/null
+++ b/tests/math/test_mat3.cpp
@@ -0,0 +1,270 @@
+#define CATCH_CONFIG_MAIN
+
+#include <blast>
+#include "catch2/catch.hpp"
+
+// ---------------------------------------------------------------------------
+// Construction
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - default constructor zeroes all elements", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m;
+  for (int i = 0; i < 9; i++)
+    CHECK(m[i] == 0.0);
+}
+
+TEST_CASE("Mat3 - 9-element constructor stores values in column-major order", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  // Column 0
+  CHECK(m(0, 0) == 1.0);
+  CHECK(m(1, 0) == 2.0);
+  CHECK(m(2, 0) == 3.0);
+  // Column 1
+  CHECK(m(0, 1) == 4.0);
+  CHECK(m(1, 1) == 5.0);
+  CHECK(m(2, 1) == 6.0);
+  // Column 2
+  CHECK(m(0, 2) == 7.0);
+  CHECK(m(1, 2) == 8.0);
+  CHECK(m(2, 2) == 9.0);
+}
+
+TEST_CASE("Mat3 - copy constructor produces an independent copy", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 copy(m);
+  CHECK(is_close(copy, m));
+  copy[0] = 99.0;
+  CHECK(m[0] == 1.0);
+}
+
+// ---------------------------------------------------------------------------
+// Element access
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - operator()(row, col) provides mutable access", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m;
+  m(1, 2) = 42.0;
+  CHECK(m(1, 2) == 42.0);
+}
+
+TEST_CASE("Mat3 - operator()(row, col) provides const access", "[Math][Mat3]") {
+  using namespace blast;
+  const Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  CHECK(m(0, 0) == 1.0);
+  CHECK(m(2, 2) == 9.0);
+}
+
+TEST_CASE("Mat3 - operator[] provides flat index access matching constructor order", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  for (int i = 0; i < 9; i++)
+    CHECK(m[i] == static_cast<real>(i + 1));
+}
+
+// ---------------------------------------------------------------------------
+// Equality
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - is_close() returns true when all elements are within epsilon", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 a(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 b(1 + 1e-6, 2 + 1e-6, 3 + 1e-6, 4 + 1e-6, 5 + 1e-6, 6 + 1e-6, 7 + 1e-6, 8 + 1e-6, 9 + 1e-6);
+  CHECK(is_close(a, b));
+}
+
+TEST_CASE("Mat3 - is_close() returns false when any element difference exceeds epsilon", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 a(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 b(1 + 2e-5, 2, 3, 4, 5, 6, 7, 8, 9);
+  CHECK_FALSE(is_close(a, b));
+}
+
+// ---------------------------------------------------------------------------
+// Arithmetic
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - operator+ adds element-wise", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 a(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 b(9, 8, 7, 6, 5, 4, 3, 2, 1);
+  Mat3 expected(10, 10, 10, 10, 10, 10, 10, 10, 10);
+  CHECK(is_close(a + b, expected));
+}
+
+TEST_CASE("Mat3 - operator- subtracts element-wise", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 a(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 b(9, 8, 7, 6, 5, 4, 3, 2, 1);
+  Mat3 expected(-8, -6, -4, -2, 0, 2, 4, 6, 8);
+  CHECK(is_close(a - b, expected));
+}
+
+TEST_CASE("Mat3 - operator+= adds element-wise in place", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 n(1, 1, 1, 1, 1, 1, 1, 1, 1);
+  m += n;
+  Mat3 expected(2, 3, 4, 5, 6, 7, 8, 9, 10);
+  CHECK(is_close(m, expected));
+}
+
+TEST_CASE("Mat3 - left scalar operator* scales all elements", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  real c = 2.0;
+  Mat3 expected(2, 4, 6, 8, 10, 12, 14, 16, 18);
+  CHECK(is_close(c * m, expected));
+}
+
+// ---------------------------------------------------------------------------
+// Mat3 x Mat3 multiplication
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - operator* produces correct matrix product", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 a(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 b(9, 8, 7, 6, 5, 4, 3, 2, 1);
+  // Computed by hand: data layout is column-major
+  Mat3 expected(90, 114, 138, 54, 69, 84, 18, 24, 30);
+  CHECK(is_close(a * b, expected));
+}
+
+TEST_CASE("Mat3 - multiplying by identity leaves matrix unchanged", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(2, 3, 1, 5, 4, 6, 8, 7, 9);
+  CHECK(is_close(m * eye(), m));
+}
+
+TEST_CASE("Mat3 - operator*= with identity leaves matrix unchanged", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 orig(m);
+  m *= eye();
+  CHECK(is_close(m, orig));
+}
+
+// ---------------------------------------------------------------------------
+// Mat3 x Vec3 multiplication
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - operator*(Mat3, Vec3) produces correct result", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Vec3 v(1, 2, 3);
+  Vec3 result = m * v;
+  CHECK(result == Vec3(30, 36, 42));
+}
+
+// ---------------------------------------------------------------------------
+// Transpose
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - transpose() swaps rows and columns", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 t = transpose(m);
+  for (int r = 0; r < 3; r++)
+    for (int c = 0; c < 3; c++)
+      CHECK(t(r, c) == m(c, r));
+}
+
+TEST_CASE("Mat3 - transpose_inplace() produces the same result as transpose()", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3 expected = transpose(m);
+  transpose_inplace(m);
+  CHECK(is_close(m, expected));
+}
+
+TEST_CASE("Mat3 - double transpose returns the original matrix", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  CHECK(is_close(transpose(transpose(m)), m));
+}
+
+// ---------------------------------------------------------------------------
+// Identity
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - eye() returns the 3x3 identity matrix", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 I = eye();
+  CHECK(I(0, 0) == 1.0);
+  CHECK(I(1, 1) == 1.0);
+  CHECK(I(2, 2) == 1.0);
+  CHECK(I(0, 1) == 0.0);
+  CHECK(I(0, 2) == 0.0);
+  CHECK(I(1, 0) == 0.0);
+  CHECK(I(1, 2) == 0.0);
+  CHECK(I(2, 0) == 0.0);
+  CHECK(I(2, 1) == 0.0);
+}
+
+// ---------------------------------------------------------------------------
+// Column access
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - col_copy() returns the correct column as a Vec3", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  CHECK(m.col_copy(0) == Vec3(1, 2, 3));
+  CHECK(m.col_copy(1) == Vec3(4, 5, 6));
+  CHECK(m.col_copy(2) == Vec3(7, 8, 9));
+}
+
+TEST_CASE("Mat3 - col() alias allows element write-through to matrix data", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  m.col(0)[1] = 99.0;
+  CHECK(m(1, 0) == 99.0);
+}
+
+// ---------------------------------------------------------------------------
+// Utility
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Mat3 - zero() sets all elements to 0.0", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  zero(m);
+  CHECK(is_small(m));
+}
+
+TEST_CASE("Mat3 - zero() returns a reference to the same object", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3  m(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  Mat3& ref = zero(m);
+  CHECK(&ref == &m);
+}
+
+TEST_CASE("Mat3 - constant() sets all elements to the given value", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m;
+  constant(m, 5.0);
+  for (int i = 0; i < 9; i++)
+    CHECK(m[i] == 5.0);
+}
+
+TEST_CASE("Mat3 - constant() returns a reference to the same object", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3  m;
+  Mat3& ref = constant(m, 3.0);
+  CHECK(&ref == &m);
+}
+
+TEST_CASE("Mat3 - is_small() returns true when all elements are within epsilon", "[Math][Mat3]") {
+  using namespace blast;
+  Mat3 m(1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6);
+  CHECK(is_small(m));
+}
+
+TEST_CASE("Mat3 - is_small() returns false when any element exceeds epsilon", "[Math][Mat3]") {
+  using namespace blast;
+  CHECK_FALSE(is_small(Mat3(2e-5, 0, 0, 0, 0, 0, 0, 0, 0)));
+  CHECK_FALSE(is_small(Mat3(0, 0, 0, 0, 2e-5, 0, 0, 0, 0)));
+  CHECK_FALSE(is_small(Mat3(0, 0, 0, 0, 0, 0, 0, 0, 2e-5)));
+}
diff --git a/tests/math/test_matrix.cpp b/tests/math/test_matrix.cpp
new file mode 100644
index 0000000..a67e6b7
--- /dev/null
+++ b/tests/math/test_matrix.cpp
@@ -0,0 +1,433 @@
+#define CATCH_CONFIG_MAIN
+
+#include <blast>
+#include "catch2/catch.hpp"
+
+// ---------------------------------------------------------------------------
+// Construction
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - default constructor produces empty matrix", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m;
+  CHECK(m.rows == 0u);
+  CHECK(m.cols == 0u);
+  CHECK(m.size == 0u);
+  CHECK(m.data == nullptr);
+  CHECK(m.is_alias == false);
+}
+
+TEST_CASE("Matrix - size constructor allocates and zeroes all elements", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(3u, 2u);
+  CHECK(m.rows == 3u);
+  CHECK(m.cols == 2u);
+  CHECK(m.size == 6u);
+  for (u32 r = 0; r < m.rows; r++)
+    for (u32 c = 0; c < m.cols; c++)
+      CHECK(m(r, c) == 0.0);
+}
+
+TEST_CASE("Matrix - size+value constructor fills all elements", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 3u, 5.0);
+  CHECK(m.rows == 2u);
+  CHECK(m.cols == 3u);
+  for (u32 r = 0; r < m.rows; r++)
+    for (u32 c = 0; c < m.cols; c++)
+      CHECK(m(r, c) == 5.0);
+}
+
+TEST_CASE("Matrix - copy constructor produces a deep independent copy", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u, 1.0);
+  Matrix copy(m);
+  CHECK(copy.rows == 2u);
+  CHECK(copy.cols == 2u);
+  CHECK(copy.is_alias == false);
+  CHECK(copy(0, 0) == 1.0);
+  copy(0, 0) = 99.0;
+  CHECK(m(0, 0) == 1.0);
+}
+
+TEST_CASE("Matrix - move constructor transfers ownership", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 3.0);
+  Matrix b(std::move(a));
+  CHECK(b.rows == 2u);
+  CHECK(b.cols == 2u);
+  CHECK(b(0, 0) == 3.0);
+  CHECK(a.rows == 0u);
+  CHECK(a.cols == 0u);
+}
+
+// ---------------------------------------------------------------------------
+// Element access
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - operator()(row, col) provides mutable access", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(3u, 3u);
+  m(1, 2) = 42.0;
+  CHECK(m(1, 2) == 42.0);
+}
+
+TEST_CASE("Matrix - operator()(row, col) provides const access", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix        m(2u, 2u, 7.0);
+  const Matrix& cm = m;
+  CHECK(cm(0, 0) == 7.0);
+  CHECK(cm(1, 1) == 7.0);
+}
+
+TEST_CASE("Matrix - address() returns pointer to the correct element", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u);
+  m(1, 0) = 5.0;
+  CHECK(m.address(1, 0) == &m(1, 0));
+  CHECK(*m.address(1, 0) == 5.0);
+}
+
+TEST_CASE("Matrix - uses column-major storage layout", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u);
+  m(0, 0) = 1.0;
+  m(1, 0) = 2.0;
+  m(0, 1) = 3.0;
+  m(1, 1) = 4.0;
+  // Column-major: data[row + rows*col]
+  CHECK(m.data[0] == 1.0); // m(0,0)
+  CHECK(m.data[1] == 2.0); // m(1,0)
+  CHECK(m.data[2] == 3.0); // m(0,1)
+  CHECK(m.data[3] == 4.0); // m(1,1)
+}
+
+// ---------------------------------------------------------------------------
+// Dimensions
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - rows, cols, size fields reflect matrix dimensions", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(3u, 4u);
+  CHECK(m.rows == 3u);
+  CHECK(m.cols == 4u);
+  CHECK(m.size == 12u);
+}
+
+// ---------------------------------------------------------------------------
+// Equality
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - operator== returns true when all elements are within epsilon", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 1.0);
+  Matrix b(2u, 2u, 1.0 + 1e-6);
+  CHECK(a == b);
+}
+
+TEST_CASE("Matrix - operator== returns false when any element difference exceeds epsilon", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 1.0);
+  Matrix b(2u, 2u, 1.0 + 2e-5);
+  CHECK_FALSE(a == b);
+}
+
+// ---------------------------------------------------------------------------
+// Arithmetic
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - unary minus negates all elements", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u, 3.0);
+  Matrix neg = -m;
+  CHECK(neg(0, 0) == -3.0);
+  CHECK(neg(1, 1) == -3.0);
+  CHECK(m(0, 0) == 3.0);
+}
+
+TEST_CASE("Matrix - operator+ adds element-wise", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 1.0);
+  Matrix b(2u, 2u, 2.0);
+  Matrix result = a + b;
+  CHECK(result(0, 0) == 3.0);
+  CHECK(result(1, 1) == 3.0);
+}
+
+TEST_CASE("Matrix - operator- subtracts element-wise", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 5.0);
+  Matrix b(2u, 2u, 2.0);
+  Matrix result = a - b;
+  CHECK(result(0, 0) == 3.0);
+  CHECK(result(1, 1) == 3.0);
+}
+
+TEST_CASE("Matrix - left scalar operator* scales all elements", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u, 3.0);
+  real   c      = 2.0;
+  Matrix result = c * m;
+  CHECK(result(0, 0) == 6.0);
+  CHECK(result(1, 1) == 6.0);
+}
+
+TEST_CASE("Matrix - operator/ divides all elements by scalar", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u, 6.0);
+  real   c      = 2.0;
+  Matrix result = m / c;
+  CHECK(result(0, 0) == 3.0);
+  CHECK(result(1, 1) == 3.0);
+}
+
+TEST_CASE("Matrix - operator*(Matrix, Matrix) produces correct product", "[Math][Matrix]") {
+  using namespace blast;
+  // A is 2x3, B is 3x2, C = A*B is 2x2
+  Matrix A(2u, 3u);
+  A(0, 0) = 1.0;
+  A(0, 1) = 2.0;
+  A(0, 2) = 3.0;
+  A(1, 0) = 4.0;
+  A(1, 1) = 5.0;
+  A(1, 2) = 6.0;
+
+  Matrix B(3u, 2u);
+  B(0, 0) = 7.0;
+  B(0, 1) = 8.0;
+  B(1, 0) = 9.0;
+  B(1, 1) = 10.0;
+  B(2, 0) = 11.0;
+  B(2, 1) = 12.0;
+
+  Matrix C = A * B;
+  CHECK(C.rows == 2u);
+  CHECK(C.cols == 2u);
+  CHECK(std::abs(C(0, 0) - 58.0) < 1e-9);
+  CHECK(std::abs(C(0, 1) - 64.0) < 1e-9);
+  CHECK(std::abs(C(1, 0) - 139.0) < 1e-9);
+  CHECK(std::abs(C(1, 1) - 154.0) < 1e-9);
+}
+
+TEST_CASE("Matrix - operator*(Matrix, Array) produces correct product", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u);
+  m(0, 0) = 1.0;
+  m(0, 1) = 2.0;
+  m(1, 0) = 3.0;
+  m(1, 1) = 4.0;
+
+  Array v{1.0, 1.0};
+  Array result = m * v;
+  CHECK(result.size == 2u);
+  CHECK(std::abs(result[0] - 3.0) < 1e-9);
+  CHECK(std::abs(result[1] - 7.0) < 1e-9);
+}
+
+// ---------------------------------------------------------------------------
+// Transpose
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - transpose() swaps dimensions and elements", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix A(2u, 3u);
+  A(0, 0) = 1.0;
+  A(0, 1) = 2.0;
+  A(0, 2) = 3.0;
+  A(1, 0) = 4.0;
+  A(1, 1) = 5.0;
+  A(1, 2) = 6.0;
+
+  Matrix T = transpose(A);
+  CHECK(T.rows == 3u);
+  CHECK(T.cols == 2u);
+  for (u32 r = 0; r < A.rows; r++)
+    for (u32 c = 0; c < A.cols; c++)
+      CHECK(std::abs(T(c, r) - A(r, c)) < 1e-9);
+}
+
+// ---------------------------------------------------------------------------
+// Identity
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - eye(n) returns the n×n identity matrix", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix I = eye(3);
+  CHECK(I.rows == 3u);
+  CHECK(I.cols == 3u);
+  for (u32 r = 0; r < 3u; r++)
+    for (u32 c = 0; c < 3u; c++)
+      CHECK(I(r, c) == (r == c ? 1.0 : 0.0));
+}
+
+TEST_CASE("Matrix - eye(n) * eye(n) equals eye(n)", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix I = eye(3);
+  CHECK(I * I == I);
+}
+
+TEST_CASE("Matrix - multiplying by identity leaves matrix unchanged", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u);
+  m(0, 0) = 1.0;
+  m(0, 1) = 2.0;
+  m(1, 0) = 3.0;
+  m(1, 1) = 4.0;
+  CHECK(m * eye(2) == m);
+}
+
+// ---------------------------------------------------------------------------
+// Column access
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - col() reads the correct column elements", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(3u, 2u);
+  m(0, 0) = 1.0;
+  m(1, 0) = 2.0;
+  m(2, 0) = 3.0;
+  m(0, 1) = 4.0;
+  m(1, 1) = 5.0;
+  m(2, 1) = 6.0;
+
+  Array col0 = m.col(0);
+  CHECK(col0.is_alias == true);
+  CHECK(col0.size == 3u);
+  CHECK(col0[0] == 1.0);
+  CHECK(col0[1] == 2.0);
+  CHECK(col0[2] == 3.0);
+}
+
+TEST_CASE("Matrix - col() element write-through modifies the matrix", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(3u, 2u);
+  m.col(0)[1] = 77.0;
+  CHECK(m(1, 0) == 77.0);
+}
+
+// ---------------------------------------------------------------------------
+// Utility
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - zero() sets all elements to 0.0", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u, 5.0);
+  zero(m);
+  CHECK(is_small(m));
+}
+
+TEST_CASE("Matrix - zero() returns a reference to the same object", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix  m(2u, 2u, 5.0);
+  Matrix& ref = zero(m);
+  CHECK(&ref == &m);
+}
+
+TEST_CASE("Matrix - constant() sets all elements to the given value", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 3u);
+  constant(m, 7.0);
+  for (u32 r = 0; r < m.rows; r++)
+    for (u32 c = 0; c < m.cols; c++)
+      CHECK(m(r, c) == 7.0);
+}
+
+TEST_CASE("Matrix - constant() returns a reference to the same object", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix  m(2u, 2u);
+  Matrix& ref = constant(m, 1.0);
+  CHECK(&ref == &m);
+}
+
+TEST_CASE("Matrix - is_small() returns true when all elements are within epsilon", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u, 1e-6);
+  CHECK(is_small(m));
+}
+
+TEST_CASE("Matrix - is_small() returns false when any element exceeds epsilon", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u);
+  m(0, 1) = 2e-5;
+  CHECK_FALSE(is_small(m));
+}
+
+TEST_CASE("Matrix - is_close() returns true when matrices are within epsilon", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 1.0);
+  Matrix b(2u, 2u, 1.0 + 1e-6);
+  CHECK(is_close(a, b));
+}
+
+TEST_CASE("Matrix - is_close() returns false when any element difference exceeds epsilon", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix a(2u, 2u, 1.0);
+  Matrix b(2u, 2u, 1.0 + 2e-5);
+  CHECK_FALSE(is_close(a, b));
+}
+
+// ---------------------------------------------------------------------------
+// Determinant
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - determinant() of a 2x2 matrix is correct", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix m(2u, 2u);
+  m(0, 0) = 1.0;
+  m(0, 1) = 2.0;
+  m(1, 0) = 3.0;
+  m(1, 1) = 4.0;
+  // det = 1*4 - 2*3 = -2
+  CHECK(std::abs(determinant(m) - (-2.0)) < 1e-9);
+}
+
+TEST_CASE("Matrix - determinant() of a 3x3 matrix is correct", "[Math][Matrix]") {
+  using namespace blast;
+  // [[1,2,3],[0,4,5],[1,0,6]] -> det = 22
+  Matrix m(3u, 3u);
+  m(0, 0) = 1.0;
+  m(0, 1) = 2.0;
+  m(0, 2) = 3.0;
+  m(1, 0) = 0.0;
+  m(1, 1) = 4.0;
+  m(1, 2) = 5.0;
+  m(2, 0) = 1.0;
+  m(2, 1) = 0.0;
+  m(2, 2) = 6.0;
+  CHECK(std::abs(determinant(m) - 22.0) < 1e-9);
+}
+
+// ---------------------------------------------------------------------------
+// LU decomposition and solve
+// ---------------------------------------------------------------------------
+
+TEST_CASE("Matrix - solveLU() solves Ax=b via LU decomposition", "[Math][Matrix]") {
+  using namespace blast;
+  // A = [[2,1],[5,3]], b = {1,2}, solution x = {1,-1}
+  Matrix A(2u, 2u);
+  A(0, 0) = 2.0;
+  A(0, 1) = 1.0;
+  A(1, 0) = 5.0;
+  A(1, 1) = 3.0;
+
+  Array b{1.0, 2.0};
+  Array x = solveLU(LU_decomp(A), b);
+
+  CHECK(std::abs(x[0] - 1.0) < 1e-9);
+  CHECK(std::abs(x[1] - (-1.0)) < 1e-9);
+}
+
+TEST_CASE("Matrix - solveLU() solution satisfies Ax = b", "[Math][Matrix]") {
+  using namespace blast;
+  Matrix A(2u, 2u);
+  A(0, 0) = 2.0;
+  A(0, 1) = 1.0;
+  A(1, 0) = 5.0;
+  A(1, 1) = 3.0;
+
+  Array b{1.0, 2.0};
+  Array x  = solveLU(LU_decomp(A), b);
+  Array Ax = A * x;
+  Array expected{1.0, 2.0};
+  CHECK(Ax == expected);
+}
diff --git a/tests/math/test_misc.cpp b/tests/math/test_misc.cpp
new file mode 100644
index 0000000..b4415ec
--- /dev/null
+++ b/tests/math/test_misc.cpp
@@ -0,0 +1,144 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+
+#include <blast>
+#include "manipulator/UR5e.hpp"
+
+TEST_CASE("Conversions Q & rx, ry, rz", "[Math][Misc]") {
+  using namespace blast;
+  int                 n_tests = 1;
+  auto                manip   = make_UR5e();
+  ManipulatorTempData data;
+
+  for (u32 i = 0; i < n_tests; i++) {
+
+    Array joint_pos(6);
+
+    fill_random(joint_pos, PI);
+
+    forward_kinematics(manip, data, joint_pos);
+    auto rxyz_rot = rotation2rpy(data.rotations_mult[5]);
+    auto rot_rxyz = rpy2rotation(rxyz_rot);
+
+    CHECK(is_close(data.rotations_mult[5], rot_rxyz, 0.001));
+  }
+}
+
+TEST_CASE("sign() - positive, negative, zero", "[Math][Misc]") {
+  using namespace blast;
+  CHECK(sign(1.0) == 1.0);
+  CHECK(sign(-1.0) == -1.0);
+  CHECK(sign(0.0) == 0.0);
+  CHECK(sign(3.14) == 1.0);
+  CHECK(sign(-0.001) == -1.0);
+}
+
+TEST_CASE("sign_no_zero() - zero returns 1", "[Math][Misc]") {
+  using namespace blast;
+  CHECK(sign_no_zero(1.0) == 1.0);
+  CHECK(sign_no_zero(-1.0) == -1.0);
+  CHECK(sign_no_zero(0.0) == 1.0);
+}
+
+TEST_CASE("wrap2pi() - scalar wrapping", "[Math][Misc]") {
+  using namespace blast;
+  CHECK(std::abs(wrap2pi(1.0) - 1.0) < 1e-9);
+  CHECK(std::abs(wrap2pi(-1.0) - (-1.0)) < 1e-9);
+  CHECK(std::abs(wrap2pi(0.0) - 0.0) < 1e-9);
+  CHECK(std::abs(wrap2pi(4.0) - (4.0 - 2 * PI)) < 1e-9);
+  CHECK(std::abs(wrap2pi(-4.0) - (-4.0 + 2 * PI)) < 1e-9);
+}
+
+TEST_CASE("wrap2pi() - Array overload", "[Math][Misc]") {
+  using namespace blast;
+  Array a = {0.0, 4.0, -4.0};
+  auto  b = wrap2pi(a);
+  CHECK(std::abs(b[0] - 0.0) < 1e-9);
+  CHECK(std::abs(b[1] - (4.0 - 2 * PI)) < 1e-9);
+  CHECK(std::abs(b[2] - (-4.0 + 2 * PI)) < 1e-9);
+}
+
+TEST_CASE("wrap_to_180() - real and float overloads", "[Math][Misc]") {
+  using namespace blast;
+  CHECK(std::abs(wrap_to_180(90.0) - 90.0) < 1e-9);
+  CHECK(std::abs(wrap_to_180(270.0) - (-90.0)) < 1e-9);
+  CHECK(std::abs(wrap_to_180(-270.0) - 90.0) < 1e-9);
+  CHECK(std::abs(wrap_to_180(90.0f) - 90.0f) < 1e-4f);
+  CHECK(std::abs(wrap_to_180(270.0f) - (-90.0f)) < 1e-4f);
+}
+
+TEST_CASE("deg2rad() / rad2deg() - scalar", "[Math][Misc]") {
+  using namespace blast;
+  CHECK(std::abs(deg2rad(0.0) - 0.0) < 1e-12);
+  CHECK(std::abs(deg2rad(180.0) - PI) < 1e-9);
+  CHECK(std::abs(deg2rad(90.0) - PI / 2) < 1e-9);
+  CHECK(std::abs(rad2deg(0.0) - 0.0) < 1e-12);
+  CHECK(std::abs(rad2deg(PI) - 180.0) < 1e-9);
+  CHECK(std::abs(rad2deg(PI / 2) - 90.0) < 1e-9);
+  CHECK(std::abs(rad2deg(deg2rad(45.0)) - 45.0) < 1e-9);
+  CHECK(std::abs(deg2rad(rad2deg(1.0)) - 1.0) < 1e-9);
+}
+
+TEST_CASE("deg2rad() / rad2deg() - Array overloads", "[Math][Misc]") {
+  using namespace blast;
+  Array angles_deg = {0.0, 90.0, 180.0, 360.0};
+  Array angles_rad = deg2rad(angles_deg);
+  CHECK(std::abs(angles_rad[0] - 0.0) < 1e-9);
+  CHECK(std::abs(angles_rad[1] - PI / 2) < 1e-9);
+  CHECK(std::abs(angles_rad[2] - PI) < 1e-9);
+  CHECK(std::abs(angles_rad[3] - 2 * PI) < 1e-9);
+  Array back = rad2deg(angles_rad);
+  CHECK(std::abs(back[0] - 0.0) < 1e-9);
+  CHECK(std::abs(back[1] - 90.0) < 1e-9);
+  CHECK(std::abs(back[2] - 180.0) < 1e-9);
+  CHECK(std::abs(back[3] - 360.0) < 1e-9);
+}
+
+TEST_CASE("clamp() - below min, in range, above max", "[Math][Misc]") {
+  using namespace blast;
+  CHECK(clamp(5.0, 0.0, 10.0) == 5.0);
+  CHECK(clamp(-1.0, 0.0, 10.0) == 0.0);
+  CHECK(clamp(11.0, 0.0, 10.0) == 10.0);
+  CHECK(clamp(0.0, 0.0, 10.0) == 0.0);
+  CHECK(clamp(10.0, 0.0, 10.0) == 10.0);
+}
+
+TEST_CASE("clamp_inplace() - modifies value and returns same reference", "[Math][Misc]") {
+  using namespace blast;
+  real  v1 = 5.0;
+  real& r1 = clamp_inplace(v1, 0.0, 10.0);
+  CHECK(v1 == 5.0);
+  CHECK(&r1 == &v1);
+  real v2 = -1.0;
+  clamp_inplace(v2, 0.0, 10.0);
+  CHECK(v2 == 0.0);
+  real v3 = 11.0;
+  clamp_inplace(v3, 0.0, 10.0);
+  CHECK(v3 == 10.0);
+}
+
+TEST_CASE("rpy2rotation() - identity angles produce identity matrix", "[Math][Misc]") {
+  using namespace blast;
+  Mat3 R        = rpy2rotation({0.0, 0.0, 0.0});
+  Mat3 expected = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+  CHECK(is_close(R, expected, 1e-12));
+}
+
+TEST_CASE("rpy2rotation() / rotation2rpy() - explicit roundtrip", "[Math][Misc]") {
+  using namespace blast;
+  Vec3 rpy_in  = {0.1, 0.2, 0.3};
+  Mat3 R       = rpy2rotation(rpy_in);
+  Vec3 rpy_out = rotation2rpy(R);
+  CHECK(is_close(rpy_in, rpy_out, 1e-9));
+}
+
+TEST_CASE("rpy2rotation() / rotation2rpy() - gimbal lock preserves rotation matrix", "[Math][Misc]") {
+  using namespace blast;
+  // At pitch = pi/2 (gimbal lock), roll/yaw decomposition is degenerate.
+  // The round-trip rotation matrix should still be identical.
+  Vec3 rpy_in  = {0.0, PI / 2, 0.5};
+  Mat3 R       = rpy2rotation(rpy_in);
+  Vec3 rpy_out = rotation2rpy(R);
+  Mat3 R2      = rpy2rotation(rpy_out);
+  CHECK(is_close(R, R2, 1e-9));
+}
diff --git a/tests/math/test_vec3.cpp b/tests/math/test_vec3.cpp
index d63c915..33943a2 100644
--- a/tests/math/test_vec3.cpp
+++ b/tests/math/test_vec3.cpp
@@ -1,6 +1,3 @@
-//
-// Created by nikos on 2025-10-15.
-//
 #define CATCH_CONFIG_MAIN
 
 #include <blast>
diff --git a/tests/optimization/test_constraints.cpp b/tests/optimization/test_constraints.cpp
new file mode 100644
index 0000000..9112b76
--- /dev/null
+++ b/tests/optimization/test_constraints.cpp
@@ -0,0 +1,90 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+
+#include <blast>
+
+using namespace blast;
+
+TEST_CASE("bound_constraint - center is fully satisfied (-1)", "[Optimization][Constraints]") {
+  CHECK(bound_constraint(0.0, -1.0, 1.0) == Approx(-1.0));
+  CHECK(bound_constraint(5.0, 0.0, 10.0) == Approx(-1.0));
+}
+
+TEST_CASE("bound_constraint - at boundary is zero", "[Optimization][Constraints]") {
+  CHECK(bound_constraint(1.0, -1.0, 1.0) == Approx(0.0));
+  CHECK(bound_constraint(-1.0, -1.0, 1.0) == Approx(0.0));
+  CHECK(bound_constraint(10.0, 0.0, 10.0) == Approx(0.0));
+}
+
+TEST_CASE("bound_constraint - beyond boundary is positive (violated)", "[Optimization][Constraints]") {
+  CHECK(bound_constraint(2.0, -1.0, 1.0) > 0.0);
+  CHECK(bound_constraint(-2.0, -1.0, 1.0) > 0.0);
+}
+
+TEST_CASE("abs_constraint - zero is fully satisfied (-1)", "[Optimization][Constraints]") {
+  CHECK(abs_constraint(0.0, 1.0) == Approx(-1.0));
+  CHECK(abs_constraint(0.0, 5.0) == Approx(-1.0));
+}
+
+TEST_CASE("abs_constraint - at max is zero (at limit)", "[Optimization][Constraints]") {
+  CHECK(abs_constraint(1.0, 1.0) == Approx(0.0));
+  CHECK(abs_constraint(-1.0, 1.0) == Approx(0.0));
+}
+
+TEST_CASE("abs_constraint - beyond max is positive (violated)", "[Optimization][Constraints]") {
+  CHECK(abs_constraint(2.0, 1.0) > 0.0);
+  CHECK(abs_constraint(-2.0, 1.0) > 0.0);
+}
+
+TEST_CASE("abs_constraint_analytical<true> - value matches abs_constraint, gradient has correct sign", "[Optimization][Constraints]") {
+  {
+    auto [c, g] = abs_constraint_analytical<true>(2.0, 4.0);
+    CHECK(c == Approx(abs_constraint(2.0, 4.0)));
+    CHECK(g == Approx(1.0 / 4.0));
+  }
+  {
+    auto [c, g] = abs_constraint_analytical<true>(-2.0, 4.0);
+    CHECK(c == Approx(abs_constraint(-2.0, 4.0)));
+    CHECK(g == Approx(-1.0 / 4.0));
+  }
+}
+
+TEST_CASE("abs_constraint_analytical<false> - gradient is zero", "[Optimization][Constraints]") {
+  auto [c, g] = abs_constraint_analytical<false>(2.0, 4.0);
+  CHECK(c == Approx(abs_constraint(2.0, 4.0)));
+  CHECK(g == 0.0);
+}
+
+TEST_CASE("bound_constraint_analytical<true> - value matches bound_constraint, gradient correct sign", "[Optimization][Constraints]") {
+  {
+    // value > center: gradient should be +2/range
+    auto [c, g] = bound_constraint_analytical<true>(0.5, -1.0, 1.0);
+    CHECK(c == Approx(bound_constraint(0.5, -1.0, 1.0)));
+    CHECK(g == Approx(2.0 / 2.0));
+  }
+  {
+    // value < center: gradient should be -2/range
+    auto [c, g] = bound_constraint_analytical<true>(-0.5, -1.0, 1.0);
+    CHECK(c == Approx(bound_constraint(-0.5, -1.0, 1.0)));
+    CHECK(g == Approx(-2.0 / 2.0));
+  }
+}
+
+TEST_CASE("bound_constraint_analytical<false> - gradient is zero", "[Optimization][Constraints]") {
+  auto [c, g] = bound_constraint_analytical<false>(0.5, -1.0, 1.0);
+  CHECK(c == Approx(bound_constraint(0.5, -1.0, 1.0)));
+  CHECK(g == 0.0);
+}
+
+TEST_CASE("bound_constraint_analytical - INF limits return -1 (unconstrained)", "[Optimization][Constraints]") {
+  {
+    auto [c, g] = bound_constraint_analytical<true>(100.0, -INF_REAL, 1.0);
+    CHECK(c == Approx(-1.0));
+    CHECK(g == 0.0);
+  }
+  {
+    auto [c, g] = bound_constraint_analytical<true>(100.0, -1.0, INF_REAL);
+    CHECK(c == Approx(-1.0));
+    CHECK(g == 0.0);
+  }
+}
diff --git a/tests/optimization/test_optimization_basic.cpp b/tests/optimization/test_optimization_basic.cpp
new file mode 100644
index 0000000..4ea77bf
--- /dev/null
+++ b/tests/optimization/test_optimization_basic.cpp
@@ -0,0 +1,85 @@
+#define CATCH_CONFIG_MAIN
+#include <blast>
+#include <catch2/catch.hpp>
+
+using namespace blast;
+
+// Returns a representative stop-to-stop task for the UR5e.
+inline blast::Task make_UR5e_task() {
+  blast::Array start = {1.94822, 0.473555, -0.0255247, -0.448375, 0.370356, -3.12883};
+  blast::Array end   = {2.5825, 0.0700, -0.3892, 0.3196, 0.9927, -3.17328};
+  return blast::Task::stop_to_stop(start, end);
+}
+
+TEST_CASE("optimize stop-to-stop default pva + tool_speed constraints active", "[Optimization]") {
+  Manipulator robot = make_UR5e();
+  Task        task  = make_UR5e_task();
+
+  Optimization opt(robot, task); // default enables pva + tool_speed constraints
+  opt.success_tolerance = 0.01f;
+
+  Result result = optimize(&opt);
+
+  CHECK(result.success == true);
+  CHECK(result.compute_time > 0.0f);
+  CHECK(result.trajectory.t.size > 0);
+}
+
+TEST_CASE("optimize stop-to-stop pva + tool_speed + torque constraints active", "[Optimization]") {
+  Manipulator robot = make_UR5e();
+  Task        task  = make_UR5e_task();
+
+  Optimization opt(robot, task); // default enables pva + tool_speed constraints
+  opt.constraints.torque = true;
+  opt.success_tolerance  = 0.01f;
+
+  Result result = optimize(&opt);
+
+  CHECK(result.success == true);
+  CHECK(result.compute_time > 0.0f);
+  CHECK(result.trajectory.t.size > 0);
+}
+
+TEST_CASE("optimization stop-to-stop pva + tool_speed + torque + self_collisions active", "[Optimization") {
+  Manipulator robot = make_UR5e();
+  Task        task  = make_UR5e_task();
+
+  Optimization opt(robot, task); // default enables pva + tool_speed constraints
+  opt.constraints.torque          = true;
+  opt.constraints.self_collisions = true;
+  opt.success_tolerance           = 0.01f;
+
+  Result result = optimize(&opt);
+
+  CHECK(result.success == true);
+  CHECK(result.compute_time > 0.0f);
+  CHECK(result.trajectory.t.size > 0);
+}
+
+TEST_CASE("optimization stop-to-stop all constraints active", "[Optimization") {
+  Manipulator robot = make_UR5e();
+  Task        task  = make_UR5e_task();
+
+  World world;
+  world.add_box(
+          Vec3{0.4, 0.0, 0.6},            // centre: 40 cm in front, 60 cm high
+          Vec3{0.05, 0.3, 0.3},           // half-extents: thin vertical slab
+          Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1} // upright, axis-aligned
+  );
+
+  Optimization opt(robot, task); // default enables pva + tool_speed constraints
+  opt.world = world;
+
+  opt.constraints.torque              = true;
+  opt.constraints.self_collisions     = true; // avoid self-contact
+  opt.constraints.external_collisions = true; // avoid world obstacles
+
+  opt.success_tolerance = 0.01f;
+
+
+  Result result = optimize(&opt);
+
+  CHECK(result.success == true);
+  CHECK(result.compute_time > 0.0f);
+  CHECK(result.trajectory.t.size > 0);
+}
diff --git a/tests/optimization/test_optimization_task_violation.cpp b/tests/optimization/test_optimization_task_violation.cpp
new file mode 100644
index 0000000..c5f051d
--- /dev/null
+++ b/tests/optimization/test_optimization_task_violation.cpp
@@ -0,0 +1,41 @@
+#define CATCH_CONFIG_MAIN
+#define CATCH_CONFIG_ENABLE_BENCHMARKING
+#include <blast>
+#include <catch2/catch.hpp>
+
+using namespace blast;
+
+// Returns a representative stop-to-stop task for the UR5e.
+inline blast::Task make_UR5e_task() {
+  blast::Array start = {1.94822, 0.473555, -0.0255247, -0.448375, 0.370356, -3.12883};
+  blast::Array end   = {2.5825, 0.0700, -0.3892, 0.3196, 0.9927, -3.17328};
+  return blast::Task::stop_to_stop(start, end);
+}
+
+TEST_CASE("optimization - task not valid", "[Optimization") {
+  Manipulator robot = make_UR5e();
+  Task        task  = make_UR5e_task();
+
+  World world;
+  world.add_box(
+          Vec3{0.0, 0.0, 0.0},            // centre: 0,0,0
+          Vec3{0.05, 0.3, 0.3},           // half-extents: thin vertical slab
+          Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1} // upright, axis-aligned
+  );
+
+  Optimization opt(robot, task); // default enables pva + tool_speed constraints
+  opt.world = world;
+
+  opt.constraints.torque              = true;
+  opt.constraints.self_collisions     = true; // avoid self-contact
+  opt.constraints.external_collisions = true; // avoid world obstacles
+
+  opt.success_tolerance = 0.01f;
+
+
+  Result result = optimize(&opt);
+
+  CHECK(result.success == false);
+  CHECK(result.compute_time == 0.0f);
+  CHECK(result.trajectory.t.size == 0);
+}
diff --git a/tests/optimization/test_task.cpp b/tests/optimization/test_task.cpp
new file mode 100644
index 0000000..e7f875d
--- /dev/null
+++ b/tests/optimization/test_task.cpp
@@ -0,0 +1,46 @@
+#define CATCH_CONFIG_MAIN
+#define CATCH_CONFIG_ENABLE_BENCHMARKING
+#include <blast>
+#include <catch2/catch.hpp>
+
+using namespace blast;
+
+TEST_CASE("Task stop_to_stop sets start and goal positions") {
+  Array q_start = {0.0f, -PI / 2, 0.0f, -PI / 2, 0.0f, 0.0f};
+  Array q_goal  = {-1.54f, -1.83f, -2.28f, -0.59f, 1.60f, 0.023f};
+  Task  task    = Task::stop_to_stop(q_start, q_goal);
+
+  CHECK(is_close(task.start.position, q_start));
+  CHECK(is_close(task.goal.position, q_goal));
+  CHECK(is_small(task.start.velocity));
+  CHECK(is_small(task.goal.velocity));
+  CHECK(is_small(task.start.acceleration));
+  CHECK(is_small(task.goal.acceleration));
+}
+
+TEST_CASE("Task n_joints constructor initializes to zero") {
+  Task task(6);
+  for (u32 i = 0; i < 6; i++) {
+    CHECK(task.start.position[i] == 0.0f);
+    CHECK(task.start.velocity[i] == 0.0f);
+    CHECK(task.start.acceleration[i] == 0.0f);
+    CHECK(task.goal.position[i] == 0.0f);
+    CHECK(task.goal.velocity[i] == 0.0f);
+    CHECK(task.goal.acceleration[i] == 0.0f);
+  }
+}
+
+TEST_CASE("Task round-trips through to_matrix") {
+  Array  q_start = {0.0f, -PI / 2, 0.0f, -PI / 2, 0.0f, 0.0f};
+  Array  q_goal  = {-1.54f, -1.83f, -2.28f, -0.59f, 1.60f, 0.023f};
+  Task   task1   = Task::stop_to_stop(q_start, q_goal);
+  Matrix m       = task1.to_matrix();
+  Task   task2(m);
+
+  CHECK(is_close(task2.start.position, task1.start.position));
+  CHECK(is_close(task2.goal.position, task1.goal.position));
+  CHECK(is_close(task2.start.velocity, task1.start.velocity));
+  CHECK(is_close(task2.goal.velocity, task1.goal.velocity));
+  CHECK(is_close(task2.start.acceleration, task1.start.acceleration));
+  CHECK(is_close(task2.goal.acceleration, task1.goal.acceleration));
+}
diff --git a/tests/test_helper/test_helper.hpp b/tests/test_helper/test_helper.hpp
deleted file mode 100644
index 697f97a..0000000
--- a/tests/test_helper/test_helper.hpp
+++ /dev/null
@@ -1,1259 +0,0 @@
-#pragma once
-#include "blast"
-
-namespace blast {
-
-
-// todo: Add camera capsule
-inline Manipulator get_generic_Link6_fixed() {
-  // Manipulator
-  u32 joints = 6;
-  // limits
-  ManipulatorLimits limits;
-  limits.position_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad
-  limits.position_min = -limits.position_max;
-
-  limits.velocity_max = {0.9 * 3.4907f, 0.9 * 3.4907f, 0.9 * 3.4907f, 0.9 * 5.5851f, 0.9 * 5.5851f, 0.9 * 5.5851f};                                   // rad/s
-
-  limits.acceleration_max = {0.9 * deg2rad(600), 0.9 * deg2rad(600), 0.9 * deg2rad(600), 0.9 * deg2rad(600), 0.9 * deg2rad(600), 0.9 * deg2rad(600)}; // rad/s^2
-
-  limits.torque_max = {210, 210, 210, 100, 100, 100};                                                                                                 // Nm
-
-  limits.tool_speed_max = 2.0;
-
-  // kinematic properties
-  ManipulatorKinematics kinematics; // using default Q_base
-  kinematics.joint_offsets = {
-          Vec3{0.11024, -0.06926, -0.1375},
-          {0.0, 0.4850, 0.0},
-          {0.0, -0.15216, -0.0917},
-          {0.0, -0.06296, -0.22275},
-          {0.08703, 0.0860, -0.07692},
-          {0.0, 0.0, -0.0920}}; // vector to next joint
-  kinematics.joint_axes = {
-          Vec3{0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1}}; // direction vectors of joint
-  kinematics.first_joint_position = {0.0, 0.0, 0.0530f};
-  kinematics.base_position        = {0.0, 0.0, 0.0};
-  kinematics.base_rotation        = {1, 0, 0, 0, 1, 0, 0, 0, 1};
-  // kinematics.static_rotations.resize(7);
-  kinematics.static_rotations[0] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-  kinematics.static_rotations[1] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[2] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-  kinematics.static_rotations[3] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[4] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[5] = {0, 0, 1, 0, 1, 0, -1, 0, 0};
-  kinematics.static_rotations[6] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-
-  // dynamic properties
-  ManipulatorDynamics dynamics;
-  dynamics.link_masses = {
-          4.8257f,
-          5.9860f,
-          3.4159f,
-          2.0849f,
-          2.0076f,
-          1.5193f}; // link masses
-  dynamics.inertia_tensors = {
-          Mat3{0.0192746f, -0.00239802f, -0.00896331f, -0.00239802f, 0.03087806f, 0.0016298f, -0.00896331f, 0.0016298f, 0.02134949f},
-          {0.25899206f, -2.89E-05f, -1.23E-06f, -2.89E-05f, 0.01755445f, -0.02128064f, -1.23E-06f, -0.02128064f, 0.25291674f},
-          {0.01742043f, -3.55E-06f, 8.4E-07f, -3.55E-06f, 0.01119175f, 0.00518163f, 8.4E-07f, 0.00518163f, 0.01212876f},
-          {0.02454276f, 2.61E-06f, 1.799E-05f, 2.61E-06f, 0.02385702f, 0.00315758f, 1.799E-05f, 0.00315758f, 0.00294903f},
-          {0.00734684f, 0.00124927f, -0.00090156f, 0.00124927f, 0.00464684f, -0.00236128f, -0.00090156f, -0.00236128f, 0.00589508f},
-          {0.00390762f, -1.13E-06f, 1.16E-06f, -1.13E-06f, 0.00390722f, -2.21E-05f, 1.16E-06f, -2.21E-05f, 0.0013928f}}; // Inertial tensors
-  dynamics.cog_offsets = {
-          Vec3{0.03930119f, -0.00705889f, -0.08462154f},
-          {2.53E-06f, 0.18829586f, -0.03988382f},
-          {4.64E-06f, -0.02451414f, -0.02997969f},
-          {-0.00010793f, -0.01056422f, -0.08091102f},
-          {0.01243595f, 0.03284165f, -0.04091434f},
-          {0.0f, 0.00050624f, -0.00388589f}}; // centers of mass
-
-  // capsules & internal collision data
-  // Collision model
-  ManipulatorCapsules collisions;
-  Sphere              base;
-  base.center               = {0, 0, 0.0}; // because this is relative to p_base and p_base is {0, 0, 0.053}
-  base.radius               = 0.2375;
-  collisions.base_sphere    = base;
-  collisions.collision_base = {0, 0, 0, 1, 1, 1};
-  // collisions.collision_base = {0, 0, 0, 1, 1, 1, 1};  // includes camera capsule
-
-  collisions.collision_matrix.resize(6, 6);
-  // collisions.collision_matrix.resize(7, 7); // includes camera capsule
-  collisions.collision_matrix(5, 0) = 1;
-  // collisions.collision_matrix(6, 0) = 1; // includes camera capsule
-  collisions.collision_matrix(4, 1) = 1;
-  collisions.collision_matrix(5, 1) = 1;
-  // collisions.collision_matrix(6, 1) = 1; // includes camera capsule
-
-  // collisions.collision_matrix.resize(6, 6);
-  // collisions.collision_matrix(5, 0) = 1;
-  // collisions.collision_matrix(4, 1) = 1;
-  // collisions.collision_matrix(5, 1) = 1;
-
-  // Collision model
-  CollisionModelCapsule model_caps;
-
-  // Capsule 1
-  model_caps.joint_frame = 1;
-  model_caps.p1          = {0, 0, -0.065};
-  model_caps.p2          = {0, 0, 0.045};
-  model_caps.radius      = 0.065;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 2
-  model_caps.joint_frame = 1;
-  // model_caps.p1 = {0, 0, -0.065};
-  model_caps.p1 = {0, 0, -0.08};
-  // model_caps.p2 = {0, 0.485, -0.065};
-  model_caps.p2     = {0, 0.485, -0.08};
-  model_caps.radius = 0.065;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 3
-  model_caps.joint_frame = 2;
-  model_caps.p1          = {0, 0, -0.065};
-  model_caps.p2          = {0, 0, 0.085};
-  model_caps.radius      = 0.065;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 4
-  model_caps.joint_frame = 2;
-  model_caps.p1          = {0, 0.00695, -0.0917};
-  model_caps.p2          = {0, -0.36805, -0.0917};
-  model_caps.radius      = 0.061;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 5
-  model_caps.joint_frame = 4;
-  model_caps.p1          = {0, 0, 0};
-  model_caps.p2          = {0, 0, -0.08};
-  model_caps.radius      = 0.060;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 6
-  model_caps.joint_frame = 5;
-  model_caps.p1          = {0, 0, 0.08583};
-  model_caps.p2          = {0, 0, -0.06417};
-  model_caps.radius      = 0.060;
-  collisions.capsule_list.push_back(model_caps);
-
-  // // Capsule 7
-  // model_caps.joint_frame = 5;
-  // model_caps.p1 = {0, 0.02125, -0.007};
-  // // model_caps.p2 = {0, 0.02125, -0.013};
-  // model_caps.p2 = {0, 0.02125, 0.143};
-  // model_caps.radius = 0.085;
-  // collisions.capsule_list.push_back(model_caps); // todo: change camera capsule
-
-  // create manipulator link6
-  Manipulator link6(joints, limits, kinematics, &dynamics, &collisions);
-
-  return link6;
-}
-
-// note: p_base and Q_base are set in respect to lab's layout (in Link6 frame)
-Manipulator get_generic_gen3_fixed() { // todo: fix capsules, not working
-  // Manipulator
-  u32 joints = 7;
-
-  // limits
-  ManipulatorLimits limits;
-  limits.position_max = {INF_REAL, 2.25, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad
-  limits.position_min = -limits.position_max;
-
-  // limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f}; // rad/s (used for demo 2)
-  limits.velocity_max = {1.39f, 1.39f, 1.39f, 1.39f, 1.22f, 1.22f, 1.22f}; // rad/s (updated for demo 3)
-
-  // limits.acceleration_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad/s^2
-  limits.acceleration_max = {5.2, 5.2, 5.2, 5.2, 10.0, 10.0, 10.0}; // rad/s^2
-
-  limits.torque_max = {52, 52, 52, 52, 17, 17, 17};                 // Nm
-
-  limits.tool_speed_max = 1.0;                                      // todo: validate with webapp (Bruno from Kinova said 1.0 could be adequate), webapp says 0.5
-  // limits.tool_speed_max = 0.5; // todo: validate with webapp (Bruno from Kinova said 1.0 could be adequate), webapp says 0.5
-
-  // kinematic properties
-  ManipulatorKinematics kinematics; // using default Q_base
-  kinematics.joint_offsets = {
-          Vec3{0.0, 0.0054, -0.1284},
-          {0.0, -0.2104, -0.0064},
-          {0.0, -0.0064, -0.2104},
-          {0.0, -0.2084, -0.0064},
-          {0.0, 0.0, -0.1059},
-          {0.0, -0.1059, 0.0},
-          {0.0, 0.0, -0.0615 /*- 0.164*/} // todo: why did we subtract this? Ans : We subtracted because we used the gripper
-  }; // vector to next joint
-  kinematics.joint_axes = {
-          Vec3{0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1}}; // direction vectors of joint
-  // kinematics.static_rotations.resize(7);
-  kinematics.static_rotations[0]  = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-  kinematics.static_rotations[1]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
-  kinematics.static_rotations[2]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[3]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
-  kinematics.static_rotations[4]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[5]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
-  kinematics.static_rotations[6]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.first_joint_position = {0, 0, 0.1564f};
-  kinematics.base_position        = {0, 0, 0};
-  kinematics.base_rotation        = {1, 0, 0, 0, 1, 0, 0, 0, 1};
-
-  // dynamic properties
-  ManipulatorDynamics dynamics;
-
-  dynamics.link_masses = {
-          1.377f,
-          1.1636f,
-          1.1636f,
-          0.93f,
-          0.678f,
-          0.678f,
-          0.364f}; // link masses
-  dynamics.inertia_tensors = {
-          Mat3{0.004570f, 0.000001f, 0.000002f, 0.000001f, 0.004831f, 0.000448f, 0.000002f, 0.000448f, 0.001409f},
-          {0.011088f, 0.000005f, 0.000000f, 0.000005f, 0.001072f, -0.000691f, 0.000000f, -0.000691f, 0.011255f},
-          {0.010932f, 0.000000f, -0.000007f, 0.000000f, 0.011127f, 0.000606f, -0.000007f, 0.000606f, 0.001043f},
-          {0.008147f, -0.000001f, 0.000000f, -0.000001f, 0.000631f, -0.000500f, 0.000000f, -0.000500f, 0.008316f},
-          {0.001596f, 0.000000f, 0.000000f, 0.000000f, 0.001607f, 0.000256f, 0.000000f, 0.000256f, 0.000399f},
-          {0.001641f, 0.000000f, 0.000000f, 0.000000f, 0.000410f, -0.000278f, 0.000000f, -0.000278f, 0.001641f},
-          {0.000214f, 0.000000f, 0.000001f, 0.000000f, 0.000223f, -0.000002f, 0.000001f, -0.000002f, 0.000240f}}; // Inertial tensors
-  dynamics.cog_offsets = {
-          Vec3{-0.000023f, -0.010364f, -0.073360f},
-          {-0.000044f, -0.099580f, -0.013278f},
-          {-0.000044f, -0.006641f, -0.117892f},
-          {-0.000018f, -0.075478f, -0.015006f},
-          {0.000001f, -0.009432f, -0.063883f},
-          {0.000001f, -0.045483f, -0.009650f},
-          {-0.000093f, 0.000132f, -0.022905f}}; // centers of mass
-
-  // create manipulator gen3
-  Manipulator generic_manip(joints, limits, kinematics, &dynamics);
-
-  // Collision model
-  ManipulatorCapsules collisions;
-  Sphere              sphere;
-  sphere.center             = {0, 0, 0.035};
-  sphere.radius             = 0.14;
-  collisions.base_sphere    = sphere;
-  collisions.collision_base = {0, 0, 1};
-
-  collisions.collision_matrix.resize(3, 3);
-  collisions.collision_matrix(2, 0) = 1;
-  collisions.collision_matrix(0, 2) = 1;
-
-  // Collision model
-  CollisionModelCapsule model_caps;
-
-  // Capsule 1
-  model_caps.joint_frame = 1;
-  model_caps.p1          = {0, 0.035, 0};
-  model_caps.p2          = {0, -0.425, 0};
-  model_caps.radius      = 0.06;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 2
-  model_caps.joint_frame = 3;
-  model_caps.p1          = {0, 0, -0.025};
-  model_caps.p2          = {0, -0.3, -0.01};
-  model_caps.radius      = 0.06;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 3
-  model_caps.joint_frame = 5;
-  model_caps.p1          = {0, 0, -0.015};
-  model_caps.p2          = {0.0, -0.15, -0.015};
-  model_caps.radius      = 0.055;
-  collisions.capsule_list.push_back(model_caps);
-
-  generic_manip.set_capsules(collisions);
-
-  return generic_manip;
-}
-
-// todo: Change camera capsule
-inline host_fn Manipulator get_generic_Link6() {
-  // Manipulator
-  constexpr u32 joints = 6;
-  // limits
-  ManipulatorLimits limits;
-  limits.position_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad
-  limits.position_min = -limits.position_max;
-
-  limits.velocity_max = {3.4907f, 3.4907f, 3.4907f, 5.5851f, 5.5851f, 5.5851f};                                   // rad/s
-                                                                                                                  //   limits.vmin = -limits.velocity_max;
-
-  limits.acceleration_max = {deg2rad(600), deg2rad(600), deg2rad(600), deg2rad(600), deg2rad(600), deg2rad(600)}; // rad/s^2
-                                                                                                                  //   limits.amin = -limits.acceleration_max;
-
-  limits.torque_max = {210, 210, 210, 100, 100, 100};                                                             // Nm
-                                                                                                                  //   limits.tau_min = -limits.torque_max;
-
-  limits.tool_speed_max = 2.0;
-
-  // kinematic properties
-  ManipulatorKinematics kinematics; // using default Q_base
-  kinematics.joint_offsets = {
-          Vec3{0.11024, -0.06926, -0.1375},
-          Vec3{0.0, 0.4850, 0.0},
-          Vec3{0.0, -0.15216, -0.0917},
-          Vec3{0.0, -0.06296, -0.22275},
-          Vec3{0.08703, 0.0860, -0.07692},
-          Vec3{0.0, 0.0, -0.0920}}; // vector to next joint
-  kinematics.joint_axes = {
-          Vec3{0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1}}; // direction vectors of joint
-  kinematics.first_joint_position = {0.0, 0.0, 0.0530f};
-  kinematics.base_position        = {0.0, 0.0, 0.0};
-  kinematics.base_rotation        = {1, 0, 0, 0, 1, 0, 0, 0, 1};
-  kinematics.static_rotations[0]  = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-  kinematics.static_rotations[1]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[2]  = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-  kinematics.static_rotations[3]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[4]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[5]  = {0, 0, 1, 0, 1, 0, -1, 0, 0};
-  kinematics.static_rotations[6]  = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-
-  // dynamic properties
-  ManipulatorDynamics dynamics;
-  dynamics.link_masses = {
-          4.8257f,
-          5.9860f,
-          3.4159f,
-          2.0849f,
-          2.0076f,
-          1.5193f}; // link masses
-  dynamics.inertia_tensors = {
-          Mat3{0.0192746f, -0.00239802f, -0.00896331f, -0.00239802f, 0.03087806f, 0.0016298f, -0.00896331f, 0.0016298f, 0.02134949f},
-          {0.25899206f, -2.89E-05f, -1.23E-06f, -2.89E-05f, 0.01755445f, -0.02128064f, -1.23E-06f, -0.02128064f, 0.25291674f},
-          {0.01742043f, -3.55E-06f, 8.4E-07f, -3.55E-06f, 0.01119175f, 0.00518163f, 8.4E-07f, 0.00518163f, 0.01212876f},
-          {0.02454276f, 2.61E-06f, 1.799E-05f, 2.61E-06f, 0.02385702f, 0.00315758f, 1.799E-05f, 0.00315758f, 0.00294903f},
-          {0.00734684f, 0.00124927f, -0.00090156f, 0.00124927f, 0.00464684f, -0.00236128f, -0.00090156f, -0.00236128f, 0.00589508f},
-          {0.00390762f, -1.13E-06f, 1.16E-06f, -1.13E-06f, 0.00390722f, -2.21E-05f, 1.16E-06f, -2.21E-05f, 0.0013928f}}; // Inertial tensors
-  dynamics.cog_offsets = {
-          Vec3{0.03930119f, -0.00705889f, -0.08462154f},
-          {2.53E-06f, 0.18829586f, -0.03988382f},
-          {4.64E-06f, -0.02451414f, -0.02997969f},
-          {-0.00010793f, -0.01056422f, -0.08091102f},
-          {0.01243595f, 0.03284165f, -0.04091434f},
-          {0.0f, 0.00050624f, -0.00388589f}}; // centers of mass
-
-  // capsules & internal collision data
-  // Collision model
-  ManipulatorCapsules collisions;
-  Sphere              base;
-  base.center               = {0, 0, 0.0}; // because this is relative to p_base and p_base is {0, 0, 0.053}
-  base.radius               = 0.2375;
-  collisions.base_sphere    = base;
-  collisions.collision_base = {0, 0, 0, 1, 1, 1, 1};
-
-  collisions.collision_matrix.resize(7, 7);
-  collisions.collision_matrix(5, 0) = 1;
-  collisions.collision_matrix(6, 0) = 1;
-  collisions.collision_matrix(4, 1) = 1;
-  collisions.collision_matrix(5, 1) = 1;
-  collisions.collision_matrix(6, 1) = 1;
-
-  // collisions.collision_matrix.resize(6, 6);
-  // collisions.collision_matrix(5, 0) = 1;
-  // collisions.collision_matrix(4, 1) = 1;
-  // collisions.collision_matrix(5, 1) = 1;
-
-  // Collision model
-  CollisionModelCapsule model_caps;
-
-  // Capsule 1
-  model_caps.joint_frame = 1;
-  model_caps.p1          = {0, 0, -0.065};
-  model_caps.p2          = {0, 0, 0.045};
-  model_caps.radius      = 0.065;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 2
-  model_caps.joint_frame = 1;
-  // model_caps.p1 = {0, 0, -0.065};
-  model_caps.p1 = {0, 0, -0.08};
-  // model_caps.p2 = {0, 0.485, -0.065};
-  model_caps.p2     = {0, 0.485, -0.08};
-  model_caps.radius = 0.065;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 3
-  model_caps.joint_frame = 2;
-  model_caps.p1          = {0, 0, -0.065};
-  model_caps.p2          = {0, 0, 0.085};
-  model_caps.radius      = 0.065;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 4
-  model_caps.joint_frame = 2;
-  model_caps.p1          = {0, 0.00695, -0.0917};
-  model_caps.p2          = {0, -0.36805, -0.0917};
-  model_caps.radius      = 0.061;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 5
-  model_caps.joint_frame = 4;
-  model_caps.p1          = {0, 0, 0};
-  model_caps.p2          = {0, 0, -0.08};
-  model_caps.radius      = 0.060;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 6
-  model_caps.joint_frame = 5;
-  model_caps.p1          = {0, 0, 0.08583};
-  model_caps.p2          = {0, 0, -0.06417};
-  model_caps.radius      = 0.060;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 7
-  model_caps.joint_frame = 5;
-  model_caps.p1          = {0, 0.02125, -0.007};
-  // model_caps.p2 = {0, 0.02125, -0.013};
-  model_caps.p2     = {0, 0.02125, 0.143};
-  model_caps.radius = 0.085;
-  collisions.capsule_list.push_back(model_caps); // todo: change camera capsule
-
-  // create manipulator link6
-  Manipulator link6(joints, limits, kinematics, &dynamics, &collisions);
-
-  return link6;
-}
-
-// note: p_base and Q_base are set in respect to lab's layout (in Link6 frame)
-inline host_fn Manipulator get_generic_gen3() { // todo: fix capsules, not working
-  // Manipulator
-  u32 joints = 7;
-
-  // limits
-  ManipulatorLimits limits;
-  limits.position_max = {INF_REAL, 2.25, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad
-  // limits.position_max = {INF_REAL, 4.5, INF_REAL, 5.16, INF_REAL, 4.2, INF_REAL}; // rad
-  // limits.position_min = {-INF_REAL, 0, -INF_REAL, 0, -INF_REAL, 0, -INF_REAL}; // rad
-  limits.position_min = -limits.position_max;
-
-  limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f};                   // rad/s
-                                                                                                    //   limits.vmin = -limits.velocity_max;
-
-  limits.acceleration_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad/s^2
-                                                                                                    //   limits.amin = -limits.acceleration_max;
-
-  limits.torque_max = {52, 52, 52, 52, 17, 17, 17};                                                 // Nm
-                                                                                                    //   limits.tau_min = -limits.torque_max;
-
-  limits.tool_speed_max = 0.5;                                                                      // todo: validate with webapp (Bruno from Kinova said 1.0 could be adequate), webapp says 0.5
-
-  // kinematic properties
-  ManipulatorKinematics kinematics; // using default Q_base
-  kinematics.joint_offsets = {
-          Vec3{0.0, 0.0054, -0.1284},
-          {0.0, -0.2104, -0.0064},
-          {0.0, -0.0064, -0.2104},
-          {0.0, -0.2084, -0.0064},
-          {0.0, 0.0, -0.1059},
-          {0.0, -0.1059, 0.0},
-          {0.0, 0.0, -0.0615 /*- 0.164*/} // todo: why did we subtract this? Ans : We subtracted because we used the gripper
-  }; // vector to next joint
-  kinematics.joint_axes = {
-          Vec3{0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1}}; // direction vectors of joint
-  kinematics.static_rotations[0]  = {1, 0, 0, 0, -1, 0, 0, 0, -1};
-  kinematics.static_rotations[1]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
-  kinematics.static_rotations[2]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[3]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
-  kinematics.static_rotations[4]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.static_rotations[5]  = {1, 0, 0, 0, 0, 1, 0, -1, 0};
-  kinematics.static_rotations[6]  = {1, 0, 0, 0, 0, -1, 0, 1, 0};
-  kinematics.first_joint_position = {0, 0, 0.1564f};
-  kinematics.base_position        = {1.4, 0, 0};
-  kinematics.base_rotation        = {-1, 0, 0, 0, -1, 0, 0, 0, 1};
-
-  // dynamic properties
-  ManipulatorDynamics dynamics;
-
-  dynamics.link_masses = {
-          1.377f,
-          1.1636f,
-          1.1636f,
-          0.93f,
-          0.678f,
-          0.678f,
-          0.364f}; // link masses
-  dynamics.inertia_tensors = {
-          Mat3{0.004570f, 0.000001f, 0.000002f, 0.000001f, 0.004831f, 0.000448f, 0.000002f, 0.000448f, 0.001409f},
-          {0.011088f, 0.000005f, 0.000000f, 0.000005f, 0.001072f, -0.000691f, 0.000000f, -0.000691f, 0.011255f},
-          {0.010932f, 0.000000f, -0.000007f, 0.000000f, 0.011127f, 0.000606f, -0.000007f, 0.000606f, 0.001043f},
-          {0.008147f, -0.000001f, 0.000000f, -0.000001f, 0.000631f, -0.000500f, 0.000000f, -0.000500f, 0.008316f},
-          {0.001596f, 0.000000f, 0.000000f, 0.000000f, 0.001607f, 0.000256f, 0.000000f, 0.000256f, 0.000399f},
-          {0.001641f, 0.000000f, 0.000000f, 0.000000f, 0.000410f, -0.000278f, 0.000000f, -0.000278f, 0.001641f},
-          {0.000214f, 0.000000f, 0.000001f, 0.000000f, 0.000223f, -0.000002f, 0.000001f, -0.000002f, 0.000240f}}; // Inertial tensors
-  dynamics.cog_offsets = {
-          Vec3{-0.000023f, -0.010364f, -0.073360f},
-          {-0.000044f, -0.099580f, -0.013278f},
-          {-0.000044f, -0.006641f, -0.117892f},
-          {-0.000018f, -0.075478f, -0.015006f},
-          {0.000001f, -0.009432f, -0.063883f},
-          {0.000001f, -0.045483f, -0.009650f},
-          {-0.000093f, 0.000132f, -0.022905f}}; // centers of mass
-
-  // create manipulator gen3
-  Manipulator generic_manip(joints, limits, kinematics, &dynamics);
-
-  // Collision model
-  ManipulatorCapsules collisions;
-  Sphere              sphere;
-  sphere.center = {0, 0, -0.075}; // todo: Verify this sphere
-  // sphere.center = {0, 0, -0.1564f};
-  sphere.radius             = 0.15;
-  collisions.base_sphere    = sphere;
-  collisions.collision_base = {0, 0, 1};
-
-  collisions.collision_matrix.resize(3, 3);
-  collisions.collision_matrix(2, 0) = 1;
-
-  // Collision model
-  CollisionModelCapsule model_caps;
-
-  // Capsule 1
-  model_caps.joint_frame = 1;
-  model_caps.p1          = {0, 0.035, 0};
-  model_caps.p2          = {0, -0.425, 0};
-  model_caps.radius      = 0.06;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 2
-  model_caps.joint_frame = 3;
-  model_caps.p1          = {0, 0, -0.025};
-  model_caps.p2          = {0, -0.3, -0.01};
-  model_caps.radius      = 0.06;
-  collisions.capsule_list.push_back(model_caps);
-
-  // Capsule 3
-  model_caps.joint_frame = 5;
-  model_caps.p1          = {0, 0, -0.015};
-  model_caps.p2          = {0.0, -0.15, -0.015};
-  model_caps.radius      = 0.055;
-  collisions.capsule_list.push_back(model_caps);
-
-  generic_manip.set_capsules(collisions);
-
-  return generic_manip;
-}
-
-// Warning: Will not do self-collisions
-inline host_fn Manipulator get_generic_fanuc_crx25ia() {
-  u32 joints = 6;
-
-  ManipulatorLimits limits;
-  limits.position_max = {3.1241, 3.1241, 4.6949, 3.2987, 3.1241, 3.9095};
-  limits.position_min = {-3.1241, -3.1241, -4.6949, -3.2987, -3.1241, -3.9095};
-  limits.velocity_max = {2.0944, 2.0944, 3.1416, 3.1416, 3.1416, 3.1416};
-  //   limits.vmin    = {-2.0944, -2.0944, -3.1416, -3.1416, -3.1416, -3.1416};
-  limits.torque_max = {0, 0, 0, 0, 0, 0};
-  //   limits.tau_min = {-0, -0, -0, -0, -0, -0};
-  limits.tool_speed_max = 0;
-
-  ManipulatorKinematics kinematics;
-  kinematics.joint_offsets = {
-          Vec3{0, 0, 0},
-          {0, -0.95, 0},
-          {0, 0, 0},
-          {-0.185, 0, -0.75},
-          {-0.18, 0, 0},
-          {0, 0, 0}};
-  kinematics.joint_axes = {
-          Vec3{0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1},
-          {0, 0, 1}};
-  kinematics.static_rotations[0]  = {1, 0, 0, 0, 1, 0, -0, 0, 1};
-  kinematics.static_rotations[1]  = {1, 0, -0, 0, -3.67321e-06, -1, -0, 1, -3.67321e-06};
-  kinematics.static_rotations[2]  = {-1, 0, 2.65359e-06, -0, 1, 0, -2.65359e-06, -0, -1};
-  kinematics.static_rotations[3]  = {-3.67321e-06, 0, -1, -0, 1, 0, 1, -0, -3.67321e-06};
-  kinematics.static_rotations[4]  = {-3.67321e-06, -0, 1, -0, 1, -0, -1, -0, -3.67321e-06};
-  kinematics.static_rotations[5]  = {-3.67321e-06, 0, -1, -0, 1, 0, 1, -0, -3.67321e-06};
-  kinematics.first_joint_position = {0, 0, 0};
-  kinematics.base_position        = {0, 0, 0};
-
-  ManipulatorDynamics dynamics;
-  dynamics.link_masses     = {0, 0, 0, 0, 0, 0};
-  dynamics.inertia_tensors = {
-          Mat3{0, 0, 0, 0, 0, 0, 0, 0, 0},
-          {0, 0, 0, 0, 0, 0, 0, 0, 0},
-          {0, 0, 0, 0, 0, 0, 0, 0, 0},
-          {0, 0, 0, 0, 0, 0, 0, 0, 0},
-          {0, 0, 0, 0, 0, 0, 0, 0, 0},
-          {0, 0, 0, 0, 0, 0, 0, 0, 0}};
-  dynamics.cog_offsets = {
-          Vec3{0, 0, 0},
-          {0, 0, 0},
-          {0, 0, 0},
-          {0, 0, 0},
-          {0, 0, 0},
-          {0, 0, 0}};
-
-  Manipulator generic_manip(joints, limits, kinematics, &dynamics);
-
-  ManipulatorCapsules collisions;
-
-  CollisionModelCapsule capsule;
-  capsule.joint_frame = 0;
-  capsule.p1          = {-1.93343e-06, -0.0256679, -0.0423751};
-  capsule.joint_frame = 0;
-  capsule.p2          = {-1.93343e-06, -0.0256679, -0.0253869};
-  capsule.radius      = 0.15152;
-  collisions.capsule_list.push_back(capsule);
-
-  capsule.joint_frame = 1;
-  capsule.p1          = {-0.000221565, -0.897957, -0.264728};
-  capsule.joint_frame = 1;
-  capsule.p2          = {-0.000221565, -0.0264647, -0.264728};
-  capsule.radius      = 0.164111;
-  collisions.capsule_list.push_back(capsule);
-
-  capsule.joint_frame = 2;
-  capsule.p1          = {-0.0821085, 2.98396e-06, 0.00941996};
-  capsule.joint_frame = 2;
-  capsule.p2          = {-0.00950472, 2.98396e-06, 0.00941996};
-  capsule.radius      = 0.106481;
-  collisions.capsule_list.push_back(capsule);
-
-  capsule.joint_frame = 3;
-  capsule.p1          = {-0.00857209, 0, -0.717588};
-  capsule.joint_frame = 3;
-  capsule.p2          = {-0.00857209, 0, -0.280478};
-  capsule.radius      = 0.103252;
-  collisions.capsule_list.push_back(capsule);
-
-  capsule.joint_frame = 4;
-  capsule.p1          = {-0.0348868, 4.9632e-05, -0.00215328};
-  capsule.joint_frame = 4;
-  capsule.p2          = {0.00902324, 4.9632e-05, -0.00215328};
-  capsule.radius      = 0.0836233;
-  collisions.capsule_list.push_back(capsule);
-
-  capsule.joint_frame = 5;
-  capsule.p1          = {-0.0097164, 0.000276191, 0.0315251};
-  capsule.joint_frame = 5;
-  capsule.p2          = {0.0172386, 0.000276191, 0.0315251};
-  capsule.radius      = 0.0562761;
-  collisions.capsule_list.push_back(capsule);
-
-  ObjMatrix<u8> collision_matrix(collisions.capsule_list.size(), collisions.capsule_list.size());
-  collision_matrix(2, 0)      = 1;
-  collision_matrix(3, 0)      = 1;
-  collision_matrix(4, 0)      = 1;
-  collision_matrix(5, 0)      = 1;
-  collision_matrix(3, 1)      = 1;
-  collision_matrix(4, 1)      = 1;
-  collision_matrix(5, 1)      = 1;
-  collisions.collision_matrix = collision_matrix;
-
-  Array collision_base(collisions.capsule_list.size());
-  collision_base            = {0, 0, 1, 1, 1};
-  collisions.collision_base = collision_base;
-
-  generic_manip.set_capsules(collisions);
-
-  // add end effector properties
-  Tool tool;
-  tool.tool_offset   = {0, 0, 0.3};
-  tool.tool_rotation = {0, 1, 0, 1, 0, 0, 0, 0, -1};
-
-  generic_manip.add_tool(tool);
-
-  return generic_manip;
-}
-
-inline host_fn World get_demo2_world() {
-  World result;
-  result.add_box({0.7, 0, 0.381}, {1.0, 0.75, 0.381}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1});       // table 76 cm high
-  result.add_box({0.67, -0.1475, 0.96}, {0.35, 0.025, 0.2}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // vertical plate (no coll)
-  // result.add_box({0.7, 0, -0.55}, {1.0, 0.75, 0.4}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1});                 // table 76 cm high
-  // result.add_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // vertical plate (no coll)
-  return result;
-}
-
-inline host_fn World get_lab_world() {
-  World world;
-  // Lab environment
-  // add_box({0.6415, 0.0237, -0.53815}, {2.0, 2.0, 0.381}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); //table
-  world.add_box({0.7, 0, -0.55}, {1.0, 0.75, 0.4}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1});                 // table
-  world.add_box({-0.5654, -0.8145, 0.3248}, {0.381, 0.635, 0.381}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // computer and screens next to link6
-  world.add_box({0.4506, -1.3479, 0.3248}, {0.635, 0.381, 0.381}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // computer and screens next to gen3lite
-  world.add_box({0.0, 0.0, -0.4091}, {0.1459, 0.2018, 0.4091}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));     // link6 base
-  world.add_box({0.6665, 1.1286, 0.0}, {0.508, 0.3175, 1.0457}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));    // UR5
-  world.add_box({0, -0.75, 1.0}, {0.10, 0.15, 2.0}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));                // cables
-
-  // DEMO 1 bin 1
-  // world.add_box({0.4114, -0.3784, -0.04465}, {0.2125, 0.155, 0.0125}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // bottom
-  // world.add_box({0.2114, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // front
-  // world.add_box({0.6114, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // back
-  // world.add_box({0.4114, -0.2359, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // left
-  // world.add_box({0.4114, -0.5209, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // right
-
-  // // DEMO 1 bin 2
-  // world.add_box({0.8716, -0.3784, -0.04465}, {0.2125, 0.155, 0.0125}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // bottom
-  // world.add_box({0.6716, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // front
-  // world.add_box({1.0716, -0.3784, 0.03035}, {0.0125, 0.155, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1));  // back
-  // world.add_box({0.8716, -0.2359, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // left
-  // world.add_box({0.8716, -0.5209, 0.03035}, {0.1875, 0.0125, 0.0625}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // right
-
-  // DEMO 1 obstacle
-  world.add_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, Mat3(1, 0, 0, 0, 1, 0, 0, 0, 1)); // vertical plate (no coll)
-
-  return world;
-}
-
-inline host_fn Task get_link6_task() {
-  Array pi = deg2rad({-40.445762634277344,
-                      -26.876392364501953,
-                      83.60868835449219,
-                      1.49383544921875,
-                      19.951095581054688,
-                      -42.22943115234375}); // w1 + 10
-  Array pf = deg2rad({51.851436614990234,
-                      -13.578636169433594,
-                      107.87167358398438,
-                      3.6194305419921875,
-                      33.133209228515625,
-                      51.21833801269531}); // wb1
-  return Task::stop_to_stop(pi, pf);
-}
-
-inline host_fn Task get_gen3_task() {
-  Array pi = wrap2pi(deg2rad({215.6, 282.3, 337.6, 325.2, 159.0, 21.7, 323.0}));
-  Array pf = wrap2pi(deg2rad({114.3, 319.8, 29.1, 266.1, 243.0, 25.3, 164.3}));
-  return Task::stop_to_stop(pi, pf);
-}
-
-inline host_fn std::vector<Task> get_Link6_demo1_tasks() {
-  // Read tasks Demo 1 (13 tasks)
-  // Array pos_zero = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-  Array pos_home = deg2rad({0.00177001953125,
-                            -19.997268676757812,
-                            110.00163269042969,
-                            0.001434326171875,
-                            39.99864196777344,
-                            0.0013427734375});
-
-  Array pos_wb1 = deg2rad({51.851436614990234,
-                           -13.578636169433594,
-                           107.87167358398438,
-                           3.6194305419921875,
-                           33.133209228515625,
-                           51.21833801269531});
-  Array pos_wb3 = deg2rad({53.879573822021484,
-                           -6.246559143066406,
-                           121.92112731933594,
-                           5.29833984375,
-                           34.81269836425781,
-                           45.617919921875});
-  Array pos_wb4 = deg2rad({62.90712356567383,
-                           -2.7591018676757812,
-                           119.23771667480469,
-                           8.18115234375,
-                           28.954452514648438,
-                           47.103912353515625});
-
-  Array pos_bb2 = deg2rad({27.443552017211914,
-                           -54.93880081176758,
-                           28.312477111816406,
-                           1.807525634765625,
-                           -4.7853546142578125,
-                           29.731155395507812});
-  Array pos_bb5 = deg2rad({32.459312438964844,
-                           -41.956546783447266,
-                           48.75336837768555,
-                           4.1028289794921875,
-                           1.5039520263671875,
-                           15.067718505859375});
-  Array pos_bb6 = deg2rad({29.169939041137695,
-                           -44.1719856262207,
-                           55.125301361083984,
-                           0.5225830078125,
-                           12.9627685546875,
-                           25.127288818359375});
-
-  Array pos_w1      = deg2rad({-40.19879913330078,
-                               -34.87216567993164,
-                               96.39874267578125,
-                               1.4193572998046875,
-                               41.67781066894531,
-                               -41.464874267578125});
-  Array pos_w1_10cm = deg2rad({-40.445762634277344,
-                               -26.876392364501953,
-                               83.60868835449219,
-                               1.49383544921875,
-                               19.951095581054688,
-                               -42.22943115234375});
-
-  Array pos_b2      = deg2rad({-23.84050750732422,
-                               -23.07632064819336,
-                               121.39236450195312,
-                               -0.2214813232421875,
-                               52.64436340332031,
-                               -23.962554931640625});
-  Array pos_b2_10cm = deg2rad({-24.826316833496094,
-                               -9.07125473022461,
-                               110.22100830078125,
-                               1.2542877197265625,
-                               30.40911865234375,
-                               -27.223480224609375});
-
-  Array pos_w3      = deg2rad({-34.8427734375,
-                               -42.51811599731445,
-                               81.55162811279297,
-                               0.0153350830078125,
-                               33.299774169921875,
-                               -35.105316162109375});
-  Array pos_w3_10cm = deg2rad({-33.93860626220703,
-                               -35.56209945678711,
-                               70.89104461669922,
-                               -0.6509857177734375,
-                               16.881134033203125,
-                               -31.8890380859375});
-
-  Array pos_w4      = deg2rad({-20.46697998046875,
-                               -30.862834930419922,
-                               105.23368835449219,
-                               1.3421478271484375,
-                               43.346038818359375,
-                               -22.13421630859375});
-  Array pos_w4_10cm = deg2rad({-19.595504760742188,
-                               -19.332134246826172,
-                               95.92303466796875,
-                               -3.042205810546875,
-                               23.505020141601562,
-                               -17.61700439453125});
-
-  Array pos_b5      = deg2rad({-30.808380126953125,
-                               -53.44142532348633,
-                               60.08732986450195,
-                               0.0470733642578125,
-                               20.517074584960938,
-                               -33.375030517578125});
-  Array pos_b5_10cm = deg2rad({-31.708511352539062,
-                               -46.73225784301758,
-                               46.94545364379883,
-                               1.8437347412109375,
-                               3.8740386962890625,
-                               -30.906951904296875});
-
-  Array pos_b6      = deg2rad({-16.236770629882812,
-                               -40.21072769165039,
-                               86.60179901123047,
-                               -2.0247955322265625,
-                               32.93904113769531,
-                               -12.799804687});
-  Array pos_b6_10cm = deg2rad({-17.104469299316406,
-                               -30.512935638427734,
-                               77.69818115234375,
-                               0.307220458984375,
-                               17.362060546875,
-                               -12.498809814453125});
-
-  // For every bloc, the sequence is  +10cm -> bloc -> +10cm -> dbox -> next bloc's +10cm
-
-  std::vector<Task> tasks;
-
-  // Sequence tasks (13 tasks)
-  tasks.push_back(Task::stop_to_stop(pos_home, pos_w1_10cm)); // 0: home to start
-  tasks.push_back(Task::stop_to_stop(pos_w1_10cm, pos_wb1));  // 3: bloc 1
-  tasks.push_back(Task::stop_to_stop(pos_wb1, pos_b2_10cm));  // 4: bloc 2 approach
-  tasks.push_back(Task::stop_to_stop(pos_b2_10cm, pos_bb2));  // 7: bloc 2
-  tasks.push_back(Task::stop_to_stop(pos_bb2, pos_w3_10cm));  // 8: bloc 3 approach
-  tasks.push_back(Task::stop_to_stop(pos_w3_10cm, pos_wb3));  // 11: bloc 3
-  tasks.push_back(Task::stop_to_stop(pos_wb3, pos_w4_10cm));  // 12: bloc 4 approach
-  tasks.push_back(Task::stop_to_stop(pos_w4_10cm, pos_wb4));  // 15: bloc 4
-  tasks.push_back(Task::stop_to_stop(pos_wb4, pos_b5_10cm));  // 16: bloc 5 approach
-  tasks.push_back(Task::stop_to_stop(pos_b5_10cm, pos_bb5));  // 19: bloc 5
-  tasks.push_back(Task::stop_to_stop(pos_bb5, pos_b6_10cm));  // 20: bloc 6 approach
-  tasks.push_back(Task::stop_to_stop(pos_b6_10cm, pos_bb6));  // 23: bloc 6
-  tasks.push_back(Task::stop_to_stop(pos_bb6, pos_home));     // 24: home to finish
-
-  return tasks;
-}
-
-// HierarchicalTask get_demo2_task() {
-//     // Initialization
-//     MultiRobotTask task;
-//
-//     // --- Define manipulators ---
-//     task.robots.reserve(2);
-//     task.add_robot(get_generic_Link6());
-//     task.add_robot(get_generic_gen3());
-//
-//     // --- Define world ---
-//     World world;
-//     add_box({0.67, -0.1475, -0.0562}, {0.35, 0.025, 0.4}, {1, 0, 0, 0, 1, 0, 0, 0, 1}); // vertical plate
-//     // add_box({0.7, 0.0, -0.8}, {1.0, 0.75, 0.4}, {1, 0, 0, 0, 1, 0, 0, 0, 1}); //table
-//     // task.world = get_lab_world();
-//     task.world = world;
-//
-//
-//     // --- Define tasks ---
-//     // Gripper toggles
-//     Matrix gripper_toggle(13, 2);
-//
-//     // Link6 positions
-//     Array pos_0_link6(6);
-//
-//     Array pos_w1_10cm = deg2rad({-40.4, -26.9, 83.6, 1.5, 20.0, -42.2});
-//     Array pos_w1 = deg2rad({-40.2, -34.9, 96.4, 1.4, 41.7, -41.5});
-//     Array pos_wb1 = deg2rad({51.9, -13.6, 107.9, 3.6, 33.1, 51.2});
-//
-//     Array pos_b2_10cm_link6 = deg2rad({-24.8, -9.1, 110.2, 1.254, 30.4, -27.2});
-//     Array pos_b2_link6 = deg2rad({-23.8, -23.1, 121.4, -0.2, 52.6, -24.0});
-//     Array pos_bb2_link6 = deg2rad({27.4, -54.9, 28.3, 1.8, -4.8, 29.7});
-//
-//     Array pos_w3_10cm = deg2rad({-33.9, -35.6, 70.9, -0.7, 16.9, -31.8});
-//     Array pos_w3 = deg2rad({-34.8, -42.5, 81.6, 0.0, 33.3, -35.1});
-//     Array pos_wb3 = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});
-//
-//     Array pos_w4_10cm = deg2rad({-20.0, -19.3, 95.9, -3.0, 23.5, -17.6});
-//     Array pos_w4 = deg2rad({-20.5, -30.9, 105.2, 1.3, 43.3, -22.1});
-//     Array pos_wb4 = deg2rad({53.9, -6.2, 121.9, 5.3, 34.8, 45.6});
-//
-//     // Gen3 positions
-//     Array pos_0_gen3(7);
-//
-//     Array pos_b2_10cm_gen3 = wrap2pi(deg2rad({36.1,  79.9,  114.8, 339.7, 41.9, 321.6, 116.2}));
-//     Array pos_b2_gen3 = wrap2pi(deg2rad(     {37.6,  89.1,  114.6, 335.7, 42.7, 332.0, 119.4}));
-//     Array pos_bb2_gen3 = wrap2pi(deg2rad(    {315.3, 33.9,  162.4, 264.0, 84.6, 306.9, 334.0}));
-//
-//     Array pos_b5_10cm = wrap2pi(deg2rad({72.4, 98.5,  98.5, 318.8, 42.0, 315.7, 135.7}));
-//     Array pos_b5 = wrap2pi(deg2rad(     {73.3, 88.8,  98.4, 318.0, 43.4, 321.6, 142.4}));
-//     Array pos_bb5 = wrap2pi(deg2rad(    {315.3, 33.9,  162.4, 264.0, 84.6, 306.9, 334.0}));
-//
-//     Array pos_b6_10cm = wrap2pi(deg2rad({69.2,  59.7,  122.7, 274.6, 55.1, 352.1, 87.7}));
-//     Array pos_b6 = wrap2pi(deg2rad(     {62.2,  64.9,  142.5, 270.5, 53.1, 347.5,  88.7}));
-//     Array pos_bb6 = wrap2pi(deg2rad(    {315.3, 33.9,  162.4, 264.0, 84.6, 306.9, 334.0}));
-//
-//     Array v0(6);
-//     Array vf(6);
-//     Array a0(6);
-//     Array af(6);
-//
-//     // Task 1 Link6
-//     task.add_angular_task(0, pos_0_link6, pos_w1_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_w1_10cm, pos_w1, v0, vf, a0, af);
-//     gripper_toggle(1, 0) = 1;
-//     task.add_angular_task(0, pos_w1, pos_w1_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_w1_10cm, pos_wb1, v0, vf, a0, af);
-//     gripper_toggle(3, 0) = 1;
-//
-//     // Task 2 Link6
-//     // task.add_angular_task(0, pos_wb1, pos_b2_10cm_link6, v0, vf, a0, af);
-//     // task.add_angular_task(0, pos_b2_10cm_link6, pos_b2_link6, v0, vf, a0, af);
-//     // gripper_toggle(5, 0) = 1;
-//     // task.add_angular_task(0, pos_b2_link6, pos_b2_10cm_link6, v0, vf, a0, af);
-//     // task.add_angular_task(0, pos_b2_10cm_link6, pos_bb2_link6, v0, vf, a0, af);
-//     // gripper_toggle(7, 0) = 1;
-//
-//     // Task 3 Link6
-//     // task.add_angular_task(0, pos_bb2, pos_w3_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_wb1, pos_w3_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_w3_10cm, pos_w3, v0, vf, a0, af);
-//     gripper_toggle(5, 0) = 1;
-//     task.add_angular_task(0, pos_w3, pos_w3_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_w3_10cm, pos_wb3, v0, vf, a0, af); // did not work after 20 restarts
-//     gripper_toggle(7, 0) = 1;
-//
-//     // Task 4 Link6
-//     task.add_angular_task(0, pos_wb3, pos_w4_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_w4_10cm, pos_w4, v0, vf, a0, af); // task is not valid (did not work after 5 restarts)
-//     gripper_toggle(9, 0) = 1;
-//     task.add_angular_task(0, pos_w4, pos_w4_10cm, v0, vf, a0, af);
-//     task.add_angular_task(0, pos_w4_10cm, pos_wb4, v0, vf, a0, af);
-//     gripper_toggle(11, 0) = 1;
-//
-//     // // Back to home
-//     task.add_angular_task(0, pos_wb4, pos_0_link6, v0, vf, a0, af);
-//     // task.add_angular_task(0, pos_w1_10cm, get_Link6_home(), v0, vf, a0, af);
-//
-//
-//     // Gen3 tasks
-//     v0.resize(7);
-//     vf.resize(7);
-//     a0.resize(7);
-//     af.resize(7);
-//
-//     // Task 1 Gen3
-//     task.add_angular_task(1, pos_0_gen3, pos_b2_10cm_gen3, v0, vf, a0, af);
-//     task.add_angular_task(1, pos_b2_10cm_gen3, pos_b2_gen3, v0, vf, a0, af); // task does not work (pos_b2 is not right)
-//     gripper_toggle(1, 1) = 1;
-//     task.add_angular_task(1, pos_b2_gen3, pos_b2_10cm_gen3, v0, vf, a0, af);
-//     task.add_angular_task(1, pos_b2_10cm_gen3, pos_bb2_gen3, v0, vf, a0, af);
-//     gripper_toggle(3, 1) = 1;
-//
-//     // Task 2 Gen3
-//     task.add_angular_task(1, pos_bb2_gen3, pos_b5_10cm, v0, vf, a0, af);
-//     task.add_angular_task(1, pos_b5_10cm, pos_b5, v0, vf, a0, af); // task does not work (pos_b5 is not right)
-//     gripper_toggle(5, 1) = 1;
-//     task.add_angular_task(1, pos_b5, pos_b5_10cm, v0, vf, a0, af);
-//     task.add_angular_task(1, pos_b5_10cm, pos_bb5, v0, vf, a0, af);
-//     gripper_toggle(7, 1) = 1;
-//
-//     // Task 3 Gen3
-//     task.add_angular_task(1, pos_bb5, pos_b6_10cm, v0, vf, a0, af);
-//     task.add_angular_task(1, pos_b6_10cm, pos_b6, v0, vf, a0, af); // task does not work (pos_b6 is not right)
-//     gripper_toggle(9, 1) = 1;
-//     task.add_angular_task(1, pos_b6, pos_b6_10cm, v0, vf, a0, af);
-//     task.add_angular_task(1, pos_b6_10cm, pos_bb6, v0, vf, a0, af);
-//     gripper_toggle(11, 1) = 1;
-//
-//     // Back to home
-//     task.add_angular_task(1, pos_bb6, pos_0_gen3, v0, vf, a0, af);
-//     // task.add_angular_task(1, pos_b2_10cm, get_gen3_home(), v0, vf, a0, af);
-//
-//     task.gripper_toggle = gripper_toggle;
-//
-//     // B-spline
-//     Bspline bspline_link6(10, 50, 5, 6);
-//     Bspline bspline_gen3(8, 200, 5, 7);
-//     std::vector<Bspline> robot_bspline;
-//     robot_bspline.reserve(2);
-//     robot_bspline.push_back(bspline_link6);
-//     robot_bspline.push_back(bspline_gen3);
-//
-//     // Guess
-//     Guess guess; // using default guess
-//     guess.n_shot = 300;
-//     std::vector<Guess> robot_guess;
-//     robot_guess.push_back(guess);
-//     robot_guess.push_back(guess);
-//
-//     // Constraints
-//     Constraints<Manipulator> constraints;
-//     constraints.position = true;
-//     constraints.velocity = true;
-//     constraints.acceleration = true;
-//     constraints.torque = true;
-//     constraints.tool_speed = true;
-//     constraints.self_collisions = true;
-//     constraints.external_collisions = true;
-//     constraints.n_collision_constraints = 100;
-//     constraints.n_collision_skip = 2;
-//
-//     std::vector<Constraints<Manipulator>> robot_constraints;
-//     robot_constraints.push_back(constraints);
-//     robot_constraints.push_back(constraints);
-//
-//     // Objective
-//     Objective<Manipulator> objective;
-//     objective.time_weight = 1;
-//
-//     std::vector<Objective<Manipulator>> robot_objective;
-//     robot_objective.push_back(objective);
-//     robot_objective.push_back(objective);
-//
-//     // --- Setup Optimization ---
-//     Array hierarchy = {0, 1};
-//     auto hierarchy_optim = create_hierarchical_object(task, hierarchy, robot_constraints, robot_objective, robot_guess, robot_bspline);
-//     return hierarchy_optim;
-// }
-
-inline host_fn Optimization get_generic_link6_opt() {
-  // Manip
-  Manipulator manip = get_generic_Link6();
-
-  // Task
-  auto task = get_link6_task();
-
-  // Create optimization
-  Optimization opt(manip, task);
-
-  // world
-  World world = get_lab_world();
-  opt.set_world(world);
-
-  // Constraints
-  ConstraintSelection constraints;
-  constraints.position            = true;
-  constraints.velocity            = true;
-  constraints.acceleration        = true;
-  constraints.torque              = true;
-  constraints.tool_speed          = true;
-  constraints.self_collisions     = true;
-  constraints.external_collisions = true;
-  opt.set_constraints(constraints);
-
-  // Objective
-  Objective objective;
-  objective.time_weight = 1;
-  opt.set_objective(objective);
-
-  // B-spline
-  Bspline bspline(manip.n_joints);
-  opt.set_bspline(bspline);
-
-  // Guess
-  opt.guess.type           = Guess::random;
-  opt.guess.n_random_shots = 100;
-
-  return opt;
-}
-
-// Optimization<Link6> get_hardcoded_link6_opt() {
-//     // Manip
-//     Link6 manip;
-//
-//     // Task
-//     auto task = get_link6_task();
-//
-//     // Create optimization
-//     Optimization<Link6> opt(manip, task);
-//
-//     // world
-//     World world = get_lab_world();
-//     opt.set_world(world);
-//
-//     // Constraints
-//     Constraints<Link6> constraints;
-//     constraints.position = true;
-//     constraints.velocity = true;
-//     constraints.acceleration = true;
-//     constraints.torque = true;
-//     constraints.tool_speed = true;
-//     constraints.self_collisions = true;
-//     constraints.external_collisions = true;
-//     opt.set_constraints(constraints);
-//
-//     // Objective
-//     Objective<Link6> objective;
-//     objective.time_weight = 1;
-//     opt.set_objective(objective);
-//
-//     // B-spline
-//     Bspline bspline(manip.joints);
-//     opt.set_bspline(bspline);
-//
-//     // Guess
-//     opt.guess.type = Guess::random;
-//     opt.guess.n_random_shots = 100;
-//
-//     return opt;
-// }
-
-inline host_fn Optimization get_generic_gen3_opt() {
-  // Manip
-  Manipulator manip = get_generic_gen3();
-
-  // Task
-  auto task = get_gen3_task();
-
-  // Create optimization
-  Optimization opt(manip, task);
-
-  // world
-  World world = get_lab_world();
-  opt.set_world(world);
-
-  // Constraints
-  ConstraintSelection constraints;
-  constraints.position            = true;
-  constraints.velocity            = true;
-  constraints.acceleration        = true;
-  constraints.torque              = true;
-  constraints.tool_speed          = true;
-  constraints.self_collisions     = true;
-  constraints.external_collisions = true;
-  opt.set_constraints(constraints);
-
-  // Objective
-  Objective objective;
-  objective.time_weight = 1;
-  opt.set_objective(objective);
-
-  // B-spline
-  Bspline bspline(manip.n_joints);
-  opt.set_bspline(bspline);
-
-  // Guess
-  opt.guess.type           = Guess::random;
-  opt.guess.n_random_shots = 100;
-
-  return opt;
-}
-
-// Optimization get_gen3_gen3_opt() {
-//     // Manip
-//     Gen3 manip;
-//     manip.p_base  = {1.4, 0, 0.1564f};
-//     manip.Q_base = {-1, 0, 0, 0, -1, 0, 0, 0, 1};
-//
-//     // Task
-//     auto task = get_gen3_task();
-//
-//     // Create optimization
-//     Optimization opt(manip, task);
-//
-//     // world
-//     World world = get_lab_world();
-//     opt.set_world(world);
-//
-//     // Constraints
-//     ConstraintSelection constraints;
-//     constraints.position = true;
-//     constraints.velocity = true;
-//     constraints.acceleration = true;
-//     constraints.torque = true;
-//     constraints.tool_speed = true;
-//     constraints.self_collisions = true;
-//     constraints.external_collisions = true;
-//     opt.set_constraints(constraints);
-//
-//     // Objective
-//     Objective objective;
-//     objective.time_weight = 1;
-//     opt.set_objective(objective);
-//
-//     // B-spline
-//     Bspline bspline(manip.joints);
-//     opt.set_bspline(bspline);
-//
-//     // Guess
-//     opt.guess.type = Guess::random;
-//     opt.guess.n_random_shots = 100;
-//
-//     return opt;
-// }
-
-inline host_fn Array get_Link6_home() {
-  return {0, -0.349, 1.92, 0, 0.698, 0};
-}
-
-// Case D1 from paper "Singularity Analysis of Kinova’s Link 6 Robot Arm via Grassmann Line Geometry"
-// (https://espace2.etsmtl.ca/id/eprint/28453/1/Bonev-I-2024-28453.pdf)
-inline host_fn Array get_Link6_singularity() {
-  return deg2rad({0, 17.19, 22.25, 62.10, 160, 0});
-}
-
-inline host_fn Array get_gen3_home() {
-  return wrap2pi(deg2rad(Array({0, 15, 180, -130, 0, 55, 90})));
-}
-
-
-} // namespace blast
diff --git a/tests/test_manipulator/test_Link6.cpp b/tests/test_manipulator/test_Link6.cpp
deleted file mode 100644
index 96e2df7..0000000
--- a/tests/test_manipulator/test_Link6.cpp
+++ /dev/null
@@ -1,175 +0,0 @@
-#include <blast>
-using namespace blast;
-
-#define CATCH_CONFIG_MAIN
-#define CATCH_CONFIG_ENABLE_BENCHMARKING
-#include <catch2/catch.hpp>
-
-Link6 manip;
-auto  joints = manip.joints;
-
-TEST_CASE("Link6 Initializer") {
-  CHECK(manip.position_max.size == joints);
-  CHECK(manip.velocity_max.size == joints);
-  CHECK(manip.acceleration_max.size == joints);
-  CHECK(manip.torque_max.size == joints);
-}
-
-// todo:
-// TEST_CASE("Link6 Set Payload") {
-// }
-
-TEST_CASE("Link6 Internal Constraints") {
-  Trajectory traj(1, joints);
-  auto       ncon = manip.ncon(1);
-  Array      constraints(ncon);
-
-  manip.dynamics(traj);
-  Array torque_const_zero(joints);
-  for (u32 i = 0; i < joints; i++)
-    torque_const_zero[i] = (abs(manip._efforts(i, 0)) - manip.torque_max[i]) / manip.torque_max[i];
-
-  manip.internal_constraints(traj, constraints.data);
-
-  CHECK(constraints.size == ncon); // todo: is there a way to actually test this ?
-  for (u32 i = 9; i < ncon - joints; i++)
-    CHECK(constraints[i] == -1);
-  for (u32 i = ncon - joints; i < ncon; i++)
-    CHECK(constraints[i] == torque_const_zero[i + joints - ncon]);
-
-  // todo: check internal_collisions
-}
-
-// todo:
-// TEST_CASE("Link6 Validate Task") {
-// }
-
-TEST_CASE("Link6", "Dynamics") {
-  // todo: check dyn at a specific known point
-  Trajectory traj(1, joints);
-  Array      pos(joints);
-  Array      vel(joints);
-  Array      acc(joints);
-
-  manip.dynamics(traj);
-  auto dyn_array = manip.dynamics(pos, vel, acc);
-  for (u32 i = 0; i < joints; i++)
-    CHECK(manip._efforts(i, 0) == dyn_array[i]);
-
-  for (u32 i = 0; i < 100; i++) {
-    Trajectory traj_rnd(1, joints);
-    auto       pos_rnd = random_array(joints, PI);
-    auto       vel_rnd = random_array(joints, PI);
-    auto       acc_rnd = random_array(joints, PI);
-    traj_rnd.pos       = pos_rnd;
-    traj_rnd.vel       = vel_rnd;
-    traj_rnd.acc       = acc_rnd;
-
-    manip.dynamics(traj_rnd);
-    auto dyn_array = manip.dynamics(pos_rnd, vel_rnd, acc_rnd);
-    for (u32 i = 0; i < joints; i++)
-      CHECK(manip._efforts(i, 0) == dyn_array[i]);
-  }
-}
-
-TEST_CASE("Link6 Forward Kinematics") {
-  // Testing initialized at 0
-  Array  pos_array(joints);
-  Matrix pos_matrix(joints, 1);
-
-  Array  tmp_array = {0, 0, 0, 0, 0, 0};
-  Matrix tmp_matrix(tmp_array);
-
-  CHECK(is_close(pos_array, tmp_array));
-  CHECK(is_close(pos_matrix, tmp_matrix));
-
-  // Testing for zeros
-  auto f_kin_array  = manip.forward_kinematics(pos_array);
-  auto f_kin_matrix = manip.forward_kinematics(pos_matrix);
-
-
-  CHECK(f_kin_array[0] == f_kin_matrix(0, 0));
-  CHECK(f_kin_array[1] == f_kin_matrix(1, 0));
-  CHECK(f_kin_array[2] == f_kin_matrix(2, 0));
-
-  f_kin_array.size = 3; // note: don't try this at home.
-
-  Array actual_pos(3);
-  actual_pos[0] = 0.489;
-  actual_pos[1] = 0.117;
-  actual_pos[2] = 1.136;
-  CHECK(is_close(f_kin_array, actual_pos, 0.001));
-
-  // Testing for home position
-  pos_array = deg2rad({0.0, -20.0, 110.0, 0.0, 40.0, 0.0});
-
-  pos_matrix(0, 0) = deg2rad(0.0);
-  pos_matrix(1, 0) = deg2rad(-20.0);
-  pos_matrix(2, 0) = deg2rad(110.0);
-  pos_matrix(3, 0) = deg2rad(0.0);
-  pos_matrix(4, 0) = deg2rad(40.0);
-  pos_matrix(5, 0) = deg2rad(0.0);
-
-  f_kin_array  = manip.forward_kinematics(pos_array);
-  f_kin_matrix = manip.forward_kinematics(pos_matrix);
-
-  CHECK(f_kin_array[0] == f_kin_matrix(0, 0));
-  CHECK(f_kin_array[1] == f_kin_matrix(1, 0));
-  CHECK(f_kin_array[2] == f_kin_matrix(2, 0));
-
-  f_kin_array.size = 3; // note: don't try this at home.
-
-  actual_pos[0] = 0.649;
-  actual_pos[1] = 0.117;
-  actual_pos[2] = 0.026; // todo: Verify?
-  CHECK(is_close(f_kin_array, actual_pos, 0.001));
-
-  // todo: check if both f_kin are the same at multiple random positions
-  for (u32 i = 0; i < 100; i++) {
-    auto   rnd_array = random_array(joints, PI);
-    Matrix rnd_matrix(rnd_array);
-
-    auto f_kin_array_rnd  = manip.forward_kinematics(rnd_array);
-    auto f_kin_matrix_rnd = manip.forward_kinematics(rnd_matrix);
-
-    CHECK(f_kin_array_rnd[0] == f_kin_matrix_rnd(0, 0));
-    CHECK(f_kin_array_rnd[1] == f_kin_matrix_rnd(1, 0));
-    CHECK(f_kin_array_rnd[2] == f_kin_matrix_rnd(2, 0));
-  }
-}
-
-// todo:
-// TEST_CASE("Link6 Jacobian"){
-// }
-
-TEST_CASE("Link6 Capsules") {
-  Array  pos_array(joints);
-  Matrix pos_matrix(joints, 1);
-
-  Array  tmp_array = {0, 0, 0, 0, 0, 0};
-  Matrix tmp_matrix(tmp_array);
-
-  CHECK(is_close(pos_array, tmp_array));
-  CHECK(is_close(pos_matrix, tmp_matrix));
-
-  auto robot_caps_matrix = manip.robot_capsules(pos_array);
-  auto robot_caps_vector = manip.robot_capsules(pos_matrix, 1);
-
-  CHECK(robot_caps_matrix.size == (u32) robot_caps_vector.size() * (sizeof(Capsule) / sizeof(real) - 2)); // Vec3 has 4 real, with one being _pad that is not desired in this calculation
-
-  for (u32 i = 0; i < 100; i++) {
-    auto   rnd_array = random_array(joints, PI);
-    Matrix rnd_matrix(rnd_array);
-    auto   robot_caps_matrix = manip.robot_capsules(rnd_array);
-    auto   robot_caps_vector = manip.robot_capsules(rnd_matrix, 1);
-    for (u32 j = 0; j < robot_caps_matrix.cols; j++) {
-      CHECK(robot_caps_matrix(0, j) == robot_caps_vector[j].p1.x);
-      CHECK(robot_caps_matrix(1, j) == robot_caps_vector[j].p1.y);
-      CHECK(robot_caps_matrix(2, j) == robot_caps_vector[j].p1.z);
-      CHECK(robot_caps_matrix(3, j) == robot_caps_vector[j].p2.x);
-      CHECK(robot_caps_matrix(4, j) == robot_caps_vector[j].p2.y);
-      CHECK(robot_caps_matrix(5, j) == robot_caps_vector[j].p2.z);
-      CHECK(robot_caps_matrix(6, j) == robot_caps_vector[j].r);
-    }
-  }
-}
diff --git a/tests/test_profiler.cpp b/tests/test_profiler.cpp
deleted file mode 100644
index 5c89260..0000000
--- a/tests/test_profiler.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include <blast>
-#include <chrono>
-#include <iostream>
-#include <thread>
-
-int main() {
-  using namespace std::chrono_literals;
-
-  blast::begin_profile();
-  {
-    blast_time_function;
-    {
-      blast_time_block("Sleep");
-      std::this_thread::sleep_for(1s);
-    }
-  }
-
-  blast::end_profile();
-  return 0;
-}
-
-blast_profiler_end_compilation_unit;
diff --git a/tests/trajectory/test_bsplines.cpp b/tests/trajectory/test_bsplines.cpp
index d898e86..db0d414 100644
--- a/tests/trajectory/test_bsplines.cpp
+++ b/tests/trajectory/test_bsplines.cpp
@@ -1,6 +1,4 @@
 #include <blast>
-#include "test_helper/test_functions.hpp"
-#include "test_helper/test_helper.hpp"
 
 #define CATCH_CONFIG_MAIN
 #define CATCH_CONFIG_ENABLE_BENCHMARKING
diff --git a/tests/trajectory/test_polynomial.cpp b/tests/trajectory/test_polynomial.cpp
new file mode 100644
index 0000000..7e310c6
--- /dev/null
+++ b/tests/trajectory/test_polynomial.cpp
@@ -0,0 +1,125 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+
+#include <blast>
+
+using namespace blast;
+
+TEST_CASE("compute_5order_trajectory - boundary conditions for single joint", "[Trajectory][Polynomial]") {
+  Matrix task(1, 6);
+  task(0, 0) = 0.0; // start pos
+  task(0, 1) = 0.0; // start vel
+  task(0, 2) = 0.0; // start acc
+  task(0, 3) = 1.0; // end pos
+  task(0, 4) = 0.0; // end vel
+  task(0, 5) = 0.0; // end acc
+
+  real T    = 2.0;
+  auto traj = compute_5order_trajectory(T, task);
+
+  constexpr real eps  = 1e-6;
+  const int      last = (int) traj.t.size - 1;
+  CHECK(std::abs(traj.pos(0, 0) - 0.0) < eps);
+  CHECK(std::abs(traj.vel(0, 0) - 0.0) < eps);
+  CHECK(std::abs(traj.acc(0, 0) - 0.0) < eps);
+  CHECK(std::abs(traj.pos(0, last) - 1.0) < eps);
+  CHECK(std::abs(traj.vel(0, last) - 0.0) < eps);
+  CHECK(std::abs(traj.acc(0, last) - 0.0) < eps);
+}
+
+TEST_CASE("compute_5order_trajectory - boundary conditions with nonzero start/end vel and acc", "[Trajectory][Polynomial]") {
+  Matrix task(1, 6);
+  task(0, 0) = -0.5; // start pos
+  task(0, 1) = 0.2;  // start vel
+  task(0, 2) = 0.1;  // start acc
+  task(0, 3) = 0.5;  // end pos
+  task(0, 4) = -0.1; // end vel
+  task(0, 5) = 0.0;  // end acc
+
+  real T    = 1.0;
+  auto traj = compute_5order_trajectory(T, task);
+
+  constexpr real eps  = 1e-5;
+  const int      last = (int) traj.t.size - 1;
+  CHECK(std::abs(traj.pos(0, 0) - task(0, 0)) < eps);
+  CHECK(std::abs(traj.vel(0, 0) - task(0, 1)) < eps);
+  CHECK(std::abs(traj.acc(0, 0) - task(0, 2)) < eps);
+  CHECK(std::abs(traj.pos(0, last) - task(0, 3)) < eps);
+  CHECK(std::abs(traj.vel(0, last) - task(0, 4)) < eps);
+  CHECK(std::abs(traj.acc(0, last) - task(0, 5)) < eps);
+}
+
+TEST_CASE("compute_5order_trajectory - output size matches expected point count", "[Trajectory][Polynomial]") {
+  Matrix task(1, 6);
+  task(0, 0) = 0.0;
+  task(0, 1) = 0.0;
+  task(0, 2) = 0.0;
+  task(0, 3) = 1.0;
+  task(0, 4) = 0.0;
+  task(0, 5) = 0.0;
+
+  real T        = 2.0;
+  auto traj     = compute_5order_trajectory(T, task);
+  u32  expected = (u32) std::ceil(T * 1000 + 1);
+  CHECK(traj.t.size == expected);
+  CHECK(traj.pos.rows == 1u);
+  CHECK(traj.pos.cols == expected);
+}
+
+TEST_CASE("compute_5order_trajectory - time array starts at zero, is monotonic", "[Trajectory][Polynomial]") {
+  Matrix task(1, 6);
+  task(0, 0) = 0.0;
+  task(0, 1) = 0.0;
+  task(0, 2) = 0.0;
+  task(0, 3) = 1.0;
+  task(0, 4) = 0.0;
+  task(0, 5) = 0.0;
+
+  auto traj = compute_5order_trajectory(3.0, task);
+  CHECK(traj.t[0] == Approx(0.0));
+  for (u32 i = 1; i < traj.t.size; i++) {
+    CHECK(traj.t[i] > traj.t[i - 1]);
+  }
+}
+
+TEST_CASE("compute_5order_trajectory - trivial motion (start == end, all zero derivatives) stays constant", "[Trajectory][Polynomial]") {
+  Matrix task(1, 6);
+  task(0, 0) = 0.7;
+  task(0, 1) = 0.0;
+  task(0, 2) = 0.0;
+  task(0, 3) = 0.7;
+  task(0, 4) = 0.0;
+  task(0, 5) = 0.0;
+
+  auto traj = compute_5order_trajectory(1.0, task);
+  for (u32 i = 0; i < traj.t.size; i++) {
+    CHECK(std::abs(traj.pos(0, (int) i) - 0.7) < 1e-9);
+    CHECK(std::abs(traj.vel(0, (int) i) - 0.0) < 1e-9);
+    CHECK(std::abs(traj.acc(0, (int) i) - 0.0) < 1e-9);
+  }
+}
+
+TEST_CASE("compute_5order_trajectory - multi-joint boundary conditions", "[Trajectory][Polynomial]") {
+  const int n_joints = 6;
+  Matrix    task(n_joints, 6);
+  Array     p0 = {0.0, -1.57, 0.0, -1.57, 0.0, 0.0};
+  Array     pf = {1.0, -1.0, 0.5, -1.0, 0.5, -0.5};
+
+  for (int j = 0; j < n_joints; j++) {
+    task(j, 0) = p0[j];
+    task(j, 1) = 0.0;
+    task(j, 2) = 0.0;
+    task(j, 3) = pf[j];
+    task(j, 4) = 0.0;
+    task(j, 5) = 0.0;
+  }
+
+  auto      traj = compute_5order_trajectory(2.0, task);
+  const int last = (int) traj.t.size - 1;
+  for (int j = 0; j < n_joints; j++) {
+    CHECK(std::abs(traj.pos(j, 0) - p0[j]) < 1e-6);
+    CHECK(std::abs(traj.pos(j, last) - pf[j]) < 1e-6);
+    CHECK(std::abs(traj.vel(j, 0) - 0.0) < 1e-6);
+    CHECK(std::abs(traj.vel(j, last) - 0.0) < 1e-6);
+  }
+}
diff --git a/tests/world/test_primitives.cpp b/tests/world/test_primitives.cpp
index 4ed8390..9226610 100644
--- a/tests/world/test_primitives.cpp
+++ b/tests/world/test_primitives.cpp
@@ -3,7 +3,6 @@
 
 #include <blast>
 #include "catch2/catch.hpp"
-#include "test_helper/test_functions.hpp"
 
 using namespace blast;