Closed MrCCaesar closed 2 months ago
Hi @MrCCaesar , It looks like one of your drives is ill-configured and another in fault state
[ros2_control_node-1] STATE: Fault Reaction Active with status word :53775 [ros2_control_node-1] STATE: Undefined State with status word :8052
use the controller from PR #61 to see which drive Also, OP is the state of your EtherCAT communication module and not the functional drive state
Hi @MrCCaesar , It looks like one of your drives is ill-configured and another in fault state
[ros2_control_node-1] STATE: Fault Reaction Active with status word :53775 [ros2_control_node-1] STATE: Undefined State with status word :8052
use the controller from PR #61 to see which drive Also, OP is the state of your EtherCAT communication module and not the functional drive state
Thanks, I have tried. And no more this kind of fault, but motor seems still don't work properly. Motor can't get in to Operation Enable state, I have tried use SOEM, that works; but does't work with ros2_control. Is this because of my driver elmo's config.yaml have problem?
Hi @MrCCaesar , can you post your config yam file ?
Hi @MrCCaesar , can you post your config yam file ?
Yes ,and sorry for delayed @mcbed .This my config for elmo right now [https://gist.github.com/MrCCaesar/52c53485bc7ce0b9d23da4df75fed818]() .Thanks for your patient
After I changed my config file according to https://github.com/leggedrobotics/elmo_ethercat_sdk/tree/feature/ament_cmake's RxPDO.h and TxPDO.h, it worked.. But I got a new question, when I run generic_cia402_controller , it shows both slaves are in Operation Enabled mode.
[ros2_control_node-1] 4 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] 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] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] [INFO] [1689923365.886981284] [EthercatDriver]: updated!
[ros2_control_node-1] [INFO] [1689923365.887026604] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1689923365.887042180] [resource_manager]: Successful 'activate' of hardware 'wlrzj_electronic'
[ros2_control_node-1] [INFO] [1689923365.894376820] [controller_manager]: update rate is 100 Hz
[ros2_control_node-1] [INFO] [1689923365.894691246] [controller_manager]: RT kernel is recommended for better performance
[ros2_control_node-1] [INFO] [1689923365.947136939] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-3] [INFO] [1689923365.966423426] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [INFO] [1689923365.968258076] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1689923365.968431432] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1689923365.996625416] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[INFO] [spawner-3]: process has finished cleanly [pid 64168]
[INFO] [spawner-4]: process started with pid [64211]
[ros2_control_node-1] [INFO] [1689923366.946834541] [controller_manager]: Loading controller 'generic_cia402_controller'
[spawner-4] [INFO] [1689923366.999900902] [spawner_generic_cia402_controller]: Loaded generic_cia402_controller
[ros2_control_node-1] [INFO] [1689923367.001463546] [controller_manager]: Configuring controller 'generic_cia402_controller'
[ros2_control_node-1] [INFO] [1689923367.004769259] [generic_cia402_controller]: configure successful
[spawner-4] [INFO] [1689923367.026270183] [spawner_generic_cia402_controller]: Configured and activated generic_cia402_controller
[INFO] [spawner-4]: process has finished cleanly [pid 64211]
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Domain: WC 1.
[ros2_control_node-1] Domain: State 1.
[ros2_control_node-1] STATE: Switch on Disabled with status word :592
[ros2_control_node-1] Domain: WC 3.
[ros2_control_node-1] Slave: State 0x04.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[ros2_control_node-1] Slave: State 0x01.
[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] Slave: State 0x02.
[ros2_control_node-1] STATE: Operation Enabled with status word :5687
[ros2_control_node-1] Domain: WC 4.
[ros2_control_node-1] STATE: Switch on Disabled with status word :592
[ros2_control_node-1] Domain: WC 6.
[ros2_control_node-1] Domain: State 2.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[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] STATE: Operation Enabled with status word :5687
When I run ros2 topic echo /generic_cia402_controller/drive_states, it shows
---
header:
stamp:
sec: 1689922972
nanosec: 149206033
frame_id: ''
dof_names:
- lw
- rw
drive_states:
- STATE_NOT_READY_TO_SWITCH_ON
- STATE_NOT_READY_TO_SWITCH_ON
modes_of_operation:
- MODE_UNDEFINED
- MODE_UNDEFINED
status_words:
- 0
- 0
---
it seems not consist with ros2 launch output. Am i misunderstanding the states here or some @mcbed Thanks.
This is my controller config file
controller_manager:
ros__parameters:
update_rate: 100 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
generic_cia402_controller:
type: ethercat_controllers/CiA402Controller
generic_cia402_controller:
ros__parameters:
dofs:
- lw
- rw
And this is my ros2_control.urdf.xacro file
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- the communication is done on actuator level using proprietary or standardized API -->
<!-- Data for all actuators and sensors is exchanged separately from each other -->
<xacro:macro name="wlrzj_electronic_actuator" params="name prefix">
<ros2_control name="${name}_electronic" type="system">
<hardware>
<plugin>ethercat_driver/EthercatDriver</plugin>
<param name="master_id">0</param>
<param name="control_frequency">100</param>
</hardware>
<joint name="${prefix}rw">
<state_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="reset_fault"/>
<state_interface name="mode_of_operation" />
<state_interface name="status_word" />
<command_interface name="velocity"/>
<command_interface name="control_word"/>
<command_interface name="mode_of_operation"/>
<command_interface name="reset_fault" />
<ec_module name="elmo_gold">
<plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
<param name="alias">0</param>
<param name="position">2</param>
<param name="mode_of_operation">3</param>
<param name="slave_config">$(find wlrzj_description)/config/elmo.yaml</param>
</ec_module>
</joint>
<joint name="${prefix}lw">
<state_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="reset_fault"/>
<state_interface name="mode_of_operation" />
<state_interface name="status_word" />
<command_interface name="velocity"/>
<command_interface name="control_word"/>
<command_interface name="mode_of_operation"/>
<command_interface name="reset_fault" />
<ec_module name="elmo_gold">
<plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
<param name="alias">0</param>
<param name="position">3</param>
<param name="mode_of_operation">3</param>
<param name="slave_config">$(find wlrzj_description)/config/elmo.yaml</param>
</ec_module>
</joint>
</ros2_control>
</xacro:macro>
</robot>
and this is my elmo.yaml
vendor_id: 0x0000009a
product_id: 0x00030924
assign_activate: 0x0300 # DC Synch register
auto_fault_reset: true # 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: 0x1605
channels:
- {index: 0x607a, sub_index: 0, type: int32, default: .nan} # Target position
- {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity, default: .nan} # Target velocity
- {index: 0x6071, sub_index: 0, type: int16, default: .nan} # Target torque
- {index: 0x6072, sub_index: 0, type: uint16, default: 0} #Max torque
- {index: 0x6040, sub_index: 0, type: uint16, command_interface: control_word, default: 0} # Control word
- {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 0} # Mode of operation
tpdo: # TxPDO = transmit PDO Mapping
- index: 0x1a03
channels:
- {index: 0x6064, sub_index: 0, type: int32, state_interface: position} # Position actual value
- {index: 0x60fd, sub_index: 0, type: int32} # Digital Inputs
- {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity} # Velocity actual value
- {index: 0x6041, sub_index: 0, type: uint16, state_interface: status_word} # Status word
- index: 0x1a1d
channels:
- {index: 0x2205, sub_index: 1, type: int16} #Analog Input1
- index: 0x1a1f
channels:
- {index: 0x6078, sub_index: 0, type: int16} #Current actual value
- index: 0x1a18
channels:
- {index: 0x6079, sub_index: 0, type: uint32} #DC link circuit voltage
Hi @MrCCaesar,
it looks like the controller is not reading the status word of your drive, thus the results. Are you sure about the names lw
, rw
as there is some prefix
in urdf ?
Hi @MrCCaesar, it looks like the controller is not reading the status word of your drive, thus the results. Are you sure about the names
lw
,rw
as there is someprefix
in urdf ?
yes, the prefix is empty. And somehow this is now working, but i have to make the name of ec_module different, or only one of this 2 motor works, and that would always be the motor in position 3, is this a bug or just a feature? Thanks @mcbed
I'm using generic cia402 plugin to work woth elmo driver, but after launch the node, the motor is not enabled, the result of ethercat slaves is all OP + 0 0:0 OP + 0x0000009a:0x00030924; and the launch output is [INFO] [launch]: All log files can be found below /home/wlr/.ros/log/2023-05-02-21-01-39-828618-wlr-SBC-T875-5600 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [ros2_control_node-1]: process started with pid [5602] [INFO] [robot_state_publisher-2]: process started with pid [5604] [INFO] [spawner-3]: process started with pid [5606] [robot_state_publisher-2] [WARN] [1683032500.483305839] [kdl_parser]: The root link pelvic has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF. [robot_state_publisher-2] [INFO] [1683032500.483528196] [robot_state_publisher]: got segment left_calf [robot_state_publisher-2] [INFO] [1683032500.483611316] [robot_state_publisher]: got segment left_elbow_link [robot_state_publisher-2] [INFO] [1683032500.483625323] [robot_state_publisher]: got segment left_hip_roll_link [robot_state_publisher-2] [INFO] [1683032500.483635805] [robot_state_publisher]: got segment left_hip_yaw_link [robot_state_publisher-2] [INFO] [1683032500.483646174] [robot_state_publisher]: got segment left_shoulder_pitch_link [robot_state_publisher-2] [INFO] [1683032500.483656827] [robot_state_publisher]: got segment left_shoulder_roll_link [robot_state_publisher-2] [INFO] [1683032500.483666843] [robot_state_publisher]: got segment left_shoulder_yaw_link [robot_state_publisher-2] [INFO] [1683032500.483676630] [robot_state_publisher]: got segment left_thigh [robot_state_publisher-2] [INFO] [1683032500.483686696] [robot_state_publisher]: got segment left_wheel [robot_state_publisher-2] [INFO] [1683032500.483696550] [robot_state_publisher]: got segment pelvic [robot_state_publisher-2] [INFO] [1683032500.483706591] [robot_state_publisher]: got segment right_calf [robot_state_publisher-2] [INFO] [1683032500.483716387] [robot_state_publisher]: got segment right_elbow_link [robot_state_publisher-2] [INFO] [1683032500.483726448] [robot_state_publisher]: got segment right_hip_roll_link [robot_state_publisher-2] [INFO] [1683032500.483736559] [robot_state_publisher]: got segment right_hip_yaw_link [robot_state_publisher-2] [INFO] [1683032500.483746458] [robot_state_publisher]: got segment right_shoulder_pitch_link [robot_state_publisher-2] [INFO] [1683032500.483756602] [robot_state_publisher]: got segment right_shoulder_roll_link [robot_state_publisher-2] [INFO] [1683032500.483766469] [robot_state_publisher]: got segment right_shoulder_yaw_link [robot_state_publisher-2] [INFO] [1683032500.483776441] [robot_state_publisher]: got segment right_thigh [robot_state_publisher-2] [INFO] [1683032500.483786412] [robot_state_publisher]: got segment right_wheel [robot_state_publisher-2] [INFO] [1683032500.483796358] [robot_state_publisher]: got segment upper_body [ros2_control_node-1] [INFO] [1683032500.499043010] [resource_manager]: Loading hardware 'wlrzj_electronic' [ros2_control_node-1] [INFO] [1683032500.502325383] [resource_manager]: Initialize hardware 'wlrzj_electronic'
[ros2_control_node-1] [INFO] [1683032500.510420593] [EthercatDriver]: Got 4 modules [ros2_control_node-1] [INFO] [1683032500.510446175] [resource_manager]: Successful initialization of hardware 'wlrzj_electronic' [ros2_control_node-1] [INFO] [1683032500.510519468] [resource_manager]: 'configure' hardware 'wlrzj_electronic'
[ros2_control_node-1] [INFO] [1683032500.510538699] [resource_manager]: 'activate' hardware 'wlrzj_electronic' [ros2_control_node-1] [INFO] [1683032500.510546649] [EthercatDriver]: Starting ...please wait... [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6040, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x607a, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x60ff, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6071, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x60b0, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x60b1, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x60b2, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6060, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x2078, 0x1} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x60b8, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6041, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6064, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x606c, 0x0} [ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6061, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x6040, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x607a, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x60ff, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x6071, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x60b0, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x60b1, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x60b2, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x6060, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x2078, 0x1} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x60b8, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x6041, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x6064, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x606c, 0x0} [ros2_control_node-1] {0, 1, 0x9a, 0x30924, 0x6061, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x6040, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x607a, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x60ff, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x6071, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x60b0, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x60b1, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x60b2, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x6060, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x2078, 0x1} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x60b8, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x6041, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x6064, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x606c, 0x0} [ros2_control_node-1] {0, 2, 0x9a, 0x30924, 0x6061, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x6040, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x607a, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x60ff, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x6071, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x60b0, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x60b1, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x60b2, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x6060, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x2078, 0x1} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x60b8, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x6041, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x6064, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x606c, 0x0} [ros2_control_node-1] {0, 3, 0x9a, 0x30924, 0x6061, 0x0} [ros2_control_node-1] [INFO] [1683032500.511178327] [EthercatDriver]: Activated EcMaster! [ros2_control_node-1] 4 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] Slave: State 0x02. [ros2_control_node-1] Slave: online. [ros2_control_node-1] Slave: State 0x02. [ros2_control_node-1] Slave: online. [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] STATE: Not Ready to Switch On with status word :0 [ros2_control_node-1] STATE: Not Ready to Switch On with status word :0 [ros2_control_node-1] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-1] [INFO] [1683032501.511603556] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1683032501.518462313] [controller_manager]: update rate is 100 Hz [ros2_control_node-1] [INFO] [1683032501.518820748] [controller_manager]: RT kernel is recommended for better performance [ros2_control_node-1] [INFO] [1683032501.578940988] [controller_manager]: Loading controller 'joint_state_broadcaster' [spawner-3] [INFO] [1683032501.590461500] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [ros2_control_node-1] [INFO] [1683032501.592068998] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2_control_node-1] [INFO] [1683032501.592257139] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published [spawner-3] [INFO] [1683032501.620573105] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster [INFO] [spawner-3]: process has finished cleanly [pid 5606] [INFO] [spawner-4]: process started with pid [5649] [ros2_control_node-1] Slave: State 0x01. [ros2_control_node-1] Slave: State 0x02. [ros2_control_node-1] Domain: WC 1. [ros2_control_node-1] Domain: State 1. [ros2_control_node-1] STATE: Fault Reaction Active with status word :53775 [ros2_control_node-1] [INFO] [1683032502.307148684] [controller_manager]: Loading controller 'wlrzj_position_controller' [spawner-4] [INFO] [1683032502.349735410] [spawner_wlrzj_position_controller]: Loaded wlrzj_position_controller [ros2_control_node-1] [INFO] [1683032502.351252680] [controller_manager]: Configuring controller 'wlrzj_position_controller' [ros2_control_node-1] [INFO] [1683032502.352374653] [wlrzj_position_controller]: configure successful [ros2_control_node-1] [INFO] [1683032502.359094586] [wlrzj_position_controller]: activate successful [spawner-4] [INFO] [1683032502.370400458] [spawner_wlrzj_position_controller]: Configured and activated wlrzj_position_controller [INFO] [spawner-4]: process has finished cleanly [pid 5649] [ros2_control_node-1] Domain: WC 3. [ros2_control_node-1] Slave: State 0x08. [ros2_control_node-1] Slave: operational. [ros2_control_node-1] Slave: State 0x01. [ros2_control_node-1] Slave: State 0x02. [ros2_control_node-1] Domain: WC 4. [ros2_control_node-1] STATE: Undefined State with status word :8052 ^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[ros2_control_node-1] Domain: WC 6. [ros2_control_node-1] Slave: State 0x08. [ros2_control_node-1] Slave: operational. [ros2_control_node-1] Slave: State 0x01. [ros2_control_node-1] Slave: State 0x02. [ros2_control_node-1] Domain: WC 7. [ros2_control_node-1] Domain: WC 9. [ros2_control_node-1] Slave: State 0x08. [ros2_control_node-1] Slave: operational. [ros2_control_node-1] Slave: State 0x01. [ros2_control_node-1] Slave: State 0x02. [ros2_control_node-1] Domain: WC 10. [ros2_control_node-1] Domain: WC 12. [ros2_control_node-1] Domain: State 2. [ros2_control_node-1] Master AL states: 0x08. [ros2_control_node-1] Slave: State 0x08. [ros2_control_node-1] Slave: operational.
How can i fix this? thanks