ICube-Robotics / ethercat_driver_ros2

Hardware Interface for EtherCAT module integration with ros2_control
https://icube-robotics.github.io/ethercat_driver_ros2/
Apache License 2.0
126 stars 32 forks source link

EcCiA402Drive as hardware interface for custom robot #58

Closed AlessioMosca closed 2 months ago

AlessioMosca commented 1 year ago

Hi there,

I am using the generic plugin EcCiA402Drive in order to develop my custom robot. However, I don not understand how i should use in a good way the ethercat library.

I have configured all the configuration files as (ingenia xcr link) Screenshot from 2023-04-21 14-41-39

I have defined in the urdf file the ro2_control : Screenshot from 2023-04-21 14-45-00

Then, when i launch the real robot the ethercat drive is loaded and the masted activated: Screenshot from 2023-04-21 14-48-45

However, all the drives are not switched on. What I am doing wrong? What am I missing?

By looking the list of hardware interfaces (command and state), it seems that they are properly loaded except for the reset_fault: Screenshot from 2023-04-21 14-59-36

an overview given by rqt_graph: uma

Besides, it is not clear to me (sorry for the high number of question, i am new on ros2 ):

Many thanks for your help Best Regards

Alessio

mcbed commented 1 year ago

Hi @AlessioMosca, On first sight it seems like your drive is rejecting the setup in you config file. In your log you should have a list of all configures pdo channels which is missing in you case. To check master log in debug, in a terminal run:

Besides, it is not clear to me (sorry for the high number of question, i am new on ros2 ):

  • where can i find the data coming from the drive ? is there any topics ?

once communication is running, you can see all state data on the \dynamic_joint_states topic.

  • by looking inside the joint_states i cannot see any message also if i launched the joint_broadcaster ? is this because i need some ros_controller to publish on joint states?

I suppose, this is because you are not receiving any data yet

  • the difference between alias and position in the ec_plugin tag? If I understood well, the position is the real position in the ethercat chain but the alias i do not understand what it means.

you can give your slave some id and use it, but mostly you will use position and put alias to 0

  • why the reset fault is unclaimed ?

non of your controllers is claiming it, you can claim it using the gpio_controller

  • where can i set the control world ? should i add the pdo channel flag "command_interface" ? is possible to set by command line or only from code ?

you can add a command_interface flag and use the gpio_controller, by command line using the ethercat tool or by a ros service using the tool provided in PR #56

  • where can i read the status word ?

you need to associate a state_interface and/or rest same as before

  • how can i run a fault reset command?

with a controller ex gpio_controller

  • how can i use the plugin features (Drive State transitions, Drive Fault reset, Mode of Operation)? are they usable by command line or only in a code ?

drive state is automatic or can be handled by a controller using status and control words, mode of operation is set in urdf or by a controller, drive fault reset is available. a dedicated controller is on the road map #46

  • can pdo's be custom? how are they loaded on the drive? if i use custom pdo's do i have to do any extra steps?

pdo's are configured in the drive. If your drive support custom pdo you need to configure the drive and only add the corresponding line in the slave_config.yaml file.

I hope this helps you

AlessioMosca commented 1 year ago

Hi @mcbed Many thanks for your help

On first sight it seems like your drive is rejecting the setup in you config file. In your log you should have a list of all configures pdo channels which is missing in you case. To check master log in debug, in a terminal run:

ethercat debug 1
sudo dmesg -c
run your code
sudo dmesg

You are right, it looks like that my pdos are wrong

Screenshot from 2023-04-21 17-40-01

pdo's are configured in the drive. If your drive support custom pdo you need to configure the drive and only add the corresponding line in the slave_config.yaml file.

This means that i have first upload the custom PDO and the configure the yaml file ? is it possible upload the PDO by ethercat tool?

non of your controllers is claiming it, you can claim it using the gpio_controller

can i use two plugin in ros2 at the same time #10 if so, how ? it is not clear to me? could you give me more information ? or an example please ?

you can add a command_interface flag and use the gpio_controller, by command line using the ethercat tool or by a ros service using the tool provided in PR https://github.com/ICube-Robotics/ethercat_driver_ros2/pull/56

Thus, if i add the gpio controller (in some way) then i can use the ros2 service, correct ?

you need to associate a state_interface and/or rest same as before

Can I associate with any controller type e.g. ForwardCommandController ? if so, something like that is ok ? the drive confing file: Screenshot from 2023-04-21 17-55-25 the urdf: Screenshot from 2023-04-21 17-56-56 the controller yalm config file Screenshot from 2023-04-21 17-57-48 I have added the fault reset in both command and state interface as you suggest in #54. Did I do properly?

or it is possible only by using the gpio ?

Many many thanks for your time

Best Regards Alessio

mcbed commented 1 year ago

This means that i have first upload the custom PDO and the configure the yaml file ? is it possible upload the PDO by ethercat tool?

your yaml config needs to fit the architecture that is expected by the drive. You can get the currently expected architecture using ethercat cstruct. This architecture can be overwritten but only by a valid one -> see drive documentation for valid PDO mappings.

can i use two plugin in ros2 at the same time https://github.com/ICube-Robotics/ethercat_driver_ros2/issues/10 if so, how ? it is not clear to me? could you give me more information ? or an example please ?

Yes, but I do not see why you would need 2 plugins here ? The gpio_controller can give you access to whatever command_interface you want.

Thus, if i add the gpio controller (in some way) then i can use the ros2 service, correct ?

No, you can use either the controller to access on the flight process data or the service. The service is using SDO and not PDO thus you can in async communicate with the drive without ros2_control node running.

Can I associate with any controller type e.g. ForwardCommandController ? if so, something like that is ok ?

Yes, if set up correctly you can do whatever you want with the command and state interfaces.

I have added the fault reset in both command and state interface as you suggest in https://github.com/ICube-Robotics/ethercat_driver_ros2/issues/54#issuecomment-1517981718. Did I do properly?

I am not sure if this works with the forward position controller but you can try. this state is only required if you use the gpio_controller to control the reset_fault as in #54. Here you do not need it.

AlessioMosca commented 1 year ago

Many thanks @mcbed,

By doing ethercat cstruct i get: Screenshot from 2023-04-24 10-45-03

It looks like that no pdos are configured ? am I wrong? If so, should I upload them on the drives ?

your yaml config needs to fit the architecture that is expected by the drive. You can get the currently expected architecture using ethercat cstruct. This architecture can be overwritten but only by a valid one -> see drive documentation for valid PDO mappings.

I checked the datasheet and I did not find nothing about the valid pdo. In the eds I found some pdos but I think they will be used for configuring the slave when they are launched.

Yes, but I do not see why you would need 2 plugins here ? The gpio_controller can give you access to whatever command_interface you want.

mmm i think that the gpio was another plugin, sorry.

If I would like to define new custom pdos, Is it possible from yaml file or should I use different approach ?

Many thanks for your time. Best Regards Alessio

mcbed commented 1 year ago

The pdo mapping needs to fit the one expected by the drive. In your case and according to the description of Ingenia, your slave_config file should be :

# Configuration file for Ingenia EVE-XCR-E drive
vendor_id: 0x0000029c
product_id: 0x02c31001
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: 0x6040, sub_index: 0, type: uint16, command_interface: control_word, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position}  # Target position
      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity, default: 0}  # Target velocity
      - {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_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: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}  # Mode of operation display

If this still does not work, the issue may come from here to check this try running a read only config:

# Configuration file for Ingenia EVE-XCR-E drive
vendor_id: 0x0000029c
product_id: 0x02c31001
assign_activate: 0x0300  # DC Synch register
auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_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: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}  # Mode of operation display
sm:  # Sync Manager
  - {index: 0, type: output, pdo: ~, watchdog: disable}
  - {index: 1, type: input, pdo: ~, watchdog: disable}
  - {index: 2, type: output, pdo: ~, watchdog: disable}
  - {index: 3, type: input, pdo: tpdo, watchdog: disable}
AlessioMosca commented 1 year ago

Hi @mcbed, Many thanks for your help.

So I have tried both solutions and here the results:

The first is with the possibility to write and read with the new yaml file see test_conf_1.txt. Here, it looks that the master tries to map the pdo but something goes wrong (there are some ERROR printed) test_conf_2.txt

The second test is with the second configuration that you have suggested (see file test_conf_2.txt) test_conf_1.txt

Unfortunately, in both case the drives did not switch to enable operation.

Do you have any suggestions?

Best Regards Alessio

mcbed commented 1 year ago

Hi @AlessioMosca Can you run sudo dmesg -c before running the launch file to clear log history? Also, try running just a single drive to have a better overview of what is going on.

AlessioMosca commented 1 year ago

Hi @mcbed ,

so I have commented out in the ros2 control xacro file all the joint except the first. test_3.txt

Many thanks for your time

Bets Regards Alessio

Attached the file with the terminal output

mcbed commented 1 year ago

Hi @AlessioMosca What is the state of your drive at startup ? can you run ethercat slaves and ethercat sl -v after power up and before running the launch and the same after running the launch ?

AlessioMosca commented 1 year ago

Hi @mcbed

the result of the ethercat slaves

0 65535:0 PREOP + 0x0000029c:0x02c31001 1 65535:0 PREOP + 0x0000029c:0x02c31001 2 65535:0 PREOP + 0x0000029c:0x02c31001 3 65535:0 PREOP + 0x0000029c:0x02c31001 4 65535:0 PREOP + 0x0000029c:0x02c31001 5 65535:1 PREOP + 0x0000029c:0x03b31001 6 65535:0 PREOP + 0x0000029c:0x02c31001 7 65535:0 PREOP + 0x0000029c:0xffffffff

the result of ethercat sl -v

=== Master 0, Slave 0 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x02c31001 Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 0 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes - 1244327085 0 0 1 MII up open yes 1 1244341265 14180 1072 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 1 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x02c31001 Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 1072 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 0 1253756805 0 1072 1 MII up open yes 2 1253768840 12035 1082 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 2 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x02c31001 Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 2154 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 1 1252872875 0 1082 1 MII up open yes 3 1252882745 9870 1072 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 3 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x02c31001 Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 3226 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 2 1248855810 0 1072 1 MII up open yes 4 1248863535 7725 1087 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 4 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x02c31001 Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 4313 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 3 1248459805 0 1087 1 MII up open yes 5 1248465355 5550 835 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 5 === Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x03b31001 Revision number: 0x00050007 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 5148 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 4 2539249634 0 835 1 MII up open yes 6 2539253514 3880 865 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 6 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0x02c31001 Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 6013 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 5 1287153280 0 865 1 MII up open yes 7 1287155430 2150 1075 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA === Master 0, Slave 7 === Alias: 65535 Device: Main State: PREOP Flag: + Identity: Vendor Id: 0x0000029c Product code: 0xffffffff Revision number: 0x00050010 Serial number: 0x00000000 DL information: FMMU bit operation: no Distributed clocks: yes, 64 bit DC system time transmission delay: 7088 ns Port Type Link Loop Signal NextSlave RxTime [ns] Diff [ns] NextDc [ns] 0 MII up open yes 6 1296301895 0 1075 1 MII down closed no - - - - 2 N/A down closed no - - - - 3 N/A down closed no - - - - Mailboxes: Bootstrap RX: 0x1000/128, TX: 0x1400/128 Standard RX: 0x1000/128, TX: 0x1400/128 Supported protocols: EoE, CoE, FoE General: Group: Servo Drives Image name: Order number: Device name: CoE details: Enable SDO: yes Enable SDO Info: yes Enable PDO Assign: yes Enable PDO Configuration: yes Enable Upload at startup: no Enable SDO complete access: yes Flags: Enable SafeOp: no Enable notLRW: no Current consumption: 0 mA

Besides this problem, i would like to ask:

Many thanks

Best regards

Alessio

mcbed commented 1 year ago

hi @AlessioMosca, This looks weird

0 65535:0 PREOP + 0x0000029c:0x02c31001 1 65535:0 PREOP + 0x0000029c:0x02c31001 2 65535:0 PREOP + 0x0000029c:0x02c31001 3 65535:0 PREOP + 0x0000029c:0x02c31001 4 65535:0 PREOP + 0x0000029c:0x02c31001 5 65535:1 PREOP + 0x0000029c:0x03b31001 6 65535:0 PREOP + 0x0000029c:0x02c31001 7 65535:0 PREOP + 0x0000029c:0xffffffff

did you play with salve aliases or is it the out-of-the-box config ? also, what is the last module you have on position 7 ?

AlessioMosca commented 1 year ago

Hi @mcbed, No i did not paly with the aliases it is the out of the box config.

The last drive is the the new version of the ingenia everest drive.

Best Regards Alessio