diff --git a/joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml b/joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml
new file mode 100644
index 0000000..221db32
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml
@@ -0,0 +1,43 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint_controller:
+      type: joint_controller/JointController
+    
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - joint_controller/body_1_joint
+    interface_name: position
+
+joint_controller:
+  ros__parameters:
+    joint_names:
+      - body_1_joint
+
+    joint_params:
+      body_1_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+    
+    reference_interfaces: 
+      - "position"
+
+    frequency: 500.0
+
+    pid_gains:
+      body_1_joint:
+        p: 1.0
+        d: 0.5
+        i: 0.0
+        ilimit: 1.0
diff --git a/joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml b/joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml
new file mode 100644
index 0000000..5a26931
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml
@@ -0,0 +1,84 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint_controller:
+      type: joint_controller/JointController
+
+
+joint_controller:
+  ros__parameters:
+    joint_names:
+      - body_1_joint
+      - body_2_joint
+      - body_3_joint
+      - body_4_joint
+      - body_5_joint
+
+    joint_params:
+      body_1_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+      body_2_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+      body_3_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+      body_4_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+      body_5_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+    
+    reference_interfaces: 
+      - "position"
+    
+    frequency: 500.0
+
+    pid_gains:
+      body_1_joint:
+        p: 1.0
+        d: 0.5
+        i: 0.0
+        ilimit: 1.0
+      body_2_joint:
+        p: 10.0
+        d: 0.5
+        i: 0.0
+        ilimit: 1.0
+      body_3_joint:
+        p: 1.0
+        d: 1.5
+        i: 0.0
+        ilimit: 1.0
+      body_4_joint:
+        p: 10.0
+        d: 5.0
+        i: 0.0
+        ilimit: 1.0
+      body_5_joint:
+        p: 1.0
+        d: 0.5
+        i: 0.25
+        ilimit: 1.0
+      
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml b/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml
index 603e82e..25aaca1 100644
--- a/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml
+++ b/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml
@@ -21,7 +21,10 @@ joint_controller:
         position_offset: 0.0
         velocity_max: 10.0
         effort_max: 10.0
-       
+    
+    reference_interfaces: 
+      - "position"
+
     frequency: 500.0
 
     pid_gains:
diff --git a/joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py b/joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py
new file mode 100644
index 0000000..e5953c7
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py
@@ -0,0 +1,174 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="joint_controller",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="chaining_joint_test.urdf.xacro",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type_file",
+            default_value="chaining_joint_test.yaml",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type",
+            default_value="joint_controller",
+        )
+    )
+
+    # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+    controller_file = LaunchConfiguration("controller_type_file")
+    controller_type = LaunchConfiguration("controller_type")
+
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "urdf",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+
+    # Set up dictionary parameters:
+    robot_description = {"robot_description": robot_description_content}
+    use_sim_time = {"use_sim_time": True}
+
+    # Load world for gazebo sim
+    world = PathJoinSubstitution(
+        [
+            FindPackageShare(package_name),
+            "worlds",
+            'empty.world'
+        ]
+    )
+
+    # robot_state_publisher node:
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description, use_sim_time],
+    )
+
+    # Spawn Meldog:
+    spawn_entity = Node(package='ros_gz_sim', executable='create',
+                    arguments=['-topic', 'robot_description',
+                                '-name', 'Meldog'],
+                    output='screen')
+
+    # Load joint_state_broadcaster
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    # Load joint_trajectory_controller 
+    load_joint_effort_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller_type],
+        output='screen'
+    )
+
+    load_forward_position_controller =  ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'forward_position_controller'],
+        output='screen'
+    )
+
+    # Bridge between ros2 and Ignition Gazebo
+    # ros2_gazebo_sim_bridge =  IncludeLaunchDescription(
+    #         PythonLaunchDescriptionSource(
+    #             [os.path.join(get_package_share_directory('ros_ign_gazebo'),
+    #                           'launch', 'ign_gazebo.launch.py')]),
+    #         launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])])
+    gazebo_sim_bridge = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
+            launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items()
+        )
+    
+    nodes = [
+        gazebo_sim_bridge,
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action= load_joint_state_broadcaster,
+                on_exit=[load_joint_effort_controller],
+            )
+        ),
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action= load_joint_effort_controller,
+                on_exit=[load_forward_position_controller],
+            )
+        ),
+
+        robot_state_pub_node,
+        spawn_entity,
+    ]
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py b/joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py
new file mode 100644
index 0000000..3a1a82c
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py
@@ -0,0 +1,162 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="joint_controller",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="multiple_joints_test.urdf.xacro",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type_file",
+            default_value="multiple_joints_test.yaml",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type",
+            default_value="joint_controller",
+        )
+    )
+
+    # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+    controller_file = LaunchConfiguration("controller_type_file")
+    controller_type = LaunchConfiguration("controller_type")
+
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "urdf",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+
+    # Set up dictionary parameters:
+    robot_description = {"robot_description": robot_description_content}
+    use_sim_time = {"use_sim_time": True}
+
+    # Load world for gazebo sim
+    world = PathJoinSubstitution(
+        [
+            FindPackageShare(package_name),
+            "worlds",
+            'empty.world'
+        ]
+    )
+
+    # robot_state_publisher node:
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description, use_sim_time],
+    )
+
+    # Spawn Meldog:
+    spawn_entity = Node(package='ros_gz_sim', executable='create',
+                    arguments=['-topic', 'robot_description',
+                                '-name', 'Meldog'],
+                    output='screen')
+
+    # Load joint_state_broadcaster
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    # Load joint_trajectory_controller 
+    load_joint_effort_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller_type],
+        output='screen'
+    )
+
+    # Bridge between ros2 and Ignition Gazebo
+    # ros2_gazebo_sim_bridge =  IncludeLaunchDescription(
+    #         PythonLaunchDescriptionSource(
+    #             [os.path.join(get_package_share_directory('ros_ign_gazebo'),
+    #                           'launch', 'ign_gazebo.launch.py')]),
+    #         launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])])
+    gazebo_sim_bridge = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
+            launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items()
+        )
+    
+    nodes = [
+        gazebo_sim_bridge,
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action= load_joint_state_broadcaster,
+                on_exit=[load_joint_effort_controller],
+            )
+        ),
+
+        robot_state_pub_node,
+        spawn_entity,
+    ]
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py b/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py
index d830fde..d04de51 100644
--- a/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py
+++ b/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py
@@ -31,7 +31,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "urdf_file",
-            default_value="single_joint_test.urdf.xacro",
+            default_value="multiple_joints_test.urdf.xacro",
         )
     )
 
diff --git a/joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt b/joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt
new file mode 100644
index 0000000..9ed9531
--- /dev/null
+++ b/joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt
@@ -0,0 +1,5 @@
+// Sending position 1.0 for joint "body_1_joint" through forward_position_controller
+
+
+ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
+- 0.5"
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt b/joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt
new file mode 100644
index 0000000..2fdfc50
--- /dev/null
+++ b/joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt
@@ -0,0 +1,3 @@
+// Sending positions 1.0 for all joints "body_x_joint" 
+
+ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: ['body_1_joint', 'body_2_joint', 'body_3_joint', 'body_4_joint', 'body_5_joint'], desired_position:[1.0, 1.0, 1.0, 1.0, 1.0], desired_velocity: [0.0, 0.0, 0.0, 0.0, 0.0], kp_scale: [0.0, 0.0, 0.0, 0.0, 0.0], kd_scale: [0.0, 0.0, 0.0, 0.0, 0.0], feedforward_effort: [0.0, 0.0, 0.0, 0.0, 0.0]}"
diff --git a/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt b/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt
index 0da172a..55df33e 100644
--- a/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt
+++ b/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt
@@ -1,4 +1,4 @@
-// Wysyłanie pozycji 1.0 dla jointa "body_1_joint" 
+// Sending position 1.0 for joint "body_1_joint" 
 
 ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: {'body_1_joint'}, desired_position:{1.0}, desired_velocity: {0.0}, kp_scale: {0.0}, kd_scale: {0.0}, feedforward_effort: {0.0}}"
 
diff --git a/joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro
new file mode 100644
index 0000000..de61eec
--- /dev/null
+++ b/joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="single_joint_test">
+
+    <xacro:property name="PI" value="3.141592"/>
+
+    <xacro:property name="rotor_inertia" value="1.0"/>
+    <xacro:property name="rotor_position_max" value="${PI}"/>
+    <xacro:property name="rotor_position_min" value="${-PI}"/>
+    <xacro:property name="rotor_velocity_max" value="10.0"/>
+    <xacro:property name="rotor_effort_max" value="10.0"/>
+    
+    <link name="world"/>
+    
+    <!-- Single joint macro -->
+    <xacro:include filename="core/single_joint.urdf.xacro"/>
+
+    <xacro:single_joint_macro name="body_1" parent_name="world" x="0.0" y="0.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+    
+
+    <ros2_control name="single_joint_test" type="system">
+        <hardware>  
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </hardware>
+        <!-- Add joint hardware interfaces -->
+        <xacro:include filename="core/joint_hardware_macros.urdf.xacro"/>   
+        <xacro:joint_hardware_macro name="body_1" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+    </ros2_control>
+
+    <gazebo>
+        <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+            <parameters>$(find joint_controller)/config/chaining_joint_test.yaml</parameters>
+        </plugin>
+    </gazebo>
+
+</robot>
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro b/joint_controller_ros2_control/test/urdf/core/joint_hardware_macros.urdf.xacro
similarity index 100%
rename from joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro
rename to joint_controller_ros2_control/test/urdf/core/joint_hardware_macros.urdf.xacro
diff --git a/joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro b/joint_controller_ros2_control/test/urdf/core/single_joint.urdf.xacro
similarity index 100%
rename from joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro
rename to joint_controller_ros2_control/test/urdf/core/single_joint.urdf.xacro
diff --git a/joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro
new file mode 100644
index 0000000..99c3f86
--- /dev/null
+++ b/joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro
@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="multiple_joints_test">
+
+<xacro:property name="PI" value="3.141592"/>
+
+    <xacro:property name="rotor_inertia" value="1.0"/>
+    <xacro:property name="rotor_position_max" value="${10*PI}"/>
+    <xacro:property name="rotor_position_min" value="${-10*PI}"/>
+    <xacro:property name="rotor_velocity_max" value="10.0"/>
+    <xacro:property name="rotor_effort_max" value="10.0"/>
+    
+    <link name="world"/>
+    
+    <!-- Single joint macro -->
+    <xacro:include filename="core/single_joint.urdf.xacro"/>
+
+    <xacro:single_joint_macro name="body_1" parent_name="world" x="0.0" y="0.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+    <xacro:single_joint_macro name="body_2" parent_name="world" x="1.0" y="0.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+    <xacro:single_joint_macro name="body_3" parent_name="world" x="-1.0" y="0.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+    <xacro:single_joint_macro name="body_4" parent_name="world" x="0.0" y="1.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+    <xacro:single_joint_macro name="body_5" parent_name="world" x="0.0" y="-1.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+
+    <ros2_control name="multiple_joints_test" type="system">
+        <hardware>  
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </hardware>
+        <!-- Add joint hardware interfaces -->
+        <xacro:include filename="core/joint_hardware_macros.urdf.xacro"/>   
+        <xacro:joint_hardware_macro name="body_1" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+        <xacro:joint_hardware_macro name="body_2" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+        <xacro:joint_hardware_macro name="body_3" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+        <xacro:joint_hardware_macro name="body_4" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+        <xacro:joint_hardware_macro name="body_5" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+    </ros2_control>
+
+    <gazebo>
+        <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+            <parameters>$(find joint_controller)/config/multiple_joints_test.yaml</parameters>
+        </plugin>
+    </gazebo>
+
+
+
+
+</robot>
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro
index f805af9..4407149 100644
--- a/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro
+++ b/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro
@@ -12,7 +12,7 @@
     <link name="world"/>
     
     <!-- Single joint macro -->
-    <xacro:include filename="single_joint.urdf.xacro"/>
+    <xacro:include filename="core/single_joint.urdf.xacro"/>
 
     <xacro:single_joint_macro name="body_1" parent_name="world" x="0.0" y="0.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
     
@@ -22,7 +22,7 @@
             <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
         <!-- Add joint hardware interfaces -->
-        <xacro:include filename="joint_hardware_macros.urdf.xacro"/>   
+        <xacro:include filename="core/joint_hardware_macros.urdf.xacro"/>   
         <xacro:joint_hardware_macro name="body_1" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
     </ros2_control>