diff --git a/src/Libraries/robot_cpp/CMakeLists.txt b/src/Libraries/robot_cpp/CMakeLists.txt
index 91e0193..ed5881b 100644
--- a/src/Libraries/robot_cpp/CMakeLists.txt
+++ b/src/Libraries/robot_cpp/CMakeLists.txt
@@ -31,88 +31,24 @@ if(BUILDING_WITH_CATKIN)
 endif()
 
 ## System dependencies
-# Find yaml-cpp shared library first, prioritizing system installation over /usr/local
-# This avoids using static library from /usr/local/lib
+# Find yaml-cpp library
+find_package(yaml-cpp REQUIRED)
 
-# First, try pkg-config to get the correct library path
-find_package(PkgConfig QUIET)
-if(PkgConfig_FOUND)
-  pkg_check_modules(YAMLCPP_PKG yaml-cpp QUIET)
-  if(YAMLCPP_PKG_FOUND AND YAMLCPP_PKG_LIBRARIES)
-    set(YAML_CPP_TARGET ${YAMLCPP_PKG_LIBRARIES})
-    message(STATUS "Using yaml-cpp from pkg-config: ${YAML_CPP_TARGET}")
-  endif()
-endif()
-
-# If pkg-config didn't work, find shared library directly, prioritizing /usr/lib
-if(NOT YAML_CPP_TARGET)
-  # First try system paths (avoid /usr/local which may have static library)
-  find_library(YAML_CPP_SHARED_LIB
-    NAMES yaml-cpp
-    PATHS /usr/lib
-    PATH_SUFFIXES x86_64-linux-gnu
-    NO_DEFAULT_PATH
-  )
-  
-  if(YAML_CPP_SHARED_LIB AND EXISTS "${YAML_CPP_SHARED_LIB}")
-    # Verify it's actually a shared library (not a symlink to static)
-    get_filename_component(YAML_CPP_REAL_LIB "${YAML_CPP_SHARED_LIB}" REALPATH)
-    if(YAML_CPP_REAL_LIB MATCHES "\\.so")
-      set(YAML_CPP_TARGET ${YAML_CPP_SHARED_LIB})
-      message(STATUS "Found yaml-cpp shared library in /usr/lib: ${YAML_CPP_SHARED_LIB}")
-      # Also find include directory
-      find_path(YAML_CPP_INCLUDE_DIR
-        NAMES yaml-cpp/yaml.h
-        PATHS /usr/include
-        NO_DEFAULT_PATH
-      )
-      if(YAML_CPP_INCLUDE_DIR)
-        message(STATUS "Found yaml-cpp include directory: ${YAML_CPP_INCLUDE_DIR}")
-      endif()
-    endif()
-  endif()
-endif()
-
-# Now use find_package, but temporarily exclude /usr/local to avoid static library
-set(CMAKE_PREFIX_PATH_SAVED ${CMAKE_PREFIX_PATH})
-list(REMOVE_ITEM CMAKE_PREFIX_PATH "/usr/local")
-find_package(yaml-cpp QUIET)
-set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH_SAVED})
-
-# Use target if available and we haven't found a library yet
-if(NOT YAML_CPP_TARGET)
-  if(TARGET yaml-cpp::yaml-cpp)
-    set(YAML_CPP_TARGET yaml-cpp::yaml-cpp)
-    message(STATUS "Using yaml-cpp target: yaml-cpp::yaml-cpp")
-  elseif(TARGET yaml-cpp)
-    set(YAML_CPP_TARGET yaml-cpp)
-    message(STATUS "Using yaml-cpp target: yaml-cpp")
-  else()
-    # Last resort: try to find any library, but check if it's static
-    find_library(YAML_CPP_ANY_LIB
-      NAMES yaml-cpp
-      PATHS /usr/lib /usr/local/lib /lib
-      PATH_SUFFIXES x86_64-linux-gnu
-    )
-    
-    if(YAML_CPP_ANY_LIB)
-      if(YAML_CPP_ANY_LIB MATCHES "\\.a$")
-        message(FATAL_ERROR 
-          "yaml-cpp static library found at: ${YAML_CPP_ANY_LIB}\n"
-          "This cannot be linked into a shared library.\n"
-          "Solution: Remove static library or use system shared library:\n"
-          "  sudo rm /usr/local/lib/libyaml-cpp.a\n"
-          "Or ensure /usr/lib/x86_64-linux-gnu/libyaml-cpp.so is found first."
-        )
-      else()
-        set(YAML_CPP_TARGET ${YAML_CPP_ANY_LIB})
-        message(STATUS "Found yaml-cpp library: ${YAML_CPP_TARGET}")
-      endif()
-    else()
-      set(YAML_CPP_TARGET yaml-cpp)
-      message(WARNING "yaml-cpp library not found. Using library name 'yaml-cpp'")
-    endif()
-  endif()
+# Use target if available (modern CMake)
+if(TARGET yaml-cpp::yaml-cpp)
+  set(YAML_CPP_TARGET yaml-cpp::yaml-cpp)
+  message(STATUS "Using yaml-cpp target: yaml-cpp::yaml-cpp")
+elseif(TARGET yaml-cpp)
+  set(YAML_CPP_TARGET yaml-cpp)
+  message(STATUS "Using yaml-cpp target: yaml-cpp")
+elseif(yaml-cpp_LIBRARIES)
+  # Fallback to library variable if target not available
+  set(YAML_CPP_TARGET ${yaml-cpp_LIBRARIES})
+  message(STATUS "Using yaml-cpp library: ${YAML_CPP_TARGET}")
+else()
+  # Last resort: use library name
+  set(YAML_CPP_TARGET yaml-cpp)
+  message(STATUS "Using yaml-cpp library name: yaml-cpp")
 endif()
 
 find_package(console_bridge REQUIRED)
@@ -179,16 +115,6 @@ target_link_libraries(${PROJECT_NAME}
     dl  # Required for dladdr() function used in plugin_loader_helper.cpp
 )
 
-# Check if we're using static library and warn user
-if(YAML_CPP_TARGET MATCHES "\\.a$" OR YAML_CPP_TARGET MATCHES "libyaml-cpp\\.a")
-  message(FATAL_ERROR 
-    "yaml-cpp static library detected at: ${YAML_CPP_TARGET}\n"
-    "Static libraries cannot be linked into shared libraries without -fPIC.\n"
-    "Solution: Install shared library version:\n"
-    "  sudo apt-get install libyaml-cpp-dev\n"
-    "Or rebuild yaml-cpp with -fPIC flag."
-  )
-endif()
 
 
 ## Add cmake target dependencies