diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp
index c811e473..7aac2ad5 100644
--- a/lbr_fri_ros2/src/interfaces/torque_command.cpp
+++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp
@@ -2,24 +2,30 @@
 
 namespace lbr_fri_ros2 {
 TorqueCommandInterface::TorqueCommandInterface(
-    const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters,
+    const double &joint_position_tau,
+    const CommandGuardParameters &command_guard_parameters,
     const std::string &command_guard_variant)
-    : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {}
+    : BaseCommandInterface(joint_position_tau, command_guard_parameters,
+                           command_guard_variant) {}
 
-void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
-                                                     const_idl_state_t_ref state) {
+void TorqueCommandInterface::buffered_command_to_fri(
+    fri_command_t_ref command, const_idl_state_t_ref state) {
   if (state.client_command_mode != KUKA::FRI::EClientCommandMode::TORQUE) {
-    std::string err = "Expected robot in '" +
-                      EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::TORQUE) +
-                      "' command mode got '" +
-                      EnumMaps::client_command_mode_map(state.client_command_mode) + "'";
+    std::string err =
+        "Expected robot in '" +
+        EnumMaps::client_command_mode_map(
+            KUKA::FRI::EClientCommandMode::TORQUE) +
+        "' command mode got '" +
+        EnumMaps::client_command_mode_map(state.client_command_mode) + "'";
     RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC);
     throw std::runtime_error(err);
   }
-  if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(),
+  if (std::any_of(command_target_.joint_position.cbegin(),
+                  command_target_.joint_position.cend(),
                   [](const double &v) { return std::isnan(v); }) ||
-      std::any_of(command_target_.torque.cbegin(), command_target_.torque.cend(),
+      std::any_of(command_target_.torque.cbegin(),
+                  command_target_.torque.cend(),
                   [](const double &v) { return std::isnan(v); })) {
     this->init_command(state);
   }
@@ -30,18 +36,13 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
     throw std::runtime_error(err);
   }
 
-  // PID
-  // if (!joint_position_pid_.is_initialized()) {
-  //   joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  // }
-  // joint_position_pid_.compute(
-  //     command_target_.joint_position, state.measured_joint_position,
-  //     std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),
-  //     command_.joint_position);
-  
+  // exponential smooth
+  if (!joint_position_filter_.is_initialized()) {
+    joint_position_filter_.initialize(state.sample_time);
+  }
+  joint_position_filter_.compute(command_target_.joint_position,
+                                 command_.joint_position);
   command_.torque = command_target_.torque;
-  command_.joint_position = command_target_.joint_position;
-  // RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), "Command: " << command_.torque.data()[0]<< " " << command_.torque.data()[1]<< " " << command_.torque.data()[2]<< " " << command_.torque.data()[3]<< " " << command_.torque.data()[4]<< " " << command_.torque.data()[5]<< " " << command_.torque.data()[6]);
   // validate
   if (!command_guard_->is_valid_command(command_, state)) {
     std::string err = "Invalid command.";
@@ -53,4 +54,4 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
   command.setJointPosition(command_.joint_position.data());
   command.setTorque(command_.torque.data());
 }
-} // namespace lbr_fri_ros2
+}  // namespace lbr_fri_ros2