Problem with ROS2 and setting control_word values

[paulwetzel]

Hey there!
We are currently experiencing problems with setting a control_word on our motor drives via 'ethercat_driver_ros2'. For context: We want to control a set of actuators, controlled by Synapticon Circulo 9 (CiA402 conform) motor drives using position control from within ROS2 to playback predefined trajectorys. In order to configure some functions on the drive we need to set the control_word to some specific value, at some point in time, transmitting the control_word using ethercat.

We defined the ethercat slave in a .yaml file, that looks like this:

#Configuration file for Synapticon Circulo Motor Drive
vendor_id: 0x000022d2
product_id: 0x00000302
# assign_activate: 0x0300  # 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: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: controlword, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan}  # Target position
      - {index: 0x60ff, sub_index: 0, type: int32, default: 0}  # Target velocity
      - {index: 0x6071, sub_index: 0, type: int16, default: 0}  # Target torque
      - {index: 0x60b2, sub_index: 0, type: int16, default: 0}  # Offset torque
      - {index: 0x6060, sub_index: 0, type: int8, command_interface: modeOfOperation, default: 8}  # Mode of operation
      - {index: 0x2701, sub_index: 0, type: uint32, default: 0}  # Tuning command
  - index: 0x1601
    channels:
      - {index: 0x60fe, sub_index: 1, type: uint32, default: 0}  # Digital Outputs: Physical Outputs
      - {index: 0x60fe, sub_index: 2, type: uint32, default: 0}  # Digital Outputs: Bit Mask
  - index: 0x1602
    channels:
      - {index: 0x60b1, sub_index: 0, type: int32, default: 0}  # Offset velocity
      - {index: 0x2703, sub_index: 0, type: uint32, default: 0}  # User MOSI

tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: statusword}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8}  # Mode of operation display
      - {index: 0x60f4, sub_index: 0, type: int32}  # Following error actual value
  - index: 0x1a01
    channels:
      - {index: 0x2401, sub_index: 0, type: uint16}  # Analog input 1
      - {index: 0x2402, sub_index: 0, type: uint16}  # Analog input 2
      - {index: 0x2403, sub_index: 0, type: uint16}  # Analog input 3
      - {index: 0x2404, sub_index: 0, type: uint16}  # Analog input 4
      - {index: 0x2702, sub_index: 0, type: uint32}  # Tuning status
  - index: 0x1a02
    channels:
      - {index: 0x60fd, sub_index: 0, type: uint32}  # Digital Inputs
  - index: 0x1a03
    channels:
      - {index: 0x2704, sub_index: 0, type: uint32}  # User MISO
      - {index: 0x20f0, sub_index: 0, type: uint32}  # Timestamp
      - {index: 0x60fc, sub_index: 0, type: int32}  # Position demand internal value
      - {index: 0x606b, sub_index: 0, type: int32}  # Velocity demand value
      - {index: 0x6074, sub_index: 0, type: int16}  # Torque demand

We already checked the indicies, they are fine, data types work as well. The xacro file therefore looks like this, it's much longer as it obviously includes multiple drives, this is an example. We also check positions and order in the ethercat signal chain, this is fine as well.

<joint name="left_hip_extension_joint">
            <state_interface name="statusword"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
            <state_interface name="effort"/>
            <command_interface name="controlword"/>
            <command_interface name="position"/>
            <command_interface name="reset_fault"/>
            <ec_module name="left_hip_extension_joint">
              <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
              <param name="alias">0</param>
              <param name="position">4</param>
              <param name="mode_of_operation">8</param>
              <param name="slave_config">$(find ethercat_slaves)/config/SynapticonDrive1.slave_config.yaml</param>
            </ec_module>
          </joint>

While building the ROS2 control codebase, we roughly followed the tutorial here. Control of the drive works great from here, however we can't figure out if there is a way to set values to the control_word or other registers on the drive that are part of the standard CiA402 set, using the configuration from the aforementioned tutorial.
We tried configuring the drive both as a joint and as a gpio module, but seemingly this doesn't work. The ROS2 node we used in testing looks like this:

import time

import rclpy
from control_msgs.msg import DynamicJointState, InterfaceValue
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray


class HomingNode(Node):
    def __init__(self):
        super().__init__("homing_node")
        self.CURRENT_STATUS = []

        self.status_word_sub = self.create_subscription(
            DynamicJointState,
            "/joint_state_broadcaster/dynamic_joint_states",
            self.status_word_callback,
            10,
            # JointState, "/joint_state_broadcaster/dynamic_joint_states", self.status_word_callback, 10
        )
        self.control_word_pub = self.create_publisher(
            Float64MultiArray, "/gpio_controller/commands", 10
            # DynamicJointState, "/joint_state_broadcaster/dynamic_joint_states", 10
        )

    def status_word_callback(self, msg):
        current_status = []
        for i, joint in enumerate(msg.interface_values):
            # Maybe don't hardcode index of value
            try:
                print(f"Joint #{i}:", joint.values[3], bin(int(joint.values[3])))
                current_status.append(int(joint.values[3]))
            except IndexError:
                return
        self.CURRENT_STATUS = msg.interface_values

        DISABLE_OPERATION = 0b00000111
        SHUTDOWN = 0b00000110
        joint_id = 2
        # time.sleep(1)
        for joint_id in range(6):
            self.send_control_word(SHUTDOWN, joint_id=joint_id)


    def send_control_word(self, control_word, joint_id: int):
        print("Sending control word ...")

        dynamic_joint_state = DynamicJointState()
        dynamic_joint_state.interface_values = self.CURRENT_STATUS
        dynamic_joint_state.interface_values[joint_id].values = [float(control_word)]
        # self.control_word_pub.publish(dynamic_joint_state)

        foo = Float64MultiArray()
        foo.data = [float(0b1111)]
        self.control_word_pub.publish(foo)


def main(args=None):
    rclpy.init(args=args)
    homing_node = HomingNode()
    rclpy.spin(homing_node)

    DISABLE_OPERATION = 0b00000111
    SHUTDOWN = 0b00000110
    joint_id = 2
    # time.sleep(1)
    for joint_id in range(6):
        homing_node.send_control_word(SHUTDOWN, joint_id=joint_id)

    homing_node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

We are somewhat clueless as to how we can set the control_word value from a drive configured as a joint, if that is even possible at all and if not if there is any recommended workaround.

We already tried:

• Setting the drive up as both a joint and a GPIO
• Sending the message to different topics

If someone gives us a hint as to how this can be implemented, we are more than happy to work on this in a pull request and submit it to the project.

Thanks a million in advance!

[ammaraljodah]

You need ros2 controller that claims the control command interface and subscribe to your topic "/gpio_controller/commands" and forward the command to the hardware plugin.
There is a cia402 controller in a PR to this git repo that you can adapt for this purpose.

[paulwetzel]

Thanks so much @ammaraljodah