diff --git a/prbt_robot_support/CMakeLists.txt b/prbt_robot_support/CMakeLists.txt
index bf6fb34..3430203 100644
--- a/prbt_robot_support/CMakeLists.txt
+++ b/prbt_robot_support/CMakeLists.txt
@@ -10,14 +10,15 @@ find_package(ament_cmake REQUIRED)
 find_package(canopen REQUIRED)
 find_package(lely_core_libraries REQUIRED)
 
-generate_dcf(prbt)
+cogen_dcf(prbt)
 
 # install launch file
 install(DIRECTORY
-  launch urdf meshes config
+  launch urdf meshes
   DESTINATION share/${PROJECT_NAME})
 
-
+install(FILES config/prbt_ros2_control.yaml
+  DESTINATION share/${PROJECT_NAME}/config)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
diff --git a/prbt_robot_support/config/prbt/bus.yml b/prbt_robot_support/config/prbt/bus.yml
index b69ae8f..5ddb2fd 100644
--- a/prbt_robot_support/config/prbt/bus.yml
+++ b/prbt_robot_support/config/prbt/bus.yml
@@ -1,24 +1,24 @@
 options:
   dcf_path: "@BUS_CONFIG_PATH@"
+
 master:
   node_id: 1
   driver: "ros2_canopen::MasterDriver"
   package: "canopen_master_driver"
   sync_period: 10000
 
-prbt_joint_1:
-  node_id: 3
+defaults:
   dcf: "prbt_0_1.dcf"
-  # dcf_path: "install/prbt_robot_support/share/prbt_robot_support/config/prbt"
   driver: "ros2_canopen::Cia402Driver"
   package: "canopen_402_driver"
   period: 10
-  enable_lazy_load: false
   heartbeat_producer: 1000
+  switching_state: 2
+  position_mode: 1
   sdo:
     #- {index: 0x2060, sub_index: 2, value: 1} # Request brake test
-    #- {index: 0x6081, sub_index: 0, value: 1000}
-    #- {index: 0x6083, sub_index: 0, value: 2000}
+    - {index: 0x6081, sub_index: 0, value: 1000}
+    - {index: 0x6083, sub_index: 0, value: 2000}
     - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period.
     - {index: 0x6060, sub_index: 0, value: 7} # Make default mode to interpolated position mode.
   tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
@@ -54,245 +54,22 @@ prbt_joint_1:
       mapping:
       - {index: 0x607A, sub_index: 0} # target position
 
-prbt_joint_2:
-  node_id: 4
-  dcf: "prbt_0_1.dcf"
-  # dcf_path: "install/prbt_robot_support/share/prbt_robot_support/config/prbt"
-  driver: "ros2_canopen::Cia402Driver"
-  package: "canopen_402_driver"
-  period: 10
-  enable_lazy_load: false
-  heartbeat_producer: 1000
-  sdo:
-    #- {index: 0x2060, sub_index: 2, value: 1} # Request brake test
-    #- {index: 0x6081, sub_index: 0, value: 1000}
-    #- {index: 0x6083, sub_index: 0, value: 2000}
-    - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period.
-    - {index: 0x6060, sub_index: 0, value: 7} # Make default mode to interpolated position mode.
-  tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6041, sub_index: 0} # status word
-        - {index: 0x6061, sub_index: 0} # mode of operaiton display
-    2:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6064, sub_index: 0} # position actual value
-        - {index: 0x606c, sub_index: 0} # velocity actual position
-    3:
-      enabled: false
-    4:
-      enabled: false
-  rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x6040, sub_index: 0} # controlword
-      - {index: 0x6060, sub_index: 0} # mode of operation
-      - {index: 0x60C1, sub_index: 1} # interpolated position data
-    2:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x607A, sub_index: 0} # target position
-# 
-# 
-prbt_joint_3:
-  node_id: 5
-  dcf: "prbt_0_1.dcf"
-  # dcf_path: "install/prbt_robot_support/share/prbt_robot_support/config/prbt"
-  driver: "ros2_canopen::Cia402Driver"
-  package: "canopen_402_driver"
-  period: 10
-  enable_lazy_load: false
-  heartbeat_producer: 1000
-  sdo:
-    #- {index: 0x2060, sub_index: 2, value: 1} # Request brake test
-    #- {index: 0x6081, sub_index: 0, value: 1000}
-    #- {index: 0x6083, sub_index: 0, value: 2000}
-    - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period.
-    - {index: 0x6060, sub_index: 0, value: 7} # Make default mode to interpolated position mode.
-  tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6041, sub_index: 0} # status word
-        - {index: 0x6061, sub_index: 0} # mode of operaiton display
-    2:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6064, sub_index: 0} # position actual value
-        - {index: 0x606c, sub_index: 0} # velocity actual position
-    3:
-      enabled: false
-    4:
-      enabled: false
-  rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x6040, sub_index: 0} # controlword
-      - {index: 0x6060, sub_index: 0} # mode of operation
-      - {index: 0x60C1, sub_index: 1} # interpolated position data
-    2:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x607A, sub_index: 0} # target position
-# 
-prbt_joint_4:
-  node_id: 6
-  dcf: "prbt_0_1.dcf"
-  # dcf_path: "install/prbt_robot_support/share/prbt_robot_support/config/prbt"
-  driver: "ros2_canopen::Cia402Driver"
-  package: "canopen_402_driver"
-  period: 10
-  enable_lazy_load: false
-  heartbeat_producer: 1000
-  sdo:
-    #- {index: 0x2060, sub_index: 2, value: 1} # Request brake test
-    #- {index: 0x6081, sub_index: 0, value: 1000}
-    #- {index: 0x6083, sub_index: 0, value: 2000}
-    - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period.
-    - {index: 0x6060, sub_index: 0, value: 7} # Make default mode to interpolated position mode.
-  tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6041, sub_index: 0} # status word
-        - {index: 0x6061, sub_index: 0} # mode of operaiton display
-    2:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6064, sub_index: 0} # position actual value
-        - {index: 0x606c, sub_index: 0} # velocity actual position
-    3:
-      enabled: false
-    4:
-      enabled: false
-  rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x6040, sub_index: 0} # controlword
-      - {index: 0x6060, sub_index: 0} # mode of operation
-      - {index: 0x60C1, sub_index: 1} # interpolated position data
-    2:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x607A, sub_index: 0} # target position
-# 
-prbt_joint_5:
-  node_id: 7
-  dcf: "prbt_0_1.dcf"
-  # dcf_path: "install/prbt_robot_support/share/prbt_robot_support/config/prbt"
-  driver: "ros2_canopen::Cia402Driver"
-  package: "canopen_402_driver"
-  period: 10
-  enable_lazy_load: false
-  heartbeat_producer: 1000
-  sdo:
-    #- {index: 0x2060, sub_index: 2, value: 1} # Request brake test
-    #- {index: 0x6081, sub_index: 0, value: 1000}
-    #- {index: 0x6083, sub_index: 0, value: 2000}
-    - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period.
-    - {index: 0x6060, sub_index: 0, value: 7} # Make default mode to interpolated position mode.
-  tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6041, sub_index: 0} # status word
-        - {index: 0x6061, sub_index: 0} # mode of operaiton display
-    2:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6064, sub_index: 0} # position actual value
-        - {index: 0x606c, sub_index: 0} # velocity actual position
-    3:
-      enabled: false
-    4:
-      enabled: false
-  rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x6040, sub_index: 0} # controlword
-      - {index: 0x6060, sub_index: 0} # mode of operation
-      - {index: 0x60C1, sub_index: 1} # interpolated position data
-    2:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x607A, sub_index: 0} # target position
-# 
-prbt_joint_6:
-  node_id: 8
-  dcf: "prbt_0_1.dcf"
-  # dcf_path: "install/prbt_robot_support/share/prbt_robot_support/config/prbt"
-  driver: "ros2_canopen::Cia402Driver"
-  package: "canopen_402_driver"
-  period: 10
-  enable_lazy_load: false
-  heartbeat_producer: 1000
-  sdo:
-    #- {index: 0x2060, sub_index: 2, value: 1} # Request brake test
-    #- {index: 0x6081, sub_index: 0, value: 1000}
-    #- {index: 0x6083, sub_index: 0, value: 2000}
-    - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period.
-    - {index: 0x6060, sub_index: 0, value: 7} # Make default mode to interpolated position mode.
-  tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6041, sub_index: 0} # status word
-        - {index: 0x6061, sub_index: 0} # mode of operaiton display
-    2:
-      enabled: true
-      cob_id: "auto"
-      transmission: 0x01
-      mapping:
-        - {index: 0x6064, sub_index: 0} # position actual value
-        - {index: 0x606c, sub_index: 0} # velocity actual position
-    3:
-      enabled: false
-    4:
-      enabled: false
-  rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
-    1:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x6040, sub_index: 0} # controlword
-      - {index: 0x6060, sub_index: 0} # mode of operation
-      - {index: 0x60C1, sub_index: 1} # interpolated position data
-    2:
-      enabled: true
-      cob_id: "auto"
-      mapping:
-      - {index: 0x607A, sub_index: 0} # target position
-# 
-# 
\ No newline at end of file
+
+nodes:
+  prbt_joint_1:
+    node_id: 3
+  
+  prbt_joint_2:
+    node_id: 4
+  
+  prbt_joint_3: 
+    node_id: 5
+  
+  prbt_joint_4:     
+    node_id: 6
+  
+  prbt_joint_5:
+    node_id: 7
+  
+  prbt_joint_6:
+    node_id: 8
\ No newline at end of file
diff --git a/prbt_robot_support/config/prbt_ros2_control.yaml b/prbt_robot_support/config/prbt_ros2_control.yaml
index 1f9cbb7..68f89c0 100644
--- a/prbt_robot_support/config/prbt_ros2_control.yaml
+++ b/prbt_robot_support/config/prbt_ros2_control.yaml
@@ -1,69 +1,27 @@
-# controller_manager:
-#   ros__parameters:
-#     update_rate: 10  # Hz
-
-#     joint_state_broadcaster:
-#       type: joint_state_broadcaster/JointStateBroadcaster
-
-#     prbt_joint_1_controller:
-#       type: canopen_ros2_controllers/Cia402DeviceController
-    
-#     prbt_joint_2_controller:
-#       type: canopen_ros2_controllers/Cia402DeviceController
-    
-#     prbt_joint_3_controller:
-#       type: canopen_ros2_controllers/Cia402DeviceController
-
-#     prbt_joint_4_controller:
-#       type: canopen_ros2_controllers/Cia402DeviceController
-
-#     prbt_joint_5_controller:
-#       type: canopen_ros2_controllers/Cia402DeviceController
-
-#     prbt_joint_6_controller:
-#       type: canopen_ros2_controllers/Cia402DeviceController
-
-#     joint_trajectory_controller:
-#       type: joint_trajectory_controller/JointTrajectoryController
-
-#     forward_position_controller:
-#       type: forward_command_controller/ForwardCommandController
-
-# prbt_joint_1_controller:
-#   ros__parameters:
-#     joint: prbt_joint_1
-
-# prbt_joint_2_controller:
-#   ros__parameters:
-#     joint: prbt_joint_2
-
-# prbt_joint_3_controller:
-#   ros__parameters:
-#     joint: prbt_joint_3
-
-# prbt_joint_4_controller:
-#   ros__parameters:
-#     joint: prbt_joint_4
-
-# prbt_joint_5_controller:
-#   ros__parameters:
-#     joint: prbt_joint_5
-
-# prbt_joint_6_controller:
-#   ros__parameters:
-#     joint: prbt_joint_6
-
-
-# forward_position_controller:
-#   ros__parameters:
-#     joints:
-#       - prbt_joint_1
-#       - prbt_joint_2
-#       - prbt_joint_3
-#       - prbt_joint_4
-#       - prbt_joint_5
-#       - prbt_joint_6
-#     interface_name: position
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    # joint_trajectory_controller:
+    #   type: joint_trajectory_controller/JointTrajectoryController
+
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - prbt_joint_1
+      - prbt_joint_2
+      - prbt_joint_3
+      - prbt_joint_4
+      - prbt_joint_5
+      - prbt_joint_6
+    interface_name: position
 
 # joint_trajectory_controller:
 #   ros__parameters:
diff --git a/prbt_robot_support/launch/prbt_lifecycle_setup.launch.py b/prbt_robot_support/launch/prbt_lifecycle_setup.launch.py
index 3c40e58..258953f 100644
--- a/prbt_robot_support/launch/prbt_lifecycle_setup.launch.py
+++ b/prbt_robot_support/launch/prbt_lifecycle_setup.launch.py
@@ -64,7 +64,7 @@ def generate_launch_description():
                 "prbt_lifecycle",
                 "bus.yml",
             ),
-            "can_interface_name_name": "vcan0",
+            "can_interface_name": "vcan0",
         }.items(),
     )
 
diff --git a/prbt_robot_support/launch/prbt_ros2_control.launch.py b/prbt_robot_support/launch/prbt_ros2_control.launch.py
index cb54c78..4bbb6f5 100644
--- a/prbt_robot_support/launch/prbt_ros2_control.launch.py
+++ b/prbt_robot_support/launch/prbt_ros2_control.launch.py
@@ -82,7 +82,7 @@ def launch_setup(context, *args, **kwargs):
             name,
             " ",
             "prefix:=",
-            prefix,
+            name,
             " ",
             "bus_config:=",
             bus_config,
@@ -123,42 +123,6 @@ def launch_setup(context, *args, **kwargs):
         arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
     )
 
-    prbt_joint_1_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["prbt_joint_1_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    prbt_joint_2_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["prbt_joint_2_controller", "--controller-manager", "/controller_manager"],
-    )    
-    
-    prbt_joint_3_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["prbt_joint_3_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    prbt_joint_4_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["prbt_joint_4_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    prbt_joint_5_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["prbt_joint_5_controller", "--controller-manager", "/controller_manager"],
-    )    
-    
-    prbt_joint_6_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["prbt_joint_6_controller", "--controller-manager", "/controller_manager"],
-    )
-
     forward_position_controller = Node(
         package="controller_manager",
         executable="spawner",
@@ -239,12 +203,6 @@ def launch_setup(context, *args, **kwargs):
         control_node,
         robot_state_publisher_node,
         joint_state_broadcaster_spawner,
-        prbt_joint_1_controller_spawner,
-        prbt_joint_2_controller_spawner,
-        prbt_joint_3_controller_spawner,
-        prbt_joint_4_controller_spawner,
-        prbt_joint_5_controller_spawner,
-        prbt_joint_6_controller_spawner,
         forward_position_controller,
     ]
 
diff --git a/prbt_robot_support/launch/prbt_setup.launch.py b/prbt_robot_support/launch/prbt_setup.launch.py
index 8023815..68d59ad 100644
--- a/prbt_robot_support/launch/prbt_setup.launch.py
+++ b/prbt_robot_support/launch/prbt_setup.launch.py
@@ -64,7 +64,7 @@ def generate_launch_description():
                 "prbt",
                 "bus.yml",
             ),
-            "can_interface_name_name": "vcan0",
+            "can_interface_name": "vcan0",
         }.items(),
     )
 
diff --git a/prbt_robot_support/urdf/prbt.ros2_control.xacro b/prbt_robot_support/urdf/prbt.ros2_control.xacro
index 41fbac4..2fc39f7 100644
--- a/prbt_robot_support/urdf/prbt.ros2_control.xacro
+++ b/prbt_robot_support/urdf/prbt.ros2_control.xacro
@@ -6,34 +6,34 @@
       prefix
       bus_config
       master_config
-      can_interface
+      can_interface_name
       master_bin">
 
         <ros2_control name="${name}" type="system">
             <hardware>
-                <plugin>canopen_ros2_control/Cia402System</plugin>
+                <plugin>canopen_ros2_control/RobotSystem</plugin>
                 <param name="bus_config">${bus_config}</param>
                 <param name="master_config">${master_config}</param>
-                <param name="can_interface">${can_interface}</param>
+                <param name="can_interface_name">${can_interface_name}</param>
                 <param name="master_bin">"${master_bin}"</param>
             </hardware>
             <joint name="${prefix}joint_1">
-                <param name="node_id">3</param>
+                <param name="device_name">prbt_joint_1</param>
             </joint>
             <joint name="${prefix}joint_2">
-                <param name="node_id">4</param>
+                <param name="device_name">prbt_joint_2</param>
             </joint>
             <joint name="${prefix}joint_3">
-                <param name="node_id">5</param>
+                <param name="device_name">prbt_joint_3</param>
             </joint>
             <joint name="${prefix}joint_4">
-                <param name="node_id">6</param>
+                <param name="device_name">prbt_joint_4</param>
             </joint>
             <joint name="${prefix}joint_5">
-                <param name="node_id">7</param>
+                <param name="device_name">prbt_joint_5</param>
             </joint>
             <joint name="${prefix}joint_6">
-                <param name="node_id">8</param>
+                <param name="device_name">prbt_joint_6</param>
             </joint>
         </ros2_control>
 
diff --git a/prbt_robot_support/urdf/prbt.xacro b/prbt_robot_support/urdf/prbt.xacro
index f3e42bb..af24e8b 100644
--- a/prbt_robot_support/urdf/prbt.xacro
+++ b/prbt_robot_support/urdf/prbt.xacro
@@ -3,12 +3,12 @@
     <xacro:arg name="robot_prefix" default="prbt_"/>
     <xacro:arg name="bus_config" default="$(find prbt_robot_support)/config/prbt/bus.yml"/>
     <xacro:arg name="master_config" default="$(find prbt_robot_support)/config/prbt/master.dcf"/>
-    <xacro:arg name="can_interface" default="vcan0"/>
+    <xacro:arg name="can_interface_name" default="vcan0"/>
     <xacro:arg name="master_bin" default=""/>
 
     <!-- Import prbt urdf file -->
     <xacro:include filename="$(find prbt_robot_support)/urdf/prbt_macro.xacro" />
-    <!--xacro:include filename="$(find prbt_robot_support)/urdf/prbt.ros2_control.xacro" /-->
+    <xacro:include filename="$(find prbt_robot_support)/urdf/prbt.ros2_control.xacro" />
 
 
     <!-- instantiate the robot -->
@@ -18,5 +18,12 @@
         <parent link="world"/>
         <child link="$(arg robot_prefix)base_link"/>
     </joint>
+    <xacro:prbt_ros2_control
+        name="$(arg robot_prefix)"
+        prefix="$(arg robot_prefix)"
+        bus_config="$(arg bus_config)"
+        master_config="$(arg master_config)"
+        can_interface_name="$(arg can_interface)"
+        master_bin="$(arg master_bin)" />
 
 </robot>