ros2 + Ethercat + Kollmorgen RGM

[Daisy-lzc]

Hi all, I'm going to use ros2 to drive a Kollmorgen RGM joint module, but I can't drive the motor, it's been bugging me for almost a month now! My main goal is to implement closed-loop servo control of motor speed in csv mode, When I start the ros2_control node, the motors can go into the OP state, but the joint motors are not enabled at this point, because if the joint motors are enabled, the holding brake on the joint motors will release and make a popping sound. So I went through a node and sent control words to the controller of ros2_control which are decimal 6,7 and 15 respectively but the state of the motor didn't change, why is that?
I made the following file configuration：
Here is the slave.yaml file:

vendor_id: 0x0000006a
product_id: 0x00002020
# assign_activate: 0x0330  # DC Synch register
auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
sdo:  # sdo data to be transferred at drive startup
#   - {index: 0x60C2, sub_index: 1, type: int8, value: 10} # Set interpolation time for cyclic modes to 10 ms
#   - {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
rpdo:  # RxPDO = receive PDO Mapping
  - index: 0x1700
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word, default: .nan}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, default: 0.0}  # Target position
      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: target_velocity, default: .nan}  # Target velocity
      - {index: 0x6071, sub_index: 0, type: int16, default: 0}  # Target torque
      - {index: 0x60b0, sub_index: 0, type: int32, default: 0}  # Offset position
      - {index: 0x60b1, sub_index: 0, type: int32, default: 0}  # Offset velocity
      - {index: 0x60b2, sub_index: 0, type: int16, default: 0}  # Offset torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 9}  # Mode of operation
      - {index: 0x2078, sub_index: 1, type: uint16, default: 0}  # Digital Output Functionalities
      - {index: 0x60b8, sub_index: 0, type: uint16, default: 0}  # Touch Probe Function
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1b00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: state_word}  # Status word
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}  # Position actual value
      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity}  # Velocity actual value
      - {index: 0x60f4, sub_index: 0, type: int32}  # Position loop error
      - {index: 0x6077, sub_index: 0, type: int16}  # Torque actual value
      - {index: 0x60C2, sub_index: 1, type: int8, value: 10} # Set interpolation time for cyclic modes to 10 ms
      - {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
      - {index: 0x6061, sub_index: 0, type: int8}  # Mode of operation display
      - {index: 0x2071, sub_index: 1, type: int16}  # Digital Input Functionalities State
      - {index: 0x60b9, sub_index: 0, type: int16}  # Touch Probe Status
      - {index: 0x60ba, sub_index: 0, type: int32}  # Touch Probe Position 1 Positive Value
      - {index: 0x60bb, sub_index: 0, type: int32}  # Touch Probe Position 1 Negative Value
sm:
  - {index: 0, type: output, pdo: ~, watchdog: disable}
  - {index: 1, type: input, pdo: ~, watchdog: disable}
  - {index: 2, type: output, pdo: rpdo, watchdog: enable}
  - {index: 3, type: input, pdo: tpdo, watchdog: enable}

here is the xacro：

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

  <xacro:macro name="rrbot_ros2_control" params="
               name
               prefix
               use_mock_hardware:=^|false
               mock_sensor_commands:=^|false
               sim_gazebo_classic:=^|false
               sim_gazebo:=^|false"
               >
    <ros2_control name="${name}" type="system">
      <hardware>
        <xacro:if value="${use_mock_hardware}">
          <plugin>mock_components/GenericSystem</plugin>
          <param name="mock_sensor_commands">${mock_sensor_commands}</param>
        </xacro:if>
        <xacro:if value="${sim_gazebo_classic}">
          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </xacro:if>
        <xacro:if value="${sim_gazebo}">
          <plugin>ign_ros2_control/IgnitionSystem</plugin>
        </xacro:if>
        <xacro:unless value="${use_mock_hardware or sim_gazebo_classic or sim_gazebo}">
          <!-- <plugin>controlko_hardware_interface/RRBotHardwareInterface</plugin> -->
          <!-- <plugin>my_hardware_interface/RRBotHardwareInterface</plugin> -->
          <plugin>ethercat_driver/EthercatDriver</plugin>
          <param name="master_id">0</param>
          <param name="control_frequency">50</param>
        </xacro:unless>
      </hardware>
      <joint name="${prefix}joint0">
        <command_interface name="control_word"/>
        <command_interface name="target_velocity"/>
        <state_interface name = "state_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <ec_module name="Kollmorgen RGM">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">0</param>
          <param name="mode_of_operation">9</param>
          <param name="slave_config">$(find controlko_bringup)/config/slave_large.yaml</param>
        </ec_module>
      </joint>
      <joint name="${prefix}joint1">
        <command_interface name="control_word"/>
        <command_interface name="target_velocity"/>
        <state_interface name = "state_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <ec_module name="Kollmorgen RGM">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">1</param>
          <param name="mode_of_operation">9</param>
          <param name="slave_config">$(find controlko_bringup)/config/slave_large.yaml</param>
        </ec_module>
      </joint>
      <joint name="${prefix}joint2">
        <command_interface name="control_word"/>
        <command_interface name="target_velocity"/>
        <state_interface name = "state_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <ec_module name="Kollmorgen RGM">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">2</param>
          <param name="mode_of_operation">9</param>
          <param name="slave_config">$(find controlko_bringup)/config/slave_large.yaml</param>
        </ec_module>
      </joint>
      <joint name="${prefix}joint3">
        <command_interface name="control_word"/>
        <command_interface name="target_velocity"/>
        <state_interface name = "state_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <ec_module name="Kollmorgen RGM">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">3</param>
          <param name="mode_of_operation">9</param>
          <param name="slave_config">$(find controlko_bringup)/config/slave_small.yaml</param>
        </ec_module>
      </joint>
      <joint name="${prefix}joint4">
        <command_interface name="control_word"/>
        <command_interface name="target_velocity"/>
        <state_interface name = "state_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <ec_module name="Kollmorgen RGM">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">4</param>
          <param name="mode_of_operation">9</param>
          <param name="slave_config">$(find controlko_bringup)/config/slave_small.yaml</param>
        </ec_module>
      </joint>
      <joint name="${prefix}joint5">
        <command_interface name="control_word"/>
        <command_interface name="target_position"/>
        <command_interface name="target_velocity"/>
        <state_interface name = "state_word"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <ec_module name="Kollmorgen RGM">
          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
          <param name="alias">0</param>
          <param name="position">5</param>
          <param name="mode_of_operation">9</param>
          <param name="slave_config">$(find controlko_bringup)/config/slave_small.yaml</param>
        </ec_module>
      </joint>
    </ros2_control>

  </xacro:macro>
</robot>

here is controller_manage.yaml：

controller_manager:
  ros__parameters:
    update_rate: 50  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    # forward_position_controller:
    #   type: forward_command_controller/ForwardCommandController

    # joint_trajectory_controller:
    #   type: joint_trajectory_controller/JointTrajectoryController

    # displacement_controller:
    #   type: displacement_controller/DisplacementController
    
    my_controller:
      type: my_position_controller/MyPositionController
    
    

my_controller:
  ros__parameters:
    joints:
      - joint0
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
    interface_name: position

    command_interfaces: 
      - control_word
      # - target_position
      - target_velocity

    state_interfaces: 
      - state_word 
      - position
      - velocity
    ```
   The last 4 images are the terminal output
![1](https://github.com/ICube-Robotics/ethercat_driver_ros2/assets/98206359/3e9d454d-f454-4c97-9405-9c1c694d3aee)
![2](https://github.com/ICube-Robotics/ethercat_driver_ros2/assets/98206359/6a27914c-76c3-4a2f-80d2-86e9e866a139)
![3](https://github.com/ICube-Robotics/ethercat_driver_ros2/assets/98206359/54a11b63-ac99-49b0-9299-085aaa244e9e)
![4](https://github.com/ICube-Robotics/ethercat_driver_ros2/assets/98206359/1d1e4fc4-3fa2-47b5-91f8-c47c5748b4db)

and in the cia402_common_defs.hpp :
```jsx:
#ifndef ETHERCAT_GENERIC_PLUGINS__CIA402_COMMON_DEFS_HPP_
#define ETHERCAT_GENERIC_PLUGINS__CIA402_COMMON_DEFS_HPP_

#define CiA402D_RPDO_CONTROLWORD  ((uint16_t) 0x6040)
#define CiA402D_RPDO_POSITION  ((uint16_t) 0x607a)
#define CiA402D_RPDO_VELOCITY  ((uint16_t) 0x60ff)
#define CiA402D_RPDO_EFFORT  ((uint16_t) 0x6071)
#define CiA402D_RPDO_MODE_OF_OPERATION  ((uint16_t) 0x6060)

#define CiA402D_TPDO_POSITION ((uint16_t) 0x6064)
#define CiA402D_TPDO_STATUSWORD  ((uint16_t) 0x6041)
#define CiA402D_TPDO_VELOCITY  ((uint16_t) 0x606C)
#define CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY  ((uint16_t) 0x6061)

All the types are defined as uint16_t, can i change it?

[Daisy-lzc]

[yguel]

Hi,
The line
[ros2_control_node-1] STATE: Not Ready to Switch On with status word: 0
said that your drive is not well configured.
Did you test your drive alone with the software provided with your drive ?
You have to find the root cause of this problem first.

[wei1224hf]

I suggest you to use this project to fit your motors:
https://github.com/ICube-Robotics/ethercat_driver_ros2_examples