From 97e990729fee86e7bc677cdd544b602d9b7b87b0 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Tue, 2 May 2023 19:27:10 +0000 Subject: [PATCH 1/4] Bugfix Signed-off-by: Christoph Hellmann Santos --- prbt_robot_support/launch/prbt_lifecycle_setup.launch.py | 2 +- prbt_robot_support/launch/prbt_setup.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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_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(), ) From 020fb875442338eb23af7dc525b7ae89369db66a Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 3 May 2023 21:45:10 +0200 Subject: [PATCH 2/4] Changes for robot controller Signed-off-by: Christoph Hellmann Santos --- prbt_robot_support/config/prbt/bus.yml | 32 +++--- .../config/prbt_ros2_control.yaml | 105 +++++++----------- .../launch/prbt_ros2_control.launch.py | 43 +------ .../urdf/prbt.ros2_control.xacro | 6 +- prbt_robot_support/urdf/prbt.xacro | 11 +- 5 files changed, 74 insertions(+), 123 deletions(-) diff --git a/prbt_robot_support/config/prbt/bus.yml b/prbt_robot_support/config/prbt/bus.yml index b69ae8f..5f7081a 100644 --- a/prbt_robot_support/config/prbt/bus.yml +++ b/prbt_robot_support/config/prbt/bus.yml @@ -15,10 +15,11 @@ prbt_joint_1: period: 10 enable_lazy_load: false heartbeat_producer: 1000 + switching_state: 2 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 @@ -63,10 +64,11 @@ prbt_joint_2: period: 10 enable_lazy_load: false heartbeat_producer: 1000 + switching_state: 2 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 @@ -112,10 +114,11 @@ prbt_joint_3: period: 10 enable_lazy_load: false heartbeat_producer: 1000 + switching_state: 2 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 @@ -160,10 +163,11 @@ prbt_joint_4: period: 10 enable_lazy_load: false heartbeat_producer: 1000 + switching_state: 2 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 @@ -208,10 +212,11 @@ prbt_joint_5: period: 10 enable_lazy_load: false heartbeat_producer: 1000 + switching_state: 2 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 @@ -256,10 +261,11 @@ prbt_joint_6: period: 10 enable_lazy_load: false heartbeat_producer: 1000 + switching_state: 2 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 @@ -288,11 +294,11 @@ prbt_joint_6: 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 + - {index: 0x60C1, sub_index: 1} # interpolated position data # # \ 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..751b884 100644 --- a/prbt_robot_support/config/prbt_ros2_control.yaml +++ b/prbt_robot_support/config/prbt_ros2_control.yaml @@ -1,69 +1,42 @@ -# 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 + + robot_controller: + type: canopen_ros2_controllers/Cia402RobotController + + # joint_trajectory_controller: + # type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +robot_controller: + ros__parameters: + joints: + - prbt_joint_1 + - prbt_joint_2 + - prbt_joint_3 + - prbt_joint_4 + - prbt_joint_5 + - prbt_joint_6 + operation_mode: 1 + command_poll_freq: 5 + + +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_ros2_control.launch.py b/prbt_robot_support/launch/prbt_ros2_control.launch.py index cb54c78..3df9f0e 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,40 +123,10 @@ 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( + robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["prbt_joint_6_controller", "--controller-manager", "/controller_manager"], + arguments=["robot_controller", "--controller-manager", "/controller_manager"], ) forward_position_controller = Node( @@ -239,12 +209,7 @@ 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, + robot_controller_spawner, forward_position_controller, ] diff --git a/prbt_robot_support/urdf/prbt.ros2_control.xacro b/prbt_robot_support/urdf/prbt.ros2_control.xacro index 41fbac4..68ecd98 100644 --- a/prbt_robot_support/urdf/prbt.ros2_control.xacro +++ b/prbt_robot_support/urdf/prbt.ros2_control.xacro @@ -6,15 +6,15 @@ prefix bus_config master_config - can_interface + can_interface_name master_bin"> - canopen_ros2_control/Cia402System + canopen_ros2_control/Cia402RobotSystem ${bus_config} ${master_config} - ${can_interface} + ${can_interface_name} "${master_bin}" 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 @@ - + - + @@ -18,5 +18,12 @@ + From 0ffd8b74c1b48ad946d54c6d3aacc44721e89041 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Mon, 15 May 2023 10:56:39 +0200 Subject: [PATCH 3/4] Add robot system interface Signed-off-by: Christoph Hellmann Santos --- prbt_robot_support/CMakeLists.txt | 5 +++-- prbt_robot_support/config/prbt/bus.yml | 6 ++++++ prbt_robot_support/config/prbt_ros2_control.yaml | 15 --------------- .../launch/prbt_ros2_control.launch.py | 7 ------- prbt_robot_support/urdf/prbt.ros2_control.xacro | 14 +++++++------- 5 files changed, 16 insertions(+), 31 deletions(-) diff --git a/prbt_robot_support/CMakeLists.txt b/prbt_robot_support/CMakeLists.txt index bf6fb34..6f93142 100644 --- a/prbt_robot_support/CMakeLists.txt +++ b/prbt_robot_support/CMakeLists.txt @@ -14,10 +14,11 @@ generate_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 5f7081a..1d86ee5 100644 --- a/prbt_robot_support/config/prbt/bus.yml +++ b/prbt_robot_support/config/prbt/bus.yml @@ -16,6 +16,7 @@ prbt_joint_1: 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} @@ -65,6 +66,7 @@ prbt_joint_2: 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} @@ -115,6 +117,7 @@ prbt_joint_3: 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} @@ -164,6 +167,7 @@ prbt_joint_4: 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} @@ -213,6 +217,7 @@ prbt_joint_5: 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} @@ -262,6 +267,7 @@ prbt_joint_6: 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} diff --git a/prbt_robot_support/config/prbt_ros2_control.yaml b/prbt_robot_support/config/prbt_ros2_control.yaml index 751b884..68f89c0 100644 --- a/prbt_robot_support/config/prbt_ros2_control.yaml +++ b/prbt_robot_support/config/prbt_ros2_control.yaml @@ -5,27 +5,12 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - robot_controller: - type: canopen_ros2_controllers/Cia402RobotController - # joint_trajectory_controller: # type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: forward_command_controller/ForwardCommandController -robot_controller: - ros__parameters: - joints: - - prbt_joint_1 - - prbt_joint_2 - - prbt_joint_3 - - prbt_joint_4 - - prbt_joint_5 - - prbt_joint_6 - operation_mode: 1 - command_poll_freq: 5 - forward_position_controller: ros__parameters: diff --git a/prbt_robot_support/launch/prbt_ros2_control.launch.py b/prbt_robot_support/launch/prbt_ros2_control.launch.py index 3df9f0e..4bbb6f5 100644 --- a/prbt_robot_support/launch/prbt_ros2_control.launch.py +++ b/prbt_robot_support/launch/prbt_ros2_control.launch.py @@ -123,12 +123,6 @@ def launch_setup(context, *args, **kwargs): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["robot_controller", "--controller-manager", "/controller_manager"], - ) - forward_position_controller = Node( package="controller_manager", executable="spawner", @@ -209,7 +203,6 @@ def launch_setup(context, *args, **kwargs): control_node, robot_state_publisher_node, joint_state_broadcaster_spawner, - robot_controller_spawner, forward_position_controller, ] diff --git a/prbt_robot_support/urdf/prbt.ros2_control.xacro b/prbt_robot_support/urdf/prbt.ros2_control.xacro index 68ecd98..2fc39f7 100644 --- a/prbt_robot_support/urdf/prbt.ros2_control.xacro +++ b/prbt_robot_support/urdf/prbt.ros2_control.xacro @@ -11,29 +11,29 @@ - canopen_ros2_control/Cia402RobotSystem + canopen_ros2_control/RobotSystem ${bus_config} ${master_config} ${can_interface_name} "${master_bin}" - 3 + prbt_joint_1 - 4 + prbt_joint_2 - 5 + prbt_joint_3 - 6 + prbt_joint_4 - 7 + prbt_joint_5 - 8 + prbt_joint_6 From fa6a54c2429347afcf185bc21023286ff28fcc10 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Mon, 15 May 2023 23:43:23 +0200 Subject: [PATCH 4/4] Update to cogen Signed-off-by: Christoph Hellmann Santos --- prbt_robot_support/CMakeLists.txt | 2 +- prbt_robot_support/config/prbt/bus.yml | 277 ++----------------------- 2 files changed, 22 insertions(+), 257 deletions(-) diff --git a/prbt_robot_support/CMakeLists.txt b/prbt_robot_support/CMakeLists.txt index 6f93142..3430203 100644 --- a/prbt_robot_support/CMakeLists.txt +++ b/prbt_robot_support/CMakeLists.txt @@ -10,7 +10,7 @@ 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 diff --git a/prbt_robot_support/config/prbt/bus.yml b/prbt_robot_support/config/prbt/bus.yml index 1d86ee5..5ddb2fd 100644 --- a/prbt_robot_support/config/prbt/bus.yml +++ b/prbt_robot_support/config/prbt/bus.yml @@ -1,19 +1,17 @@ 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 @@ -56,255 +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 - 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: 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 - 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: 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 - 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: 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 - 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: 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 - 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: 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 - 2: - enabled: true - cob_id: "auto" - mapping: - - {index: 0x607A, sub_index: 0} # target position - - {index: 0x60C1, sub_index: 1} # interpolated position data -# -# \ 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