diff --git a/CMakeLists.txt b/CMakeLists.txt
index 19b12bc..40fd0da 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,6 +9,9 @@ find_package(ament_cmake REQUIRED)
 # Common cmake code applied to all moveit packages
 find_package(moveit_common REQUIRED)
 moveit_package()
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
 
 find_package(ament_cmake REQUIRED)
 find_package(Eigen3 REQUIRED)
@@ -24,6 +27,11 @@ find_package(rclcpp REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
 find_package(warehouse_ros REQUIRED)
 find_package(shape_msgs REQUIRED)
+find_package(ros2_control REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
 generate_parameter_library(ktopt_moveit_parameters
                            parameters/ktopt_moveit_parameters.yaml)
@@ -49,6 +57,7 @@ add_library(
   src/ktopt_planning_context.cpp
   # TOPPRA
   src/add_toppra_time_parameterization.cpp
+  src/joint_stiffness_controller.cpp
   # Conversions
   src/conversions.cpp)
 
@@ -59,7 +68,12 @@ ament_target_dependencies(
   moveit_msgs
   pluginlib
   rclcpp
-  shape_msgs)
+  shape_msgs
+  rclcpp
+  controller_interface
+  hardware_interface
+  std_msgs
+  ros2_control)
 target_link_libraries(moveit_drake drake::drake ktopt_moveit_parameters)
 
 # Ensure that the plugin finds libdrake.so at runtime
diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt
index 4efc9b1..cfe6db2 100644
--- a/demo/CMakeLists.txt
+++ b/demo/CMakeLists.txt
@@ -5,6 +5,12 @@ ament_target_dependencies(pipeline_testbench_example
 add_executable(constrained_planning_demo src/constrained_planning_demo.cpp)
 ament_target_dependencies(constrained_planning_demo
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# GUI
+add_executable(stiffness_gui src/control_gui.cpp)
+target_link_libraries(stiffness_gui Qt5::Widgets)
+ament_target_dependencies(stiffness_gui rclcpp std_msgs)
+
+install(TARGETS stiffness_gui DESTINATION lib/${PROJECT_NAME})
 
 install(TARGETS pipeline_testbench_example constrained_planning_demo
         DESTINATION lib/${PROJECT_NAME})
diff --git a/demo/src/control_gui.cpp b/demo/src/control_gui.cpp
new file mode 100644
index 0000000..cf10c11
--- /dev/null
+++ b/demo/src/control_gui.cpp
@@ -0,0 +1,74 @@
+#include <QApplication>
+#include <QPushButton>
+#include <QVBoxLayout>
+#include <QDoubleSpinBox>
+#include <QWidget>
+#include <QLabel>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+
+class StiffnessGUI : public QWidget
+{
+  Q_OBJECT
+
+public:
+  StiffnessGUI(std::shared_ptr<rclcpp::Node> node) : node_(node)
+  {
+    publisher_ = node_->create_publisher<std_msgs::msg::Float64MultiArray>("/joint_stiffness_controller/command", 10);
+
+    auto* layout = new QVBoxLayout(this);
+
+    pos_spin1 = new QDoubleSpinBox();
+    pos_spin2 = new QDoubleSpinBox();
+    stiff_spin1 = new QDoubleSpinBox();
+    stiff_spin2 = new QDoubleSpinBox();
+
+    layout->addWidget(new QLabel("Position Joint 1"));
+    layout->addWidget(pos_spin1);
+    layout->addWidget(new QLabel("Stiffness Joint 1"));
+    layout->addWidget(stiff_spin1);
+    layout->addWidget(new QLabel("Position Joint 2"));
+    layout->addWidget(pos_spin2);
+    layout->addWidget(new QLabel("Stiffness Joint 2"));
+    layout->addWidget(stiff_spin2);
+
+    auto* send_btn = new QPushButton("Send Command");
+    layout->addWidget(send_btn);
+    connect(send_btn, &QPushButton::clicked, this, &StiffnessGUI::send_command);
+  }
+
+private slots:
+  void send_command()
+  {
+    auto msg = std_msgs::msg::Float64MultiArray();
+    msg.data = { pos_spin1->value(), pos_spin2->value(), stiff_spin1->value(), stiff_spin2->value() };
+    publisher_->publish(msg);
+  }
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_;
+
+  QDoubleSpinBox* pos_spin1;
+  QDoubleSpinBox* pos_spin2;
+  QDoubleSpinBox* stiff_spin1;
+  QDoubleSpinBox* stiff_spin2;
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  QApplication app(argc, argv);
+
+  auto node = rclcpp::Node::make_shared("stiffness_gui");
+  StiffnessGUI gui(node);
+  gui.show();
+
+  std::thread spin_thread([&]() { rclcpp::spin(node); });
+
+  int ret = app.exec();
+  rclcpp::shutdown();
+  spin_thread.join();
+  return ret;
+}
+#include "control_gui.moc"
diff --git a/include/stiffness_control/joint_stiffness_controller.hpp b/include/stiffness_control/joint_stiffness_controller.hpp
new file mode 100644
index 0000000..ddf2307
--- /dev/null
+++ b/include/stiffness_control/joint_stiffness_controller.hpp
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <controller_interface/controller_interface.hpp>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/loaned_state_interface.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+
+namespace joint_stiffness_controller
+{
+
+class JointStiffnessController : public controller_interface::ControllerInterface
+{
+public:
+  JointStiffnessController();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+  controller_interface::return_type update(const rclcpp::Time&, const rclcpp::Duration&) override;
+  controller_interface::CallbackReturn on_init() override;
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override;
+
+private:
+  std::vector<hardware_interface::LoanedCommandInterface> position_cmd_handles_;
+  std::vector<hardware_interface::LoanedStateInterface> position_state_handles_;
+
+  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr command_sub_;
+  std::vector<double> desired_positions_;
+  std::vector<double> stiffness_gains_;
+
+  void command_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+};
+
+}  // namespace joint_stiffness_controller
diff --git a/plugin_descriptions.xml b/plugin_descriptions.xml
index 13ed720..501bccd 100644
--- a/plugin_descriptions.xml
+++ b/plugin_descriptions.xml
@@ -1,4 +1,4 @@
-<library  path="moveit_drake">
+<library path="moveit_drake">
   <!-- KTOPT adapter -->
   <class name="ktopt_interface/KTOptPlanner"
          type="ktopt_interface::KTOptPlannerManager"
@@ -13,4 +13,10 @@
       Given a path, add time parameterization with TOPRRA.
     </description>
   </class>
+  <!-- Joint Stiffness Controller -->
+  <class name="moveit_drake/JointStiffnessController" type="moveit_drake::JointStiffnessController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      A controller for joint stiffness control.
+    </description>
+  </class>
 </library>
diff --git a/src/joint_stiffness_controller.cpp b/src/joint_stiffness_controller.cpp
new file mode 100644
index 0000000..ecb6af9
--- /dev/null
+++ b/src/joint_stiffness_controller.cpp
@@ -0,0 +1,70 @@
+#include "stiffness_control/joint_stiffness_controller.hpp"
+
+namespace joint_stiffness_controller
+{
+
+JointStiffnessController::JointStiffnessController()
+{
+}
+
+controller_interface::CallbackReturn JointStiffnessController::on_init()
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn JointStiffnessController::on_configure(const rclcpp_lifecycle::State&)
+{
+  auto node = get_node();
+  command_sub_ = node->create_subscription<std_msgs::msg::Float64MultiArray>(
+      "~/command", 10, std::bind(&JointStiffnessController::command_callback, this, std::placeholders::_1));
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration JointStiffnessController::command_interface_configuration() const
+{
+  return {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    { "joint1/position", "joint2/position" }  // replace with your actual joints
+  };
+}
+
+controller_interface::InterfaceConfiguration JointStiffnessController::state_interface_configuration() const
+{
+  return {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    { "joint1/position", "joint2/position" }  // same joints
+  };
+}
+
+void JointStiffnessController::command_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
+{
+  size_t n = msg->data.size() / 2;
+  desired_positions_.resize(n);
+  stiffness_gains_.resize(n);
+  for (size_t i = 0; i < n; ++i)
+  {
+    desired_positions_[i] = msg->data[i];
+    stiffness_gains_[i] = msg->data[i + n];
+  }
+}
+
+controller_interface::return_type JointStiffnessController::update(const rclcpp::Time&, const rclcpp::Duration&)
+{
+  for (size_t i = 0; i < position_cmd_handles_.size(); ++i)
+  {
+    double error = desired_positions_[i] - position_state_handles_[i].get_optional().value_or(0.0);
+    double command = stiffness_gains_[i] * error;
+    bool success =
+        position_cmd_handles_[i].set_value(position_state_handles_[i].get_optional().value_or(0.0) + command);
+    if (!success)
+    {
+      RCLCPP_WARN(get_node()->get_logger(), "Failed to set command for joint %zu", i);
+    }
+  }
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace joint_stiffness_controller
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(joint_stiffness_controller::JointStiffnessController, controller_interface::ControllerInterface)