[ZeroErr eRob] Joint not moving using EcCia402Drive + Joint trajectory position

[alesof]

Hi there!

I am trying to move an actuator using the generic plugin EcCiA402Drive. The device cannot switch to OP state due to a configuration error, can someone provide any help?

The error i get is: AL status message 0x001E: "Invalid input configuration".
The actuator name is: erob ZeroErr

sudo dmseg:

[ 7812.078596] EtherCAT DEBUG 0: Adding datagram pair with expected WC 3.
[ 7812.078597] EtherCAT 0: Domain0: Logical address 0x00000000, 20 byte, expected working counter 3.
[ 7812.078598] EtherCAT 0:   Datagram domain0-0-main: Logical offset 0x00000000, 20 byte, type LRW.
[ 7812.078599] EtherCAT DEBUG 0: Stopping master thread.
[ 7812.078632] EtherCAT DEBUG 0: Master IDLE thread exiting...
[ 7812.078651] EtherCAT 0: Master thread exited.
[ 7812.078652] EtherCAT DEBUG 0: FSM datagram is 0000000088c1f073.
[ 7812.078653] EtherCAT 0: Starting EtherCAT-OP thread.
[ 7812.078815] EtherCAT DEBUG 0: Operation thread running with fsm interval = 4000 us, max data size=45000
[ 7812.078817] EtherCAT DEBUG 0: mmap()
[ 7812.078818] EtherCAT WARNING 0: 1 datagram UNMATCHED!
[ 7812.078820] EtherCAT DEBUG 0: Vma fault, offset = 0, page = 00000000dfd09629
[ 7813.086330] EtherCAT DEBUG 0: Configuration changed.
[ 7813.086333] EtherCAT DEBUG 0-0: Checking system time offset.
[ 7813.106353] EtherCAT DEBUG 0-0: DC 64 bit system time offset calculation: system_time=751649702317787824 (corrected with 16000000), app_time=751649702316035076, diff=-1752748
[ 7813.106356] EtherCAT DEBUG 0-0: Setting time offset to 751641523853848985 (was 751641523855601733)
[ 7813.122353] EtherCAT DEBUG 0: Requesting OP...
[ 7813.162369] EtherCAT DEBUG 0-0: Changing state from PREOP to OP.
[ 7813.162374] EtherCAT DEBUG 0-0: Configuring...
[ 7813.190944] EtherCAT DEBUG 0-0: Now in INIT.
[ 7813.190945] EtherCAT DEBUG 0-0: Clearing FMMU configurations...
[ 7813.201000] EtherCAT DEBUG 0-0: Clearing sync manager configurations...
[ 7813.211005] EtherCAT DEBUG 0-0: Clearing DC assignment...
[ 7813.221006] EtherCAT DEBUG 0-0: Configuring mailbox sync managers...
[ 7813.221007] EtherCAT DEBUG 0-0: SM0: Addr 0x1000, Size 128, Ctrl 0x26, En 1
[ 7813.221008] EtherCAT DEBUG 0-0: SM1: Addr 0x1080, Size 128, Ctrl 0x22, En 1
[ 7813.231004] EtherCAT DEBUG 0-0: Assigning SII access to PDI.
[ 7813.260944] EtherCAT DEBUG 0-0: Now in PREOP.
[ 7813.260945] EtherCAT DEBUG 0-0: Assigning SII access back to EtherCAT.
[ 7813.270968] EtherCAT DEBUG 0-0: SM2: Addr 0x1100, Size   0, Ctrl 0x64, En 0
[ 7813.270969] EtherCAT DEBUG 0-0: SM3: Addr 0x1400, Size   0, Ctrl 0x20, En 0
[ 7813.300998] EtherCAT ERROR 0-0: Failed to set SAFEOP state, slave refused state change (PREOP + ERROR).
[ 7813.310940] EtherCAT ERROR 0-0: AL status message 0x001E: "Invalid input configuration".
[ 7813.330965] EtherCAT 0-0: Acknowledged state PREOP.

zeroerr_config.yaml

# Configuration file for ZERO ERR drive
vendor_id: 0x5A65726F
product_id: 0x00029252
assign_activate: 0x0300  # DC Synch register - 
auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
rpdo:  # RxPDO = receive PDO Mapping
 - index: 0x1600
   channels:
     - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan}  # Target position
     - {index: 0x60fe, sub_index: 0, type: int32, default: 0}  # Digital Output Functionalities
     - {index: 0x6040, sub_index: 0, type: uint16, default: 0}  # Control word
tpdo:  # TxPDO = transmit PDO Mapping 
 - index: 0x1A00
   channels:
     - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}  # Position actual value
     - {index: 0x60fd, sub_index: 0, type: int32}  # Digital Input Functionalities
     - {index: 0x6041, sub_index: 0, type: uint16}  # Status word
sm:  # Sync Manager
 - {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: disable}

$ ethercat cstruct

/* Master 0, Slave 0, "ZeroErr Driver"
* Vendor ID:       0x5a65726f
* Product code:    0x00029252
* Revision number: 0x00000001
*/

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
   {0x607a, 0x00, 32}, /* Target Position */
   {0x60fe, 0x00, 32}, /* Digital outputs */
   {0x6040, 0x00, 16}, /* Control Word */
   {0x6064, 0x00, 32}, /* Position Actual Value */
   {0x60fd, 0x00, 32}, /* Digital inputs */
   {0x6041, 0x00, 16}, /* Status Word */
};

ec_pdo_info_t slave_0_pdos[] = {
   {0x1600, 3, slave_0_pdo_entries + 0}, /* R0PDO */
   {0x1a00, 3, slave_0_pdo_entries + 3}, /* T0PDO */
};

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}
};

Thank you!

[alesof]

EDIT: the config file is correct and I'm able to set the OP state. For some reason the config file it was being launched wasn't updated.

Anyway now when I send a new position the controller doesn't work and the motor doesn't move. I am able to move the motor, and the communication is ok as if I set the default position to a value different then zero it moves there. Therefore, the problem is when using the trajectory controller, any help would be appreciated.

[mcbed]

Hi, can you include the log of your terminal running ros2_control ?

[JensVanhooydonck]

Which operation mode is used?

[MEJamnezhad]

Hi, I have the same problem with the erob ZeroErr. I can't reach OP state. Can you describe how you reached the OP state?
Thanks

[JensVanhooydonck]

You must include the operation mode in the tpdo, otherwise your position isn't forwarded. I searched long after this..

I will try to open a PR which falls back to the setup operation mode if there is no config in tpdo.

[alesof]

Hi there, the working configuration file for me was:

zeroerror_config.yaml

# Configuration file for ZERO ERR drive
vendor_id: 0x5A65726F
product_id: 0x00029252
assign_activate: 0x003c
auto_fault_reset: true 
rpdo:  
  - index: 0x1600
    channels:
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: 0}  
      - {index: 0x60fe, sub_index: 0, type: int32, default: 0} 
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}
tpdo:  
  - index: 0x1A00
    channels:
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}
      - {index: 0x60fd, sub_index: 0, type: int32}
      - {index: 0x6041, sub_index: 0, type: uint16} 
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: disable}

One thing was that the default position couldn't be set to .nan but works with zero. Of course this produces another error, if the motor is powered on when its position is not near zero the OP state won't be set to ready but in fault.

This is the ros2_control config file:

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

  <xacro:macro name="motor_drive">

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

      <joint name="joint_1">
        <state_interface name="position"/>
        <command_interface name="position"/>
        <ec_module name="ZeroErr">
          <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">$(find ethercat_motor_drive)/config/zeroerr_config.yaml</param>
        </ec_module>
      </joint>
    </ros2_control>

  </xacro:macro>

</robot>

Sorry if I didn't answer earlier but I didn't have the motor to make any test for a while. I'll try in the next days to make it work with ros2 control, if anyone else has attempted and succeeded let us know!

[brennand]

I would also be interested in the solution for these motors. I can talk to them and also set and read the values for example the position and write the control words eta. But I still can't move the motors and have them all hooked up to a ros2 controller, as I can print the desired position. Any input on how you managed to get them to work would be good.

[JensVanhooydonck]

The main problem is that the mode of operation display is set to 0 -> ModeOfOperation::MODE_NO_MODE

ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp

Line 50 in 9657cf3

int8_t mode_of_operation_display_ = 0;

This should initially be set to the operation mode given in de config mapping.
Otherwise no commands will be send to the position adres.

Temporary work around is to add
- {index: 0x6061, sub_index: 0, type: uint8 } # Mode of operation
to tpdo:

example from above:

# Configuration file for ZERO ERR drive
vendor_id: 0x5A65726F
product_id: 0x00029252
assign_activate: 0x003c
auto_fault_reset: true 
rpdo:  
  - index: 0x1600
    channels:
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan}  
      - {index: 0x60fe, sub_index: 0, type: int32, default: 0} 
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}
tpdo:  
  - index: 0x1A00
    channels:
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}
      - {index: 0x6061, sub_index: 0, type: uint8 }  # Mode of operation
      - {index: 0x60fd, sub_index: 0, type: int32}
      - {index: 0x6041, sub_index: 0, type: uint16} 
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: disable}

This wil set the vallue correctly here:

ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp

Line 78 in 9657cf3

mode_of_operation_display_ = pdo_channels_info_[index].last_value;

[alesof]

Hi @JensVanhooydonck! I tried adding the Mode of operation using - {index: 0x6061, sub_index: 0, type: uint8 } as you suggested, but this makes the motor go into preop error state. Did you manage to use it on the Zero err?

ethercat slaves returns: 0 0:0 PREOP + ZeroErr Driver

@mcbed when i launch it with the configuration that i published in previous comments the ros2_control returns:

[ros2_control_node-1] [INFO] [1706526480.322534728] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-1] {0, 0, 0x5a65726f, 0x29252, 0x607a, 0x0}
[ros2_control_node-1] {0, 0, 0x5a65726f, 0x29252, 0x60fe, 0x0}
[ros2_control_node-1] {0, 0, 0x5a65726f, 0x29252, 0x6040, 0x0}
[ros2_control_node-1] {0, 0, 0x5a65726f, 0x29252, 0x6064, 0x0}
[ros2_control_node-1] {0, 0, 0x5a65726f, 0x29252, 0x60fd, 0x0}
[ros2_control_node-1] {0, 0, 0x5a65726f, 0x29252, 0x6041, 0x0}
[ros2_control_node-1] [INFO] [1706526480.322851982] [EthercatDriver]: Activated EcMaster!
[ros2_control_node-1] 1 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] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] [INFO] [1706526481.323316459] [EthercatDriver]: updated!
[ros2_control_node-1] [INFO] [1706526481.323329363] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1706526481.323336627] [resource_manager]: Successful 'activate' of hardware 'motor_drive'
[ros2_control_node-1] [INFO] [1706526481.325584271] [controller_manager]: update rate is 1000 Hz
[ros2_control_node-1] [WARN] [1706526481.325740030] [controller_manager]: Could not enable FIFO RT scheduling policy
[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 2.
[ros2_control_node-1] STATE: Fault with status word :4616
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[ros2_control_node-1] STATE: Switch on Disabled with status word :4688
[ros2_control_node-1] STATE: Ready to Switch On with status word :4657
[ros2_control_node-1] STATE: Switch On with status word :4659
[ros2_control_node-1] Master AL states: 0x08.
[ros2_control_node-1] STATE: Operation Enabled with status word :4663
[ros2_control_node-1] [INFO] [1706526481.508627839] [controller_manager]: Loading controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1706526481.513056653] [controller_manager]: Loading controller 'trajectory_controller'
[spawner-3] [INFO] [1706526481.515047005] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [WARN] [1706526481.516423306] [trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2_control_node-1] [INFO] [1706526481.517045064] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1706526481.517089710] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-4] [INFO] [1706526481.517324299] [spawner_trajectory_controller]: Loaded trajectory_controller
[ros2_control_node-1] [INFO] [1706526481.519045399] [controller_manager]: Configuring controller 'trajectory_controller'
[ros2_control_node-1] [INFO] [1706526481.519093188] [trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-1] [INFO] [1706526481.519102575] [trajectory_controller]: Command interfaces are [position] and state interfaces are [position].
[ros2_control_node-1] [INFO] [1706526481.519108770] [trajectory_controller]: Using 'splines' interpolation method.
[ros2_control_node-1] [INFO] [1706526481.519489758] [trajectory_controller]: Controller state will be published at 50.00 Hz.
[ros2_control_node-1] [INFO] [1706526481.520063999] [trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-3] [INFO] [1706526481.523317968] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[spawner-4] [INFO] [1706526481.525604155] [spawner_trajectory_controller]: Configured and activated trajectory_controller
[INFO] [spawner-3]: process has finished cleanly [pid 71207]
[INFO] [spawner-4]: process has finished cleanly [pid 71209]

[alesof]

@JensVanhooydonck by hardcoding 8 as mode_of_operation_display_ i am able to move it. What's a clean way to solve this? 😅

[JensVanhooydonck]

@JensVanhooydonck by hardcoding 8 as mode_of_operation_display_ i am able to move it. What's a clean way to solve this? 😅

That was my next possible try..

I wanted to add a PR which fills in the inital value of mode_of_operation_display_ with the value defined here:

<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="joint_0">
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <command_interface name="position"/>
      <command_interface name="reset_fault"/>
      <ec_module name="MAXON">
        <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">/path/to/maxon.yaml</param>
      </ec_module>
    </joint>
  </ros2_control>

Currently not had the time for this. I guess this can easily be done in:

ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp

Line 131 in 9657cf3

mode_of_operation_ = std::stod(paramters_["mode_of_operation"]);

So we also set display instead of only mode_of_operation_

Haven't tested this yet..

Could you let me know if this works?

[alesof]

Ok never mind, I wrote too soon ahah. I am able to set it from the tpdo config but CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY is a uint16!

For whoever is watching, the correct config file, in order to move the motor using ros2_control is the following:

# Configuration file for ZERO ERR drive
vendor_id: 0x5A65726F
product_id: 0x00029252
assign_activate: 0x003c
auto_fault_reset: true 

rpdo: 
  - index: 0x1600
    channels:
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan} 
      - {index: 0x60fe, sub_index: 0, type: int32, default: 0}
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}
tpdo:  
  - index: 0x1A00
    channels:
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position} 
      - {index: 0x60fd, sub_index: 0, type: int32} 
      - {index: 0x6041, sub_index: 0, type: uint16} 
      - {index: 0x6061, sub_index: 0, type: uint16, default: 8}
      
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: disable}

[JensVanhooydonck]

Ok never mind, I wrote too soon ahah. I am able to set it from the tpdo config but CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY is a uint16!

For whoever is watching, the correct config file, in order to move the motor using ros2_control is the following:

# Configuration file for ZERO ERR drive
vendor_id: 0x5A65726F
product_id: 0x00029252
assign_activate: 0x003c
auto_fault_reset: true 

rpdo: 
  - index: 0x1600
    channels:
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan} 
      - {index: 0x60fe, sub_index: 0, type: int32, default: 0}
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}
tpdo:  
  - index: 0x1A00
    channels:
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position} 
      - {index: 0x60fd, sub_index: 0, type: int32} 
      - {index: 0x6041, sub_index: 0, type: uint16} 
      - {index: 0x6061, sub_index: 0, type: uint16, default: 8}
      
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: disable}

I think this is dependend on the mapping of the drive. On my drive this was a uint8 (:
I still think the proper solution would be to alter

ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp

Line 131 in 9657cf3

mode_of_operation_ = std::stod(paramters_["mode_of_operation"]);

to also set the mode_of_operation_display_

But good that it works now!

EDIT
Also the default: 8 should not be necessary.. This value is now read from the drive it self.