diff --git a/turtlebot3_manipulation_bringup/config/hardware_controller_manager.yaml b/turtlebot3_manipulation_bringup/config/hardware_controller_manager.yaml
index 8890d4a..a296265 100644
--- a/turtlebot3_manipulation_bringup/config/hardware_controller_manager.yaml
+++ b/turtlebot3_manipulation_bringup/config/hardware_controller_manager.yaml
@@ -36,12 +36,12 @@ diff_drive_controller:
     pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
     twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
 
-    open_loop: true
+    open_loop: false
     enable_odom_tf: true
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Preserve turning radius when limiting speed/acceleration/jerk
diff --git a/turtlebot3_manipulation_description/ros2_control/turtlebot3_manipulation_system.ros2_control.xacro b/turtlebot3_manipulation_description/ros2_control/turtlebot3_manipulation_system.ros2_control.xacro
index 1bbd0c5..18b21e1 100644
--- a/turtlebot3_manipulation_description/ros2_control/turtlebot3_manipulation_system.ros2_control.xacro
+++ b/turtlebot3_manipulation_description/ros2_control/turtlebot3_manipulation_system.ros2_control.xacro
@@ -171,10 +171,10 @@ https://github.com/ros-controls/ros2_control_demos/blob/foxy/ros2_control_demo_d
       <joint name="${prefix}gripper_right_joint">
         <param name="mimic">gripper_left_joint</param>
         <param name="multiplier">1</param>
-        <command_interface name="position">
+        <!-- <command_interface name="position">
           <param name="min">-0.010</param>
           <param name="max">0.019</param>
-        </command_interface>
+        </command_interface> -->
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
diff --git a/turtlebot3_manipulation_hardware/include/turtlebot3_manipulation_hardware/turtlebot3_manipulation_system.hpp b/turtlebot3_manipulation_hardware/include/turtlebot3_manipulation_hardware/turtlebot3_manipulation_system.hpp
index af40d78..4240846 100644
--- a/turtlebot3_manipulation_hardware/include/turtlebot3_manipulation_hardware/turtlebot3_manipulation_system.hpp
+++ b/turtlebot3_manipulation_hardware/include/turtlebot3_manipulation_hardware/turtlebot3_manipulation_system.hpp
@@ -92,6 +92,9 @@ class TurtleBot3ManipulationSystemHardware
   std::vector<double> dxl_positions_;
   std::vector<double> dxl_velocities_;
 
+  std::array<double, 2> dxl_wheel_offsets_;
+  bool init_wheel_offsets_;
+
   std::vector<double> opencr_sensor_states_;
 };
 }  // namespace turtlebot3_manipulation_hardware
diff --git a/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp b/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp
index 46f03dc..ede7672 100644
--- a/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp
+++ b/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp
@@ -44,6 +44,7 @@ hardware_interface::CallbackReturn TurtleBot3ManipulationSystemHardware::on_init
   usb_port_ = info_.hardware_parameters["opencr_usb_port"];
   baud_rate_ = stoi(info_.hardware_parameters["opencr_baud_rate"]);
   heartbeat_ = 0;
+  init_wheel_offsets_ = true;
 
   joints_acceleration_[0] = stoi(info_.hardware_parameters["dxl_joints_profile_acceleration"]);
   joints_acceleration_[1] = stoi(info_.hardware_parameters["dxl_joints_profile_acceleration"]);
@@ -225,10 +226,10 @@ hardware_interface::return_type TurtleBot3ManipulationSystemHardware::read(
     RCLCPP_WARN(logger, "Failed to read all control table");
   }
 
-  dxl_positions_[0] = opencr_->get_wheel_positions()[opencr::wheels::LEFT];
+  dxl_positions_[0] = opencr_->get_wheel_positions()[opencr::wheels::LEFT] - dxl_wheel_offsets_[opencr::wheels::LEFT];
   dxl_velocities_[0] = opencr_->get_wheel_velocities()[opencr::wheels::LEFT];
 
-  dxl_positions_[1] = opencr_->get_wheel_positions()[opencr::wheels::RIGHT];
+  dxl_positions_[1] = opencr_->get_wheel_positions()[opencr::wheels::RIGHT] - dxl_wheel_offsets_[opencr::wheels::RIGHT];
   dxl_velocities_[1] = opencr_->get_wheel_velocities()[opencr::wheels::RIGHT];
 
   dxl_positions_[2] = opencr_->get_joint_positions()[opencr::joints::JOINT1];
@@ -287,6 +288,12 @@ hardware_interface::return_type TurtleBot3ManipulationSystemHardware::write(
   if (opencr_->set_gripper_position(dxl_gripper_commands_[0]) == false) {
     RCLCPP_ERROR(logger, "Can't control gripper");
   }
+ 
+  if (init_wheel_offsets_){
+    dxl_wheel_offsets_ = opencr_->get_wheel_positions(); // First time returns all zeros
+    dxl_wheel_offsets_ = opencr_->get_wheel_positions(); // Second time returns the offsets
+    init_wheel_offsets_ = false;
+  }
 
   return hardware_interface::return_type::OK;
 }