diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index 7f10022..0767700 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -15,4 +15,21 @@ jobs:
       - run: sudo apt-get install -y libpciaccess-dev libomp-dev
       - uses: ros-tooling/action-ros-ci@v0.3
         with:
+          package-name: test_calibration
           target-ros2-distro: humble
+          skip-tests: true
+      - uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          package-name: test_admittance
+          target-ros2-distro: humble
+          skip-tests: true
+      - uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          package-name: test_impedance
+          target-ros2-distro: humble
+          skip-tests: true
+      - uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          package-name: haptic_control
+          target-ros2-distro: humble
+          skip-tests: true
diff --git a/src/haptic_control/include/haptic_control.hpp b/src/haptic_control/include/haptic_control.hpp
index eb22e51..f13e3d3 100644
--- a/src/haptic_control/include/haptic_control.hpp
+++ b/src/haptic_control/include/haptic_control.hpp
@@ -3,8 +3,8 @@
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/LinearMath/Quaternion.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 
 #include <Eigen/Core>
@@ -34,7 +34,7 @@ using std::placeholders::_2;
 using namespace std::chrono_literals;
 
 class HapticControl : public rclcpp::Node {
-public:
+ public:
   HapticControl(const std::string &name = "haptic_control",
                 const std::string &namespace_ = "",
                 const rclcpp::NodeOptions &options =
@@ -53,12 +53,12 @@ class HapticControl : public rclcpp::Node {
       const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg);
   void out_virtuose_statusCB(
       const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg);
-  void call_impedance_service();
+  void callImpedanceService();
   void impedanceThread();
-  void project_target_on_sphere(Eigen::Vector3d &target_position_vec,
-                                double safety_sphere_radius_);
+  void projectTargetOnSphere(Eigen::Vector3d &target_position_vec,
+                             double safety_sphere_radius_);
 
-private:
+ private:
   // ROS2 subscribtions
   rclcpp::Subscription<raptor_api_interfaces::msg::OutVirtuoseStatus>::SharedPtr
       out_virtuose_status_;
@@ -131,4 +131,4 @@ class HapticControl : public rclcpp::Node {
   Eigen::Vector3d x_tilde_old_, x_tilde_new_;
 };
 
-#endif // HAPTIC_CONTROL_HPP
+#endif  // HAPTIC_CONTROL_HPP
diff --git a/src/haptic_control/src/haptic_control.cpp b/src/haptic_control/src/haptic_control.cpp
index 56a1292..324bd67 100644
--- a/src/haptic_control/src/haptic_control.cpp
+++ b/src/haptic_control/src/haptic_control.cpp
@@ -254,7 +254,7 @@ void HapticControl::out_virtuose_statusCB(
   status_button_ = msg->buttons;
 }
 
-void HapticControl::call_impedance_service() {
+void HapticControl::callImpedanceService() {
   received_ee_pose_ = false;
   while (!received_ee_pose_) {
     try {
@@ -444,7 +444,7 @@ void HapticControl::impedanceThread() {
                          target_position_vec.norm(), safety_sphere_radius_);
     if (target_position_vec.norm() > safety_sphere_radius_) {
       // project the target on the safety sphere to prevent singularities
-      project_target_on_sphere(target_position_vec, safety_sphere_radius_);
+      projectTargetOnSphere(target_position_vec, safety_sphere_radius_);
     } else {
       // update the old pose only if the target is within the safety sphere
       x_tilde_old_ = x_tilde_new_;
@@ -485,8 +485,8 @@ void HapticControl::impedanceThread() {
   tf_broadcaster_->sendTransform(target_pose_tf_);
   target_pos_publisher_->publish(target_pose_);
 }
-void HapticControl::project_target_on_sphere(
-    Eigen::Vector3d &target_position_vec, double safety_sphere_radius_) {
+void HapticControl::projectTargetOnSphere(Eigen::Vector3d &target_position_vec,
+                                          double safety_sphere_radius_) {
   target_position_vec =
       target_position_vec.normalized() * safety_sphere_radius_;
 }
@@ -498,7 +498,7 @@ int main(int argc, char **argv) {
 
   RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:");
 
-  node->call_impedance_service();
+  node->callImpedanceService();
   rclcpp::spin(node);
   rclcpp::shutdown();
   return 0;
diff --git a/src/test_impedance/src/test_impedance.cpp b/src/test_impedance/src/test_impedance.cpp
index 7f3c153..aa3566e 100755
--- a/src/test_impedance/src/test_impedance.cpp
+++ b/src/test_impedance/src/test_impedance.cpp
@@ -52,7 +52,7 @@ class TestImpedance : public rclcpp::Node {
         "virtuose_impedance");
 
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:");
-    call_impedance_service();
+    callImpedanceService();
   }
 
  private:
@@ -86,7 +86,7 @@ class TestImpedance : public rclcpp::Node {
     status_button = msg->buttons;
   }
 
-  void call_impedance_service() {
+  void callImpedanceService() {
     // Request impedance mode
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending impedance request");
 
diff --git a/src/vf_control/include/haptic_control_base.hpp b/src/vf_control/include/haptic_control_base.hpp
index ce88733..ce46ff3 100644
--- a/src/vf_control/include/haptic_control_base.hpp
+++ b/src/vf_control/include/haptic_control_base.hpp
@@ -30,7 +30,7 @@
 #include "tf2_ros/transform_broadcaster.h"
 
 #include "utils.hpp"
-#include "vf_enforcer.hpp"
+#include "vf/vf_enforcer.hpp"
 
 class HapticControlBase : public rclcpp::Node {
  public:
@@ -48,15 +48,15 @@ class HapticControlBase : public rclcpp::Node {
   void set_safety_box_length_CB(const rclcpp::Parameter &p);
   void set_safety_box_height_CB(const rclcpp::Parameter &p);
   void update_current_ee_pos();
-  void SetWrenchCB(const geometry_msgs::msg::WrenchStamped target_wrench);
+  void set_wrench(const geometry_msgs::msg::WrenchStamped target_wrench);
   void out_virtuose_pose_CB(
       const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg);
   void out_virtuose_statusCB(
       const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg);
   void call_impedance_service();
-  void impedanceThread();
-  geometry_msgs::msg::TransformStamped getEndEffectorTransform();
-  void InitVFEnforcer();
+  void impedance_thread();
+  void get_ee_trans(geometry_msgs::msg::TransformStamped &trans);
+  void init_vf_enforcer();
   void project_target_on_sphere(Eigen::Vector3d &target_position_vec,
                                 double safety_sphere_radius_);
   geometry_msgs::msg::TransformStamped target_pose_tf_;
diff --git a/src/vf_control/include/system_interface.cpp b/src/vf_control/include/system_interface.cpp
new file mode 100644
index 0000000..8677612
--- /dev/null
+++ b/src/vf_control/include/system_interface.cpp
@@ -0,0 +1,15 @@
+#ifndef __SYSTEM_INTERFACE_HPP__
+#define __SYSTEM_INTERFACE_HPP__
+// this is the class that controls the haptic interface using its API,
+// you can redefine it to adapt the library to your device
+#include "raptor_api_interfaces/msg/in_virtuose_force.hpp"
+#include "raptor_api_interfaces/msg/in_virtuose_pose.hpp"
+#include "raptor_api_interfaces/msg/in_virtuose_speed.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_force.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_physical_pose.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_pose.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_speed.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_status.hpp"
+#include "raptor_api_interfaces/srv/virtuose_impedance.hpp"
+#include "raptor_api_interfaces/srv/virtuose_reset.hpp"
+#endif  // __SYSTEM_INTERFACE_HPP__
\ No newline at end of file
diff --git a/src/vf_control/include/utils.hpp b/src/vf_control/include/utils.hpp
index 3c03c89..0384f21 100644
--- a/src/vf_control/include/utils.hpp
+++ b/src/vf_control/include/utils.hpp
@@ -1,4 +1,4 @@
-geometry_msgs::msg::Point vector3ToPoint(
+geometry_msgs::msg::Point vector3_to_point(
     const geometry_msgs::msg::Vector3& vec) {
   geometry_msgs::msg::Point point;
   point.x = vec.x;
diff --git a/src/vf_control/include/closest_on_triangle.hpp b/src/vf_control/include/vf/closest_on_triangle.hpp
similarity index 99%
rename from src/vf_control/include/closest_on_triangle.hpp
rename to src/vf_control/include/vf/closest_on_triangle.hpp
index d9c3d17..45416e8 100644
--- a/src/vf_control/include/closest_on_triangle.hpp
+++ b/src/vf_control/include/vf/closest_on_triangle.hpp
@@ -3,7 +3,7 @@
 
 #include <Eigen/Dense>
 #include <cmath>
-#include "location.hpp"
+#include "vf/location.hpp"
 
 double round_up(const double value, const int decimal_places) {
   const double multiplier = std::pow(10.0, decimal_places);
diff --git a/src/vf_control/include/json.hpp b/src/vf_control/include/vf/json.hpp
similarity index 100%
rename from src/vf_control/include/json.hpp
rename to src/vf_control/include/vf/json.hpp
diff --git a/src/vf_control/include/location.hpp b/src/vf_control/include/vf/location.hpp
similarity index 91%
rename from src/vf_control/include/location.hpp
rename to src/vf_control/include/vf/location.hpp
index 585e2cd..588db0c 100644
--- a/src/vf_control/include/location.hpp
+++ b/src/vf_control/include/vf/location.hpp
@@ -10,7 +10,7 @@ enum class Location {
   V1V3 = 5,
   V2V3 = 6
 };
-const inline char* LocationToString(Location l) {
+const inline char* location_to_string(Location l) {
   switch (l) {
     case Location::VOID:
       return "VOID";
@@ -32,7 +32,7 @@ const inline char* LocationToString(Location l) {
       return "INVALID";
   }
 };
-inline Location intToLocation(int i) {
+inline Location int_to_location(int i) {
   switch (i) {
     case -1:
       return Location::VOID;
diff --git a/src/vf_control/include/mesh.hpp b/src/vf_control/include/vf/mesh.hpp
similarity index 94%
rename from src/vf_control/include/mesh.hpp
rename to src/vf_control/include/vf/mesh.hpp
index 52bbf2e..dc990fc 100644
--- a/src/vf_control/include/mesh.hpp
+++ b/src/vf_control/include/vf/mesh.hpp
@@ -80,7 +80,7 @@ class Mesh {
       triangle_xfm_inv[i] = triangle_xfm[i].inverse();
     }
   }
-  void extrudeMeshRadially(const Eigen::Vector3d &origin, const double scale) {
+  void extrude_radially(const Eigen::Vector3d &origin, const double scale) {
     (void)origin;
     // find the faces with no neighbors
     std::vector<int> border_faces;
@@ -95,23 +95,26 @@ class Mesh {
 
       if (adjacency_dict.at({i, Location::V1V2}).size() == 0) {
         std::cout << "Extruding face " << i << " along V1V2" << std::endl;
-        extrudeEdgeAlongDirection(faces[i][0], faces[i][1], direction, scale);
+        extrude_edge_along_direction(faces[i][0], faces[i][1], direction,
+                                     scale);
       }
       if (adjacency_dict.at({i, Location::V2V3}).size() == 0) {
         std::cout << "Extruding face " << i << " along V2V3" << std::endl;
-        extrudeEdgeAlongDirection(faces[i][1], faces[i][2], direction, scale);
+        extrude_edge_along_direction(faces[i][1], faces[i][2], direction,
+                                     scale);
       }
       if (adjacency_dict.at({i, Location::V1V3}).size() == 0) {
         std::cout << "Extruding face " << i << " along V1V3" << std::endl;
-        extrudeEdgeAlongDirection(faces[i][0], faces[i][2], direction, scale);
+        extrude_edge_along_direction(faces[i][0], faces[i][2], direction,
+                                     scale);
       }
     }
     std::cout << "Face before extrusion: " << num_faces << std::endl;
     std::cout << "Face after extrusion: " << faces.size() << std::endl;
   }
-  void extrudeEdgeAlongDirection(const int v1, const int v2,
-                                 const Eigen::Vector3d &direction,
-                                 const double scale) {
+  void extrude_edge_along_direction(const int v1, const int v2,
+                                    const Eigen::Vector3d &direction,
+                                    const double scale) {
     // extrude the border faces
     const Eigen::Vector3d v1_ = vertices[v1];
     const Eigen::Vector3d v2_ = vertices[v2];
diff --git a/src/vf_control/include/qp_wrapper.hpp b/src/vf_control/include/vf/qp_wrapper.hpp
similarity index 100%
rename from src/vf_control/include/qp_wrapper.hpp
rename to src/vf_control/include/vf/qp_wrapper.hpp
diff --git a/src/vf_control/include/vf_computation.hpp b/src/vf_control/include/vf/vf_computation.hpp
similarity index 92%
rename from src/vf_control/include/vf_computation.hpp
rename to src/vf_control/include/vf/vf_computation.hpp
index 43aa3a3..9f90a55 100644
--- a/src/vf_control/include/vf_computation.hpp
+++ b/src/vf_control/include/vf/vf_computation.hpp
@@ -6,18 +6,18 @@
 
 namespace compute_vf {
 template <typename Scalar>
-inline bool AlmostEqual(const Eigen::Matrix<Scalar, 3, 1> &vec1,
-                        const Eigen::Matrix<Scalar, 3, 1> &vec2,
-                        Scalar tolerance = static_cast<Scalar>(1e-6)) {
+inline bool almost_equal(const Eigen::Matrix<Scalar, 3, 1> &vec1,
+                         const Eigen::Matrix<Scalar, 3, 1> &vec2,
+                         Scalar tolerance = static_cast<Scalar>(1e-6)) {
   // Calculate the angle between the two vectors
   Scalar dotProduct = vec1.normalized().dot(vec2.normalized());
   Scalar angle = std::acos(dotProduct);
 
   // Calculate the axis difference (check if the vectors are almost parallel)
-  bool axisAlmostEqual =
+  bool axisalmost_equal =
       vec1.normalized().isApprox(vec2.normalized(), tolerance);
 
-  return ((angle <= tolerance) && axisAlmostEqual);
+  return ((angle <= tolerance) && axisalmost_equal);
 }
 
 Eigen::Vector3d enforce_virtual_fixture(
@@ -47,7 +47,7 @@ Eigen::Vector3d enforce_virtual_fixture(
   for (int Ti : T) {
     CP[Ti] = mesh.get_closest_on_triangle(current_position, Ti);
   }
-  // vis.DrawClosestPoints(CP, 0.002);
+  // vis.draw_closest_points(CP, 0.002);
   int iteration = 0;
   for (auto it = T.begin(); it != T.end();) {
     // std::cout << "-----\nIteration: " << iteration++ << std::endl;
@@ -66,11 +66,11 @@ Eigen::Vector3d enforce_virtual_fixture(
       if (cpi_loc == Location::V1 || cpi_loc == Location::V2 ||
           cpi_loc == Location::V3 || cpi_loc == Location::VOID) {
         // Constraint plane is the tangent plane to the sphere
-        if (AlmostEqual(current_position, CPi, eps)) {
+        if (almost_equal(current_position, CPi, eps)) {
           Eigen::Vector3d n = mesh.normals[*it].normalized();
           for (auto j = it + 1; j != T.end();) {
             auto [CPj, cpj_loc] = CP.at(*j);
-            if (AlmostEqual(CPi, CPj, eps) &&
+            if (almost_equal(CPi, CPj, eps) &&
                 mesh.is_locally_concave(*it, *j, cpi_loc)) {
               n += mesh.normals[*j].normalized();
               T.erase(j);
@@ -110,7 +110,7 @@ Eigen::Vector3d enforce_virtual_fixture(
 
           if (neighborIdxList1.size() > 0) {
             neighborIdx1 = neighborIdxList1[0];
-            if (AlmostEqual(CPi, CPn1, eps)) {
+            if (almost_equal(CPi, CPn1, eps)) {
               for (auto j = it + 1; j != T.end();) {
                 if (*j == neighborIdx1) {
                   T.erase(j);
@@ -125,7 +125,7 @@ Eigen::Vector3d enforce_virtual_fixture(
           }
           if (neighborIdxList2.size() > 0) {
             neighborIdx2 = neighborIdxList2[0];
-            if (AlmostEqual(CPi, CPn2, eps)) {
+            if (almost_equal(CPi, CPn2, eps)) {
               for (auto j = it + 1; j != T.end();) {
                 if (*j == neighborIdx2) {
                   T.erase(j);
@@ -157,8 +157,8 @@ Eigen::Vector3d enforce_virtual_fixture(
           }
           auto [CPia, cpia_loc] = CP.at(neighborIdx);
           if (mesh.is_locally_concave(Ti, neighborIdx, cpi_loc)) {
-            if (AlmostEqual(CPia, CPi, eps)) {
-              if (AlmostEqual(current_position, CPi, eps)) {
+            if (almost_equal(CPia, CPi, eps)) {
+              if (almost_equal(current_position, CPi, eps)) {
                 Eigen::Vector3d n =
                     (Ni + mesh.normals[neighborIdx].normalized()).normalized();
                 constraint_planes.push_back({n, CPi});
diff --git a/src/vf_control/include/vf_enforcer.hpp b/src/vf_control/include/vf/vf_enforcer.hpp
similarity index 93%
rename from src/vf_control/include/vf_enforcer.hpp
rename to src/vf_control/include/vf/vf_enforcer.hpp
index acda1b7..de442c2 100644
--- a/src/vf_control/include/vf_enforcer.hpp
+++ b/src/vf_control/include/vf/vf_enforcer.hpp
@@ -50,9 +50,9 @@ class VFEnforcer {
     }
     open3d::io::WriteTriangleMesh(output_mesh_path_, *o3d_mesh);
     if (mesh_type_ == "file") {
-      visualizer_->AddPatientMesh(output_mesh_path_, skin_mesh_path_);
+      visualizer_->add_patient_mesh(output_mesh_path_, skin_mesh_path_);
     } else {
-      visualizer_->AddMesh(output_mesh_path_, 0);
+      visualizer_->add_mesh(output_mesh_path_, 0);
     }
     RCLCPP_INFO_STREAM(node->get_logger(),
                        "Loaded mesh with "
@@ -82,10 +82,10 @@ class VFEnforcer {
     this->x_des_old = x_des;
     this->delta_x_ << 0, 0, 0;
 
-    visualizer_->UpdateScene(constraint_planes_, x_des, x_old_, radius_);
+    visualizer_->update_scene(constraint_planes_, x_des, x_old_, radius_);
   }
 
-  Eigen::Vector3d EnforceVF(Eigen::Vector3d x_des) {
+  Eigen::Vector3d enforce_vf(Eigen::Vector3d x_des) {
     if ((x_des - x_des_old).norm() < 0.0005) {
       // return last delta_x_
       return Eigen::Vector3d(0, 0, 0);
@@ -103,7 +103,7 @@ class VFEnforcer {
     target_pose_vf.pose.position.y = x_new[1];
     target_pose_vf.pose.position.z = x_new[2];
     target_pose_vf.pose.orientation.w = 1.0;
-    visualizer_->UpdateScene(constraint_planes_, x_des, x_new, radius_);
+    visualizer_->update_scene(constraint_planes_, x_des, x_new, radius_);
     return delta_x_;
   }
 
diff --git a/src/vf_control/include/visualization.hpp b/src/vf_control/include/vf/visualization.hpp
similarity index 92%
rename from src/vf_control/include/visualization.hpp
rename to src/vf_control/include/vf/visualization.hpp
index 7c09e0e..7338e88 100644
--- a/src/vf_control/include/visualization.hpp
+++ b/src/vf_control/include/vf/visualization.hpp
@@ -14,7 +14,7 @@ class Visualizer {
   std::string reference_frame_;
   double plane_size_;
 
-  void InitMsg(visualization_msgs::msg::Marker &marker) {
+  void init_msg(visualization_msgs::msg::Marker &marker) {
     marker.header.frame_id = reference_frame_;
     marker.header.stamp = node_->now();
     marker.id = 0;
@@ -33,10 +33,10 @@ class Visualizer {
     marker.color.a = 1.0;
     marker.pose.orientation.w = 1.0;
   }
-  void AddMesh(std::string path, int id) {
+  void add_mesh(std::string path, int id) {
     visualization_msgs::msg::MarkerArray marker_array;
     visualization_msgs::msg::Marker marker;
-    InitMsg(marker);
+    init_msg(marker);
     marker.id = id;
     marker.ns = "mesh";
     marker.mesh_resource = "file://" + path;
@@ -47,8 +47,8 @@ class Visualizer {
       rclcpp::sleep_for(100ms);
     }
   }
-  void AddPatientMesh(std::string output_mesh_path,
-                      std::string skin_mesh_path) {
+  void add_patient_mesh(std::string output_mesh_path,
+                        std::string skin_mesh_path) {
     RCLCPP_INFO_STREAM(node_->get_logger(),
                        "Adding rib case mesh with path: " << output_mesh_path);
     RCLCPP_INFO_STREAM(node_->get_logger(),
@@ -56,7 +56,7 @@ class Visualizer {
 
     visualization_msgs::msg::MarkerArray marker_array;
     visualization_msgs::msg::Marker marker;
-    InitMsg(marker);
+    init_msg(marker);
     marker.ns = "patient";
     // add skin mesh
     marker.id = 0;
@@ -76,12 +76,12 @@ class Visualizer {
     }
     RCLCPP_INFO(node_->get_logger(), "Mesh visualization completed");
   }
-  void DrawClosestPoints(
+  void draw_closest_points(
       std::unordered_map<int, std::pair<Eigen::Vector3d, Location>> &points,
       double radius) {
     visualization_msgs::msg::MarkerArray marker_array;
     visualization_msgs::msg::Marker marker;
-    InitMsg(marker);
+    init_msg(marker);
     marker.ns = "CP";
     marker.action = visualization_msgs::msg::Marker::DELETEALL;
     marker_array.markers.push_back(marker);
@@ -118,10 +118,10 @@ class Visualizer {
     marker_pub_->publish(marker_array);
   }
 
-  void UpdateScene(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>
-                       constraint_planes,
-                   Eigen::Vector3d target, Eigen::Vector3d vf_pose,
-                   double radius) {
+  void update_scene(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>
+                        constraint_planes,
+                    Eigen::Vector3d target, Eigen::Vector3d vf_pose,
+                    double radius) {
     visualization_msgs::msg::MarkerArray marker_array;
     visualization_msgs::msg::Marker marker;
     marker.header.frame_id = reference_frame_;
diff --git a/src/vf_control/launch/vf.launch.py b/src/vf_control/launch/vf.launch.py
index 3db5f42..d8ce068 100644
--- a/src/vf_control/launch/vf.launch.py
+++ b/src/vf_control/launch/vf.launch.py
@@ -6,58 +6,62 @@
 from launch.launch_context import LaunchContext
 from launch_ros.parameter_descriptions import ParameterFile
 from launch.actions import TimerAction, DeclareLaunchArgument
+
 # get package share directory
 from ament_index_python.packages import get_package_share_directory
 from launch.actions import DeclareLaunchArgument
+
 # PythonExpression
 from launch.conditions import LaunchConfigurationEquals
-from launch.actions import LogInfo 
+from launch.actions import LogInfo
 import time
 import sys
 
 
 def generate_launch_description():
-    
+
     ld = LaunchDescription()
 
-    # use_fixtures = DeclareLaunchArgument(
-    #     'use_fixtures',
-    #     default_value='false',
-    #     description='Use this argument to activate the fixtures (true or false)'
-    # )
-        
-    # simulated_delay = DeclareLaunchArgument(
-    #     'simulated_delay',
-    #     default_value=0.0,
-    #     description='Use this argument to simulate a delay in the system (in seconds), use 0.0 for no delay'
-    # )
-    
+    # LOGINFO
+    ld.add_action(
+        DeclareLaunchArgument(
+            "use_fixtures",
+            default_value="false",
+            description="Use this argument to activate the fixtures (true or false)",
+        )
+    )
+    ld.add_action(
+        DeclareLaunchArgument(
+            "delay",
+            default_value="0.0",
+            description="Use this argument to simulate a delay in the system (in seconds), use 0.0 for no delay",
+        )
+    )
+
     # haptic_wrapper
     haptic_wrapper = TimerAction(
-        period = 0.0,
-        actions = [Node(
-        package="haption_raptor_api",
-        executable="raptor_api_wrapper"
-        )]
-
+        period=0.0,
+        actions=[Node(package="haption_raptor_api", executable="raptor_api_wrapper")],
     )
 
-    haptic_parameters_calibration = get_package_share_directory('test_calibration') + "/config/parameters.yaml"
-    print("CALIBRATION FILE: ",haptic_parameters_calibration)
+    haptic_parameters_calibration = (
+        get_package_share_directory("test_calibration") + "/config/parameters.yaml"
+    )
 
     # CALIBRATION NODE
     haptic_calibration_node = TimerAction(
-        period = 0.0,
-        actions = [Node(
-        package="test_calibration",
-        executable="test_calibration",
-        parameters=[ParameterFile(haptic_parameters_calibration)]
-        )]
+        period=0.0,
+        actions=[
+            Node(
+                package="test_calibration",
+                executable="test_calibration",
+                parameters=[ParameterFile(haptic_parameters_calibration)],
+            )
+        ],
     )
 
     # SET RIGHT PATH TO YAML
-    vf_params = get_package_share_directory('vf_control') + "/config/parameters.yaml"
-    print("VF PARAMETERS FILE: ",vf_params)
+    vf_params = get_package_share_directory("vf_control") + "/config/parameters.yaml"
     vf_node = TimerAction(
         period=2.5,
         actions=[
@@ -68,33 +72,32 @@ def generate_launch_description():
                 parameters=[
                     ParameterFile(vf_params),
                     {"use_fixtures": LaunchConfiguration("use_fixtures")},
-                    {"delay": LaunchConfiguration("delay")}
+                    {"delay": LaunchConfiguration("delay")},
+                ],
+                remappings=[
+                    (
+                        "bus0/ft_sensor0/ft_sensor_readings/wrench",
+                        "/force_torque_sensor_broadcaster/wrench",
+                    )
                 ],
-                remappings=[('bus0/ft_sensor0/ft_sensor_readings/wrench', '/force_torque_sensor_broadcaster/wrench')],
-
                 # prefix=["xterm -hold -fa 'Monospace' -fs 14 -e "],
                 # output='screen',
                 # emulate_tty=True,
                 # arguments=[('__log_level:=debug')]
             )
-        ]
+        ],
     )
-    info = LogInfo(msg=LaunchConfiguration("use_fixtures"))
     # load rviz if use_fixtures is true using ifcondition
     rviz = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        output='screen',
-        arguments=['-d', get_package_share_directory('vf_control') + '/rviz/vf.rviz'],
-        condition=LaunchConfigurationEquals('use_fixtures', 'true')
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+        arguments=["-d", get_package_share_directory("vf_control") + "/rviz/vf.rviz"],
+        # condition=LaunchConfigurationEquals("use_fixtures", "true"),
     )
-    
-    ld.add_action(haptic_wrapper)
-    ld.add_action(haptic_calibration_node)
-    ld.add_action(info)
+    # ld.add_action(haptic_wrapper)
+    # ld.add_action(haptic_calibration_node)
     ld.add_action(vf_node)
-    ld.add_action(rviz)
+    # ld.add_action(rviz)
     return ld
-
-
diff --git a/src/vf_control/src/haptic_control_base.cpp b/src/vf_control/src/haptic_control_base.cpp
index acd7fb0..b270df9 100644
--- a/src/vf_control/src/haptic_control_base.cpp
+++ b/src/vf_control/src/haptic_control_base.cpp
@@ -50,7 +50,7 @@ HapticControlBase::HapticControlBase(const std::string &name,
   this->delay_ = this->get_parameter("delay").as_double();
   this->delay_ = std::clamp(this->delay_, 0.0, 10.0);
   if (std::abs(this->delay_) > 1e-6) {
-    RCLCPP_INFO(this->get_logger(), "A Delay of %f seconds is set",
+    RCLCPP_WARN(this->get_logger(), "A Delay of %f seconds is set",
                 this->delay_);
   }
 
@@ -91,7 +91,7 @@ HapticControlBase::HapticControlBase(const std::string &name,
   // create force/wrench subscriber
   ft_subscriber_ = this->create_subscription<geometry_msgs::msg::WrenchStamped>(
       "bus0/ft_sensor0/ft_sensor_readings/wrench", 1,
-      std::bind(&HapticControlBase::SetWrenchCB, this, _1));
+      std::bind(&HapticControlBase::set_wrench, this, _1));
 
   client_ = this->create_client<raptor_api_interfaces::srv::VirtuoseImpedance>(
       "virtuose_impedance");
@@ -146,7 +146,7 @@ HapticControlBase::HapticControlBase(const std::string &name,
   x_tilde_old_ << 0.0, 0.0, 0.0;
 }
 
-void HapticControlBase::InitVFEnforcer() {
+void HapticControlBase::init_vf_enforcer() {
   // Init virtual fixture enforcer
   vf_enforcer_ = std::make_shared<VFEnforcer>(this->shared_from_this(), x_new_);
 }
@@ -180,7 +180,7 @@ void HapticControlBase::set_safety_box_height_CB(const rclcpp::Parameter &p) {
   safety_box_height_ = std::clamp(p.as_double(), 0.0, 2.0);
 }
 
-void HapticControlBase::SetWrenchCB(
+void HapticControlBase::set_wrench(
     const geometry_msgs::msg::WrenchStamped target_wrench) {
   current_wrench_.header.stamp = target_wrench.header.stamp;
   geometry_msgs::msg::WrenchStamped force;
@@ -209,7 +209,7 @@ void HapticControlBase::out_virtuose_pose_CB(
     // Store last pose date
     received_haptic_pose_ = true;
     haptic_starting_position_.pose.position =
-        vector3ToPoint(msg->virtuose_pose.translation);
+        vector3_to_point(msg->virtuose_pose.translation);
     haptic_starting_position_.pose.orientation = msg->virtuose_pose.rotation;
     x_new_ << haptic_starting_position_.pose.position.x,
         haptic_starting_position_.pose.position.y,
@@ -253,28 +253,26 @@ void HapticControlBase::out_virtuose_statusCB(
   status_button_ = msg->buttons;
 }
 
-geometry_msgs::msg::TransformStamped
-HapticControlBase::getEndEffectorTransform() {
-  geometry_msgs::msg::TransformStamped ee_pose;
+void HapticControlBase::get_ee_trans(
+    geometry_msgs::msg::TransformStamped &trans) {
   try {
-    ee_pose = tf_buffer_->lookupTransform(base_link_name_, tool_link_name_,
-                                          tf2::TimePointZero);
+    trans = tf_buffer_->lookupTransform(base_link_name_, tool_link_name_,
+                                        tf2::TimePointZero);
     received_ee_pose_ = true;
   } catch (tf2::TransformException &ex) {
     RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                           "End Effector pose not available: %s", ex.what());
   }
-  return ee_pose;
 }
 
 void HapticControlBase::call_impedance_service() {
   received_ee_pose_ = false;
   geometry_msgs::msg::TransformStamped trans;
   while (!received_ee_pose_) {
-    trans = getEndEffectorTransform();
+    get_ee_trans(trans);
   }
   ee_starting_position.pose.position =
-      vector3ToPoint(trans.transform.translation);
+      vector3_to_point(trans.transform.translation);
   ee_starting_position.pose.orientation = trans.transform.rotation;
   if (enable_safety_sphere_) {
     Eigen::Vector3d ee_start(ee_starting_position.pose.position.x,
@@ -353,12 +351,12 @@ void HapticControlBase::call_impedance_service() {
 
   // Perform impedance loop at 1000 Hz
   impedanceThread_ = this->create_wall_timer(
-      1ms, std::bind(&HapticControlBase::impedanceThread, this));
+      1ms, std::bind(&HapticControlBase::impedance_thread, this));
   RCLCPP_INFO(this->get_logger(), "\033[0;32mImpedance thread started\033[0m");
 }
 
 // This function is called at 1000 Hz
-void HapticControlBase::impedanceThread() {
+void HapticControlBase::impedance_thread() {
   if (!received_haptic_pose_) {
     RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                           "Haptic pose not available");
@@ -492,7 +490,7 @@ void HapticControlBase::impedanceThread() {
     Eigen::Vector3d x_desired(target_pose_.pose.position.x,
                               target_pose_.pose.position.y,
                               target_pose_.pose.position.z);
-    auto delta_x = vf_enforcer_->EnforceVF(x_desired);
+    auto delta_x = vf_enforcer_->enforce_vf(x_desired);
     target_pose_vf_.header.stamp = this->get_clock()->now();
     target_pose_vf_.header.frame_id = base_link_name_;
     target_pose_vf_.header.frame_id = target_pose_.header.frame_id;
@@ -508,11 +506,12 @@ void HapticControlBase::impedanceThread() {
   }
 
   // Publish the current ee pose
-  auto ee_pose = getEndEffectorTransform();
+  geometry_msgs::msg::TransformStamped ee_pose;
+  get_ee_trans(ee_pose);
   geometry_msgs::msg::PoseStamped ee_pose_msg;
   ee_pose_msg.header.stamp = ee_pose.header.stamp;
   ee_pose_msg.header.frame_id = base_link_name_;
-  ee_pose_msg.pose.position = vector3ToPoint(ee_pose.transform.translation);
+  ee_pose_msg.pose.position = vector3_to_point(ee_pose.transform.translation);
   ee_pose_msg.pose.orientation = ee_pose.transform.rotation;
   current_frame_pub_->publish(ee_pose_msg);
 
@@ -544,7 +543,7 @@ int main(int argc, char **argv) {
   auto node = std::make_shared<HapticControlBase>();
   RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:");
   if (node->use_fixtures_) {
-    node->InitVFEnforcer();
+    node->init_vf_enforcer();
   }
   node->call_impedance_service();
   rclcpp::spin(node);
diff --git a/src/vf_control/src/mesh_test.cpp b/src/vf_control/src/mesh_test.cpp
index bc90480..57b8f63 100644
--- a/src/vf_control/src/mesh_test.cpp
+++ b/src/vf_control/src/mesh_test.cpp
@@ -1,11 +1,11 @@
-#include "mesh.hpp"
 
 #include <pwd.h>
 #include <sys/types.h>
 #include <unistd.h>
 
-#include "location.hpp"
 #include "open3d/Open3D.h"
+#include "vf/location.hpp"
+#include "vf/mesh.hpp"
 
 void testFindNearest() {
   auto o3d_mesh = open3d::geometry::TriangleMesh::CreateSphere(0.2, 40);
@@ -36,7 +36,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 1" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 2: Point at vertex P2
@@ -45,7 +46,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 2" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 3: Point at vertex P3
@@ -54,7 +56,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 3" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 4: Point inside triangle
@@ -63,7 +66,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 4" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 5: Point on edge P1P3 but not at vertices
@@ -72,7 +76,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 5" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 6: Point on edge P1P3 but not at vertices
@@ -81,7 +86,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 6" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 7: Point inside triangle
@@ -90,7 +96,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 7" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 7: Point inside triangle
@@ -99,7 +106,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 8" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   point << -0.5, -1.5, 0.0;
@@ -107,7 +115,8 @@ void testClosestOnTriangle() {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 9" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Try with mesh
@@ -129,7 +138,7 @@ void testClosestOnTriangle() {
 
   auto [CPi, loc] = findClosestPointOnTriangle(point, xfm, xfm_inv, V1, V2, V3);
   std::cout << "CPI: \n"
-            << CPi << "\n Loc: " << LocationToString(loc) << std::endl;
+            << CPi << "\n Loc: " << location_to_string(loc) << std::endl;
 }
 void colorMeshFacesNew(open3d::t::geometry::TriangleMesh &mesh_new, int faceIdx,
                        std::vector<int> adjacent_faces) {
@@ -209,8 +218,10 @@ void testAdjacencyList() {
         geometries.push_back(sphere_face);
         std::cout << "Retrieving neighbors for face " << face_idx
                   << " at location "
-                  << LocationToString(static_cast<Location>(loc)) << std::endl;
-        auto neighbors = mesh.adjacency_dict.at({face_idx, intToLocation(loc)});
+                  << location_to_string(static_cast<Location>(loc))
+                  << std::endl;
+        auto neighbors =
+            mesh.adjacency_dict.at({face_idx, int_to_location(loc)});
         std::cout << "Face " << face_idx << " has neighbors: \n";
         geometries.push_back(o3d_mesh);
         for (auto neighbor : neighbors) {
@@ -241,7 +252,7 @@ void testExtrusion() {
   o3d_mesh->ComputeTriangleNormals();
   Mesh mesh(o3d_mesh->vertices_, o3d_mesh->triangles_,
             o3d_mesh->vertex_normals_);
-  mesh.extrudeMeshRadially(Eigen::Vector3d(0, 0, 0), 0.05);
+  mesh.extrude_radially(Eigen::Vector3d(0, 0, 0), 0.05);
   auto o3d_mesh_extruded = std::make_shared<open3d::geometry::TriangleMesh>();
   o3d_mesh_extruded->vertices_ = mesh.vertices;
   o3d_mesh_extruded->triangles_ = mesh.faces;
diff --git a/src/vf_control/src/system_interface.cpp b/src/vf_control/src/system_interface.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/src/vf_control/test/vf_test.cpp b/src/vf_control/test/vf_test.cpp
index d7937bd..6445759 100644
--- a/src/vf_control/test/vf_test.cpp
+++ b/src/vf_control/test/vf_test.cpp
@@ -1,13 +1,12 @@
 #include <gtest/gtest.h>
 
-#include "mesh.hpp"
-
 #include <pwd.h>
 #include <sys/types.h>
 #include <unistd.h>
 
-#include "location.hpp"
 #include "open3d/Open3D.h"
+#include "vf/location.hpp"
+#include "vf/mesh.hpp"
 
 TEST(vf_control, testFindNearest) {
   auto o3d_mesh = open3d::geometry::TriangleMesh::CreateSphere(0.2, 40);
@@ -38,7 +37,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 1" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   EXPECT_EQ(triXfm, Eigen::Matrix4d::Identity());
@@ -49,7 +49,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 2" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 3: Point at vertex P3
@@ -58,7 +59,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 3" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 4: Point inside triangle
@@ -67,7 +69,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 4" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 5: Point on edge P1P3 but not at vertices
@@ -76,7 +79,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 5" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 6: Point on edge P1P3 but not at vertices
@@ -85,7 +89,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 6" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 7: Point inside triangle
@@ -94,7 +99,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 7" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Test Case 7: Point inside triangle
@@ -103,7 +109,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 8" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   point << -0.5, -1.5, 0.0;
@@ -111,7 +118,8 @@ TEST(vf_control, testClosestOnTriangle) {
       findClosestPointOnTriangle(point, triXfm, triXfmInv, P1, P2, P3);
   std::cout << "Test 9" << std::endl
             << closestPoint.first
-            << "\n Loc: " << LocationToString(closestPoint.second) << std::endl;
+            << "\n Loc: " << location_to_string(closestPoint.second)
+            << std::endl;
   std::cout << "-----------------------------------" << std::endl;
 
   // Try with mesh
@@ -133,7 +141,7 @@ TEST(vf_control, testClosestOnTriangle) {
 
   auto [CPi, loc] = findClosestPointOnTriangle(point, xfm, xfm_inv, V1, V2, V3);
   std::cout << "CPI: \n"
-            << CPi << "\n Loc: " << LocationToString(loc) << std::endl;
+            << CPi << "\n Loc: " << location_to_string(loc) << std::endl;
 }
 void colorMeshFacesNew(open3d::t::geometry::TriangleMesh &mesh_new, int faceIdx,
                        std::vector<int> adjacent_faces) {
@@ -213,8 +221,10 @@ void testAdjacencyList() {
         geometries.push_back(sphere_face);
         std::cout << "Retrieving neighbors for face " << face_idx
                   << " at location "
-                  << LocationToString(static_cast<Location>(loc)) << std::endl;
-        auto neighbors = mesh.adjacency_dict.at({face_idx, intToLocation(loc)});
+                  << location_to_string(static_cast<Location>(loc))
+                  << std::endl;
+        auto neighbors =
+            mesh.adjacency_dict.at({face_idx, int_to_location(loc)});
         std::cout << "Face " << face_idx << " has neighbors: \n";
         geometries.push_back(o3d_mesh);
         for (auto neighbor : neighbors) {
@@ -245,7 +255,7 @@ void testExtrusion() {
   o3d_mesh->ComputeTriangleNormals();
   Mesh mesh(o3d_mesh->vertices_, o3d_mesh->triangles_,
             o3d_mesh->vertex_normals_);
-  mesh.extrudeMeshRadially(Eigen::Vector3d(0, 0, 0), 0.05);
+  mesh.extrude_radially(Eigen::Vector3d(0, 0, 0), 0.05);
   auto o3d_mesh_extruded = std::make_shared<open3d::geometry::TriangleMesh>();
   o3d_mesh_extruded->vertices_ = mesh.vertices;
   o3d_mesh_extruded->triangles_ = mesh.faces;