diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt
index ceca0e1..63d45cb 100644
--- a/cpp/CMakeLists.txt
+++ b/cpp/CMakeLists.txt
@@ -3,38 +3,95 @@ cmake_minimum_required(VERSION 3.14)
 project(
   LEAP_HAND_API
   LANGUAGES CXX C
-  VERSION 1.0.0)
+  VERSION 1.0.0
+)
 
-set(PLATEFORM "linux64" CACHE STRING "Plateform to build dynamixel sdk")
-message(STATUS "Please set your plateform with -DPLATEFORM={linux_sbc, linux32, linux64, mac} default: linux64")
+# Platform selection
+set(PLATEFORM "linux64" CACHE STRING "Platform to build dynamixel sdk (linux_sbc, linux32, linux64, mac)")
+message(STATUS "Building for platform: ${PLATEFORM}")
 
+# Dependencies
 find_package(Eigen3 REQUIRED)
 
-## Build dynamixel sdk
+# === Build Dynamixel SDK via Makefile ===
 add_custom_target(
     run_make_dynamixel_sdk ALL
     COMMAND make && sudo make install
     WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/dynamixel_sdk/build/${PLATEFORM}
-    COMMENT "Running dynamixel_sdk Makefile"
+    COMMENT "Building and installing Dynamixel SDK"
 )
 
+# === Library sources ===
 set(SRC_CLIENT
   src/dynamixel_client.cpp
   src/leap_hand_utils.cpp
   src/leap_controller.cpp
 )
 
-## Build dynamixel client library
-add_library(dynamixel_client SHARED ${SRC_CLIENT})
+# === Include directories ===
+set(PUBLIC_HEADERS
+  include/leap_hand_utils/dynamixel_client.h
+  include/leap_hand_utils/leap_hand_utils.h
+  include/leap_hand_utils/leap_controller.h
+)
+
+# === Shared library ===
+add_library(dynamixel_client SHARED ${SRC_CLIENT} ${PUBLIC_HEADERS})
 add_dependencies(dynamixel_client run_make_dynamixel_sdk)
-target_link_libraries(dynamixel_client PUBLIC
-  dxl_x64_cpp
-  Eigen3::Eigen
+
+target_include_directories(dynamixel_client
+  PUBLIC
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
 )
-target_include_directories(dynamixel_client PUBLIC
-  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+
+target_link_libraries(dynamixel_client
+  PUBLIC
+    dxl_x64_cpp
+    Eigen3::Eigen
 )
 
-# Build test_leap_hand
+# === Executable ===
 add_executable(test_leap_hand src/main.cpp)
-target_link_libraries(test_leap_hand PRIVATE dynamixel_client)
\ No newline at end of file
+target_link_libraries(test_leap_hand PRIVATE dynamixel_client)
+
+# === Install targets ===
+include(GNUInstallDirs)
+
+install(
+  TARGETS dynamixel_client
+  EXPORT leap_hand_clientTargets
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)
+
+install(DIRECTORY include/leap_hand_utils DESTINATION include)
+
+# Export the library for use with find_package
+install(EXPORT leap_hand_clientTargets
+  FILE leap_hand_clientTargets.cmake
+  NAMESPACE leap_hand_client::
+  DESTINATION lib/cmake/leap_hand_client
+)
+
+# === Config file for find_package ===
+include(CMakePackageConfigHelpers)
+
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/leap_hand_clientConfigVersion.cmake"
+  VERSION ${PROJECT_VERSION}
+  COMPATIBILITY AnyNewerVersion
+)
+
+configure_package_config_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/cmake/leap_hand_clientConfig.cmake.in"
+  "${CMAKE_CURRENT_BINARY_DIR}/leap_hand_clientConfig.cmake"
+  INSTALL_DESTINATION lib/cmake/leap_hand_client
+)
+
+install(FILES
+  "${CMAKE_CURRENT_BINARY_DIR}/leap_hand_clientConfig.cmake"
+  "${CMAKE_CURRENT_BINARY_DIR}/leap_hand_clientConfigVersion.cmake"
+  DESTINATION lib/cmake/leap_hand_client
+)
\ No newline at end of file
diff --git a/cpp/cmake/leap_hand_clientConfig.cmake.in b/cpp/cmake/leap_hand_clientConfig.cmake.in
new file mode 100644
index 0000000..027a087
--- /dev/null
+++ b/cpp/cmake/leap_hand_clientConfig.cmake.in
@@ -0,0 +1,3 @@
+@PACKAGE_INIT@
+
+include("${CMAKE_CURRENT_LIST_DIR}/leap_hand_clientTargets.cmake")
\ No newline at end of file
diff --git a/cpp/include/leap_hand_utils/leap_controller.h b/cpp/include/leap_hand_utils/leap_controller.h
index 5481d78..37bdff6 100644
--- a/cpp/include/leap_hand_utils/leap_controller.h
+++ b/cpp/include/leap_hand_utils/leap_controller.h
@@ -19,6 +19,8 @@ class LeapController
 public:
     LeapController(const std::string &usb_port = "/dev/ttyUSB0");
 
+    ~LeapController();
+
     /**
      * @brief Connect leap hand
      * 
diff --git a/cpp/readme.md b/cpp/readme.md
index e1a08be..55a5283 100644
--- a/cpp/readme.md
+++ b/cpp/readme.md
@@ -15,6 +15,18 @@ In order to use the LEAP Hand with C++ sdk please read the following information
 
 #### Usage 
 
+To use leap_hand_client in your project please consider adding the following line to your cmake :
+
+```cmake
+find_package(leap_hand_client REQUIRED)
+
+add_library(test SHARED src/test.cpp)
+target_link_libraries(test PUBLIC leap_hand_client::dynamixel_client)
+```
+
+
+#### Test
+
 Please execute the following commands to run the test.
 
 ```bash
@@ -23,4 +35,5 @@ cd ~/LEAP_Hand_API/cpp/build/src
 ```
 
 
+
 Thank you to Albert Paik (apaik458) at the University of Auckland for the first version!
\ No newline at end of file
diff --git a/cpp/src/dynamixel_client.cpp b/cpp/src/dynamixel_client.cpp
index 9f8a7ec..a14cab0 100644
--- a/cpp/src/dynamixel_client.cpp
+++ b/cpp/src/dynamixel_client.cpp
@@ -496,6 +496,7 @@ DynamixelPosVelCurReader::DynamixelPosVelCurReader(DynamixelClient *client,
     , vel_scale {vel_scale}
     , cur_scale {cur_scale}
 {
+    _initialize_data();
 }
 
 std::vector<float> DynamixelPosVelCurReader::_get_scale() {
diff --git a/cpp/src/leap_controller.cpp b/cpp/src/leap_controller.cpp
index d3db36e..72e891c 100644
--- a/cpp/src/leap_controller.cpp
+++ b/cpp/src/leap_controller.cpp
@@ -11,6 +11,11 @@ LeapController::LeapController(const std::string &usb_port) :
 
 }
 
+LeapController::~LeapController()
+{
+  disconnect();
+}
+
 void LeapController::connect()
 {
   dxl_client.connect();
@@ -29,6 +34,7 @@ void LeapController::connect()
 
 void LeapController::disconnect()
 {
+  dxl_client.set_torque_enabled(motors, false);
   dxl_client.disconnect();
 }
 
@@ -67,6 +73,12 @@ double LeapController::getJointPose(int idx)
   return read_pos()[idx];
 }
 
+std::tuple<double, double, double> LeapController::getJointData(int idx)
+{
+  const auto & pvc = read_pos_vel_cur();
+  return {pvc[0](idx, 0) , pvc[1](idx, 0), pvc[2](idx, 0)};
+}
+
 // allegro compatibility
 void LeapController::set_allegro(Eigen::VectorXd pose)
 {
diff --git a/cpp/src/main.cpp b/cpp/src/main.cpp
index 1bb5270..52cf60d 100644
--- a/cpp/src/main.cpp
+++ b/cpp/src/main.cpp
@@ -30,13 +30,17 @@ int main()
     LeapController leap_hand { "/dev/ttyUSB0" };
     leap_hand.connect();
 
+    leap_hand.setJointPose(1, 3.5);
+    leap_hand.setJointPose(2, 3.5);
+
     while (1) {
         if (flag) {
             printf("\nShutting down\n");
             break;
         }
 
-        leap_hand.set_allegro(Eigen::MatrixXd::Zero(16, 1));
-        std::cout << "Position: " << leap_hand.read_pos() << '\n';
+        std::cout << "Position :" << std::get<0>(leap_hand.getJointData(1)) << std::endl;
+        std::cout << "Velocity :" << std::get<1>(leap_hand.getJointData(1)) << std::endl;
+        std::cout << "Current :" << std::get<2>(leap_hand.getJointData(1)) << std::endl << std::endl;
     }
 }