Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Configuration problem for Ingenia EVE XCR and EVE S XCR #147

Open
iamdrfly opened this issue Oct 7, 2024 · 124 comments
Open

Configuration problem for Ingenia EVE XCR and EVE S XCR #147

iamdrfly opened this issue Oct 7, 2024 · 124 comments

Comments

@iamdrfly
Copy link

iamdrfly commented Oct 7, 2024

Hi =D

I am trying to control three Everest XCR drives using ROS2 EtherCAT driver. I followed the instructions from this guide. However, I can only enable and move one drive (Drive 3), while the others do not reach the "operation enabled" state.

Since my 3 drives have different product IDs, I created separate YAML configuration files for each of them. I also attempted to switch the RPDO and TPDO orders without success. Below are the details of the configuration for each drive.

The drive's reference manual manual
EtherCAT & CANopen registers link

Configuration for Drive 1 (EVE XCR):

# Configuration file for Ingenia EVE-XCR-E drive
vendor_id: 0x0000029c
product_id: 0x02c30001
auto_fault_reset: true
tpdo: 
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_word}
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 0.0000001198422491}
      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 0.00006283185307}
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}
      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort}
rpdo: 
  - index: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word}
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position}
      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity}
      - {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}
      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort}

Configuration for Drive 2 (EVE XCR):

vendor_id: 0x0000029c
product_id: 0x02c31001
auto_fault_reset: true
rpdo:  
  - index: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word}
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position}
      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity}
      - {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}
      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort}
tpdo:  
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_word}
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 0.0000002396844981}
      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 0.0001256637061}
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}
      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort}

Configuration for Drive 3 (Working drive EVE S XCR):

vendor_id: 0x0000029c
product_id: 0x03b31001
auto_fault_reset: true
rpdo:  
  - index: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word}
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position}
      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity}
      - {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}
      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort}
tpdo:  
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_word}
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 0.0000002396844981}
      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 0.0001256637061}
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}
      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort}

Below is the ros2_control EtherCAT configuration:

    <ros2_control name="ec_single_axis" type="system">
      <hardware>
        <plugin>ethercat_driver/EthercatDriver</plugin>
        <param name="master_id">0</param>
        <param name="control_frequency">1000</param>
      </hardware>

    <joint name="${name}_HAA">
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <state_interface name="error_code"/>
        <command_interface name="position">
          <param name="min">"${haa_min_count}"</param>
          <param name="max">"${haa_min_count}"</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">"${haa_min_mrev_sec}" </param>
          <param name="max">"${haa_max_mrev_sec}"</param>
        </command_interface>
        <command_interface name="effort"/>
        <command_interface name="reset_fault"/>
        <ec_module name="drive_1">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">0</param>
          <param name="mode_of_operation">8</param>
          <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_1c.yaml</param>
        </ec_module>
    </joint>

    <joint name="${name}_HFE">
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <state_interface name="error_code"/>
        <command_interface name="position">
          <param name="min">"${hfe_min_count}"</param>
          <param name="max">"${hfe_min_count}"</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">"${hfe_min_mrev_sec}" </param>
          <param name="max">"${hfe_max_mrev_sec}"</param>
        </command_interface>
        <command_interface name="effort"/>
        <command_interface name="reset_fault"/>
        <ec_module name="drive_2">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">1</param>
          <param name="mode_of_operation">8</param>
          <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_2c.yaml</param>
        </ec_module>
    </joint>

    <joint name="${name}_KFE">
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <state_interface name="error_code"/>
        <command_interface name="position">
          <param name="min">"${kfe_min_count}"</param>
          <param name="max">"${kfe_min_count}"</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">"${kfe_min_mrev_sec}" </param>
          <param name="max">"${kfe_max_mrev_sec}"</param>
        </command_interface>
        <command_interface name="effort"/>
        <command_interface name="reset_fault"/>
        <ec_module name="drive_1">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">2</param>
          <param name="mode_of_operation">8</param>
          <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_3b.yaml</param>
        </ec_module>
    </joint>

    </ros2_control>

My setup is based on Ubuntu 22.04 and ROS2 Rolling.

I've attached the dmesg output obtained with EtherCAT debug level set to 1 for both scenarios (with and without the 0x6060 register).

the output from ros2 for the first scenario is:

[INFO] [launch]: All log files can be found below /home/lab/.ros/log/2024-10-07-13-20-27-556845-lab-X570-AORUS-PRO-1754316
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [1754335]
[INFO] [robot_state_publisher-2]: process started with pid [1754337]
[INFO] [spawner-3]: process started with pid [1754339]
[INFO] [spawner-4]: process started with pid [1754341]
[robot_state_publisher-2] [INFO] [1728300027.781726491] [robot_state_publisher]: Robot initialized
[ros2_control_node-1] [WARN] [1728300027.787804042] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-1] [INFO] [1728300027.787959145] [resource_manager]: Loading hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300027.789292642] [resource_manager]: Initialize hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300027.789513276] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1728300027.791563557] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1728300027.792113448] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1728300027.792629878] [EthercatDriver]: Got 3 modules
[ros2_control_node-1] [INFO] [1728300027.792639488] [resource_manager]: Successful initialization of hardware 'ec_single_axis'
[ros2_control_node-1] [INFO] [1728300027.792739280] [resource_manager]: 'configure' hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300027.792742820] [resource_manager]: Successful 'configure' of hardware 'ec_single_axis'
[ros2_control_node-1] [INFO] [1728300027.792746681] [resource_manager]: 'activate' hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300027.792749211] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6040, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x607a, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x60ff, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6071, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6060, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6041, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6064, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x606c, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6077, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6061, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6040, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x607a, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x60ff, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6071, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6060, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6041, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6064, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x606c, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6077, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6061, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6040, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x607a, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x60ff, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6071, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6060, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6041, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6064, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x606c, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6077, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6061, 0x0}
[ros2_control_node-1] [INFO] [1728300027.793665469] [EthercatDriver]: Activated EcMaster!
[ros2_control_node-1] 4 slave(s).
[ros2_control_node-1] Master AL states: 0x02.
[ros2_control_node-1] Link is up.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] [INFO] [1728300028.793936625] [EthercatDriver]: updated!
[ros2_control_node-1] [INFO] [1728300028.793966966] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1728300028.793977926] [resource_manager]: Successful 'activate' of hardware 'ec_single_axis'
[ros2_control_node-1] [INFO] [1728300028.797481686] [controller_manager]: update rate is 1000 Hz
[ros2_control_node-1] [WARN] [1728300028.797540387] [controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details.
[ros2_control_node-1] [INFO] [1728300028.830893662] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-3] [INFO] [1728300028.838196588] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [INFO] [1728300028.838753069] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1728300028.838802080] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1728300028.846580915] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ros2_control_node-1] [INFO] [1728300028.854602644] [controller_manager]: Loading controller 'forward_position_controller'
[spawner-4] [INFO] [1728300028.860292478] [spawner_forward_position_controller]: Loaded forward_position_controller
[ros2_control_node-1] [INFO] [1728300028.861003422] [controller_manager]: Configuring controller 'forward_position_controller'
[ros2_control_node-1] [INFO] [1728300028.861440281] [forward_position_controller]: configure successful
[ros2_control_node-1] [INFO] [1728300028.862676176] [forward_position_controller]: activate successful
[spawner-4] [INFO] [1728300028.864230787] [spawner_forward_position_controller]: Configured and activated forward_position_controller
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Domain: WC 3.
[ros2_control_node-1] Domain: State 1.
[ros2_control_node-1] STATE: Fault with status word :1560
[INFO] [spawner-3]: process has finished cleanly [pid 1754339]
[INFO] [spawner-4]: process has finished cleanly [pid 1754341]
[ros2_control_node-1] Slave: State 0x04.
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Domain: WC 6.
[ros2_control_node-1] STATE: Fault with status word :1560
[ros2_control_node-1] Slave: State 0x04.
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Domain: WC 9.
[ros2_control_node-1] Domain: State 2.
[ros2_control_node-1] Slave: State 0x04.
[ros2_control_node-1] STATE: Fault with status word :5656
[ros2_control_node-1] Master AL states: 0x0E.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.

The output from ros2 for second scenario (without x6060) is:

[INFO] [launch]: All log files can be found below /home/lab/.ros/log/2024-10-07-13-24-39-278208-lab-X570-AORUS-PRO-1758210
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [1758220]
[INFO] [robot_state_publisher-2]: process started with pid [1758222]
[INFO] [spawner-3]: process started with pid [1758224]
[INFO] [spawner-4]: process started with pid [1758226]
[robot_state_publisher-2] [INFO] [1728300279.504662735] [robot_state_publisher]: Robot initialized
[ros2_control_node-1] [WARN] [1728300279.510317613] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-1] [INFO] [1728300279.510507547] [resource_manager]: Loading hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300279.511804534] [resource_manager]: Initialize hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300279.511998058] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1728300279.514183793] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1728300279.514732845] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1728300279.515253836] [EthercatDriver]: Got 3 modules
[ros2_control_node-1] [INFO] [1728300279.515263916] [resource_manager]: Successful initialization of hardware 'ec_single_axis'
[ros2_control_node-1] [INFO] [1728300279.515372158] [resource_manager]: 'configure' hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300279.515376178] [resource_manager]: Successful 'configure' of hardware 'ec_single_axis'
[ros2_control_node-1] [INFO] [1728300279.515379858] [resource_manager]: 'activate' hardware 'ec_single_axis' 
[ros2_control_node-1] [INFO] [1728300279.515382408] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6040, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x607a, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x60ff, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6071, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6041, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6064, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x606c, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6077, 0x0}
[ros2_control_node-1] {0, 0, 0x29c, 0x2c30001, 0x6061, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6040, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x607a, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x60ff, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6071, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6041, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6064, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x606c, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6077, 0x0}
[ros2_control_node-1] {0, 1, 0x29c, 0x2c31001, 0x6061, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6040, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x607a, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x60ff, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6071, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6060, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6041, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6064, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x606c, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6077, 0x0}
[ros2_control_node-1] {0, 2, 0x29c, 0x3b31001, 0x6061, 0x0}
[ros2_control_node-1] [INFO] [1728300279.516150554] [EthercatDriver]: Activated EcMaster!
[ros2_control_node-1] 4 slave(s).
[ros2_control_node-1] Master AL states: 0x02.
[ros2_control_node-1] Link is up.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] [INFO] [1728300280.516692270] [EthercatDriver]: updated!
[ros2_control_node-1] [INFO] [1728300280.516718850] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1728300280.516728511] [resource_manager]: Successful 'activate' of hardware 'ec_single_axis'
[ros2_control_node-1] [INFO] [1728300280.520593511] [controller_manager]: update rate is 1000 Hz
[ros2_control_node-1] [WARN] [1728300280.520670382] [controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details.
[ros2_control_node-1] Domain: WC 3.
[ros2_control_node-1] Domain: State 1.
[ros2_control_node-1] STATE: Fault with status word :1560
[ros2_control_node-1] [INFO] [1728300280.652741673] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-3] [INFO] [1728300280.661549216] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [INFO] [1728300280.662111448] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1728300280.662173569] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1728300280.669453371] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ros2_control_node-1] [INFO] [1728300280.673585577] [controller_manager]: Loading controller 'forward_position_controller'
[spawner-4] [INFO] [1728300280.679462130] [spawner_forward_position_controller]: Loaded forward_position_controller
[ros2_control_node-1] [INFO] [1728300280.680107143] [controller_manager]: Configuring controller 'forward_position_controller'
[ros2_control_node-1] [INFO] [1728300280.680632364] [forward_position_controller]: configure successful
[ros2_control_node-1] [INFO] [1728300280.681855009] [forward_position_controller]: activate successful
[spawner-4] [INFO] [1728300280.683515944] [spawner_forward_position_controller]: Configured and activated forward_position_controller
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[ros2_control_node-1] Domain: WC 6.
[ros2_control_node-1] STATE: Fault with status word :1560
[INFO] [spawner-3]: process has finished cleanly [pid 1758224]
[INFO] [spawner-4]: process has finished cleanly [pid 1758226]
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Domain: WC 9.
[ros2_control_node-1] Domain: State 2.
[ros2_control_node-1] Slave: State 0x04.
[ros2_control_node-1] STATE: Fault with status word :5656
[ros2_control_node-1] Master AL states: 0x0A.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.

test_with.txt
test_without_6060.txt

Many thanks for your time

Best
Alessio

@ammaraljodah
Copy link

Check few things like:

  1. Check the state if the drives
    ethercat slaves
    Are they all opertional
  2. If they are check the status word, and faluts, did the faluts clear, if not check why
  3. Are you sending the command to enable them, are you claiming the hardware interfaces correctly

@iamdrfly
Copy link
Author

iamdrfly commented Oct 14, 2024

Hi @ammaraljodah,
Thank you for your time,

  1. Only the third drive reaches the OP. The other two drives (first and second) become SAFEOP+E
  2. when I kill the ros2_ethercat all the drives switch to PREOP.
  3. maybe I am doing wrong but according to the documentation is not necessary to claim the hw interface to reach the OP.
    "After launching the well-configured drive, by default and without fault, motor drive module is brought automatically into the state OPERATION_ENABLED making it ready for use. Automatic transition is only enabled when the control_word command interface is either missing or set to NaN making it possible for the user to take control over the motor drive’s state machine by sending corresponding state transition values using the control_word command interface."

I have noticed that the problem can be caused by the PDO register

  • {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}

Do you have any suggestions?

Best
Alessio

@ammaraljodah
Copy link

Don't worry about the hardware interface, you are right once it comes into OP it will enable automaticly.
You have problem in configuration.
What is the output of
sudo dmesg | grep EtherCAT

@iamdrfly
Copy link
Author

iamdrfly commented Oct 15, 2024

Hi @ammaraljodah,

many thanks for your support,
attached the result of sudo dmesg -w (test_2.txt) and sudo dmesg | grep EtherCAT (test_1.txt).
test_1.txt
test_2.txt

The configuration files for the drives are equal for the pdo, only the vendor_id e product_id are different. To find these values I used the command ethercat slaves.

image

Best
Alessio

@ammaraljodah
Copy link

You need to give the permission for the user.
Ros control needs to set pre-emption for the process.

[WARN] [1728300028.797540387] [controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details.
[ros2_control_node-1] [INFO] [1728300028.830893662] [controller_manager]: Loading

@ammaraljodah
Copy link

@ammaraljodah
Copy link

Drop the controller frequency to 100Hz in the urdf of the plugin and in the yaml of the ros2 control.
Are you using preempt-rt kernel? If not, you should use it.

@ammaraljodah
Copy link

Also use native driver for your ethernet controller instead if the generic

@iamdrfly
Copy link
Author

Hi @ammaraljodah,

You are right I am no using a rt patch kernel. Does it make any difference (the problem is related to the pdo config, i do not understand the correlation)?

Unfortunately, I have not an ethernet card compatible with the ones required by link, as you can see below:


lspci -vv | grep Ethernet
04:00.0 Ethernet controller: Intel Corporation I350 Gigabit Network Connection (rev 01)
	Subsystem: Intel Corporation Ethernet Server Adapter I350-T2
04:00.1 Ethernet controller: Intel Corporation I350 Gigabit Network Connection (rev 01)
	Subsystem: Intel Corporation Ethernet Server Adapter I350-T2
05:00.0 Ethernet controller: Intel Corporation I211 Gigabit Network Connection (rev 03)
0a:00.0 Ethernet controller: Intel Corporation I350 Gigabit Network Connection (rev 01)
	Subsystem: Intel Corporation Ethernet Server Adapter I350-T2
0a:00.1 Ethernet controller: Intel Corporation I350 Gigabit Network Connection (rev 01)
	Subsystem: Intel Corporation Ethernet Server Adapter I350-T2

Now I will install the preempt patch and I will update you.

Many thanks
Alessio

@ammaraljodah
Copy link

How you know it is PDO config, have you mapped them correctly??

@ammaraljodah
Copy link

You need to map the PDOs using a set of SDOs if your drive is having flexible PDOs

@iamdrfly
Copy link
Author

iamdrfly commented Oct 16, 2024

Hi @ammaraljodah,

I have followed your suggestion to use rt kernel but it doesn't work, I have the same problems.

Yes, the pdo used in the config files should be fine. Here the sdo available for the ingenia drives sdo_map.txt (This file is obtained by using ethercat sdos -p 0 )

Here the output of dmesg -w dmesg_rt.txt

Here the output of sudo dmesg | grep EtherCAT grep_dmesg.txt

I don't understand how I can control and move only one of the 3 drives, which are identical except for the product and vendor id.

Many thanks for your help

Best regards
Alessio

@ammaraljodah
Copy link

You need to map the PDOs, currently you are not mapping any at all.

@iamdrfly
Copy link
Author

Hi @ammaraljodah,

I'm not sure what you're referring to.
I've set up the PDO configuration using the YAML files.

# Configuration file for Ingenia EVE-XCR-E drive
vendor_id: 0x0000029c
product_id: 0x02c30001
auto_fault_reset: true
tpdo: 
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_word}
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 0.0000001198422491}
      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 0.00006283185307}
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}
      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort}
rpdo: 
  - index: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word}
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position}
      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity}
      - {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}
      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort}

What am I missing? Could you please be more clear?

Best
Alessio

@ammaraljodah
Copy link

Is this servo drive has fixed mapping or flexible mapping?
Are those PDOs the same as the fixed mapping?

@iamdrfly
Copy link
Author

iamdrfly commented Oct 16, 2024

Sure! Here's the revised version with your additional details:

Hi @ammaraljodah,

Good question! I believe there are 4 predefined RPDOs and 4 predefined TPDOs (refer to page 44 of the Ingenia manual). However, when I power cycle the system and run ethercat cstruct in the terminal, all the PDOs show up as empty.
If I run the ethercat cstruct command after first launching of ethercat_driver_ros2, I can see the PDOs that I have configured.

Does this servo drive use fixed mapping or flexible mapping?

How can I verify this?

Are the current PDOs the same as the fixed mapping ones?

If the PDOs are present on the drive, I believe the ones I’m trying to set might be different from the predefined fixed mappings.

Bets
Alessio

@ammaraljodah
Copy link

Do the flexible mapping to be sure

4.1.4.3 Mapping procedure
The steps to configure the PDO mapping are (Example for setting the RPDO 1):
Place the controller into NMT pre-operational.
Destroy the PDO writing a 1 into the valid bit of SubIndex 0x01 of PDO communication parameter. (0x1400
RPDO1 - bit 31 of COB-ID used)
Unmap all registers from PDO by setting SubIndex 0x00 to zero. (0x1600 RPDO1 mapping parameter).
Modify mapping by changing the values in the corresponding SubIndexes. (0x1600 RPDO1 mapping
parameter).
Enable mapping by setting SubIndex 0x00 to the number of mapped objects. (0x1600 RPDO1 mapping
parameter).
Create PDO by setting a 0 into the valid bit of SubIndex 0x01 of PDO communication parameter. (0x1400
RPDO1 - bit 31 of COB-ID used

@iamdrfly
Copy link
Author

Hi @ammaraljodah,

Could you explain how this is handled in ros2_ethercat? Isn't this process managed automatically by ros2_ethercat?
I’m a bit confused—if it's not managed automatically, how is it possible that I can see the PDOs after the first launch?

Best regards,
Alessio

@ammaraljodah
Copy link

Ok, what is the output of
ethercat slaves

@iamdrfly
Copy link
Author

iamdrfly commented Oct 17, 2024

Hi @ammaraljodah,

Results after switching the drives off and on again before running ros2_ethercat:

  • output of ethercat slaves:
0  65535:0  PREOP  +  0x0000029c:0x02c30001
1  65535:0  PREOP  +  0x0000029c:0x02c31001
2  65535:1  PREOP  +  0x0000029c:0x03b31001
3  65535:0  PREOP  +  0x0000029c:0x02c31001
  • output of ethercat cstruct:
/* Master 0, Slave 0
 * Vendor ID:       0x0000029c
 * Product code:    0x02c30001
 * Revision number: 0x0005001f
 */

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 1
 * Vendor ID:       0x0000029c
 * Product code:    0x02c31001
 * Revision number: 0x0005001f
 */

ec_sync_info_t slave_1_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 2
 * Vendor ID:       0x0000029c
 * Product code:    0x03b31001
 * Revision number: 0x0005000d
 */

ec_sync_info_t slave_2_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 3
 * Vendor ID:       0x0000029c
 * Product code:    0x02c31001
 * Revision number: 0x0005001f
 */

ec_sync_info_t slave_3_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {0xff}
};

After launching ros2_ethercat:

-output ethercat slaves:

0  65535:0  SAFEOP+ERROR  E  0x0000029c:0x02c30001
1  65535:0  SAFEOP+ERROR  E  0x0000029c:0x02c31001
2  65535:1  OP            +  0x0000029c:0x03b31001
3  65535:0  PREOP         +  0x0000029c:0x02c31001
  • output ethercat cstruct:
/* Master 0, Slave 0
 * Vendor ID:       0x0000029c
 * Product code:    0x02c30001
 * Revision number: 0x0005001f
 */

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x60ff, 0x00, 32},
    {0x6071, 0x00, 16},
    {0x6060, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x606c, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x603f, 0x00, 16},
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1600, 5, slave_0_pdo_entries + 0},
    {0x1a00, 6, slave_0_pdo_entries + 5},
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 1
 * Vendor ID:       0x0000029c
 * Product code:    0x02c31001
 * Revision number: 0x0005001f
 */

ec_pdo_entry_info_t slave_1_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x60ff, 0x00, 32},
    {0x6071, 0x00, 16},
    {0x6060, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x606c, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x603f, 0x00, 16},
};

ec_pdo_info_t slave_1_pdos[] = {
    {0x1600, 5, slave_1_pdo_entries + 0},
    {0x1a00, 6, slave_1_pdo_entries + 5},
};

ec_sync_info_t slave_1_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_1_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_1_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 2
 * Vendor ID:       0x0000029c
 * Product code:    0x03b31001
 * Revision number: 0x0005000d
 */

ec_pdo_entry_info_t slave_2_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x60ff, 0x00, 32},
    {0x6071, 0x00, 16},
    {0x6060, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x606c, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x603f, 0x00, 16},
};

ec_pdo_info_t slave_2_pdos[] = {
    {0x1600, 5, slave_2_pdo_entries + 0},
    {0x1a00, 6, slave_2_pdo_entries + 5},
};

ec_sync_info_t slave_2_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_2_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_2_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 3
 * Vendor ID:       0x0000029c
 * Product code:    0x02c31001
 * Revision number: 0x0005001f
 */

ec_sync_info_t slave_3_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {0xff}
};

As you can see, the PDOs are configured after the first launch of ros2_ethercat.
PS: I have 4 drives but only the first 3 I would like to move.

Best Regards
Alessio

@ammaraljodah
Copy link

ammaraljodah commented Oct 17, 2024

You are not setting the alias and position correctly
The ethercat slaves has this format:

<id>  <alias>:<position>  <device_state>  +  <device_name>

Your alias
65535
But the position is wrong some drives has the same position
Try to change the alias and set the new alias for the drives

Ideally your bus be like
0 0:0
1 0:1
2 0:2
So the first drive has alias of 0 and position of 0
Second has alias of 0 and position 1.
You can not have the same position for the same alias.

@ammaraljodah
Copy link

Are you using ethercat junction?

@ammaraljodah
Copy link

ethercat alias [ OPTIONS ] < ALIAS >
Write alias addresses .
Arguments :
ALIAS must be an unsigned 16 bit number . Zero means
removing an alias address .
If multiple slaves are selected , the -- force option
is required .
Command - specific options :
-- alias -a < alias >
-- position -p <pos > Slave selection . See the help of
the ’ slaves ’ command .
-- force -f Acknowledge writing aliases of
multiple slaves .
Numerical values can be specified either with decimal ( no
prefix ) , octal ( prefix ’0 ’) or hexadecimal ( prefix ’0x ’) base .

@ammaraljodah
Copy link

Make sure to change the alias and position accordingly in the urdf file

@iamdrfly
Copy link
Author

Hi @ammaraljodah,

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="haa_min_count" value="260" />
  <xacro:property name="haa_max_count" value="764" />
  <xacro:property name="haa_min_mrev_sec" value="-120000" />
  <xacro:property name="haa_max_mrev_sec" value="120000" />

  <xacro:property name="hfe_min_count" value="260" />
  <xacro:property name="hfe_max_count" value="764" />
  <xacro:property name="hfe_min_mrev_sec" value="-120000" />
  <xacro:property name="hfe_max_mrev_sec" value="120000" />

  <xacro:property name="kfe_min_count" value="260" />
  <xacro:property name="kfe_max_count" value="764" />
  <xacro:property name="kfe_min_mrev_sec" value="-120000" />
  <xacro:property name="kfe_max_mrev_sec" value="120000" />

  <!-- mode of operation = 8 position; 9 velocity -->

  <xacro:macro name="ethercat" params="name">

    <ros2_control name="ec_single_axis" type="system">
      <hardware>
        <plugin>ethercat_driver/EthercatDriver</plugin>
        <param name="master_id">0</param>
        <param name="control_frequency">100</param>
      </hardware>

      <joint name="${name}_HAA">
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <state_interface name="error_code"/>
        <command_interface name="control_word"/>
        <command_interface name="mode_of_operation"/>
        <command_interface name="reset_fault"/>
        <state_interface name="mode_of_operation"/>
        <state_interface name="status_word"/>
        <command_interface name="position">
          <param name="min">"${haa_min_count}"</param>
          <param name="max">"${haa_min_count}"</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">"${haa_min_mrev_sec}" </param>
          <param name="max">"${haa_max_mrev_sec}"</param>
        </command_interface>
        <command_interface name="effort"/>
        <command_interface name="reset_fault"/>
        <ec_module name="drive_1">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">0</param>
          <param name="mode_of_operation">8</param>
          <!-- <param name="mode_of_operation">20</param> -->
          <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_1c.yaml</param>
          <!-- <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_3b.yaml</param> -->
        </ec_module>
      </joint>

      <joint name="${name}_HFE">
        <command_interface name="control_word"/>
        <command_interface name="mode_of_operation"/>
        <command_interface name="reset_fault"/>
        <state_interface name="mode_of_operation"/>
        <state_interface name="status_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <state_interface name="error_code"/>
        <command_interface name="position">
          <param name="min">"${hfe_min_count}"</param>
          <param name="max">"${hfe_min_count}"</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">"${hfe_min_mrev_sec}" </param>
          <param name="max">"${hfe_max_mrev_sec}"</param>
        </command_interface>
        <command_interface name="effort"/>
        <command_interface name="reset_fault"/>
        <ec_module name="drive_2">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">1</param>
          <param name="mode_of_operation">8</param>
          <!-- <param name="mode_of_operation">20</param> -->
          <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_2c.yaml</param>
          <!-- <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_3b.yaml</param> -->
        </ec_module>
      </joint>

      <joint name="${name}_KFE">
        <command_interface name="control_word"/>
        <command_interface name="mode_of_operation"/>
        <command_interface name="reset_fault"/>
        <state_interface name="mode_of_operation"/>
        <state_interface name="status_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <state_interface name="error_code"/>
        <command_interface name="position">
          <param name="min">"${kfe_min_count}"</param>
          <param name="max">"${kfe_min_count}"</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">"${kfe_min_mrev_sec}" </param>
          <param name="max">"${kfe_max_mrev_sec}"</param>
        </command_interface>
        <command_interface name="effort"/>
        <command_interface name="reset_fault"/>
        <ec_module name="drive_1">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">2</param>
          <param name="mode_of_operation">8</param>
          <!-- <param name="mode_of_operation">20</param> -->
          <param name="slave_config">/home/lab/grace_ros2_ws/src/grace_description/config/driver_3b.yaml</param>
        </ec_module>
      </joint>

    </ros2_control>

  </xacro:macro>

</robot>

As you can see the positions are all different only the aliases are different. I do not know why I have an alias of 65535, should it be 0?

I use an ethernet RJ45 couple, I have not an ethercat junction.

Best
Alessio

@ammaraljodah
Copy link

The alias in urdf should match the one from the ethercat slaves command.
Moreover you have position conflict, several drived has same position. You need to fix that. Use
ethercat alias
To change alias

@ammaraljodah
Copy link

Your urdf is
0:0
0:1
0:2
Your actual bus is
65535:0
65535:0
65535:1
65535:0

Remove the unused drive and make the actual bus match the urdf

@iamdrfly
Copy link
Author

Hi @ammaraljodah,
Many thanks =D

I have changed the alias and now the EtherCAT slaves output is:

0  0:0  PREOP  +  0x0000029c:0x02c30001
1  0:1  PREOP  +  0x0000029c:0x02c31001
2  0:2  PREOP  +  0x0000029c:0x03b31001

So now the positions and aliases are good. However, the first two drives do not work.

here the output from ros2:
out_ros2.txt

here the output from dmesg -w:
out_dmesg.txt

Many thanks for your time and help.

Best
Alessio

@ammaraljodah
Copy link

Two drives time-out when master requested to put them in op mode.
Can you try one drive on the bus and have one joint in the urdf see if it comes to OP by itself.
Then swap it to the second drive, then third.
We want to see if it is the drive issue or ros issue.

@iamdrfly
Copy link
Author

Hi @ammaraljodah,

I have modified the URDF to include only one drive at a time. For the first drive I have also disconnected the other two drives from the Ethercat chain (I cannot disconnect them for the other cases) and connected the Controller for cia402 drives #61.

The result for the first drive:

  • the output of ethercat slaves:

0 0:0 SAFEOP+ERROR E 0x0000029c:0x02c30001

header:
  stamp:
    sec: 1729234507
    nanosec: 653545968
  frame_id: ''
dof_names:
- FL_HAA
drive_states:
- STATE_SWITCH_ON_DISABLED
modes_of_operation:
- MODE_UNDEFINED
status_words:
- 1616

The result for the second drive:

  • EtherCAT slaves
0  0:0  PREOP         E  0x0000029c:0x02c30001
1  0:1  SAFEOP+ERROR  E  0x0000029c:0x02c31001
2  0:2  PREOP         +  0x0000029c:0x03b31001
header:
  stamp:
    sec: 1729235767
    nanosec: 472883875
  frame_id: ''
dof_names:
- FL_HFE
drive_states:
- STATE_SWITCH_ON_DISABLED
modes_of_operation:
- MODE_UNDEFINED
status_words:
- 1616

The result for the third drive:

  • EtherCAT slaves
0  0:0  PREOP         E  0x0000029c:0x02c30001
1  0:1  SAFEOP+ERROR  E  0x0000029c:0x02c31001
2  0:2  OP            +  0x0000029c:0x03b31001
header:
  stamp:
    sec: 1729236252
    nanosec: 7447392
  frame_id: ''
dof_names:
- FL_KFE
drive_states:
- STATE_OPERATION_ENABLED
modes_of_operation:
- MODE_UNDEFINED
status_words:
- 1591

Best
Alessio

@iamdrfly
Copy link
Author

Hi @pzanne,

so the issue seems related to the ubuntu version(24.04). I have tested your last code on Ubuntu 22.04 with ros2 rolling and it works (the drive switched to OP).

ros2 output: test_1126_ros2.txt
dmesg output: test_1126_dmesg.txt
wireshark output: test_1126_wireshark.zip

Do you know what the problem is with Ubuntu 24.04? And what changes have you made to reach the OP ?

Best,
Alessio

@pzanne
Copy link
Member

pzanne commented Nov 26, 2024

Hi Alessio,

Ok, for the moment I haven't changed anything in our driver, the package you tested is just the direct adaptation of the c++ program into a hardware interface. Everything is static and only ‘works’ for your everest driver. It validates the ros2_control pipe but doesn't solve our problem.
I'm going to prepare a new test package to try to find out where the error is.
Could you send me the wireshark traces with the old ros2 package (the one that uses our ethercat driver)? That might help me identify the problem.
Best
Philippe

@iamdrfly
Copy link
Author

Hi @pzanne,

here the result of the test performed with the following ros2 pkg:ingenia_everest_1128.zip

the dmesg output: test_dmesg_2811.txt

the ros2 output: test_ros2_2811.txt

the wireshark file: test_2811.zip

the drives was not able to reach the OP:
0 0:0 SAFEOP+ERROR E 0x0000029c:0x02c31001

Best,
Alessio

@pzanne
Copy link
Member

pzanne commented Nov 28, 2024

Hello Alessio,
I've just looked at the wireshark logs, and it seems that on initialisation the mode of operation value is not 0 (NO_MODE) but 255. This value is not taken into account and this could be the origin of the problem.
Could you switch to the humble-issue-147 branch and test with the package ingenia_everest_1128 ? If it still doesn't work, could you send me the wireshark log of this new test?

Best
Philippe

@iamdrfly
Copy link
Author

iamdrfly commented Dec 3, 2024

Hi @pzanne,

here the result:

wireshark: test_1202.zip

dmesg: test_dmesg_1203.txt

ros2: test_ros2_1202.txt

output ethercat slaves:
0 0:0 SAFEOP+ERROR E 0x0000029c:0x02c31001

Best,
Alessio

@pzanne
Copy link
Member

pzanne commented Dec 4, 2024

Hello Alessio,

I deleted the clock synchronization on the humble-issue-147 branch, could you try again?
Do you think it would be possible for us to access your driver? It would certainly be easier to try and debug this problem.
Best
Philippe

@pzanne
Copy link
Member

pzanne commented Dec 4, 2024

Hello Alessio,

I don't know if you've had time to test the new version, but we've just changed the timing of ethercat initialisation which could cause problems, they are pushed to the dedicated branch humble-issue-147. Can you test and give us some feedback?
Best
Philippe

@iamdrfly
Copy link
Author

iamdrfly commented Dec 4, 2024

Hi @pzanne,

I will try to test it as soon as possible.
I hope today late at maximum tomorrow afternoon.

Best,
Alessio

@iamdrfly
Copy link
Author

iamdrfly commented Dec 4, 2024

HI @pzanne,

here the result:

-output: 0 0:0 SAFEOP+ERROR E

Do you think it would be possible for us to access your driver?
what do you mean? remote access?

Best
Alessio

@Raphaelb10
Copy link

Raphaelb10 commented Dec 6, 2024

Hey,

I just made it work with an Ingenia Everest-XCR-E.
The only difference I have with your configuration, is mostly the register values and their data type in the yaml config.

Here is mine :

# Configuration file for Ingenia Everest XCR-E drive
vendor_id: 0x0000029c
product_id: 0x02c31001
auto_fault_reset: true  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"

rpdo:
  - index: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word, default: 0}
      - {index: 0x2020, sub_index: 0, type: int32, command_interface: position_setpoint, default: 0}
      - {index: 0x2014, sub_index: 0, type: uint16, command_interface : mode_of_operation, default: 8}

tpdo:
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_word}
      - {index: 0x2015, sub_index: 0, type: uint16, state_interface: mode_of_operation_display}
      - {index: 0x200F, sub_index: 0, type: int32, state_interface: last_error}

If I switch the " operation mode " and "operation mode display" values to the one you put, it seems that I get into the same SAFEOP E state.
Maybe it will help you,
Best

@iamdrfly
Copy link
Author

iamdrfly commented Dec 9, 2024

Hi @Raphaelb10,

Thank you for your suggestion. However, it seems the registers you are referencing fall outside the CiA 402 standard, and the drive in question is compliant with the CiA 402 specification.

Best regards,
Alessio

@Raphaelb10
Copy link

Hi @iamdrfly,

I use these registers because I already had issues before with ingenia drives not being fully compliant to CiA402 (had missing registers, and errors like you when trying to configure them as PDO).
Also had other issues with the online documentation not reflecting what was actually implemented on the drive.

The registers I used above are supposed to give the same values as the register of the CiA402, sometime the data type is not the same, but from a user point of view, it do not change much. Its worth a try imo. I also used them for over 2 years on several ingenia drives, and had no issues so far.

If you stictly cannot use the non-CiA402 registers, I would try to update the firmware of the XCR drives to latest one, as they might have patched the non-compliancy since you bought them. Especially as the EVE-XCR-S (with more recent firmware) works in your case.

Hope it helps,
BR,

@pzanne
Copy link
Member

pzanne commented Dec 9, 2024

Hello,
From the documentation of ingenia, the registers 0x2014 and 0x2015 appear to be dedicated to a specific operating mode (interpolation mode ?). The values do not seem to have the same meaning: for example, the value 8 (CSP mode) is not referenced.
In addition, in a basic version of the ethercat ros2 driver it is possible to switch the driver to OP mode with the current configuration. The problem probably lies elsewhere. But your feedback on ingenia drivers is most welcome.
We have made further modifications to the dedicated branch humble-issue-147 in order to switch the order in which tpdo and rpdo are processed. Can you test this and give us some feedback on the result?

@iamdrfly
Copy link
Author

iamdrfly commented Dec 9, 2024

HI @pzanne,

here the result:

* dmseg: [test_dmesg_1204.txt](https://github.com/user-attachments/files/18010519/test_dmesg_1204.txt)

* wireshark: [test_1204.zip](https://github.com/user-attachments/files/18010522/test_1204.zip)

* ros2: [test_ros2_1204.txt](https://github.com/user-attachments/files/18010528/test_ros2_1204.txt)

-output: 0 0:0 SAFEOP+ERROR E

Do you think it would be possible for us to access your driver?
what do you mean? remote access?

Best Alessio

Hi @pzanne,
Did you happen to see this message, by chance?
Did you modify something on the branch humble-issue-147 in the last 4 days ? If not, please check the message above.

Best
Alessio

@pzanne
Copy link
Member

pzanne commented Dec 9, 2024

Hello Alessio,
Yes, we've changed something today.
Best
Philippe

@pzanne
Copy link
Member

pzanne commented Dec 9, 2024

On the other hand, I hadn't seen your question about access to the driver.
If we could access it remotely, that would be great, it would allow us to do more tests to try and find the problem or find a way around it...
Best
Philippe

@Raphaelb10
Copy link

@pzanne
You are right the register are slightly different, but allow to get to the same drive modes.
Screenshot from 2024-12-09 15-48-58
The values are not the same, because there are more modes available on the drive than there should be for the CiA402.
But the regular modes you want are also available from this register, just using a different value.
If you want the mode 8 ( from 0x6060), you need to pass 0x24 (from 0x2014)instead and it will be the same.
You indeed have access to other "interpolated" modes, that can be relevant when sending asyncronous setpoints. Its also on the CiA402 std, using value 1. The 0x2014 registers allow you to choose the velocity profile that the interpolator will generate as well.

I am not sure to understand why you need this modified version of the driver, but if everything works on the main branch (I am surprised as it do not work for my EVE-XRC, with your config, due to the 0x6060 pdo), then I agree that the error is probably elsewhere.
Good luck !

@pzanne
Copy link
Member

pzanne commented Dec 9, 2024

@Raphaelb10
Hello,
I hadn't seen the last table... could you test the behaviour of the driver with the value 0X24 ? (which is the mode that @iamdrfly wants to use) does it switch to operational mode ?
The basic package to make the driver work with the configuration is not on our git, you can find it here :
ros2_ethercat_everest.tgz
If you get a chance to test it, I'd like to know if it works for you too.
Best
Philippe

@Raphaelb10
Copy link

Hi @pzanne ,

From my understanding, the value do not matter as long as the PDO object is configured correctly and accepted by the drive, as PDO communication only start after the slave is in OP mode.

But I tried, by putting 0x24 (=36) as a default value, and it works as before :
image
I unfortunately do not have yet a motor connected to the drive so I cannot really validate more.
But my slave is in OP mode and transmit everything correctly, indicating it accepted the value using the operation mode display.
Unfortunately I am a bit short on time for testing with the basic package, I might try later this month.

But honnestly it looks to me that the documentation is not up to date with the drive's behavior, causing an issue when configuring PDO. I get the same error as you if I use the 6060 register for some reason.
Maybe Novanta's support will give you a better explanation, they are quite responsive (when I had a missing register on the drive compared to the documentation, they found it out quickly).
Having used their drives for the past 2 years, things like that are rather common unfortunately.

Best regards,

@pzanne
Copy link
Member

pzanne commented Dec 9, 2024

@Raphaelb10
Thanks for the feedback. I agree with you, the value in register 0x2014 should not prevent the driver from switching to op mode. I'm not in possession of any Novanta's hardware, so if I contact support I'm not sure they'll reply, but I'll give it a try.
Best
Philippe

@pzanne
Copy link
Member

pzanne commented Dec 12, 2024

Hello Alessio,
Have you had time to test our latest modifications? Is the problem still there?
Best
Philippe

@iamdrfly
Copy link
Author

Hi @pzanne,

sorry for the delay. I am outside the lab and I cannot test it. Can I test it next week?

If you want I can give you access via anydesk to my PC when I am back, please write me an email at: alessio(dot)mosca(at)supi(dot).ch to setup the date.

Best,
Alessio

@pzanne
Copy link
Member

pzanne commented Dec 13, 2024

Hello Alessio,
Don't worry, we'll see about that next week.
Best
Philippe

@iamdrfly
Copy link
Author

Hi @pzanne

I have tested the new code and it seems I have a problem with the ros2 control manager now, as you can see from the output, what did you change?

output ros2
test_1219_ros2.txt

dmeg:
test_1218_dmesg.txt

wireshark:
test_1218.zip

Best
Alessio

@pzanne
Copy link
Member

pzanne commented Dec 18, 2024

Hi Alessio,

we haven't changed anything that could explain this problem (we've mainly inverted 2 sections of code to reverse the order in which the channels are loaded in the ethercat configuration we load tpdo before rpdo). Could you test with the previous version to be sure?
In the kernel logs, no slaves seem to be detected. Was the driver powered up?
Best
Philippe

@iamdrfly
Copy link
Author

iamdrfly commented Dec 18, 2024

Hi @pzanne,

It was my fault (i was using rolling instead of humble )

The drive does not reach the OP mode:
0 0:0 PREOP E 0x0000029c:0x02c30001

here the logs
wireshark:
test_1218.zip

ros2:
test_1218_ros2.txt

dmesg:
test_1218_dmesg.txt

Best
Alessio

@pzanne
Copy link
Member

pzanne commented Dec 18, 2024

Hi Alessio,

Can you enable the debug level 1 in the ethercat stack and resend us the kernel log
Best
Philippe

@pzanne
Copy link
Member

pzanne commented Dec 19, 2024

Hi Alessio,
Could you check your config file, the size for register 0x6061 is wrong (255 bits instead of 8bits). Could you check that you have this line:
- {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}
Check that there is a space between type: and int8
Best
Philippe

@iamdrfly
Copy link
Author

Hi @pzanne,

I forgot that in the code you had given me to try there was this error in the yaml, sorry.

I tried with the humble branch and now I can reach OP. May I ask you what did you change ?

Here the logs with debug level 1

wireshark:
test_1220.zip

dmesg:
test_1220_dmesg.txt

ros2:
test_1220_ros2.txt

Many thanks,
Best Regards

Alessio

@pzanne
Copy link
Member

pzanne commented Dec 20, 2024

Hello Alessio,

Good news !!
In this new version we've changed two things, I don't know which of the two changes solved the problem :

  • The first point is that we had a double read/write during the activation phase which could be a first source of the problem.
  • The second point modifies our driver so that read and write data are reversed on the EtherCAT frame compared with the initial version. I don't understand why this would have an influence but it's also a possibility.

Could you do another test to determine which of these changes is the source of the problem?
To do this, you could go back to commit 47e5db3 and test to see if the driver continues to switch to operational state.

Now that we're able to make a driver go into operational state, we need to check that we can make the motor move to be sure the problem is resolve.
Best
Philippe

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

4 participants