diff --git a/CMakeLists.txt b/CMakeLists.txt
index 361435b..00c7d5b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,8 +29,8 @@ find_package(Franka REQUIRED)
 find_package(Threads REQUIRED)
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(ZMQ REQUIRED libzmq)
-# find_package(yaml-cpp REQUIRED)
-
+find_package(yaml-cpp REQUIRED)
+find_package(spdlog REQUIRED)
 # -----------------------------
 # Source collection
 # -----------------------------
@@ -71,4 +71,5 @@ target_link_libraries(proxy
     ${ZMQ_LIBRARIES}
     ${YAML_CPP_LIBRARIES}
     Threads::Threads
+    spdlog::spdlog
 )
diff --git a/P10_franka.yaml b/P10_franka.yaml
new file mode 100644
index 0000000..5bb4e36
--- /dev/null
+++ b/P10_franka.yaml
@@ -0,0 +1,41 @@
+########################
+# communication
+########################
+robot_ip: 172.16.1.2
+service_addr: tcp://127.0.0.1:5555
+state_pub_addr: tcp://127.0.0.1:5556
+gripper_ip: 172.16.1.3
+gripper_service_addr: tcp://127.0.0.1:5557
+gripper_state_pub_addr: tcp://127.0.0.1:5558
+
+
+########################
+# arm
+########################
+# initial joint/matrix state
+arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]
+arm_default_state_O_T_EE: [1.0, 0.0, 0.0, 0.3, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.5, 0.0, 0.0, 0.0, 1.0]
+# rate/timeouts
+arm_state_pub_rate_hz: 100
+arm_socket_timeout_ms: 100
+arm_max_message_size: 4096
+# collision behavior thresholds
+arm_collision_lower_torque_thresholds_acc: [30, 30, 30, 30, 30, 30, 30]
+arm_collision_upper_torque_thresholds_acc: [45, 45, 45, 45, 45, 45, 45]
+arm_collision_lower_torque_thresholds_nom: [25, 25, 25, 25, 25, 25, 25]
+arm_collision_upper_torque_thresholds_nom: [35, 35, 35, 35, 35, 35, 35]
+arm_collision_lower_force_thresholds_acc:  [10, 10, 10, 10, 10, 10]
+arm_collision_upper_force_thresholds_acc:  [15, 15, 15, 15, 15, 15]
+arm_collision_lower_force_thresholds_nom:  [8, 8, 8, 8, 8, 8]
+arm_collision_upper_force_thresholds_nom:  [12, 12, 12, 12, 12, 12]
+
+
+########################
+# gripper
+########################
+gripper_pub_rate_hz: 100
+gripper_command_rcvtimeo_ms: 500
+gripper_default_close_open_threshold: 0.01
+gripper_default_speed_slow: 0.01
+gripper_default_speed_fast: 20.0
+gripper_default_force: 20.0
diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp
index 412b282..b18ccdb 100644
--- a/include/control_mode/abstract_control_mode.hpp
+++ b/include/control_mode/abstract_control_mode.hpp
@@ -6,9 +6,9 @@
 #include <memory>
 #include <mutex>
 #include <zmq.hpp>
-#include <iostream>
 #include <thread>
 #include <zmq.hpp>
+#include "utils/logger.hpp"
 
 #include "utils/atomic_double_buffer.hpp"
 #include "utils/zmq_context.hpp"
@@ -25,34 +25,34 @@ class AbstractControlMode {
     virtual void start() {
         startRobot();
         control_thread_ = std::thread(&AbstractControlMode::controlLoop, this);
-        std::cout << "[" << getModeName() << "] Control thread launched.\n";
+        LOG_INFO("[{}] Control thread launched.", getModeName());
         command_thread_ = std::thread(&AbstractControlMode::commandSubscriptionLoop, this);
-        std::cout << "[" << getModeName() << "] Command subscription thread launched.\n";
+        LOG_INFO("[{}] Command subscription thread launched.", getModeName());
     };
 
     void startRobot() {
 #if !LOCAL_TESTING
         if (!robot_ || !model_) {
-            std::cerr << "[ " << getModeName() << "] Robot or model not set.\n";
+            LOG_ERROR("[{}] Robot or model not set.", getModeName());
             return;
         }
         robot_->automaticErrorRecovery();
 #endif
-        std::cout << "[" << getModeName() << "] Robot control started.\n";
+        LOG_INFO("[{}] Robot control started.", getModeName());
         is_running_ = true;
     };
 
     virtual void stop() {
         is_running_ = false;
         if (control_thread_.joinable()) {
-            std::cout << "[" << getModeName() << "] Stopping control thread...\n";
+            LOG_INFO("[{}] Stopping control thread...", getModeName());
             control_thread_.join();
         }
         if (command_thread_.joinable()) {
-            std::cout << "[" << getModeName() << "] Stopping command subscription thread...\n";
+            LOG_INFO("[{}] Stopping command subscription thread...", getModeName());
             command_thread_.join();
         }
-        std::cout << "[" << getModeName() << "] Stopped.\n";
+        LOG_INFO("[{}] Stopped.", getModeName());
     };
     // Get the mode ID for this control mode
     virtual protocol::ModeID getModeID() const = 0; // Return the mode ID as an integer
@@ -96,7 +96,7 @@ class AbstractControlMode {
         zmq::socket_t sub_socket_(ZmqContext::instance(), ZMQ_SUB);
         sub_socket_.set(zmq::sockopt::rcvtimeo, 200); // 100 ms timeout
         if (command_sub_addr_.empty()) {
-            std::cerr << "[" << getModeName() << "] Command subscription address is empty. Exiting command subscription loop." << std::endl;
+            LOG_WARN("[{}] Command subscription address is empty. Exiting command subscription loop.", getModeName());
             return;
         }
         sub_socket_.connect(command_sub_addr_);
@@ -114,12 +114,12 @@ class AbstractControlMode {
                 };
                 writeCommand(data);
             } catch (const zmq::error_t& e) {
-                std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl;
+                LOG_ERROR("[FrankaProxy] ZMQ recv error: {}", e.what());
                 break;
             }
         }
         sub_socket_.close();
-        std::cout << "[" << getModeName() << "] Command subscription loop exited." << std::endl;
+        LOG_INFO("[{}] Command subscription loop exited.", getModeName());
     };
 
     virtual void writeCommand(const protocol::ByteView& data) = 0;
diff --git a/include/control_mode/control_mode_factory.hpp b/include/control_mode/control_mode_factory.hpp
index fab7da4..3a25fd4 100644
--- a/include/control_mode/control_mode_factory.hpp
+++ b/include/control_mode/control_mode_factory.hpp
@@ -4,13 +4,14 @@
 #include <unordered_map>
 #include <string>
 #include <functional>
+#include "utils/logger.hpp"
 
 
 class ControlModeFactory {
 public:
 
     static void registerMode(const std::string& name, std::function<std::shared_ptr<AbstractControlMode>()> creator) {
-        std::cout << "[ControlModeFactory] Registering mode: " << name << std::endl;
+        LOG_INFO("[ControlModeFactory] Registering mode: {}", name);
         getRegistry()[name] = std::move(creator);
     }
 
diff --git a/include/franka_arm_proxy.hpp b/include/franka_arm_proxy.hpp
index 939863a..965ab55 100644
--- a/include/franka_arm_proxy.hpp
+++ b/include/franka_arm_proxy.hpp
@@ -13,13 +13,14 @@
 #include "control_mode/abstract_control_mode.hpp"
 #include "utils/atomic_double_buffer.hpp"
 #include "utils/service_registry.hpp" 
+#include "utils/franka_config.hpp"
 #include "protocol/codec.hpp"
 
 class FrankaArmProxy {
 
 public:
     // Constructor & Destructor
-    explicit FrankaArmProxy(const std::string& config_path);// Constructor that initializes the proxy with a configuration file
+    explicit FrankaArmProxy(const FrankaConfigData& config);// Constructor that initializes the proxy with a configuration file
     ~FrankaArmProxy();// Destructor to clean up resources
 
     // Core server operations
@@ -76,6 +77,9 @@ class FrankaArmProxy {
     // Current robot state
     AtomicDoubleBuffer<franka::RobotState> current_state_;
 
+    FrankaConfigData config_;
+    franka::RobotState default_state_;
+
     // initialize
     void initializeControlMode();
     void initializeService();
diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp
index cc27019..232b745 100644
--- a/include/franka_gripper_proxy.hpp
+++ b/include/franka_gripper_proxy.hpp
@@ -5,6 +5,7 @@
 #include <mutex>
 #include <string>
 #include <memory>
+#include "utils/logger.hpp"
 
 #include <franka/gripper.h>
 #include <franka/robot_state.h>
@@ -14,7 +15,7 @@
 #include "utils/franka_config.hpp"
 #include "protocol/codec.hpp"
 #include "protocol/grasp_command.hpp"
-
+#include <iostream>
 enum class FrankaGripperFlag {
     STOP = 0,
     STOPPING = 1,
@@ -27,26 +28,34 @@ class FrankaGripperProxy {
 
 public:
     // Constructor & Destructor
-    explicit FrankaGripperProxy(const std::string& config_path):
+    explicit FrankaGripperProxy(const FrankaConfigData& config):
         is_running(false),
         is_on_control_mode(false),
         gripper_flag(FrankaGripperFlag::STOP),
         current_state_(AtomicDoubleBuffer<franka::GripperState>(franka::GripperState{})),
-        command_(AtomicDoubleBuffer<protocol::GraspCommand>(protocol::GraspCommand{}))
+        command_(AtomicDoubleBuffer<protocol::GraspCommand>(protocol::GraspCommand{})),
+        config_(config)
     {
-        FrankaConfig config(config_path);
-        gripper_ip_ = config.getValue("gripper_ip");
-        state_pub_addr_ = config.getValue("gripper_state_pub_addr");
-        service_registry_.bindSocket(config.getValue("gripper_service_addr"));
+        gripper_ip_ = config_.gripper_ip;
+        state_pub_addr_ = config_.gripper_state_pub_addr;
+        service_registry_.bindSocket(config_.gripper_service_addr);
         //initialize franka gripper
 # if !LOCAL_TESTING
         gripper_ = std::make_shared<franka::Gripper>(gripper_ip_);
         gripper_->homing();
         current_state_.write(gripper_->readOnce());
-        command_.write(protocol::GraspCommand{current_state_.read().width, 0.01f, 20.0f});
+        command_.write(protocol::GraspCommand{
+            current_state_.read().width,
+            static_cast<float>(config_.gripper_default_speed_slow),
+            static_cast<float>(config_.gripper_default_force)
+        });
 # else
         current_state_.write(franka::GripperState{0.0f, 0.0f, false, false, franka::Duration{}});
-        command_.write(protocol::GraspCommand{0.0f, 0.01f, 20.0f});
+        command_.write(protocol::GraspCommand{
+            0.0f,
+            static_cast<float>(config_.gripper_default_speed_slow),
+            static_cast<float>(config_.gripper_default_force)
+        });
 # endif
         initializeService();
     };
@@ -57,18 +66,21 @@ class FrankaGripperProxy {
     // Core server operations
     void start() {
         is_running = true;
-        is_moving = false;
-        std::cout << is_running <<"gripper control"<< std::endl;
+        LOG_INFO("Gripper proxy running flag set to {}", is_running.load());
         // state_pub_thread_ = std::thread(&FrankaGripperProxy::statePublishThread, this);
         service_registry_.start();
-        command_.write(protocol::GraspCommand{(float)current_state_.read().width, 20.0f, 20.0f});
+        command_.write(protocol::GraspCommand{
+            (float)current_state_.read().width,
+            static_cast<float>(config_.gripper_default_speed_fast),
+            static_cast<float>(config_.gripper_default_force)
+        });
         state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this);
         check_thread_ = std::thread(&FrankaGripperProxy::checkWidthThread, this);
         control_thread_ = std::thread(&FrankaGripperProxy::controlLoopThread, this);
     };
 
     void stop() {
-        std::cout << "[INFO] Stopping FrankaGripperProxy..." << std::endl;
+        LOG_INFO("Stopping FrankaGripperProxy...");
         is_running = false;
         is_on_control_mode = false;
         if (state_pub_thread_.joinable()) state_pub_thread_.join();
@@ -79,7 +91,7 @@ class FrankaGripperProxy {
         // wait for closing
         service_registry_.stop();
         gripper_.reset();
-        std::cout << "[INFO] FrankaGripperProxy stopped successfully." << std::endl;
+        LOG_INFO("FrankaGripperProxy stopped successfully.");
     };
 
 private:
@@ -115,19 +127,19 @@ class FrankaGripperProxy {
             }
             try
             {
-                if (current_width > target_width + 0.01) {
-                    std::cout << "[Gripper Close] Closing gripper to target width: " << target_width << std::endl;
+                if (current_width > target_width + config_.gripper_default_close_open_threshold) {
+                    LOG_INFO("[Gripper Close] Closing gripper to target width: {}", target_width);
                     gripper_flag.store(FrankaGripperFlag::CLOSING);
                     gripper_->grasp(target_width, command_.read().speed, 60.0);
-                } else if (current_width < target_width - 0.01) {
-                    std::cout << "[Gripper Open] Opening gripper to target width: " << target_width << std::endl;
+                } else if (current_width < target_width - config_.gripper_default_close_open_threshold) {
+                    LOG_INFO("[Gripper Open] Opening gripper to target width: {}", target_width);
                     gripper_flag.store(FrankaGripperFlag::OPENING);
                     gripper_->move(target_width, command_.read().speed);
                 }
             }
             catch(const std::exception& e)
             {
-                std::cerr << e.what() << '\n';
+                LOG_ERROR("{}", e.what());
             }
             gripper_flag.store(FrankaGripperFlag::STOP);
         }
@@ -144,12 +156,13 @@ class FrankaGripperProxy {
                                                 protocol::encode(gs).size()),
                                 zmq::send_flags::none);
 #endif
-            std::this_thread::sleep_for(std::chrono::milliseconds(1000 / GRIPPER_PUB_RATE_HZ));
+            const int rate = config_.gripper_pub_rate_hz > 0 ? config_.gripper_pub_rate_hz : GRIPPER_PUB_RATE_HZ;
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000 / rate));
         }
         try {
             state_pub_socket_.close();
         } catch (const zmq::error_t& e) {
-            std::cerr << "[ZMQ ERROR] " << e.what() << "\n";
+            LOG_ERROR("[ZMQ ERROR] {}", e.what());
         }
     };
 
@@ -160,22 +173,22 @@ class FrankaGripperProxy {
             float target_width = command_.read().width;
             if (
                 gripper_flag.load() == FrankaGripperFlag::STOPPING
-            || (gripper_flag.load() == FrankaGripperFlag::CLOSING && current_width >= target_width + 0.01)
-            || (gripper_flag.load() == FrankaGripperFlag::OPENING && current_width <= target_width - 0.01)
-            || (gripper_flag.load() == FrankaGripperFlag::STOP && std::abs(current_width - target_width) <= 0.01)
+            || (gripper_flag.load() == FrankaGripperFlag::CLOSING && current_width >= target_width + config_.gripper_default_close_open_threshold)
+            || (gripper_flag.load() == FrankaGripperFlag::OPENING && current_width <= target_width - config_.gripper_default_close_open_threshold)
+            || (gripper_flag.load() == FrankaGripperFlag::STOP && std::abs(current_width - target_width) <= config_.gripper_default_close_open_threshold)
             ) {
                 std::this_thread::sleep_for(std::chrono::milliseconds(10));
                 continue;
             }
             try
             {
-                std::cout << "[Gripper Stop] Gripper reached target width: " << target_width << std::endl;
+                LOG_INFO("[Gripper Stop] Stopping gripper at current width: {}", current_width);
                 gripper_flag.store(FrankaGripperFlag::STOP);
                 gripper_->stop();
             }
             catch(const std::exception& e)
             {
-                std::cerr << e.what() << '\n';
+                LOG_ERROR("{}", e.what());
             }
 #endif
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
@@ -185,7 +198,7 @@ class FrankaGripperProxy {
     void commandSubThread(const std::string& command_sub_addr) {
         is_on_control_mode = true;
         zmq::socket_t sub_socket_(ZmqContext::instance(), ZMQ_SUB);
-        sub_socket_.set(zmq::sockopt::rcvtimeo, 500); // 0.5 second timeout
+        sub_socket_.set(zmq::sockopt::rcvtimeo, config_.gripper_command_rcvtimeo_ms); // timeout
         sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages
         sub_socket_.connect(command_sub_addr);
         while (is_running && is_on_control_mode) {
@@ -200,9 +213,8 @@ class FrankaGripperProxy {
             };
             command_.write(protocol::decode<protocol::GraspCommand>(data));
             // std::this_thread::sleep_for(std::chrono::milliseconds(10));
-            // std::cout << "[FrankaGripperProxy] Received new gripper command: width="
-            //           << command_.read().width << ", speed=" << command_.read().speed
-            //           << ", force=" << command_.read().force << std::endl;
+            // spdlog::info("[FrankaGripperProxy] Received new gripper command: width={}, speed={}, force={}",
+            //     command_.read().width, command_.read().speed, command_.read().force);
         }
     };
 
@@ -215,12 +227,13 @@ class FrankaGripperProxy {
     
     AtomicDoubleBuffer<franka::GripperState> current_state_;
     AtomicDoubleBuffer<protocol::GraspCommand> command_;
+    FrankaConfigData config_;
 
     // service registry
     ServiceRegistry service_registry_;
     void startFrankaGripperControl(const std::string& command_sub_addr) {
         if (is_on_control_mode) {
-            std::cout << "[FrankaGripperProxy] Already in control mode, now quit previous control mode." << std::endl;
+            LOG_WARN("[FrankaGripperProxy] Already in control mode, stopping previous command subscriber.");
             is_on_control_mode = false;
             if (command_sub_thread_.joinable()) command_sub_thread_.join();
         }
diff --git a/include/utils/franka_config.hpp b/include/utils/franka_config.hpp
index d868ae3..437f9bd 100644
--- a/include/utils/franka_config.hpp
+++ b/include/utils/franka_config.hpp
@@ -1,65 +1,184 @@
-#include <iostream>
-#include <fstream>
+#pragma once
+#include <algorithm>
+#include <array>
 #include <string>
-#include <map>
+#include <vector>
+#include <yaml-cpp/yaml.h>
+#include "utils/logger.hpp"
+
+struct FrankaConfigData {
+    // communication
+    std::string robot_ip;
+    std::string state_pub_addr;
+    std::string service_addr;
+    std::string gripper_ip;
+    std::string gripper_state_pub_addr;
+    std::string gripper_service_addr;
+
+    // arm
+    std::vector<double> arm_default_state_q;
+    std::array<double, 16> arm_default_state_O_T_EE{};
+    int arm_state_pub_rate_hz{100};
+    int arm_socket_timeout_ms{100};
+    int arm_max_message_size{4096};
+    // collision behavior thresholds
+    std::array<double, 7> arm_col_lower_torque_acc{};
+    std::array<double, 7> arm_col_upper_torque_acc{};
+    std::array<double, 7> arm_col_lower_torque_nom{};
+    std::array<double, 7> arm_col_upper_torque_nom{};
+    std::array<double, 6> arm_col_lower_force_acc{};
+    std::array<double, 6> arm_col_upper_force_acc{};
+    std::array<double, 6> arm_col_lower_force_nom{};
+    std::array<double, 6> arm_col_upper_force_nom{};
+
+    // gripper
+    int gripper_pub_rate_hz{100};
+    int gripper_command_rcvtimeo_ms{500};
+    double gripper_default_close_open_threshold{0.01};
+    double gripper_default_speed_slow{0.01};
+    double gripper_default_speed_fast{20.0};
+    double gripper_default_force{20.0};
+};
 
 class FrankaConfig {
 public:
-    std::map<std::string, std::string> config_data;
-
-    // Constructor to initialize and load the configuration from a file
-    FrankaConfig(const std::string& file_path) {
+    FrankaConfig() = default;
+    explicit FrankaConfig(const std::string& file_path) {
         loadFromFile(file_path);
     }
 
-    // Function to load configuration from a file
     void loadFromFile(const std::string& file_path) {
-        std::ifstream file(file_path);
-        if (!file.is_open()) {
-            std::cerr << "Error: Could not open config file: " << file_path << std::endl;
-            return;
+        try {
+            YAML::Node node = YAML::LoadFile(file_path);
+            parse(node);
+        } catch (const YAML::BadFile& e) {
+            LOG_ERROR("Error: Could not open config file: {} ({})", file_path, e.what());
+        } catch (const std::exception& e) {
+            LOG_ERROR("Error: Failed to parse config: {}", e.what());
         }
+    }
 
-        std::string line;
-        while (std::getline(file, line)) {
-            size_t delimiter_pos = line.find(':');
-            if (delimiter_pos != std::string::npos) {
-                std::string key = trim(line.substr(0, delimiter_pos));
-                std::string value = trim(line.substr(delimiter_pos + 1));
-                config_data[key] = value;
-            }
+    const FrankaConfigData& data() const { return data_; }
+
+private:
+    template <typename T>
+    static T getOr(const YAML::Node& node, const std::string& key, const T& fallback) {
+        if (!node[key]) {
+            return fallback;
+        }
+        try {
+            return node[key].as<T>();
+        } catch (const std::exception& e) {
+            LOG_WARN("Config parse warning for key {}: {}", key, e.what());
+            return fallback;
         }
-        file.close();
     }
 
-    // Function to get a value as a string
-
-    std::string getValue(const std::string& key, const std::string& default_value = "") const {
-        auto it = config_data.find(key);
-        if (it != config_data.end()) {
-            return it->second;
-        } else {
-        std::cerr << "Warning: Key not found in config: " << key << ", using default: " << default_value << std::endl;
-        return default_value;
+    static std::vector<double> getVecOrEmpty(const YAML::Node& node, const std::string& key) {
+        if (!node[key] || !node[key].IsSequence()) {
+            return {};
+        }
+        try {
+            return node[key].as<std::vector<double>>();
+        } catch (const std::exception& e) {
+            LOG_WARN("Config parse warning for key {}: {}", key, e.what());
+            return {};
         }
-    }   
+    }
 
+    static std::array<double, 16> getArray16OrDefault(const YAML::Node& node, const std::string& key, const std::array<double, 16>& fallback) {
+        if (!node[key] || !node[key].IsSequence()) {
+            return fallback;
+        }
+        try {
+            auto vec = node[key].as<std::vector<double>>();
+            if (vec.size() != 16) {
+                LOG_WARN("Config key {} expected 16 values, got {}. Using fallback.", key, vec.size());
+                return fallback;
+            }
+            std::array<double, 16> arr{};
+            std::copy(vec.begin(), vec.end(), arr.begin());
+            return arr;
+        } catch (const std::exception& e) {
+            LOG_WARN("Config parse warning for key {}: {}", key, e.what());
+            return fallback;
+        }
+    }
 
-    // Function to display the loaded configuration
-    void display() const {
-        for (const auto& pair : config_data) {
-            std::cout << pair.first << ": " << pair.second << std::endl;
+    template <size_t N>
+    static std::array<double, N> getArrayNOrDefault(const YAML::Node& node, const std::string& key, const std::array<double, N>& fallback) {
+        if (!node[key] || !node[key].IsSequence()) {
+            return fallback;
+        }
+        try {
+            auto vec = node[key].as<std::vector<double>>();
+            if (vec.size() != N) {
+                LOG_WARN("Config key {} expected {} values, got {}. Using fallback.", key, N, vec.size());
+                return fallback;
+            }
+            std::array<double, N> arr{};
+            std::copy(vec.begin(), vec.end(), arr.begin());
+            return arr;
+        } catch (const std::exception& e) {
+            LOG_WARN("Config parse warning for key {}: {}", key, e.what());
+            return fallback;
         }
     }
 
-private:
-    // Function to trim whitespace from a string
-    std::string trim(const std::string& str) const {
-        size_t first = str.find_first_not_of(" \t");
-        size_t last = str.find_last_not_of(" \t");
-        if (first == std::string::npos || last == std::string::npos) {
-            return "";
+    void parse(const YAML::Node& node) {
+        // defaults for arrays
+        const std::array<double, 16> default_O_T_EE{{
+            1.0, 0.0, 0.0, 0.3,
+            0.0, 1.0, 0.0, 0.0,
+            0.0, 0.0, 1.0, 0.5,
+            0.0, 0.0, 0.0, 1.0
+        }};
+        const std::vector<double> default_q{{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}};
+
+        // communication
+        data_.robot_ip = getOr<std::string>(node, "robot_ip", "");
+        data_.state_pub_addr = getOr<std::string>(node, "state_pub_addr", "");
+        data_.service_addr = getOr<std::string>(node, "service_addr", "");
+        data_.gripper_ip = getOr<std::string>(node, "gripper_ip", "");
+        data_.gripper_state_pub_addr = getOr<std::string>(node, "gripper_state_pub_addr", "");
+        data_.gripper_service_addr = getOr<std::string>(node, "gripper_service_addr", "");
+
+        // arm
+        data_.arm_default_state_q = getVecOrEmpty(node, "arm_default_state_q");
+        if (data_.arm_default_state_q.empty()) {
+            data_.arm_default_state_q = default_q;
         }
-        return str.substr(first, last - first + 1);
+        data_.arm_default_state_O_T_EE = getArray16OrDefault(node, "arm_default_state_O_T_EE", default_O_T_EE);
+        data_.arm_state_pub_rate_hz = getOr<int>(node, "arm_state_pub_rate_hz", 100);
+        data_.arm_socket_timeout_ms = getOr<int>(node, "arm_socket_timeout_ms", 100);
+        data_.arm_max_message_size = getOr<int>(node, "arm_max_message_size", 4096);
+        // collision behavior (defaults simple non-zero thresholds to avoid 0)
+        const std::array<double, 7> d7_acc_low{{30,30,30,30,30,30,30}};
+        const std::array<double, 7> d7_acc_up {{45,45,45,45,45,45,45}};
+        const std::array<double, 7> d7_nom_low{{25,25,25,25,25,25,25}};
+        const std::array<double, 7> d7_nom_up {{35,35,35,35,35,35,35}};
+        const std::array<double, 6> d6_acc_low{{10,10,10,10,10,10}};
+        const std::array<double, 6> d6_acc_up {{15,15,15,15,15,15}};
+        const std::array<double, 6> d6_nom_low{{8,8,8,8,8,8}};
+        const std::array<double, 6> d6_nom_up {{12,12,12,12,12,12}};
+        data_.arm_col_lower_torque_acc = getArrayNOrDefault<7>(node, "arm_collision_lower_torque_thresholds_acc", d7_acc_low);
+        data_.arm_col_upper_torque_acc = getArrayNOrDefault<7>(node, "arm_collision_upper_torque_thresholds_acc", d7_acc_up);
+        data_.arm_col_lower_torque_nom = getArrayNOrDefault<7>(node, "arm_collision_lower_torque_thresholds_nom", d7_nom_low);
+        data_.arm_col_upper_torque_nom = getArrayNOrDefault<7>(node, "arm_collision_upper_torque_thresholds_nom", d7_nom_up);
+        data_.arm_col_lower_force_acc  = getArrayNOrDefault<6>(node, "arm_collision_lower_force_thresholds_acc", d6_acc_low);
+        data_.arm_col_upper_force_acc  = getArrayNOrDefault<6>(node, "arm_collision_upper_force_thresholds_acc", d6_acc_up);
+        data_.arm_col_lower_force_nom  = getArrayNOrDefault<6>(node, "arm_collision_lower_force_thresholds_nom", d6_nom_low);
+        data_.arm_col_upper_force_nom  = getArrayNOrDefault<6>(node, "arm_collision_upper_force_thresholds_nom", d6_nom_up);
+
+        // gripper
+        data_.gripper_pub_rate_hz = getOr<int>(node, "gripper_pub_rate_hz", 100);
+        data_.gripper_command_rcvtimeo_ms = getOr<int>(node, "gripper_command_rcvtimeo_ms", 500);
+        data_.gripper_default_close_open_threshold = getOr<double>(node, "gripper_default_close_open_threshold", 0.01);
+        data_.gripper_default_speed_slow = getOr<double>(node, "gripper_default_speed_slow", 0.01);
+        data_.gripper_default_speed_fast = getOr<double>(node, "gripper_default_speed_fast", 20.0);
+        data_.gripper_default_force = getOr<double>(node, "gripper_default_force", 20.0);
     }
+
+private:
+    FrankaConfigData data_;
 };
diff --git a/include/utils/logger.hpp b/include/utils/logger.hpp
index 8823c94..e364b54 100644
--- a/include/utils/logger.hpp
+++ b/include/utils/logger.hpp
@@ -1,15 +1,20 @@
 #pragma once
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <mutex>
-#include <chrono>
-#include <iomanip>
+
+#include <memory>
 #include <string>
-#include <thread>
+#include <vector>
+#include <ctime>
+#include <filesystem>
+
+#include <spdlog/spdlog.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+#include <spdlog/sinks/basic_file_sink.h>
+#include <spdlog/async.h>
+#include <spdlog/async_logger.h>
 
 namespace utils {
 
+// Log levels for unified control
 enum class LogLevel {
     TRACE = 0,
     INFO,
@@ -20,112 +25,78 @@ enum class LogLevel {
 
 class Logger {
 public:
-    static Logger& instance() {
-        static Logger instance;
-        return instance;
-    }
-
-    void setLevel(LogLevel level) {
-        level_ = level;
-    }
-
-    void setOutputFile(const std::string& path) {
-        std::lock_guard<std::mutex> lock(mutex_);
-        if (file_.is_open()) file_.close();
-        file_.open(path, std::ios::out | std::ios::app);
-    }
+    // Initialize async logging system.
+    // enable_file_logging = true → console + file
+    // enable_file_logging = false → console only
+    static void init(bool enable_file_logging = false,
+                     const std::string& log_dir = "logs")
+    {
+        // Create async thread pool (1 thread, 8192 queue size)
+        spdlog::init_thread_pool(8192, 1);
+
+        std::vector<spdlog::sink_ptr> sinks;
+
+        // Console sink
+        auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
+        sinks.push_back(console_sink);
+
+        // Optional file sink
+        if (enable_file_logging) {
+            std::filesystem::create_directories(log_dir);
+
+            std::time_t t = std::time(nullptr);
+            char ts[32];
+            std::strftime(ts, sizeof(ts), "%Y%m%d_%H%M%S", std::localtime(&t));
+
+            std::string file_path = log_dir + "/output_" + ts + ".txt";
+
+            auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>(
+                file_path, true
+            );
+            sinks.push_back(file_sink);
+        }
 
-    void enableColor(bool enable = true) {
-        use_color_ = enable;
+        // Async logger using the global thread pool
+        auto async_logger = std::make_shared<spdlog::async_logger>(
+            "proxy_async_logger",
+            sinks.begin(),
+            sinks.end(),
+            spdlog::thread_pool(),
+            spdlog::async_overflow_policy::block
+        );
+
+        // Register and set as default logger
+        spdlog::set_default_logger(async_logger);
+
+        // Log formatting
+        // if want to see thread id:→ [%Y-%m-%d %H:%M:%S.%e] [T%t] [%^%l%$] %v
+        spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v");
     }
 
-    template <typename... Args>
-    void log(LogLevel level, Args&&... args) {
-        if (level < level_) return; // filtered out
-
-        std::ostringstream oss;
-        (oss << ... << args);  // fold expression (C++17)
-
-        std::lock_guard<std::mutex> lock(mutex_);
-        std::string output = format(level, oss.str());
-
-        if (file_.is_open())
-            file_ << output << std::endl;
-        else
-            std::cout << output << std::endl;
+    // Set global logging level
+    static void setLevel(LogLevel lvl) {
+        spdlog::level::level_enum spd_lvl = toSpdLevel(lvl);
+        spdlog::set_level(spd_lvl);
     }
 
-    // Helper macros for convenience
-    template <typename... Args> void trace(Args&&... a) { log(LogLevel::TRACE, std::forward<Args>(a)...); }
-    template <typename... Args> void info(Args&&... a)  { log(LogLevel::INFO,  std::forward<Args>(a)...); }
-    template <typename... Args> void warn(Args&&... a)  { log(LogLevel::WARN,  std::forward<Args>(a)...); }
-    template <typename... Args> void error(Args&&... a) { log(LogLevel::ERROR, std::forward<Args>(a)...); }
-    template <typename... Args> void fatal(Args&&... a) { log(LogLevel::FATAL, std::forward<Args>(a)...); }
-
 private:
-    Logger() = default;
-    ~Logger() { if (file_.is_open()) file_.close(); }
-
-    std::string format(LogLevel level, const std::string& msg) {
-        // Timestamp
-        auto now = std::chrono::system_clock::now();
-        auto t = std::chrono::system_clock::to_time_t(now);
-        std::tm tm;
-    #ifdef _WIN32
-        localtime_s(&tm, &t);
-    #else
-        localtime_r(&t, &tm);
-    #endif
-
-        std::ostringstream oss;
-        oss << std::put_time(&tm, "%H:%M:%S");
-
-        // Thread ID
-        oss << " [" << std::this_thread::get_id() << "] ";
-
-        // Level
-        if (use_color_)
-            oss << levelColor(level) << levelName(level) << "\033[0m";
-        else
-            oss << levelName(level);
-
-        oss << " : " << msg;
-        return oss.str();
-    }
-
-    std::string levelName(LogLevel level) const {
-        switch (level) {
-            case LogLevel::TRACE: return "TRACE";
-            case LogLevel::INFO:  return "INFO ";
-            case LogLevel::WARN:  return "WARN ";
-            case LogLevel::ERROR: return "ERROR";
-            case LogLevel::FATAL: return "FATAL";
+    static spdlog::level::level_enum toSpdLevel(LogLevel lvl) {
+        switch (lvl) {
+            case LogLevel::TRACE: return spdlog::level::trace;
+            case LogLevel::INFO:  return spdlog::level::info;
+            case LogLevel::WARN:  return spdlog::level::warn;
+            case LogLevel::ERROR: return spdlog::level::err;
+            case LogLevel::FATAL: return spdlog::level::critical;
         }
-        return "UNKWN";
+        return spdlog::level::info;
     }
-
-    std::string levelColor(LogLevel level) const {
-        switch (level) {
-            case LogLevel::TRACE: return "\033[37m";  // white
-            case LogLevel::INFO:  return "\033[36m";  // cyan
-            case LogLevel::WARN:  return "\033[33m";  // yellow
-            case LogLevel::ERROR: return "\033[31m";  // red
-            case LogLevel::FATAL: return "\033[41m";  // red background
-        }
-        return "";
-    }
-
-    LogLevel level_ = LogLevel::TRACE;
-    std::ofstream file_;
-    bool use_color_ = true;
-    mutable std::mutex mutex_;
 };
 
 } // namespace utils
 
-// Convenience macros
-#define LOG_TRACE(...) utils::Logger::instance().trace(__VA_ARGS__)
-#define LOG_INFO(...)  utils::Logger::instance().info(__VA_ARGS__)
-#define LOG_WARN(...)  utils::Logger::instance().warn(__VA_ARGS__)
-#define LOG_ERROR(...) utils::Logger::instance().error(__VA_ARGS__)
-#define LOG_FATAL(...) utils::Logger::instance().fatal(__VA_ARGS__)
+// Logging macros using async logger (supports {} format)
+#define LOG_TRACE(...) spdlog::trace(__VA_ARGS__)
+#define LOG_INFO(...)  spdlog::info(__VA_ARGS__)
+#define LOG_WARN(...)  spdlog::warn(__VA_ARGS__)
+#define LOG_ERROR(...) spdlog::error(__VA_ARGS__)
+#define LOG_FATAL(...) spdlog::critical(__VA_ARGS__)
diff --git a/include/utils/service_registry.hpp b/include/utils/service_registry.hpp
index c6d48cc..8712208 100644
--- a/include/utils/service_registry.hpp
+++ b/include/utils/service_registry.hpp
@@ -5,7 +5,7 @@
 #include <vector>
 #include <cstdint>
 #include <stdexcept>
-#include <iostream>
+#include "utils/logger.hpp"
 #include "protocol/codec.hpp"
 #include "protocol/request_result.hpp"
 #include "protocol/request_result.hpp"
@@ -86,7 +86,7 @@ class ServiceRegistry {
 
     void handleRequest(const std::string& service_name, const protocol::ByteView& payload, protocol::FrankaResponse& response) {
         auto it = handlers_.find(service_name);
-        std::cout << "[ServiceRegistry] Handling message of type " << service_name << std::endl;
+        LOG_INFO("[ServiceRegistry] Handling message of type {}", service_name);
         if (it == handlers_.end()) {
             // const std::string err = "Unknown handler";
             // protocol::FrankaResponse rr(protocol::RequestResultCode::FAIL, err);
@@ -101,10 +101,10 @@ class ServiceRegistry {
         try {
             response.payload = it->second(payload);
         } catch (const std::exception& e) {
-            std::cerr << "[ServiceRegistry] Exception while handling " << service_name << " service request: " << e.what() << std::endl;
+            LOG_ERROR("[ServiceRegistry] Exception while handling {} service request: {}", service_name, e.what());
             response.code = protocol::FrankaResponseCode::FAIL;
         }
-        std::cout << "[ServiceRegistry] Found handler for service " << service_name << std::endl;
+        LOG_INFO("[ServiceRegistry] Found handler for service {}", service_name);
     }
 
     void clearHandlers() {
@@ -120,7 +120,7 @@ class ServiceRegistry {
         while (is_running) {
             zmq::message_t service_name_msg;
             if (!res_socket_.recv(service_name_msg, zmq::recv_flags::none)) continue;
-            std::cout << "[FrankaArmProxy] Received service request message of size " << service_name_msg.size() << " bytes." << std::endl;
+            LOG_TRACE("[FrankaArmProxy] Received service request frame of size {} bytes.", service_name_msg.size());
             std::string service_name = protocol::decode<std::string>(protocol::ByteView{
                 static_cast<const uint8_t*>(service_name_msg.data()),
                 service_name_msg.size()
@@ -128,27 +128,27 @@ class ServiceRegistry {
 
 
             if (!service_name_msg.more()) {
-                std::cerr << "[FrankaArmProxy] Warning: No payload frame received for service request." << std::endl;
+                LOG_WARN("[FrankaArmProxy] Warning: No payload frame received for service request.");
                 continue; // Skip this iteration if no payload
             }
             zmq::message_t payload_msg;
             if (!res_socket_.recv(payload_msg, zmq::recv_flags::none)) continue;
-            std::cout << "[FrankaArmProxy] Received payload message of size " << payload_msg.size() << " bytes." << std::endl;
+            LOG_TRACE("[FrankaArmProxy] Received payload frame of size {} bytes.", payload_msg.size());
             protocol::ByteView payload{
                 static_cast<const uint8_t*>(payload_msg.data()),
                 payload_msg.size()
             };
             if (payload_msg.more()) {
-                std::cerr << "[FrankaArmProxy] Warning: More message frames received than expected." << std::endl;
+                LOG_WARN("[FrankaArmProxy] Warning: More message frames received than expected.");
             }
-            std::cout << "[FrankaArmProxy] Received request " << service_name << std::endl;
+            LOG_INFO("[FrankaArmProxy] Received request {}", service_name);
             //std::string response;
             protocol::FrankaResponse response;
             handleRequest(service_name, payload, response);
             //send response
             res_socket_.send(zmq::buffer(service_name), zmq::send_flags::sndmore);
             res_socket_.send(zmq::buffer(response.payload), zmq::send_flags::none);
-            std::cout << "[FrankaArmProxy] Sent response: msg size = " << response.payload.size() << std::endl;
+            LOG_INFO("[FrankaArmProxy] Sent response payload of {} bytes.", response.payload.size());
         }
     }
 
@@ -166,4 +166,4 @@ class ServiceRegistry {
         static constexpr int SOCKET_TIMEOUT_MS = 100;
         std::thread service_thread_;
         std::string service_addr_;
-    };
\ No newline at end of file
+    };
diff --git a/src/control_mode/cartesian_pose_mode.cpp b/src/control_mode/cartesian_pose_mode.cpp
index a5627f2..1e13833 100644
--- a/src/control_mode/cartesian_pose_mode.cpp
+++ b/src/control_mode/cartesian_pose_mode.cpp
@@ -2,66 +2,75 @@
 #include "protocol/mode_id.hpp"
 #include "protocol/codec.hpp"
 #include <franka/exception.h>
-#include <iostream>
-
+#include "utils/logger.hpp"
+#include <unistd.h>
+//Note: Pose is represented as a 4x4 matrix in column-major format.
 CartesianPoseMode::CartesianPoseMode():
     desired_pose_(franka::CartesianPose{
         {1.0, 0.0, 0.0, 0.0,
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 1.0, 0.0,
-         0.0, 0.0, 0.0, 1.0}
+         0.306, 0.0, 0.485, 1.0}
     })
 {};
 CartesianPoseMode::~CartesianPoseMode() = default;
 
 void CartesianPoseMode::controlLoop() {
-    std::cout << "[CartesianPoseMode] Started.\n";
+    LOG_INFO("[CartesianPoseMode] Started.");
     is_running_ = true;
 
     // Initialize desired Cartesian pose to identity (no movement)`
     desired_pose_.write(franka::CartesianPose{
-        {1.0, 0.0, 0.0, 0.0,   // Row 1 (rotation)
-         0.0, 1.0, 0.0, 0.0,   // Row 2 (rotation)
-         0.0, 0.0, 1.0, 0.0,   // Row 3 (rotation)
-         0.0, 0.0, 0.0, 1.0}   // Row 4 (translation, homogeneous)
+        {1.0, 0.0, 0.0, 0.0,   // col 1 (rotationx 0)
+         0.0, 1.0, 0.0, 0.0,   // col 2 (rotationy 0)
+         0.0, 0.0, 1.0, 0.0,   // col 3 (rotationz 0)
+         0.306, 0.0, 0.485}   // col 4 (translation 1)
     });
 
     if (!robot_ || !model_) {
-        std::cerr << "[CartesianPoseMode] Robot or model not set.\n";
+        LOG_ERROR("[CartesianPoseMode] Robot or model not set.");
         return;
     }
 
     robot_->automaticErrorRecovery();
 
-    std::function<franka::CartesianPose(const franka::RobotState&, franka::Duration)> callback =
+    std::function<franka::CartesianPose(const franka::RobotState&, franka::Duration)> motion_generator_callback =
         [this](const franka::RobotState& state, franka::Duration) -> franka::CartesianPose {
-            if (!is_running_) {
-                throw franka::ControlException("CartesianPoseMode stopped.");
-            }
-
+            // if (!is_running_) {
+            //     throw franka::ControlException("CartesianPoseMode stopped.");
+            // }
             updateRobotState(state);
             auto desired = desired_pose_.read();
-
             if (!is_running_) {
                 return franka::MotionFinished(desired);
             }
 
             return desired;
         };
+    bool is_robot_operational = true;
+    while (is_running_ && is_robot_operational) {
+        try {
+            robot_->control(motion_generator_callback);
+    } catch (const std::exception &ex) {
+        LOG_ERROR("[CartesianPoseMode] Robot is unable to be controlled: {}", ex.what());
+        is_robot_operational = false;
+    }
+    for (int i = 0; i < 3; i++) {
+        LOG_WARN("[CartesianPoseMode] Waiting {} seconds before recovery attempt...", 3);
 
-    try {
-        robot_->control(callback);
-    } catch (const franka::ControlException& e) {
-        std::cerr << "[CartesianPoseMode] Exception: " << e.what() << std::endl;
-        if (std::string(e.what()).find("reflex") != std::string::npos) {
-            std::cout << "Reflex detected, attempting automatic recovery...\n";
-            try {
-                robot_->automaticErrorRecovery();
-            } catch (const franka::Exception& recovery_error) {
-                std::cerr << "Recovery failed: " << recovery_error.what() << std::endl;
+        // Wait
+        usleep(1000 * 3);
+        // Attempt recovery
+        try {
+            robot_->automaticErrorRecovery();
+            LOG_INFO("[CartesianPoseMode] Robot operation recovered.");
+            is_robot_operational = true;
+            break;
+            } catch (const std::exception &ex) {
+                LOG_ERROR("[CartesianPoseMode] Recovery failed: {}", ex.what());
             }
         }
-        std::cout << "[CartesianPoseMode] Exited.\n";
+    
     }
 }
 
@@ -77,4 +86,4 @@ void CartesianPoseMode::writeCommand(const protocol::ByteView& data) {
 void CartesianPoseMode::writeZeroCommand() {
     franka::CartesianPose current_pose = current_state_->read().O_T_EE;
     desired_pose_.write(current_pose);
-}
\ No newline at end of file
+}
diff --git a/src/control_mode/cartesian_velocity_mode.cpp b/src/control_mode/cartesian_velocity_mode.cpp
index c7dc1ed..2e84013 100644
--- a/src/control_mode/cartesian_velocity_mode.cpp
+++ b/src/control_mode/cartesian_velocity_mode.cpp
@@ -3,7 +3,7 @@
 #include "protocol/codec.hpp"
 #include <franka/exception.h>
 #include <franka/control_types.h>
-#include <iostream>
+#include "utils/logger.hpp"
 #include <unistd.h>
 
 CartesianVelocityMode::CartesianVelocityMode():
@@ -12,13 +12,13 @@ CartesianVelocityMode::CartesianVelocityMode():
 CartesianVelocityMode::~CartesianVelocityMode() = default;
 
 void CartesianVelocityMode::controlLoop() {
-    std::cout << "[CartesianVelocityMode] Started.\n";
+    LOG_INFO("[CartesianVelocityMode] Started.");
     is_running_ = true;
 
     desired_velocities_.write(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
 
     if (!robot_ || !model_) {
-        std::cerr << "[CartesianVelocityMode] Robot or model not set.\n";
+        LOG_ERROR("[CartesianVelocityMode] Robot or model not set.");
         return;
     }
 
@@ -62,12 +62,11 @@ void CartesianVelocityMode::controlLoop() {
             // robot_->control(impedance_control_callback, motion_generator_callback, true, 1);
             robot_->control(motion_generator_callback, franka::ControllerMode::kCartesianImpedance, true, 1);
         } catch (const std::exception &ex) {
-            std::cout << "Robot is unable to be controlled: " << ex.what() << std::endl;
+            LOG_ERROR("[CartesianVelocityMode] Robot is unable to be controlled: {}", ex.what());
             is_robot_operational = false;
         }
         for (int i = 0; i < 3; i++) {
-            std::cout << "[CartesianVelocityMode] Waiting " << 3
-                        << " seconds before recovery attempt...\n";
+            LOG_WARN("[CartesianVelocityMode] Waiting {} seconds before recovery attempt...", 3);
 
             // Wait
             usleep(1000 * 3);
@@ -75,11 +74,11 @@ void CartesianVelocityMode::controlLoop() {
             // Attempt recovery
             try {
                 robot_->automaticErrorRecovery();
-                std::cout << "[CartesianVelocityMode] Robot operation recovered.\n";
+                LOG_INFO("[CartesianVelocityMode] Robot operation recovered.");
                 is_robot_operational = true;
                 break;
             } catch (const std::exception &ex) {
-                std::cout << "[CartesianVelocityMode] Recovery failed: " << ex.what() << std::endl;
+                LOG_ERROR("[CartesianVelocityMode] Recovery failed: {}", ex.what());
             }
         }
     }
@@ -97,4 +96,4 @@ void CartesianVelocityMode::writeCommand(const protocol::ByteView& data) {
 
 void CartesianVelocityMode::writeZeroCommand() {
     desired_velocities_.write(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
-}
\ No newline at end of file
+}
diff --git a/src/control_mode/human_control_mode.cpp b/src/control_mode/human_control_mode.cpp
index d19f96d..f5541b6 100644
--- a/src/control_mode/human_control_mode.cpp
+++ b/src/control_mode/human_control_mode.cpp
@@ -1,16 +1,16 @@
 
 #include "human_control_mode.hpp"
 #include <franka/exception.h>
-#include <iostream>
+#include "utils/logger.hpp"
 
 HumanControlMode::HumanControlMode() = default;
 HumanControlMode::~HumanControlMode() = default;
 
 void HumanControlMode::start() {
     is_running_ = true;
-    std::cout << "[HumanControlMode] Started.\n";
+    LOG_INFO("[HumanControlMode] Started.");
     if (!robot_ || !model_) {
-        std::cerr << "[HumanControlMode] Robot or model not set.\n";
+        LOG_ERROR("[HumanControlMode] Robot or model not set.");
         return;
     }
     robot_->setCollisionBehavior(
@@ -36,16 +36,16 @@ void HumanControlMode::start() {
     try {
         robot_->control(callback);
     } catch (const franka::ControlException& e) {
-            std::cerr << "[HumanControlMode] Exception: " << e.what() << std::endl;
+            LOG_ERROR("[HumanControlMode] Exception: {}", e.what());
         if (std::string(e.what()).find("reflex") != std::string::npos) {
-            std::cout << "Reflex detected, attempting automatic recovery...\n";
+            LOG_WARN("Reflex detected, attempting automatic recovery...");
             try {
                 robot_->automaticErrorRecovery();
             } catch (const franka::Exception& recovery_error) {
-                std::cerr << "Recovery failed: " << recovery_error.what() << std::endl;
+                LOG_ERROR("Recovery failed: {}", recovery_error.what());
             }
         }
-        std::cout << "[HumanControlMode] Exited.\n";
+        LOG_INFO("[HumanControlMode] Exited.");
     }
 }
 
@@ -56,14 +56,14 @@ protocol::ModeID HumanControlMode::getModeID() const {
 
 void HumanControlMode::writeCommand(const protocol::ByteView& data) {
     // HumanControlMode does not process external commands; ignore incoming data.
-    std::cout << "[HumanControlMode] Received command data, but this mode does not accept commands.\n";
+    LOG_WARN("[HumanControlMode] Received command data, but this mode does not accept commands.");
 }
 
 void HumanControlMode::controlLoop() {
     // HumanControlMode control logic is handled in start(); this function is unused.
-    std::cout << "[HumanControlMode] controlLoop() called, but control is managed in start().\n";
+    LOG_WARN("[HumanControlMode] controlLoop() called, but control is managed in start().");
 }
 
 void HumanControlMode::writeZeroCommand() {
     // No action needed for zero command in human control mode
-}
\ No newline at end of file
+}
diff --git a/src/control_mode/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp
index 8915286..609ad0f 100644
--- a/src/control_mode/idle_control_mode.cpp
+++ b/src/control_mode/idle_control_mode.cpp
@@ -1,4 +1,5 @@
 #include "idle_control_mode.hpp"
+#include "utils/logger.hpp"
 
 
 IdleControlMode::IdleControlMode() = default;
@@ -10,11 +11,11 @@ protocol::ModeID IdleControlMode::getModeID() const {
     return protocol::ModeID::IDLE;
 }
 
-void IdleControlMode::start() {
-    startRobot();
-    control_thread_ = std::thread(&IdleControlMode::controlLoop, this);
-    std::cout << "[" << getModeName() << "] Control thread launched.\n";
-}
+    void IdleControlMode::start() {
+        startRobot();
+        control_thread_ = std::thread(&IdleControlMode::controlLoop, this);
+        LOG_INFO("[{}] Control thread launched.", getModeName());
+    }
 
 
 void IdleControlMode::controlLoop() {
@@ -27,7 +28,7 @@ void IdleControlMode::controlLoop() {
     }
 #else
     if (!robot_ || !model_) {
-        std::cerr << "[IdleControlMode] Robot or model not set.\n";
+        LOG_ERROR("[IdleControlMode] Robot or model not set.");
         return;
     }
     while (is_running_) {
@@ -37,10 +38,10 @@ void IdleControlMode::controlLoop() {
                     current_state_->write(state);
                 }
             } catch (const franka::Exception& e) {
-                std::cerr << "[IdleMode] readOnce() failed: " << e.what() << std::endl;
+                LOG_ERROR("[IdleMode] readOnce() failed: {}", e.what());
             }
 }
-    std::cout << "[IdleControlMode] Exited.\n";
+    LOG_INFO("[IdleControlMode] Exited.");
 #endif
 }
 
@@ -51,4 +52,4 @@ void IdleControlMode::writeCommand(const protocol::ByteView& data) {
 
 void IdleControlMode::writeZeroCommand() {
     // No action needed for zero command in idle mode
-}
\ No newline at end of file
+}
diff --git a/src/control_mode/joint_position_mode.cpp b/src/control_mode/joint_position_mode.cpp
index 4ca2db3..7cd0792 100644
--- a/src/control_mode/joint_position_mode.cpp
+++ b/src/control_mode/joint_position_mode.cpp
@@ -1,6 +1,6 @@
 #include "control_mode/joint_position_mode.hpp"
 #include <franka/exception.h>
-#include <iostream>
+#include "utils/logger.hpp"
 #include "protocol/codec.hpp"
 
 JointPositionMode::JointPositionMode():
@@ -9,13 +9,13 @@ JointPositionMode::JointPositionMode():
 JointPositionMode::~JointPositionMode() = default;
 
 void JointPositionMode::controlLoop() {
-    std::cout << "[JointPositionMode] Started.\n";
+    LOG_INFO("[JointPositionMode] Started.");
     is_running_ = true;
 
     desired_positions_.write(franka::JointPositions{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
 
     if (!robot_ || !model_) {
-        std::cerr << "[JointPositionMode] Robot or model not set.\n";
+        LOG_ERROR("[JointPositionMode] Robot or model not set.");
         return;
     }
 
@@ -37,16 +37,16 @@ void JointPositionMode::controlLoop() {
     try {
         robot_->control(callback);
     } catch (const franka::ControlException& e) {
-        std::cerr << "[JointPositionMode] Exception: " << e.what() << std::endl;
+        LOG_ERROR("[JointPositionMode] Exception: {}", e.what());
         if (std::string(e.what()).find("reflex") != std::string::npos) {
-            std::cout << "Reflex detected, attempting automatic recovery...\n";
+            LOG_WARN("Reflex detected, attempting automatic recovery...");
             try {
                 robot_->automaticErrorRecovery();
             } catch (const franka::Exception& recovery_error) {
-                std::cerr << "Recovery failed: " << recovery_error.what() << std::endl;
+                LOG_ERROR("Recovery failed: {}", recovery_error.what());
             }
         }
-        std::cout << "[JointPositionMode] Exited.\n";
+        LOG_INFO("[JointPositionMode] Exited.");
     }
 }
 
@@ -63,4 +63,4 @@ void JointPositionMode::writeCommand(const protocol::ByteView& data) {
 void JointPositionMode::writeZeroCommand() {
     franka::JointPositions current_positions = current_state_->read().q;
     desired_positions_.write(current_positions);
-}
\ No newline at end of file
+}
diff --git a/src/control_mode/joint_velocity_mode.cpp b/src/control_mode/joint_velocity_mode.cpp
index 2a19d2c..567274b 100644
--- a/src/control_mode/joint_velocity_mode.cpp
+++ b/src/control_mode/joint_velocity_mode.cpp
@@ -1,7 +1,7 @@
 #include "joint_velocity_mode.hpp"
 #include <franka/exception.h>
 #include <franka/robot_state.h>
-#include <iostream>
+#include "utils/logger.hpp"
 #include "protocol/codec.hpp"
 
 JointVelocityMode::JointVelocityMode():
@@ -11,13 +11,13 @@ JointVelocityMode::~JointVelocityMode() = default;
 
 
 void JointVelocityMode::controlLoop() {
-    std::cout << "[JointVelocityMode] Started.\n";
+    LOG_INFO("[JointVelocityMode] Started.");
     is_running_ = true;
     // Initialize desired velocities to zero
     desired_velocities_.write(franka::JointVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
 
     if (!robot_ || !model_) {
-        std::cerr << "[JointVelocityMode] Robot or model not set.\n";
+        LOG_ERROR("[JointVelocityMode] Robot or model not set.");
         return;
     }
     robot_->automaticErrorRecovery();
@@ -38,16 +38,16 @@ void JointVelocityMode::controlLoop() {
     try {
         robot_->control(joint_velocity_callback);
     } catch (const franka::ControlException& e) {
-        std::cerr << "[JointVelocityMode] Exception: " << e.what() << std::endl;
+        LOG_ERROR("[JointVelocityMode] Exception: {}", e.what());
         if (std::string(e.what()).find("reflex") != std::string::npos) {
-            std::cout << "Reflex detected, attempting automatic recovery...\n";
+            LOG_WARN("[JointVelocityMode] Reflex detected, attempting automatic recovery...");
             try {
                 robot_->automaticErrorRecovery();
             } catch (const franka::Exception& recovery_error) {
-                std::cerr << "Recovery failed: " << recovery_error.what() << std::endl;
+                LOG_ERROR("Recovery failed: {}", recovery_error.what());
             }
         }
-        std::cout << "[JointVelocityMode] Exited.\n";
+        LOG_INFO("[JointVelocityMode] Exited.");
     }
 }
 
@@ -63,4 +63,4 @@ void JointVelocityMode::writeCommand(const protocol::ByteView& data) {
 
 void JointVelocityMode::writeZeroCommand() {
     desired_velocities_.write(franka::JointVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
-}
\ No newline at end of file
+}
diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp
index 6675ca7..cc2c2f7 100644
--- a/src/franka_arm_proxy.cpp
+++ b/src/franka_arm_proxy.cpp
@@ -1,9 +1,10 @@
-#include <iostream>
+#include <array>
 #include <chrono>
 #include <thread>
 #include <csignal>
 #include <atomic>
 #include <algorithm>
+#include "utils/logger.hpp"
 #include "franka_arm_proxy.hpp"
 #include "protocol/codec.hpp"
 #include "utils/franka_config.hpp"
@@ -18,46 +19,73 @@
 
 static std::atomic<bool> running_flag{true};  // let ctrl-c stop the server
 static void signalHandler(int signum) {
-    std::cout << "\n[INFO] Caught signal " << signum << ", shutting down..." << std::endl;
+    LOG_INFO("Caught signal {}, shutting down...", signum);
     running_flag = false;
 }
-// Todo: may add to config file later
-franka::RobotState default_state = []{
-    franka::RobotState state;
-    state.q = {{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}};
-    state.O_T_EE = {{
-        1.0, 0.0, 0.0, 0.3,
-        0.0, 1.0, 0.0, 0.0,
-        0.0, 0.0, 1.0, 0.5,
-        0.0, 0.0, 0.0, 1.0
-    }};
+namespace {
+franka::RobotState makeDefaultState(const FrankaConfigData& cfg) {
+    franka::RobotState state{};
+    std::array<double, 7> q{};
+    const auto& q_src = cfg.arm_default_state_q.size() == 7
+                        ? cfg.arm_default_state_q
+                        : std::vector<double>{{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}};
+    std::copy(q_src.begin(), q_src.begin() + 7, q.begin());
+    state.q = q;
+
+    std::array<double, 16> pose = cfg.arm_default_state_O_T_EE;
+    if (pose == std::array<double, 16>{}) {
+        pose = {{
+            1.0, 0.0, 0.0, 0.3,
+            0.0, 1.0, 0.0, 0.0,
+            0.0, 0.0, 1.0, 0.5,
+            0.0, 0.0, 0.0, 1.0
+        }};
+    }
+    state.O_T_EE = pose;
     return state;
-}();
+}
+}
 
-FrankaArmProxy::FrankaArmProxy(const std::string& config_path)
+FrankaArmProxy::FrankaArmProxy(const FrankaConfigData& config)
     : state_pub_socket_(ZmqContext::instance(), ZMQ_PUB),//arm state publish socket
       is_running(false),
-      current_state_(AtomicDoubleBuffer<franka::RobotState>(default_state)),
+      current_state_(AtomicDoubleBuffer<franka::RobotState>(makeDefaultState(config))),
+      config_(config),
+      default_state_(makeDefaultState(config)),
       service_registry_()
     {
-    FrankaConfig config(config_path);
-    // type_ = config.getValue("type", "Arm"); // Default to "Arm" if not specified
-    robot_ip_ = config.getValue("robot_ip");
+    robot_ip_ = config_.robot_ip;
     //bind state pub socket
-    state_pub_addr_ = config.getValue("state_pub_addr");
-    std::cout<<"state_pub: "<< state_pub_addr_ <<std::endl;
+    state_pub_addr_ = config_.state_pub_addr;
+    LOG_INFO("State publisher bound to {}", state_pub_addr_);
     state_pub_socket_.bind(state_pub_addr_);
-    service_registry_.bindSocket(config.getValue("service_addr"));
+    service_registry_.bindSocket(config_.service_addr);
     //initialize franka robot
 #if !LOCAL_TESTING
     try
     {
         robot_ = std::make_shared<franka::Robot>(robot_ip_);
         model_ = std::make_shared<franka::Model>(robot_->loadModel());
+        // set collision behavior thresholds from config
+        try {
+            robot_->setCollisionBehavior(
+                config_.arm_col_lower_torque_acc,
+                config_.arm_col_upper_torque_acc,
+                config_.arm_col_lower_torque_nom,
+                config_.arm_col_upper_torque_nom,
+                config_.arm_col_lower_force_acc,
+                config_.arm_col_upper_force_acc,
+                config_.arm_col_lower_force_nom,
+                config_.arm_col_upper_force_nom
+            );
+        } catch (const franka::CommandException& e) {
+            LOG_ERROR("Failed to set collision behavior: {}", e.what());
+            throw;
+        }
     }
     catch(const franka::NetworkException& e)
     {
-        std::cerr << e.what() << '\n';
+        LOG_ERROR("{}", e.what());
         this->stop();
     }
 #endif
@@ -97,22 +125,19 @@ FrankaArmProxy::~FrankaArmProxy() {
 
 bool FrankaArmProxy::start(){
     is_running = true;
-    std::cout << is_running <<"control"<< std::endl;
-    std::cout << robot_<<"robot"<< std::endl;
+    LOG_INFO("Arm proxy running flag set to {}", is_running.load());
+    LOG_INFO("Robot interface initialized: {}", robot_ != nullptr);
 #if LOCAL_TESTING
-    current_state_.write(default_state);
+    current_state_.write(default_state_);
 #else
     // current_state_.write(robot_->readOnce());
 #endif
     state_pub_thread_ = std::thread(&FrankaArmProxy::statePublishThread, this);
-    // std::cout << "done arm state pub"<< std::endl;
-    // service_thread_ = std::thread(&FrankaArmProxy::responseSocketThread, this);
-    // std::cout << "done service"<< std::endl;
     return true;
 }
 
 void FrankaArmProxy::stop() {
-    std::cout << "[INFO] Stopping FrankaArmProxy..." << std::endl;
+    LOG_INFO("Stopping FrankaArmProxy...");
     is_running = false;
     if (current_mode_)
         current_mode_->stop();
@@ -122,7 +147,7 @@ void FrankaArmProxy::stop() {
     try {
         state_pub_socket_.close();
     } catch (const zmq::error_t& e) {
-        std::cerr << "[ZMQ ERROR] " << e.what() << "\n";
+        LOG_ERROR("[ZMQ ERROR] {}", e.what());
     }
     // wait for closing
     service_registry_.stop();
@@ -131,19 +156,19 @@ void FrankaArmProxy::stop() {
     model_.reset();
 #endif
     current_mode_ = nullptr;// reset current control mode
-    std::cout << "[INFO] FrankaArmProxy stopped successfully." << std::endl;
+    LOG_INFO("FrankaArmProxy stopped successfully.");
 }
 
 // Main loop for processing requests, ctrl-c to stop the server
 void FrankaArmProxy::spin() {
     std::signal(SIGINT, signalHandler);  //  Catch Ctrl+C to stop the server
-    std::cout << "[INFO] Entering main spin loop. Press Ctrl+C to exit." << std::endl;
-    std::cout << "running_flag " << running_flag << std::endl;
+    LOG_INFO("Entering main spin loop. Press Ctrl+C to exit.");
+    LOG_INFO("Current running flag: {}", running_flag.load());
     while (running_flag) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
     stop(); 
-    std::cout << "[INFO] Shutdown complete.\n";
+    LOG_INFO("Shutdown complete.");
 }
 
 // publish threads
@@ -155,17 +180,18 @@ void FrankaArmProxy::statePublishThread() {
         const std::vector<uint8_t> payload = protocol::encode(rs);
         // Publish over ZMQ PUB socket
         state_pub_socket_.send(zmq::buffer(payload), zmq::send_flags::none);
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / STATE_PUB_RATE_HZ));
+        const int rate = config_.arm_state_pub_rate_hz > 0 ? config_.arm_state_pub_rate_hz : STATE_PUB_RATE_HZ;
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / rate));
         // std::cout << "[FrankaArmProxy] Published state message, size = " << frame.size() << " bytes." << std::endl;//debug
     }
 }
 
 void FrankaArmProxy::setControlMode(const protocol::FrankaArmControlMode& mode) {
     if (current_mode_.get() != nullptr) {
-        std::cout << "[Info] Stopping previous control mode...\n";
+        LOG_INFO("Stopping previous control mode...");
         current_mode_->stop();  // stopMotion + is_running_ = false
     }
-    std::cout << "[Info] Switching to control mode: " << protocol::toString(mode.id) << " with URL: " << mode.url << std::endl;
+    LOG_INFO("Switching to control mode: {} (command URL: {})", protocol::toString(mode.id), mode.url);
     current_mode_ = ControlModeFactory::create(mode.id);
 #if !LOCAL_TESTING
     current_mode_->setRobot(robot_);
diff --git a/src/main.cpp b/src/main.cpp
index 74ebf1a..207f369 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,18 +1,35 @@
-#include <iostream>
+#include <ctime>
+#include <memory>
+#include <vector>
+#include <spdlog/spdlog.h>
+#include <spdlog/sinks/basic_file_sink.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+#include "utils/logger.hpp"
+#include "utils/franka_config.hpp"
 #include "franka_arm_proxy.hpp"
 #include "franka_gripper_proxy.hpp"
 
 
 int main(int argc, char **argv)
 {
+    //initialize logger
+    utils::Logger::init(false);//true to enable file logging
+    utils::Logger::setLevel(utils::LogLevel::INFO);
+
+
+
+
+    //check configpath argument
     if (argc != 2) {
-        std::cerr << "Please input the config path" << std::endl;
-        return 0;
+        LOG_ERROR("Please provide the config path as the sole argument.");
+        return 1;
     }
-
+    //initialize and start proxies
     std::string config_path = argv[1];
-    FrankaArmProxy robot_proxy(config_path);
-    FrankaGripperProxy gripper_proxy(config_path);
+    FrankaConfig config(config_path);
+    const auto& cfg = config.data();
+    FrankaArmProxy robot_proxy(cfg);
+    FrankaGripperProxy gripper_proxy(cfg);
     robot_proxy.start();
     gripper_proxy.start();
     robot_proxy.spin();