diff --git a/lbr_bringup/config/moveit_servo.yaml b/lbr_bringup/config/moveit_servo.yaml
index da58d276..9d292fba 100644
--- a/lbr_bringup/config/moveit_servo.yaml
+++ b/lbr_bringup/config/moveit_servo.yaml
@@ -17,16 +17,16 @@
       # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
 
       ## Properties of outgoing commands
-      publish_period: 0.034 # 1/Nominal publish rate [seconds]
+      publish_period: 0.005 # #1/controller manager update rate [seconds]
       low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
       # What type of topic does your robot driver expect?
       # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
-      command_out_type: trajectory_msgs/JointTrajectory
+      command_out_type: std_msgs/Float64MultiArray
 
       # What to publish? Can save some bandwidth as most robots only require positions or velocities
       publish_joint_positions: true
-      publish_joint_velocities: true
+      publish_joint_velocities: false
       publish_joint_accelerations: false
 
       ## Plugins for smoothing outgoing commands
@@ -63,7 +63,7 @@
       joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
       joint_topic: /lbr/joint_states
       status_topic: ~/status # Publish status to this topic
-      command_out_topic: /lbr/joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
+      command_out_topic: /lbr/forward_position_controller/commands # Publish outgoing commands here
 
       ## Collision checking for the entire robot body
       check_collisions: true # Check collisions?
diff --git a/lbr_demos/lbr_moveit/config/forward_keyboard.yaml b/lbr_demos/lbr_moveit/config/forward_keyboard.yaml
index 4774cb5f..bd51e1e0 100644
--- a/lbr_demos/lbr_moveit/config/forward_keyboard.yaml
+++ b/lbr_demos/lbr_moveit/config/forward_keyboard.yaml
@@ -2,21 +2,21 @@
   ros__parameters:
     velocity_scales:
       translation:
-        x: 0.2
-        y: 0.2
-        z: 0.2
+        x: 1.0
+        y: 1.0
+        z: 1.0
       rotation:
-        x: 0.2
-        y: 0.2
-        z: 0.2
+        x: 1.0
+        y: 1.0
+        z: 1.0
       joints:
-        - 0.4
-        - 0.4
-        - 0.4
-        - 0.4
-        - 0.4
-        - 0.4
-        - 0.4
+        - 1.0
+        - 1.0
+        - 1.0
+        - 1.0
+        - 1.0
+        - 1.0
+        - 1.0
     keyboard_layout:
       # uses pynput: https://pypi.org/project/pynput/
       translation:
diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst
index da581574..97c2d632 100644
--- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst
+++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst
@@ -13,11 +13,12 @@ MoveIt Servo
 
 MoveIt Servo - Simulation
 ~~~~~~~~~~~~~~~~~~~~~~~~~
-#. Run the mock setup:
+#. Run the mock setup (important to run the ``forward_position_controller`` controller for servoing):
 
     .. code-block:: bash
 
         ros2 launch lbr_bringup mock.launch.py \
+            ctrl:=forward_position_controller \
             model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
    .. hint::
diff --git a/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py b/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py
index 1e93e154..6015f504 100644
--- a/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py
+++ b/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py
@@ -83,12 +83,12 @@ def _on_key_release(self, key: Key) -> None:
                 key_str
                 == self._command_forward_node.keyboard_layout.rotation.y.increase
             ):
-                self._twist_cmd[3] = 0.0  # rotation about y-axis
+                self._twist_cmd[4] = 0.0  # rotation about y-axis
             elif (
                 key_str
                 == self._command_forward_node.keyboard_layout.rotation.y.decrease
             ):
-                self._twist_cmd[3] = 0.0
+                self._twist_cmd[4] = 0.0
             elif key_str in self._valid_numbers:
                 # get index of key_str within valid_numbers
                 index = self._valid_numbers.index(key_str)
@@ -173,12 +173,12 @@ def _on_key_press(self, key: Key) -> None:
                 key_str
                 == self._command_forward_node.keyboard_layout.rotation.y.increase
             ):
-                self._twist_cmd[3] = 1.0  # rotation about y-axis
+                self._twist_cmd[4] = 1.0  # rotation about y-axis
             elif (
                 key_str
                 == self._command_forward_node.keyboard_layout.rotation.y.decrease
             ):
-                self._twist_cmd[3] = -1.0
+                self._twist_cmd[4] = -1.0
             elif key_str == self._command_forward_node.keyboard_layout.reverse_joints:
                 self._joint_vel_direction *= -1.0
             elif key_str in self._valid_numbers: