diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt
index c62a293034..4c060bb5db 100644
--- a/moveit_ros/hybrid_planning/CMakeLists.txt
+++ b/moveit_ros/hybrid_planning/CMakeLists.txt
@@ -7,11 +7,13 @@ moveit_package()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_servo REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -30,18 +32,23 @@ set(LIBRARIES
   forward_trajectory_plugin
   motion_planning_pipeline_plugin
   replan_invalidated_trajectory_plugin
+  servo_local_solver_plugin
   simple_sampler_plugin
   single_plan_execution_plugin
   # Parameters
   local_planner_parameters
   hp_manager_parameters
+  forward_trajectory_parameters
+  servo_solver_parameters
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
   moveit_core
   moveit_msgs
   moveit_ros_planning
   moveit_ros_planning_interface
+  moveit_servo
   pluginlib
   rclcpp
   rclcpp_action
@@ -90,11 +97,7 @@ install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include/moveit_hybrid
 install(DIRECTORY test/launch DESTINATION share/moveit_hybrid_planning)
 install(DIRECTORY test/config DESTINATION share/moveit_hybrid_planning)
 
-pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning moveit_planning_pipeline_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning simple_sampler_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning plugins.xml)
 
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_export_targets(moveit_hybrid_planningTargets HAS_LIBRARY_TARGET)
diff --git a/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml b/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml
deleted file mode 100644
index 3bc40fe17d..0000000000
--- a/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="forward_trajectory_plugin">
-  <class name="moveit_hybrid_planning/ForwardTrajectory" type="moveit::hybrid_planning::ForwardTrajectory" base_class_type="moveit::hybrid_planning::LocalConstraintSolverInterface">
-    <description>
-    Simple local solver plugin that forwards the next waypoint of the sampled local trajectory.
-    </description>
-  </class>
-</library>
diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt
index fa68289600..e24813635c 100644
--- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt
+++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt
@@ -1,5 +1,13 @@
-add_library(forward_trajectory_plugin SHARED
-  src/forward_trajectory.cpp
-)
+# Forward Trajectory Solver
+generate_parameter_library(forward_trajectory_parameters res/forward_trajectory_parameters.yaml)
+add_library(forward_trajectory_plugin SHARED src/forward_trajectory.cpp)
 set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(forward_trajectory_plugin forward_trajectory_parameters)
 ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Servo Local Solver
+generate_parameter_library(servo_solver_parameters res/servo_solver_parameters.yaml)
+add_library(servo_local_solver_plugin SHARED src/servo_solver.cpp)
+set_target_properties(servo_local_solver_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(servo_local_solver_plugin servo_solver_parameters local_planner_parameters)
+ament_target_dependencies(servo_local_solver_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp
new file mode 100644
index 0000000000..76463c30a9
--- /dev/null
+++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp
@@ -0,0 +1,102 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+   Description: Local solver plugin that uses moveit_servo steer the robot towards a Cartesian goal point.
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <moveit/local_planner/local_constraint_solver_interface.h>
+
+#include <moveit_servo/servo.hpp>
+#include <servo_solver_parameters.hpp>
+
+namespace moveit::hybrid_planning
+{
+/**
+ * @brief Local solver plugin that utilizes moveit_servo as a local planner
+ *
+ */
+class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterface
+{
+public:
+  /**
+   * @brief Initializes moveit servo
+   *
+   * @param node Node provided by the local planning component to this plugin. Can be used to init and use ROS interfaces
+   * @param planning_scene_monitor Planning scene monitor to access the current state of the system
+   * @param group_name UNUSED
+   * @return True if initialization was successful
+   */
+  bool initialize(const rclcpp::Node::SharedPtr& node,
+                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                  const std::string& group_name) override;
+  /**
+   * @brief Reset rolling window
+   *
+   * @return Always returns true
+   */
+  bool reset() override;
+
+  /**
+   * @brief Solves local planning problem with servo. The first waypoint of the local trajectory is used as goal point
+   * for servo. After computing FK for that waypoint, the difference between the current and goal reference frame is used
+   * to calculate a twist command for servo. The status of servo is returned as feedback to the hybrid planning manager.
+   *
+   * @param local_trajectory Reference trajectory, only the first waypoint is used
+   * @param local_goal UNUSED
+   * @param local_solution Joint trajectory containing a sequence of servo commands
+   * @return moveit_msgs::action::LocalPlanner::Feedback containing the servo status code
+   */
+  moveit_msgs::action::LocalPlanner::Feedback
+  solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+        const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
+        trajectory_msgs::msg::JointTrajectory& local_solution) override;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+  servo_solver_parameters::Params solver_parameters_;
+  servo::Params servo_parameters_;
+
+  // Servo cpp interface
+  std::unique_ptr<moveit_servo::Servo> servo_;
+
+  // Command queue to build trajectory message and add current robot state
+  std::deque<moveit_servo::KinematicState> joint_cmd_rolling_window_;
+};
+}  // namespace moveit::hybrid_planning
diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml
new file mode 100644
index 0000000000..181b982f33
--- /dev/null
+++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml
@@ -0,0 +1,6 @@
+forward_trajectory_parameters:
+  stop_before_collision: {
+    type: bool,
+    description: "If a collision is detected should the robot stop at the current pose or continue executing",
+    default_value: false,
+  }
diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml
new file mode 100644
index 0000000000..6fc5a0f1c3
--- /dev/null
+++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml
@@ -0,0 +1,24 @@
+servo_solver_parameters:
+  reference_frame: {
+    type: string,
+    description: "Frame that is tracked by servo",
+    validation: {
+      not_empty<>: []
+    }
+  }
+  trans_gain_scaling: {
+    type: double,
+    description: "Scaling for translative velocities computed from pose diff.",
+    default_value: 0.05,
+    validation: {
+      gt<>: 0.0
+    }
+  }
+  rot_gain_scaling: {
+    type: double,
+    description: "Scaling for rotational velocities computed from pose diff",
+    default_value: 5.0,
+    validation: {
+      gt<>: 0.0
+    }
+  }
diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp
index 13086091bc..a61f61e7cf 100644
--- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp
+++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp
@@ -36,6 +36,7 @@
 #include <moveit/local_planner/feedback_types.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_state/conversions.h>
+#include <forward_trajectory_parameters.hpp>
 
 namespace
 {
@@ -50,15 +51,8 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node,
                                    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                                    const std::string& /* unused */)
 {
-  // Load parameter & initialize member variables
-  if (node->has_parameter("stop_before_collision"))
-  {
-    node->get_parameter<bool>("stop_before_collision", stop_before_collision_);
-  }
-  else
-  {
-    stop_before_collision_ = node->declare_parameter<bool>("stop_before_collision", false);
-  }
+  auto param_listener = forward_trajectory_parameters::ParamListener(node, "");
+  stop_before_collision_ = param_listener.get_params().stop_before_collision;
   node_ = node;
   path_invalidation_event_send_ = false;
   num_iterations_stuck_ = 0;
diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp
new file mode 100644
index 0000000000..64094cafcc
--- /dev/null
+++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp
@@ -0,0 +1,200 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr, Adam Pettinger
+ */
+
+#include <control_msgs/msg/joint_jog.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <moveit/local_constraint_solver_plugins/servo_solver.hpp>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit/utils/logger.hpp>
+#include <local_planner_parameters.hpp>
+
+namespace moveit::hybrid_planning
+{
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("servo_solver");
+}
+}  // namespace
+
+bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node,
+                             const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                             const std::string& /*group_name*/)
+{
+  planning_scene_monitor_ = planning_scene_monitor;
+  node_ = node;
+
+  const std::shared_ptr<const servo_solver_parameters::ParamListener> solver_param_listener =
+      std::make_shared<const servo_solver_parameters::ParamListener>(node, "");
+  solver_parameters_ = solver_param_listener->get_params();
+
+  // Get Servo Parameters
+  const std::string param_namespace = "moveit_servo";
+  const std::shared_ptr<const servo::ParamListener> servo_param_listener =
+      std::make_shared<const servo::ParamListener>(node, param_namespace);
+  servo_parameters_ = servo_param_listener->get_params();
+
+  // Create Servo and start it
+  servo_ = std::make_unique<moveit_servo::Servo>(node_, servo_param_listener, planning_scene_monitor_);
+
+  // Set servo publish_period
+  auto local_param_listener = local_planner_parameters::ParamListener(node_, "");
+  const auto local_config = local_param_listener.get_params();
+
+  // Publish period is the time difference used for numerical integration. It is recommended that the period 3x smaller
+  // than the actual publish period.
+  const double servo_publish_rate = 1.0 / 3.0 * local_config.local_planning_frequency;
+  if (!node_->has_parameter("publish_period"))
+  {
+    RCLCPP_ERROR(getLogger(), "Node used by servo solver doesn't seem to have a parameter called 'publish_period' that "
+                              "shouldn't happen!");
+    return false;
+  }
+  node_->set_parameter(rclcpp::Parameter("publish_period", servo_publish_rate));
+  return true;
+}
+
+bool ServoSolver::reset()
+{
+  RCLCPP_INFO(getLogger(), "Reset Servo Solver");
+  joint_cmd_rolling_window_.clear();
+  return true;
+};
+
+moveit_msgs::action::LocalPlanner::Feedback
+ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+                   const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> /*local_goal*/,
+                   trajectory_msgs::msg::JointTrajectory& local_solution)
+{
+  // Create Feedback
+  moveit_msgs::action::LocalPlanner::Feedback feedback_result;
+
+  // Transform next robot trajectory waypoint into JointJog message
+  moveit_msgs::msg::RobotTrajectory robot_command;
+  local_trajectory.getRobotTrajectoryMsg(robot_command);
+
+  if (robot_command.joint_trajectory.points.empty())
+  {
+    feedback_result.feedback = std::string("Reference trajectory does not contain any points");
+    return feedback_result;
+  }
+
+  // Get current state
+  const auto current_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
+
+  // Create goal state
+  moveit::core::RobotState target_state = *current_state;
+  target_state.setVariablePositions(robot_command.joint_trajectory.joint_names,
+                                    robot_command.joint_trajectory.points.at(0).positions);
+  target_state.update();
+
+  // TF planning_frame -> current EE
+  Eigen::Isometry3d current_pose = current_state->getFrameTransform(solver_parameters_.reference_frame);
+  // TF planning -> target EE
+  Eigen::Isometry3d target_pose = target_state.getFrameTransform(solver_parameters_.reference_frame);
+
+  // current EE -> planning frame * planning frame -> target EE
+  Eigen::Isometry3d diff_pose = current_pose.inverse() * target_pose;
+  Eigen::AngleAxisd axis_angle(diff_pose.linear());
+
+  const double trans_gain = solver_parameters_.trans_gain_scaling / diff_pose.translation().norm();
+  const double rot_gain = solver_parameters_.rot_gain_scaling / diff_pose.rotation().norm();
+
+  // Calculate Cartesian command delta
+  // Transform current pose to command frame
+  // Transform goal pose to command frame
+  servo_->setCommandType(moveit_servo::CommandType::TWIST);
+  moveit_servo::TwistCommand target_twist{
+    solver_parameters_.reference_frame,
+    { diff_pose.translation().x() * trans_gain, diff_pose.translation().y() * trans_gain,
+      diff_pose.translation().z() * trans_gain, axis_angle.axis().x() * axis_angle.angle() * rot_gain,
+      axis_angle.axis().y() * axis_angle.angle() * rot_gain, axis_angle.axis().z() * axis_angle.angle() * rot_gain }
+  };
+
+  std::optional<trajectory_msgs::msg::JointTrajectory> trajectory_msg;
+
+  // Clear all commands that are older than older than one publish period
+  const auto cutoff_timestamp = node_->now() - rclcpp::Duration::from_seconds(3.0 * servo_parameters_.publish_period);
+  const auto cutoff_iterator = std::find_if(joint_cmd_rolling_window_.rbegin(), joint_cmd_rolling_window_.rend(),
+                                            [&cutoff_timestamp](moveit_servo::KinematicState state) {
+                                              return state.time_stamp < cutoff_timestamp;
+                                            })
+                                   .base();
+
+  if (cutoff_iterator != joint_cmd_rolling_window_.end())
+  {
+    // Erase elements from the beginning to the found position
+    joint_cmd_rolling_window_.erase(joint_cmd_rolling_window_.begin(), cutoff_iterator);
+  }
+
+  // Create servo commands until a trajectory message can be generated
+  while (!trajectory_msg)
+  {
+    // Calculate next servo command
+    moveit_servo::KinematicState joint_state = servo_->getNextJointState(current_state, target_twist);
+    const auto status = servo_->getStatus();
+    // Servo solver feedback is always the status of the first servo iteration
+    if (feedback_result.feedback.empty() && status != moveit_servo::StatusCode::NO_WARNING)
+    {
+      feedback_result.feedback = moveit_servo::SERVO_STATUS_CODE_MAP.at(status);
+    }
+    // If servo couldn't compute the next joint state, exit local solver without a solution
+    if (status == moveit_servo::StatusCode::INVALID)
+    {
+      return feedback_result;
+    }
+    moveit_servo::updateSlidingWindow(joint_state, joint_cmd_rolling_window_, servo_parameters_.max_expected_latency,
+                                      node_->now());
+    if (!joint_cmd_rolling_window_.empty())
+    {
+      current_state->setJointGroupPositions(current_state->getJointModelGroup(servo_parameters_.move_group_name),
+                                            joint_cmd_rolling_window_.back().positions);
+      current_state->setJointGroupVelocities(current_state->getJointModelGroup(servo_parameters_.move_group_name),
+                                             joint_cmd_rolling_window_.back().velocities);
+    }
+    trajectory_msg = moveit_servo::composeTrajectoryMessage(servo_parameters_, joint_cmd_rolling_window_);
+  }
+  local_solution = trajectory_msg.value();
+  return feedback_result;
+}
+}  // namespace moveit::hybrid_planning
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::ServoSolver, moveit::hybrid_planning::LocalConstraintSolverInterface);
diff --git a/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml b/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml
deleted file mode 100644
index 1b325280bd..0000000000
--- a/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="motion_planning_pipeline_plugin">
-  <class name="moveit_hybrid_planning/MoveItPlanningPipeline" type="moveit::hybrid_planning::MoveItPlanningPipeline" base_class_type="moveit::hybrid_planning::GlobalPlannerInterface">
-    <description>
-    Use default MoveIt planning pipeline as Global Planner
-    </description>
-  </class>
-</library>
diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml
index b35f8449a3..c85944884d 100644
--- a/moveit_ros/hybrid_planning/package.xml
+++ b/moveit_ros/hybrid_planning/package.xml
@@ -16,10 +16,12 @@
   <depend>moveit_common</depend>
 
   <depend>ament_index_cpp</depend>
+  <depend>control_msgs</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_servo</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
diff --git a/moveit_ros/hybrid_planning/plugins.xml b/moveit_ros/hybrid_planning/plugins.xml
new file mode 100644
index 0000000000..d4ffffaba4
--- /dev/null
+++ b/moveit_ros/hybrid_planning/plugins.xml
@@ -0,0 +1,46 @@
+<class_libraries>
+<!-- Planner Logic Plugins-->
+<library path="single_plan_execution_plugin">
+  <class name="moveit_hybrid_planning/SinglePlanExecution" type="moveit::hybrid_planning::SinglePlanExecution" base_class_type="moveit::hybrid_planning::PlannerLogicInterface">
+    <description>
+    Simple hybrid planning logic that runs the global planner once and execute the global solution with the local planner.
+    </description>
+  </class>
+</library>
+<library path="replan_invalidated_trajectory_plugin">
+  <class name="moveit_hybrid_planning/ReplanInvalidatedTrajectory" type="moveit::hybrid_planning::ReplanInvalidatedTrajectory" base_class_type="moveit::hybrid_planning::PlannerLogicInterface">
+    <description>
+    Simple hybrid planning logic that runs the global planner once and starts executing the global solution with the local planner. In case the local planner detects a collision the global planner is rerun to update the invalidated global trajectory.
+    </description>
+  </class>
+</library>
+<!-- Global Planner Plugins -->
+<library path="motion_planning_pipeline_plugin">
+  <class name="moveit_hybrid_planning/MoveItPlanningPipeline" type="moveit::hybrid_planning::MoveItPlanningPipeline" base_class_type="moveit::hybrid_planning::GlobalPlannerInterface">
+    <description>
+    Use default MoveIt planning pipeline as Global Planner
+    </description>
+  </class>
+</library>
+<!-- Trajectory Operator Plugins -->
+<library path="simple_sampler_plugin">
+  <class name="moveit_hybrid_planning/SimpleSampler" type="moveit::hybrid_planning::SimpleSampler" base_class_type="moveit::hybrid_planning::TrajectoryOperatorInterface">
+    <description>
+    Simple trajectory operator plugin that updates the local planner's reference trajectory by simple replacing it and extracts the next trajectory point based on the current robot state and an index.
+    </description>
+  </class>
+</library>
+<!-- Local solver Plugins -->
+<library path="forward_trajectory_plugin">
+  <class name="moveit_hybrid_planning/ForwardTrajectory" type="moveit::hybrid_planning::ForwardTrajectory" base_class_type="moveit::hybrid_planning::LocalConstraintSolverInterface">
+    <description>
+    Simple local solver plugin that forwards the next waypoint of the sampled local trajectory.
+    </description>
+  </class>
+</library>
+<library path="servo_local_solver_plugin">
+  <class name="moveit_hybrid_planning/ServoSolver" type="moveit::hybrid_planning::ServoSolver" base_class_type="moveit::hybrid_planning::LocalConstraintSolverInterface">
+    <description>Uses Servo as a local solver.</description>
+  </class>
+</library>
+</class_libraries>
diff --git a/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml b/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml
deleted file mode 100644
index 0bceaac42d..0000000000
--- a/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="replan_invalidated_trajectory_plugin">
-  <class name="moveit_hybrid_planning/ReplanInvalidatedTrajectory" type="moveit::hybrid_planning::ReplanInvalidatedTrajectory" base_class_type="moveit::hybrid_planning::PlannerLogicInterface">
-    <description>
-    Simple hybrid planning logic that runs the global planner once and starts executing the global solution with the local planner. In case the local planner detects a collision the global planner is rerun to update the invalidated global trajectory.
-    </description>
-  </class>
-</library>
diff --git a/moveit_ros/hybrid_planning/simple_sampler_plugin.xml b/moveit_ros/hybrid_planning/simple_sampler_plugin.xml
deleted file mode 100644
index 3678ab0fef..0000000000
--- a/moveit_ros/hybrid_planning/simple_sampler_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="simple_sampler_plugin">
-  <class name="moveit_hybrid_planning/SimpleSampler" type="moveit::hybrid_planning::SimpleSampler" base_class_type="moveit::hybrid_planning::TrajectoryOperatorInterface">
-    <description>
-    Simple trajectory operator plugin that updates the local planner's reference trajectory by simple replacing it and extracts the next trajectory point based on the current robot state and an index.
-    </description>
-  </class>
-</library>
diff --git a/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml b/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml
deleted file mode 100644
index 847ec33f58..0000000000
--- a/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="single_plan_execution_plugin">
-  <class name="moveit_hybrid_planning/SinglePlanExecution" type="moveit::hybrid_planning::SinglePlanExecution" base_class_type="moveit::hybrid_planning::PlannerLogicInterface">
-    <description>
-    Simple hybrid planning logic that runs the global planner once and execute the global solution with the local planner.
-    </description>
-  </class>
-</library>