diff --git a/kuka_rsi_driver/config/initial_positions.yaml b/kuka_rsi_driver/config/initial_positions.yaml
new file mode 100644
index 0000000..94a4e8b
--- /dev/null
+++ b/kuka_rsi_driver/config/initial_positions.yaml
@@ -0,0 +1,6 @@
+joint_a1: 0.0
+joint_a2: -1.5708
+joint_a3: 1.5708
+joint_a4: 0.0
+joint_a5: 0.0
+joint_a6: 0.0
diff --git a/kuka_rsi_driver/launch/test_bringup.launch.py b/kuka_rsi_driver/launch/test_bringup.launch.py
index 8b90d7f..c156208 100644
--- a/kuka_rsi_driver/launch/test_bringup.launch.py
+++ b/kuka_rsi_driver/launch/test_bringup.launch.py
@@ -93,6 +93,19 @@ def declare_robot_description_arg(name, **kwargs):
         "macro_name", description="Name of robot description macro"
     )
 
+    declare_robot_description_arg(
+        "use_mock_hardware",
+        default_value="false",
+        description="Whether to use mock hardware instead of real hardware interface",
+    )
+    declare_robot_description_arg(
+        "initial_positions_file",
+        default_value=PathJoinSubstitution(
+            [kuka_rsi_driver, "config", "initial_positions.yaml"]
+        ),
+        description="Initial joint positions when using mock hardware",
+    )
+
     declare_robot_description_arg(
         "rsi_listen_ip",
         default_value="127.0.0.1",
diff --git a/kuka_rsi_driver/test/move_sim.py b/kuka_rsi_driver/test/move_sim.py
index d0fd5f3..6976edf 100644
--- a/kuka_rsi_driver/test/move_sim.py
+++ b/kuka_rsi_driver/test/move_sim.py
@@ -56,7 +56,7 @@
 
 @pytest.mark.launch_test
 @launch_testing.parametrize(
-    "prefix,robot_model",
+    "prefix,robot_model,use_mock_hardware",
     itertools.product(
         ["", "prefix_"],
         [
@@ -64,9 +64,10 @@
             ("kuka_kr120_support", "kr120r2500pro_macro.xacro", "kuka_kr120r2500pro"),
             ("kuka_kr210_support", "kr210r3100_macro.xacro", "kuka_kr210r3100"),
         ],
+        [True, False],
     ),
 )
-def generate_test_description(prefix, robot_model):
+def generate_test_description(prefix, robot_model, use_mock_hardware):
     kuka_rsi_driver = FindPackageShare("kuka_rsi_driver")
 
     test_description = LaunchDescription()
@@ -84,6 +85,7 @@ def generate_test_description(prefix, robot_model):
                 "description_package": robot_model[0],
                 "description_macro_file": robot_model[1],
                 "macro_name": robot_model[2],
+                "use_mock_hardware": f"{use_mock_hardware}",
                 "launch_rviz": "False",
                 "prefix": prefix,
             }.items(),
@@ -93,13 +95,14 @@ def generate_test_description(prefix, robot_model):
     #
     # Start simulator
     #
-    test_description.add_action(
-        Node(
-            package="kuka_rsi_driver",
-            executable="simulator",
-            arguments=["--host-ip", "127.0.0.1"],
+    if not use_mock_hardware:
+        test_description.add_action(
+            Node(
+                package="kuka_rsi_driver",
+                executable="simulator",
+                arguments=["--host-ip", "127.0.0.1"],
+            )
         )
-    )
 
     test_description.add_action(ReadyToTest())
     return test_description
diff --git a/kuka_rsi_driver/urdf/kuka_rsi_driver.ros2_control.xacro b/kuka_rsi_driver/urdf/kuka_rsi_driver.ros2_control.xacro
index 92f1a64..67cd4aa 100644
--- a/kuka_rsi_driver/urdf/kuka_rsi_driver.ros2_control.xacro
+++ b/kuka_rsi_driver/urdf/kuka_rsi_driver.ros2_control.xacro
@@ -2,10 +2,12 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="kuka_rsi_driver_control_joint" params="name">
+  <xacro:macro name="kuka_rsi_driver_control_joint" params="name initial_position">
     <joint name="${name}">
       <command_interface name="position" />
-      <state_interface name="position" />
+      <state_interface name="position">
+        <param name="initial_value">${initial_position}</param>
+      </state_interface>
       <state_interface name="effort" />
     </joint>
   </xacro:macro>
@@ -13,39 +15,64 @@
   <xacro:macro name="kuka_rsi_driver_control" params="
     name
     prefix
+    use_mock_hardware:=false
+    initial_positions:=${dict(joint_a1=0.0,joint_a2=0.0,joint_a3=0.0,joint_a4=0.0,joint_a5=0.0,joint_a6=0.0)}
     rsi_listen_ip
     rsi_listen_port:=49152
     ">
     <ros2_control name="${name}" type="system">
       <hardware>
-        <plugin>kuka_rsi_driver/KukaRsiHardwareInterface</plugin>
-        <param name="listen_address">${rsi_listen_ip}</param>
-        <param name="listen_port">${rsi_listen_port}</param>
+        <xacro:if value="${use_mock_hardware}">
+          <plugin>mock_components/GenericSystem</plugin>
+        </xacro:if>
+        <xacro:unless value="${use_mock_hardware}">
+          <plugin>kuka_rsi_driver/KukaRsiHardwareInterface</plugin>
+          <param name="listen_address">${rsi_listen_ip}</param>
+          <param name="listen_port">${rsi_listen_port}</param>
+        </xacro:unless>
       </hardware>
 
-      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a1" />
-      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a2" />
-      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a3" />
-      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a4" />
-      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a5" />
-      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a6" />
+      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a1" initial_position="${initial_positions['joint_a1']}" />
+      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a2" initial_position="${initial_positions['joint_a2']}" />
+      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a3" initial_position="${initial_positions['joint_a3']}" />
+      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a4" initial_position="${initial_positions['joint_a4']}" />
+      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a5" initial_position="${initial_positions['joint_a5']}" />
+      <xacro:kuka_rsi_driver_control_joint name="${prefix}joint_a6" initial_position="${initial_positions['joint_a6']}" />
 
       <sensor name="${prefix}tcp">
-        <state_interface name="position.x"/>
-        <state_interface name="position.y"/>
-        <state_interface name="position.z"/>
-        <state_interface name="orientation.x"/>
-        <state_interface name="orientation.y"/>
-        <state_interface name="orientation.z"/>
-        <state_interface name="orientation.w"/>
+        <state_interface name="position.x">
+          <param name="initial_value">0</param>
+        </state_interface>
+        <state_interface name="position.y">
+          <param name="initial_value">0</param>
+        </state_interface>
+        <state_interface name="position.z">
+          <param name="initial_value">0</param>
+        </state_interface>
+        <state_interface name="orientation.x">
+          <param name="initial_value">0</param>
+        </state_interface>
+        <state_interface name="orientation.y">
+          <param name="initial_value">0</param>
+        </state_interface>
+        <state_interface name="orientation.z">
+          <param name="initial_value">0</param>
+        </state_interface>
+        <state_interface name="orientation.w">
+          <param name="initial_value">1</param>
+        </state_interface>
       </sensor>
 
       <gpio name="${prefix}robot_state">
-        <state_interface name="program_state" />
+        <state_interface name="program_state">
+          <param name="initial_value">3</param>
+        </state_interface>
       </gpio>
 
       <gpio name="${prefix}speed_scaling">
-        <state_interface name="speed_scaling_factor" />
+        <state_interface name="speed_scaling_factor">
+          <param name="initial_value">1</param>
+        </state_interface>
       </gpio>
 
     </ros2_control>
diff --git a/kuka_rsi_driver/urdf/test_description.urdf.xacro b/kuka_rsi_driver/urdf/test_description.urdf.xacro
index 3c41891..be0c668 100644
--- a/kuka_rsi_driver/urdf/test_description.urdf.xacro
+++ b/kuka_rsi_driver/urdf/test_description.urdf.xacro
@@ -7,11 +7,17 @@
   <xacro:arg name="description_macro_file" default="" />
   <xacro:arg name="macro_name" default="" />
 
+  <xacro:arg name="use_mock_hardware" default="false" />
+  <xacro:arg name="initial_positions_file" default="$(find kuka_rsi_driver)/config/initial_positions.yaml" />
+
   <xacro:arg name="rsi_listen_ip" default="127.0.0.1" />
   <xacro:arg name="rsi_listen_port" default="49152" />
 
   <xacro:arg name="prefix" default="" />
 
+  <!-- Convert argument to property so it can be used in a function -->
+  <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)" />
+
   <!-- Include robot description and invoke macro -->
   <xacro:property name="description_package" value="$(arg description_package)" />
   <xacro:include filename="$(find ${description_package})/urdf/$(arg description_macro_file)" />
@@ -22,6 +28,8 @@
   <xacro:kuka_rsi_driver_control
     name="$(arg macro_name)"
     prefix="$(arg prefix)"
+    use_mock_hardware="$(arg use_mock_hardware)"
+    initial_positions="${xacro.load_yaml(initial_positions_file)}"
     rsi_listen_ip="$(arg rsi_listen_ip)"
     rsi_listen_port="$(arg rsi_listen_port)"
   />