Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

maxon EPOS4 Cia402Driver homing fails #236

Open
leonce-m opened this issue Dec 12, 2023 · 5 comments
Open

maxon EPOS4 Cia402Driver homing fails #236

leonce-m opened this issue Dec 12, 2023 · 5 comments
Labels
question Further information is requested

Comments

@leonce-m
Copy link

Describe the bug
When calling the /init service with homing mode 37 I get an error, but I'm still able to switch to position mode and control the motor.

Logs

[INFO] [launch]: All log files can be found below /home/admin/.ros/log/2023-12-12-17-56-19-432204-jetson-91204
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [launch.user]: /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/bus.yml
[INFO] [launch.user]: /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/master.dcf
[INFO] [launch.user]: /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/master.bin
[INFO] [launch.user]: vcan1
[INFO] [device_container_node-1]: process started with pid [91222]
[device_container_node-1] [INFO] [1702400179.799718590] [device_container_node]: Starting Device Container with:
[device_container_node-1] [INFO] [1702400179.800007938] [device_container_node]: 	 master_config /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/master.dcf
[device_container_node-1] [INFO] [1702400179.800033058] [device_container_node]: 	 bus_config /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/bus.yml
[device_container_node-1] [INFO] [1702400179.800045378] [device_container_node]: 	 can_interface_name vcan1
[device_container_node-1] [INFO] [1702400179.801109264] [device_container_node]: Loading Master Configuration.
[device_container_node-1] [INFO] [1702400179.801847193] [device_container_node]: Load Library: /workspaces/isaac_ros-dev/install/canopen_master_driver/lib/libmaster_driver.so
[device_container_node-1] [INFO] [1702400179.806102766] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1702400179.806166735] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1702400179.811619347] [master]: NodeCanopenBasicMaster
[device_container_node-1] [INFO] [1702400179.811870070] [device_container_node]: Load master component.
[device_container_node-1] [INFO] [1702400179.811997048] [device_container_node]: Added /master to executor
[device_container_node-1] [INFO] [1702400179.833326562] [device_container_node]: Loading Driver Configuration.
[device_container_node-1] [INFO] [1702400179.833512580] [device_container_node]: Found device steering_motor with driver ros2_canopen::Cia402Driver
[device_container_node-1] [INFO] [1702400179.833959338] [device_container_node]: Load Library: /workspaces/isaac_ros-dev/install/canopen_402_driver/lib/libcia402_driver.so
[device_container_node-1] [INFO] [1702400179.844284747] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1702400179.844367468] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1702400179.850873629] [device_container_node]: Load driver component.
[device_container_node-1] [INFO] [1702400179.851085984] [device_container_node]: Added /steering_motor to executor
[device_container_node-1] [ERROR] [1702400179.873396503] [steering_motor]: Could not read enable diagnostics from config, setting to false.
[device_container_node-1] [INFO] [1702400179.873819932] [steering_motor]: scale_pos_to_dev_ 367.891156
[device_container_node-1] scale_pos_from_dev_ 0.002718
[device_container_node-1] scale_vel_to_dev_ 367.891156
[device_container_node-1] scale_vel_from_dev_ 0.002718
[device_container_node-1] 
[device_container_node-1] [INFO] [1702400179.876090328] [steering_motor]: eds file /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/epos4.dcf
[device_container_node-1] [INFO] [1702400179.876133657] [steering_motor]: bin file /workspaces/isaac_ros-dev/install/srd_canopen/share/srd_canopen/config/as_can/steering_motor.bin
[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=6060 subindex=0
[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=607a subindex=0
[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=60ff subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6061 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6064 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=606c subindex=0
[device_container_node-1] [WARN] [1702400179.884073468] [steering_motor]: Wait for device to boot.
[device_container_node-1] [INFO] [1702400179.933429093] [steering_motor]: Driver booted and ready.
[device_container_node-1] [INFO] [1702400179.934314544] [steering_motor]: Starting with event mode.
[device_container_node-1] [INFO] [1702400179.938836200] [canopen_402_driver]: Fault reset
[device_container_node-1] [INFO] [1702400180.033997709] [steering_motor]: Slave 0xC: Switched NMT state to START
[device_container_node-1] [INFO] [1702400186.662334256] [canopen_402_driver]: Init: Read State
[device_container_node-1] [INFO] [1702400186.662485554] [canopen_402_driver]: Init: Enable
[device_container_node-1] [INFO] [1702400186.667502993] [canopen_402_driver]: Fault reset
[device_container_node-1] [INFO] [1702400186.768836386] [canopen_402_driver]: Init: Switch to homing
[device_container_node-1] [INFO] [1702400186.769709229] [canopen_402_driver]: Init: Execute homing
[device_container_node-1] homing did not start
[device_container_node-1] Homing failed

Setup:

  • Device: Jetson AGX Orin
  • OS: Jetpack 5.1.2-b104
  • ROS-Distro: humble
  • Branch/Commit: humble

Additional context
bus.yml

options:
  dcf_path: "@BUS_CONFIG_PATH@"

master:
  node_id: 1
  driver: ros2_canopen::MasterDriver
  package: canopen_master_driver
  baudrate: 1000
  # heartbeat_multiplier: 1
  # heartbeat_consumer: true
  # heartbeat_producer: 100
  # emcy_inhibit_time: 0
  sync_period: 10000
  # sync_window: 0
  # sync_overflow: 0
  # error_behavior: {1: 0x00}
  # nmt_inhibit_time: 0
  # start: true
  # start_nodes: true
  # start_all_nodes: false
  # reset_all_nodes: false
  # stop_all_nodes: false
  # boot_time: 500

nodes:
  steering_motor:
    node_id: 12
    driver: ros2_canopen::Cia402Driver
    package: canopen_402_driver
    dcf: epos4.dcf
    # heartbeat_multiplier: 1
    # heartbeat_consumer: false
    # heartbeat_producer: 0
    # error_behavior: {}
    # boot: true
    # mandatory: false
    # reset_communication: true
    polling: false # Enables polling of the drive status. If true, period will be used to run a ros2 timer as update loop. If false, the update loop will be triggered by the sync signal and directly executed in the canopen realtime loop.
    period: 10
    # switching_state: 1
    scale_pos_to_dev: 367.891156463
    scale_pos_from_dev: 0.00271819526
    scale_vel_to_dev: 367.891156463
    scale_vel_from_dev: 0.00271819526
    position_mode: 1
    # velocity_mode: 0
    # torque_mode: 0
    sdo:        
      - {index: 0x6081, sub_index: 0, value: 10000} # Set velocity
      - {index: 0x6083, sub_index: 0, value: 200000} # Set acceleration
      - {index: 0x6083, sub_index: 0, value: 200000} # Set deceleration
      - {index: 0x6085, sub_index: 0, value: 200000} # Set quickstop deceleration
      - {index: 0x6098, sub_index: 0, value: 37} # Set default homing mode to 37
@leonce-m leonce-m added the question Further information is requested label Dec 12, 2023
@leonce-m
Copy link
Author

leonce-m commented Dec 12, 2023 •
edited

This is the relevant exchange of SDOs that happens after I call the /init service.

132.000074  60C  40 02 65 00 00 00 00 00
132.000419  58C  43 02 65 00 25 02 00 00
132.000854  60C  40 02 65 00 00 00 00 00
132.001210  58C  43 02 65 00 25 02 00 00
132.001540  60C  40 02 65 00 00 00 00 00
132.001813  58C  43 02 65 00 25 02 00 00
132.002108  60C  40 02 65 00 00 00 00 00
...
132.003259  58C  43 02 65 00 25 02 00 00
132.003725  60C  40 02 65 00 00 00 00 00
132.004059  58C  43 02 65 00 25 02 00 00
132.004608  60C  40 02 65 00 00 00 00 00
132.004883  58C  43 02 65 00 25 02 00 00
132.005154  60C  40 02 65 00 00 00 00 00
132.005417  58C  43 02 65 00 25 02 00 00
132.005670  60C  40 02 65 00 00 00 00 00
132.006018  58C  43 02 65 00 25 02 00 00
132.006226  60C  40 02 65 00 00 00 00 00
132.006612  58C  43 02 65 00 25 02 00 00
132.007298  60C  40 02 65 00 00 00 00 00
132.007758  58C  43 02 65 00 25 02 00 00
132.008261  60C  40 02 65 00 00 00 00 00
132.008720  58C  43 02 65 00 25 02 00 00
...
132.043197  60C  40 98 60 00 00 00 00 00
132.043605  58C  4F 98 60 00 25 00 00 00

It looks like the Master is asking for the supported drive modes and is not happy with 0x225.
0x225 is also the value that is set in the .dcf file.

@leonce-m
Copy link
Author

Things I have tried so far that did not fix the problem:

  1. Set the homing method to 0 in Epos Studio.
    • It's not possible to set the homing method to 0 in Epos Studio. It will throw an error and leave it at 37.
  2. Set the homing method to 0 in the bus.yml.
    • The driver gets stuck on bootup. 
      ...
4.598284  60C  2F 98 60 00 00 00 00 00   
4.598568  58C  80 98 60 00 30 00 09 06
...
4.602341  20C  00 00
4.602590  18C  08 82
  3. Setting the supported drive modes in the .dcf file to 0x205 (set bit 6 to 0).
    • Does nothing. The driver still requests the SDO from the slave.

@hellantos
Copy link
Member

@leonce-m this is a problem I have also encountered with certain drives. It seems to be an issue with the homing sequence (from ROS1) that does not pick up that homing has already happened when you use a mode such as 35/37 (homing in actual position). For now I have set init to not actually fail in that case homing thinks it has failed (kind of workaround). Therefore, you can actually control the device in position mode etc.

@leonce-m
Copy link
Author

@ipa-cmh I was able to bypass this issue by essentially skipping the default homing mode execution similar to how it's done with homing mode 0. It's kind of a hack but it works for now. I needed homing to succeed because otherwise the ros2_control Cia402System would get stuck when calling the /init service and not accept control commands.

  int hmode = driver->universal_get_value<int8_t>(index, 0x0);
  if (hmode == 0 || hmode == 37)
  {
    return true;
  }

@lvjinxin-l
Copy link

When I finish running the launch file,and cansend can0 702#00, the terminal responds as follows：[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=6060 subindex=0
[device_container_node-1] Found rpdo mapped object: index=607a subindex=0
[device_container_node-1] Found rpdo mapped object: index=60ff subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6061 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6064 subindex=0
[device_container_node-1] Found tpdo mapped object: index=606c subindex=0
[device_container_node-1] [WARN] [1711590016.248517424] [test3]: Wait for device to boot.
[device_container_node-1] [INFO] [1711590041.189577478] [test3]: Driver booted and ready.
[device_container_node-1] [INFO] [1711590041.191179428] [test3]: Starting with event mode.
[device_container_node-1] [INFO] [1711590041.219297034] [canopen_402_driver]: Fault reset
I think the end "Fault reset"maybe something wrong.
when I launch service :
ros2 service call /test3/init std_srvs/srv/Trigger
requester: making request: std_srvs.srv.Trigger_Request()

response:
std_srvs.srv.Trigger_Response(success=False, message='')

terminal responds as follows
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out.
[device_container_node-1] [INFO] [1711590103.632577376] [canopen_402_driver]: Init: Read State
[device_container_node-1] [INFO] [1711590103.632684677] [canopen_402_driver]: Init: Enable
[device_container_node-1] [INFO] [1711590103.668500964] [canopen_402_driver]: Fault reset
[device_container_node-1] [INFO] [1711590108.633396486] [canopen_402_driver]: Transition timed out.
[device_container_node-1] Could not enable motor
please help me
my bus.yml:
options:
dcf_path: "@BUS_CONFIG_PATH@"

master:
node_id: 1
driver: "ros2_canopen::MasterDriver"
package: "canopen_master_driver"
sync_period: 10000
baudrate: 250
start_nodes: false
heartbeat_consumer: false
heartbeat_producer: 1000
sync_period: 50000
#vendor_id: 0x00000300
#product_code: 0x4644

defaults:
dcf: "Dolphin_V100B0.eds"
#driver: "ros2_canopen::Cia402Driver"
#package: "canopen_402_driver"
driver: "ros2_canopen::ProxyDriver"
package: "canopen_proxy_driver"
polling: false
heartbeat_consumer: false
heartbeat_producer: 2000
period: 10
boot: false
period: 10
sdo:
#- {index: 0x6060, sub_index: 0, value: 3}

  • {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms
  • {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s
  • {index: 0x6081, sub_index: 0, value: 420}
  • {index: 0x6083, sub_index: 0, value: 800}
    #- {index: 0x6060, sub_index: 0, value: 3} # 3 = profile velocity mode
    tpdo:
    1:
    enabled: true
    #cob_id: "auto"
    cob_id: 0x324
    transmission: 0x01
    mapping:
  • {index: 0x6041, sub_index: 0} # status word
  • {index: 0x6061, sub_index: 0} # mode of operation

rpdo:
1:
enabled: true
#cob_id: "auto"

cob_id: 0x344
transmission: 0xFF
event_timer: 100
mapping:
- {index: 0x6040, sub_index: 0} # control word
- {index: 0x6060, sub_index: 0} # mode of operation
nodes:
proxy_1:
node_id: 2

diagnostics:
enable: false @leonce-m

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
question Further information is requested
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@hellantos @lvjinxin-l @leonce-m