Support for Multi-Channel Servo Driver

[maxwelllls]

Hi @mcbed : I have a servo driver that supports 6 axes, but the current code implementation allows each joint to support one or more ec_modules, and it is not possible to achieve one ec_module corresponding to multiple joints. Is there a feasible solution at the moment? I can enhance the adaptation for multi-channel servos. Could you provide some guidance or suggestions? Thank you.

[maxwelllls]

My current idea is to add a “axis” parameter to the ec_module’s parameters and distinguish multi-channel devices during the on_init() process for special handling. Since one ec_module corresponds to one YAML configuration file, it is necessary to add a axis parameter like “axisx/interface” to the mapping of the interface.

The expected final outcome is as follows:

            <joint name="joint1">
              <state_interface name="position"/>
              <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
              <ec_module name="rox">
                <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
                <param name="alias">1</param>
                <param name="position">0</param>
                <param name="axis">0</param>
                <param name="mode_of_operation">8</param>
                <param name="slave_config">/path_to_my_config/multiAxis.yaml</param>
              </ec_module>
            </joint> 

            <joint name="joint2">
              <state_interface name="position"/>
              <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
              <ec_module name="rox">
                <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
                <param name="alias">1</param>
                <param name="position">0</param>
                <param name="axis">1</param>
                <param name="mode_of_operation">8</param>
                <param name="slave_config">/path_to_my_config/multiAxis.yaml</param>
              </ec_module>
            </joint> 

multiAxis.yaml

rpdo:  # RxPDO = receive PDO Mapping
  - index: 0x1600
    channels:
      # axis0
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: axis0/position, default: .nan, factor: 1, offset: 0}  # Target position
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}  # Control word
      - {index: 0x6060, sub_index: 0, type: uint8, default: 0}  # Mode of operation

      # axis0
      - {index: 0x687a, sub_index: 0, type: int32, command_interface: axis1/position, default: .nan, factor: 1, offset: 0}  # Target position
      - {index: 0x6840, sub_index: 0, type: uint16, default: 0}  # Control word
      - {index: 0x6860, sub_index: 0, type: uint8, default: 0}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      # axis0
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: axis0/position, factor: 1,  offset: 0}  # Position actual value
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: axis0/status_word}  # Status word
      - {index: 0x603f, sub_index: 0, type: uint16, state_interface: axis0/error_code}  # Error Code
      - {index: 0x6061, sub_index: 0, type: uint8, state_interface: axis0/operation_mode}  # Mode of operation display

      # axis1
      - {index: 0x6864, sub_index: 0, type: int32, state_interface: axis1/position, factor: 1,  offset: 0}  # Position actual value
      - {index: 0x6841, sub_index: 0, type: uint16, state_interface: axis1/status_word}  # Status word
      - {index: 0x683f, sub_index: 0, type: uint16, state_interface: axis1/error_code}  # Error Code
      - {index: 0x6861, sub_index: 0, type: uint8, state_interface: axis1/operation_mode}  # Mode of operation display

However, considering that the servo drive-related code also involves the cia402 state machine, mode of operation, position, and other underlying logic, there may be quite a few points that need modification.

[yguel]

Hi, you can try using the following branch: https://github.com/yguel/ethercat_driver_ros2/tree/yguel-EcCiA402DriveCfgRegs I have implemented an option that will allow to change very few lines of the code (and hence be reasonably cautious about the changes).

Idea: If we just update the EcCiA402Drive class and make the following constants (from cia402_common_defs.hpp) a set of class variables:

#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_MODE_OF_OPERATION_DISPLAY  ((uint16_t) 0x6061)

(it is also interesting to add error_code)

Those variable will be set in the setupSlave method from the urdf definition (for instance):

<joint name="joint1">
              <state_interface name="position"/>
              <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
              <ec_module name="rox">
                <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
                <param name="alias">1</param>
                <param name="position">0</param>
                <param name="mode_of_operation">8</param>
                <param name="rpdo_control_word">0x6040</param>
                <param name="rpdo_target_position">0x607a</param>
                <param name="rpdo_mode_of_operation">0x6060</param>
                <param name="tpdo_actual_position">0x6064</param>
                <param name="tpdo_status_word">0x6041</param>
                <param name="tpdo_mode_of_operation_display">0x6061</param>
                <param name="tpdo_error_code">0x603f</param>
                <param name="slave_config">/path_to_my_config/multiAxis.yaml</param>
              </ec_module>
            </joint> 

            <joint name="joint2">
              <state_interface name="position"/>
              <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
              <ec_module name="rox">
                <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
                <param name="alias">1</param>
                <param name="position">0</param>
                <param name="mode_of_operation">8</param>
                <param name="rpdo_control_word">0x6840</param>
                <param name="rpdo_target_position">0x687a</param>
                <param name="rpdo_mode_of_operation">0x6860</param>
                <param name="tpdo_actual_position">0x6864</param>
                <param name="tpdo_status_word">0x6841</param>
                <param name="tpdo_mode_of_operation_display">0x6861</param>
                <param name="tpdo_error_code">0x683f</param>
                <param name="slave_config">/path_to_my_config/multiAxis.yaml</param>
              </ec_module>
            </joint> 

It is possible to just change the setupSlave method in generic_ec_cia402_drive.cpp to make everything work:

unsigned int uint_from_string(const std::string & str)
{
  // Strip leading and trailing whitespaces
  std::string s = std::regex_replace(str, std::regex("^ +| +$|( ) +"), "$1");
  // Test if the number is in hexadecimal format
  if (s.find("0x") == 0) {
    return std::stoul(s, nullptr, 16);
  }
  return std::stoul(s);
}

bool EcCiA402Drive::setupSlave(
  std::unordered_map<std::string, std::string> slave_paramters,
  std::vector<double> * state_interface,
  std::vector<double> * command_interface)
{
  state_interface_ptr_ = state_interface;
  command_interface_ptr_ = command_interface;
  paramters_ = slave_paramters;

  if (paramters_.find("slave_config") != paramters_.end()) {
    if (!setup_from_config_file(paramters_["slave_config"])) {
      return false;
    }
  } else {
    std::cerr << "EcCiA402DriveCfgRegs: failed to find 'slave_config' tag in URDF." << std::endl;
    return false;
  }

// setup PDOs entries if definition is present in URDF
  if (paramters_.find("rpdo_control_word") != paramters_.end()) {
    CiA402D_RPDO_CONTROLWORD = uint_from_string(paramters_["rpdo_control_word"]);
  }
  if (paramters_.find("rpdo_target_position") != paramters_.end()) {
    CiA402D_RPDO_POSITION = uint_from_string(paramters_["rpdo_target_position"]);
  }
  if (paramters_.find("rpdo_target_velocity") != paramters_.end()) {
    CiA402D_RPDO_VELOCITY = uint_from_string(paramters_["rpdo_target_velocity"]);
  }
  if (paramters_.find("rpdo_target_effort") != paramters_.end()) {
    CiA402D_RPDO_EFFORT = uint_from_string(paramters_["rpdo_target_effort"]);
  }
  if (paramters_.find("rpdo_mode_of_operation") != paramters_.end()) {
    CiA402D_RPDO_MODE_OF_OPERATION = uint_from_string(paramters_["rpdo_mode_of_operation"]);
  }
  if (paramters_.find("tpdo_actual_position") != paramters_.end()) {
    CiA402D_TPDO_POSITION = uint_from_string(paramters_["tpdo_actual_position"]);
  }
  if (paramters_.find("tpdo_status_word") != paramters_.end()) {
    CiA402D_TPDO_STATUSWORD = uint_from_string(paramters_["tpdo_status_word"]);
  }
  if (paramters_.find("tpdo_mode_of_operation_display") != paramters_.end()) {
    CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY =
      uint_from_string(paramters_["tpdo_mode_of_operation_display"]);
  }
  if (paramters_.find("tpdo_error_code") != paramters_.end()) {
    CiA402D_TPDO_ERROR_CODE = uint_from_string(paramters_["tpdo_error_code"]);
  }
  // < End of setup PDOs entries

  setup_interface_mapping();
  setup_syncs();

  if (paramters_.find("mode_of_operation") != paramters_.end()) {
    mode_of_operation_ = std::stod(paramters_["mode_of_operation"]);
  }

  if (paramters_.find("command_interface/reset_fault") != paramters_.end()) {
    fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]);
  }

  return true;
}

As the PDO entries are different for each axis, you should be able to use your ec_module several times without changing anything. Internally the same plugin will be loaded for the different joints, so regarding the cia402 state machine, they should be separated. It is just regarding the communication layer that some shadowing will occur and we have observed that it usually works well. The only potential problem, that is obvious, is if one PDO entries is shared by all the cia402 state machine and is use or set in non coherent manner for all the axis. But I see no such problem there.

[yguel]

If you are satisfied by this proposal, we can consider making it part of the official distribution.

[yguel]

I would like to add a comment: you could have one single yaml file for your slave containing all of your axis configuration. However as it will be loaded 6 times, all the pdos will be processed 6x6 times (the library does not check that a register is used two times by different slave declaration (even if on the bus it is the same slave, for the library it is loaded has different slaves). To avoid that, the solution is to have 6 yaml configurations files, one for each axis. Example:

axis0.yaml

rpdo:  # RxPDO = receive PDO Mapping
  - index: 0x1600
    channels:
      # axis0
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: axis0/position, default: .nan, factor: 1, offset: 0}  # Target position
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}  # Control word
      - {index: 0x6060, sub_index: 0, type: uint8, default: 0}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      # axis0
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: axis0/position, factor: 1,  offset: 0}  # Position actual value
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: axis0/status_word}  # Status word
      - {index: 0x603f, sub_index: 0, type: uint16, state_interface: axis0/error_code}  # Error Code
      - {index: 0x6061, sub_index: 0, type: uint8, state_interface: axis0/operation_mode}  # Mode of operation display

axis1.yaml

rpdo:  # RxPDO = receive PDO Mapping
  - index: 0x1600
    channels:
      # axis1
      - {index: 0x687a, sub_index: 0, type: int32, command_interface: axis1/position, default: .nan, factor: 1, offset: 0}  # Target position
      - {index: 0x6840, sub_index: 0, type: uint16, default: 0}  # Control word
      - {index: 0x6860, sub_index: 0, type: uint8, default: 0}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      # axis1
      - {index: 0x6864, sub_index: 0, type: int32, state_interface: axis1/position, factor: 1,  offset: 0}  # Position actual value
      - {index: 0x6841, sub_index: 0, type: uint16, state_interface: axis1/status_word}  # Status word
      - {index: 0x683f, sub_index: 0, type: uint16, state_interface: axis1/error_code}  # Error Code
      - {index: 0x6861, sub_index: 0, type: uint8, state_interface: axis1/operation_mode}  # Mode of operation display

It is not satisfying, and we are working to come up with a better design, however the actual solution is functional and as efficient as one can expect if you take care to have one slave_config yaml file per axis that declares only the registers that the axis uses.

The URDF must be updated accordingly

<joint name="joint1">
  ...
  <ec_module name="rox1">
    <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
    ...
    <param name="slave_config">/path_to_my_config/axis0.yaml</param>
  </ec_module>
</joint> 

<joint name="joint2">
  ...
  <ec_module name="rox2">
    <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
    ...
    <param name="slave_config">/path_to_my_config/axis1.yaml</param>
  </ec_module>
</joint> 

[maxwelllls]

Thank you for your reply.

I haven’t carefully reviewed the branch code you provided yet, but from what I understand, during program initialization, the PDO mapping from each YAML file is written into the corresponding servo drive via SDO.

However, if there are six YAML configuration files pointing to the same servo, it will cause the PDO mapping of this servo to be overwritten repeatedly, resulting in the final PDO mapping containing only the objects for axis6.

[yguel]

Calls to the igh master is done directly to configure the EtherCat frame and as there is no configuration of the sync managers (at least in the files you provide), I see little risk for the PDO mapping containing only the object for axis6. But even if that is the case (I would be interested to know), you can still have the same YAML file with all axis in it. It will be suboptimal in terms of computations, but it will definitely eliminate the risk for having a wrong PDO mapping.

Do you think, you can test it, and report ? I need feedbacks before considering merging that change into the main branch of the driver.

• all the best,

[maxwelllls]

Currently, I am initializing using a single YAML file and have added an “axis” parameter. During the on_init() phase, I filter out ec_module instances with the same position to avoid duplicate processing of PDO data. Finally, I match the corresponding joint’s interfaces with the channel based on the axis. I will first attempt to develop and verify according to this plan. I will keep you informed of the test results in a timely manner.

[yguel]

By modifying the on_init phase, you will come up with a version of the driver dedicated to your case. Your changes won't benefit the community and all the efforts to maintain it and follow the new features and bug fixes of the driver will fall on you. Depending on your long term goals that not necessary a problem but taking those points into account early is always beneficial from a project management point of view. Thanks for the proposed follow ups, we are always interested in feedbacks.