using CSP mode, would move to position 0 when launch

[wei1224hf]

using this launch code: ros2 launch ethercat_motor_drive motor_drive.launch.py my servo motor eanbled , but when it's enabled, the motor would shock, if it's not in position 0 , seems that it would send position 0 when it start up, how to make it stay at where it stay during starting up?

the start up script I used: ros2 launch ethercat_motor_drive motor_drive.launch.py

the script I used to test move: ''ros2 topic pub -r 0.2 /trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ""}, joint_names: ["joint_1"], points: [{positions: [100.0], velocities: [0.0], accelerations: [0.0], time_from_start: {sec: 1, nanosec: 0}},{positions: [10000.0], velocities: [0.0], accelerations: [0.0],time_from_start: {sec: 5, nanosec: 0}}]}'

I found this poblem has something to do with override_command attribute:

[JensVanhooydonck]

For me this only happens if the controller starts to quickly.. If i remember correctly this is something that is known: https://github.com/ICube-Robotics/ethercat_driver_ros2/pull/120#issuecomment-2092788718

In the meantime, I will merge your PR (you can expect the merge to be done by the end of the week).

The problem, you had, shows however a bigger problem of our design. It shows that our design does not follow the hardware interface lifecycle (https://control.ros.org/master/_images/hardware_interface_lifecycle.png)

Read and write calls are legit after the on_configure stage, just «no movement» should happen. Since for the moment we have neither the concept of a «motion enabled» nor the concept of a «motion disabled» state for our ethercat slaves, it seemed more «simple» to setup the ethercat communication in the activate() method, but we should change that.

Our proposal is to have something that emulate a write_inactive() and a write_active() method for each slave that will be called by the write() method according to the hardware interface state.

For the generic slave interface, we could add a new parameter to the channels providing values for the writes in the inactive mode:

either an inactive_value that will be written in place of the actual value in inactive state
or an inactive_mask that will be applied to the value to write in inactive state.
Only one of the 2 (inactive_value or inactive_mask) would be accepted or we could always use the inactive_value if it exists and applied at the end the inactive_mask.
As it will break the existing mode of operation, however being more respectful of the ros2 control specifications, we will increment the major version of the lib, making it an api breaking change.

I've temporary fixed this by first starting the ros2 control node, which starts ethercat, then add a timeout afterwards the other controllers are started:

from launch.actions import RegisterEventHandler, TimerAction
...

[ros2_control_node]
+ [
    RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=ros2_control_node,
            on_start=[
                TimerAction(
                    period=(
                        45.0
                    ),
                    actions=load_controllers,
                ),
            ],
        )
    )
]

[yguel]

Hi JensVanhooydonck, The problem solved by the PR (merged by the way) is that the call to read or write would completely fail because the network would not be configured at the on_configure stage although it should be according to the hardware interface life cycle. It is also true, that now, a write is prevented (thanks to the mutex) if the network is not completely activated (or on_activate has finished). If a write was called before activation is finished, before the PR, nothing was guaranteed.

It opens a problem with ros2 control: if on_activate takes time, what should be the read and the write values during the on_activate function call ? The only proper solution, I see, is that only after on_activate terminates, writes must correspond to activated writes to avoid having a mix of activated and non activated writes.

If the 2 problems are indeed related, that is a very good catch ! @JensVanhooydonck have you a program that reproduces the problem ? Can you confirm that it is solved by the PR ? @wei1224hf Does the PR solved your problem ? If yes, I would like to integrate a unit test to confirm that and I would like to have a very simple program reproducing the problem, so more data on your project would help towards that.

• all the best,

[wei1224hf]

For me this only happens if the controller starts to quickly.. If i remember correctly this is something that is known: #120 (comment)

In the meantime, I will merge your PR (you can expect the merge to be done by the end of the week).

The problem, you had, shows however a bigger problem of our design. It shows that our design does not follow the hardware interface lifecycle (https://control.ros.org/master/_images/hardware_interface_lifecycle.png)

Read and write calls are legit after the on_configure stage, just «no movement» should happen. Since for the moment we have neither the concept of a «motion enabled» nor the concept of a «motion disabled» state for our ethercat slaves, it seemed more «simple» to setup the ethercat communication in the activate() method, but we should change that.

Our proposal is to have something that emulate a write_inactive() and a write_active() method for each slave that will be called by the write() method according to the hardware interface state.

For the generic slave interface, we could add a new parameter to the channels providing values for the writes in the inactive mode:

either an inactive_value that will be written in place of the actual value in inactive state
or an inactive_mask that will be applied to the value to write in inactive state.
Only one of the 2 (inactive_value or inactive_mask) would be accepted or we could always use the inactive_value if it exists and applied at the end the inactive_mask.
As it will break the existing mode of operation, however being more respectful of the ros2 control specifications, we will increment the major version of the lib, making it an api breaking change.

I've temporary fixed this by first starting the ros2 control node, which starts ethercat, then add a timeout afterwards the other controllers are started:

from launch.actions import RegisterEventHandler, TimerAction
...

[ros2_control_node]
+ [
    RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=ros2_control_node,
            on_start=[
                TimerAction(
                    period=(
                        45.0
                    ),
                    actions=load_controllers,
                ),
            ],
        )
    )
]

Thank you for your advise, I tried your way , but it did not work. the motor will move to position 0 during ros2 launch ethercat_motor_drive motor_drive.launch.py, here is the launch code I used:

`

from launch import LaunchDescription from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch.actions import DeclareLaunchArgument

from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node

from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess, LogInfo, RegisterEventHandler, TimerAction) from launch.conditions import IfCondition from launch.event_handlers import (OnExecutionComplete, OnProcessExit, OnProcessIO, OnProcessStart, OnShutdown) from launch.events import Shutdown from launch.substitutions import (EnvironmentVariable, FindExecutable, LaunchConfiguration, LocalSubstitution, PythonExpression)

def generate_launch_description():

# Declare arguments
declared_arguments = []
declared_arguments.append(
    DeclareLaunchArgument(
        'description_file',
        default_value='motor_drive.config_0609.xacro',
        description='URDF/XACRO description file with the axis.',
    )
)

description_file = LaunchConfiguration('description_file')

# Get URDF via xacro
robot_description_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        PathJoinSubstitution(
            [
                FindPackageShare("ethercat_motor_drive"),
                "description/config",
                description_file,
            ]
        ),
    ]
)
robot_description = {"robot_description": robot_description_content}

robot_controllers = PathJoinSubstitution(
    [
        FindPackageShare("ethercat_motor_drive"),
        "config",
        "controllers_huichuan.yaml",
    ]
)

control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[robot_description, robot_controllers],
    output="both",
)
robot_state_pub_node = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    output="both",
    parameters=[robot_description],
)

joint_state_broadcaster_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
)

trajectory_controller_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["trajectory_controller", "-c", "/controller_manager"],
)

velocity_controller_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["velocity_controller", "-c", "/controller_manager"],
)

#effort_controller_spawner = Node(
#   package="controller_manager",
#   executable="spawner",
#   arguments=["effort_controller", "-c", "/controller_manager"],
#)

nodes = [     
    robot_state_pub_node,
    joint_state_broadcaster_spawner,
    trajectory_controller_spawner,
    #velocity_controller_spawner,
    #effort_controller_spawner,
]

event_handler = RegisterEventHandler(
    OnProcessStart(
        target_action=control_node,
        on_start=[
            TimerAction(
                period=45.0,  # Wait for 45 seconds
                actions=nodes,  # Then execute the load_controllers actions
            ),
        ],
    )
)    

declared_arguments.append(control_node)
declared_arguments.append(event_handler)
return LaunchDescription(
    declared_arguments)

`

here is the whole output:

` [INFO] [launch]: All log files can be found below /home/fudanrobotuser/.ros/log/2024-06-23-04-54-32-615991-111-19364 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [ros2_control_node-1]: process started with pid [19366] [ros2_control_node-1] [WARN] [1719118472.756832068] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead. [ros2_control_node-1] [INFO] [1719118472.756910815] [resource_manager]: Loading hardware 'motor_drive' [ros2_control_node-1] [INFO] [1719118472.757978208] [resource_manager]: Initialize hardware 'motor_drive'

[ros2_control_node-1] [INFO] [1719118472.759867761] [EthercatDriver]: Got 1 modules [ros2_control_node-1] [INFO] [1719118472.759884028] [resource_manager]: Successful initialization of hardware 'motor_drive' [ros2_control_node-1] [INFO] [1719118472.759967388] [resource_manager]: 'configure' hardware 'motor_drive'

[ros2_control_node-1] [INFO] [1719118472.759975423] [resource_manager]: 'activate' hardware 'motor_drive' [ros2_control_node-1] [INFO] [1719118472.759978491] [EthercatDriver]: Starting ...please wait... [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6040, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x607a, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x607a, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x60ff, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6071, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6060, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6041, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6064, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x606c, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6077, 0x0} [ros2_control_node-1] {0, 0, 0x100000, 0xc010d, 0x6061, 0x0} [ros2_control_node-1] [INFO] [1719118472.760319340] [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] [1719118473.760860735] [EthercatDriver]: System Successfully started!

[ros2_control_node-1] [INFO] [1719118473.766757718] [controller_manager]: update rate is 125 Hz [ros2_control_node-1] [WARN] [1719118473.766924201] [controller_manager]: Could not enable FIFO RT scheduling policy [ros2_control_node-1] Domain: WC 2. [ros2_control_node-1] Domain: State 1. [ros2_control_node-1] Slave: State 0x04. [ros2_control_node-1] Domain: WC 3. [ros2_control_node-1] Domain: State 2. [ros2_control_node-1] STATE: Fault with status word :5688 [ros2_control_node-1] STATE: Switch on Disabled with status word :5712 [ros2_control_node-1] Master AL states: 0x08. [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 :5681 [ros2_control_node-1] STATE: Switch On with status word :5683 [ros2_control_node-1] STATE: Operation Enabled with status word :5687 `

[wei1224hf]

Hi JensVanhooydonck, The problem solved by the PR (merged by the way) is that the call to read or write would completely fail because the network would not be configured at the on_configure stage although it should be according to the hardware interface life cycle. It is also true, that now, a write is prevented (thanks to the mutex) if the network is not completely activated (or on_activate has finished). If a write was called before activation is finished, before the PR, nothing was guaranteed.

It opens a problem with ros2 control: if on_activate takes time, what should be the read and the write values during the on_activate function call ? The only proper solution, I see, is that only after on_activate terminates, writes must correspond to activated writes to avoid having a mix of activated and non activated writes.

If the 2 problems are indeed related, that is a very good catch ! @JensVanhooydonck have you a program that reproduces the problem ? Can you confirm that it is solved by the PR ? @wei1224hf Does the PR solved your problem ? If yes, I would like to integrate a unit test to confirm that and I would like to have a very simple program reproducing the problem, so more data on your project would help towards that.

• all the best,

Thank you for your reply , the PR did not solve the problem. the source code I download , the main branch , has already used the 120comments ,

during my work , I found these codes may to do with the problem:

the override_command attribute , if I changed it to true , the motor will not crush to position 0 when start up, but it would not move if I send another command , like the topic below I used to test: ros2 topic pub -r 0.2 /trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ""}, joint_names: ["joint_1"], points: [{positions: [100.0], velocities: [0.0], accelerations: [0.0], time_from_start: {sec: 1, nanosec: 0}},{positions: [10000.0], velocities: [0.0], accelerations: [0.0],time_from_start: {sec: 5, nanosec: 0}}]}'

If I changed it to false, the motor will crush to position 0 during start up. the start up command I used is : ros2 launch ethercat_motor_drive motor_drive.launch.py

[JensVanhooydonck]

Hi JensVanhooydonck, The problem solved by the PR (merged by the way) is that the call to read or write would completely fail because the network would not be configured at the on_configure stage although it should be according to the hardware interface life cycle. It is also true, that now, a write is prevented (thanks to the mutex) if the network is not completely activated (or on_activate has finished). If a write was called before activation is finished, before the PR, nothing was guaranteed. It opens a problem with ros2 control: if on_activate takes time, what should be the read and the write values during the on_activate function call ? The only proper solution, I see, is that only after on_activate terminates, writes must correspond to activated writes to avoid having a mix of activated and non activated writes. If the 2 problems are indeed related, that is a very good catch ! @JensVanhooydonck have you a program that reproduces the problem ? Can you confirm that it is solved by the PR ? @wei1224hf Does the PR solved your problem ? If yes, I would like to integrate a unit test to confirm that and I would like to have a very simple program reproducing the problem, so more data on your project would help towards that.

• all the best,

Thank you for your reply , the PR did not solve the problem. the source code I download , the main branch , has already used the 120comments ,

during my work , I found these codes may to do with the problem:

the override_command attribute , if I changed it to true , the motor will not crush to position 0 when start up, but it would not move if I send another command , like the topic below I used to test: ros2 topic pub -r 0.2 /trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ""}, joint_names: ["joint_1"], points: [{positions: [100.0], velocities: [0.0], accelerations: [0.0], time_from_start: {sec: 1, nanosec: 0}},{positions: [10000.0], velocities: [0.0], accelerations: [0.0],time_from_start: {sec: 5, nanosec: 0}}]}'

If I changed it to false, the motor will crush to position 0 during start up. the start up command I used is : ros2 launch ethercat_motor_drive motor_drive.launch.py

For CSP mode the overwride_command should not be set.. Can you share your yaml-config file? What i have seen and strugled with, is that the Mode of operation needs to be set in the config file as well. Only in the ros2_control.xacro.urdf file is not enough to always have the correct mode settings..

[467480903]

Hi JensVanhooydonck, The problem solved by the PR (merged by the way) is that the call to read or write would completely fail because the network would not be configured at the on_configure stage although it should be according to the hardware interface life cycle. It is also true, that now, a write is prevented (thanks to the mutex) if the network is not completely activated (or on_activate has finished). If a write was called before activation is finished, before the PR, nothing was guaranteed. It opens a problem with ros2 control: if on_activate takes time, what should be the read and the write values during the on_activate function call ? The only proper solution, I see, is that only after on_activate terminates, writes must correspond to activated writes to avoid having a mix of activated and non activated writes. If the 2 problems are indeed related, that is a very good catch ! @JensVanhooydonck have you a program that reproduces the problem ? Can you confirm that it is solved by the PR ? @wei1224hf Does the PR solved your problem ? If yes, I would like to integrate a unit test to confirm that and I would like to have a very simple program reproducing the problem, so more data on your project would help towards that.

• all the best,

Thank you for your reply , the PR did not solve the problem. the source code I download , the main branch , has already used the 120comments , during my work , I found these codes may to do with the problem: the override_command attribute , if I changed it to true , the motor will not crush to position 0 when start up, but it would not move if I send another command , like the topic below I used to test: ros2 topic pub -r 0.2 /trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ""}, joint_names: ["joint_1"], points: [{positions: [100.0], velocities: [0.0], accelerations: [0.0], time_from_start: {sec: 1, nanosec: 0}},{positions: [10000.0], velocities: [0.0], accelerations: [0.0],time_from_start: {sec: 5, nanosec: 0}}]}' If I changed it to false, the motor will crush to position 0 during start up. the start up command I used is : ros2 launch ethercat_motor_drive motor_drive.launch.py

For CSP mode the overwride_command should not be set.. Can you share your yaml-config file? What i have seen and strugled with, is that the Mode of operation needs to be set in the config file as well. Only in the ros2_control.xacro.urdf file is not enough to always have the correct mode settings..

I am wei1224hf 's another account, I have solved the problem , by modify ec_pdo_channel_manager.hpp

void ec_update(uint8_t domain_address) { // update state interface if (pdo_type == TPDO) { ec_read(domain_address); if (interface_index >= 0) { state_interfaceptr->at(interface_index) = last_value; } } else if (pdo_type == RPDO && allow_ec_write) { if (interface_index >= 0 && !std::isnan(command_interfaceptr->at(interface_index)) && !override_command) { value_wxf = factor command_interfaceptr->at(interface_index) + offset; if(index==0x607a && value_wxf==0.0){ value_wxf = default_value; ec_write(domain_address, value_wxf); } else{ std::cout<<"value_wxf "<<value_wxf<<std::endl; ec_write(domain_address, value_wxf); } } else { if (!std::isnan(default_value)) { // if(index==0x607a){ std::cout<<"default_value "<<default_value<<std::endl; // } ec_write(domain_address, default_value); } } } }

I found that this did not work: std::isnan(command_interfaceptr->at(interface_index)) if there is nothing send into /trajectory_controller/joint_trajectory , this code should take the data as nan , but it take the data as 0 so the motor would move to position zero at start.

[467480903]

modify in this way would solve the problem

[JensVanhooydonck]

modify in this way would solve the problem

Are you sure there isn't just a 0 command on the command interface? Can you give the yaml-config file? You did set default to .nan ?

And do you have a

 - { index: 0x6060, sub_index: 0, type: int8, default: 8, command_interface: mode_of_operation }

and a

- { index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation, } # Mode of operation display +

[467480903]

[467480903]

modify in this way would solve the problem

Are you sure there isn't just a 0 command on the command interface? Can you give the yaml-config file? You did set default to .nan ?

And do you have a

 - { index: 0x6060, sub_index: 0, type: int8, default: 8, command_interface: mode_of_operation }

and a

- { index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation, } # Mode of operation display +

Yes , I have set .nan to position command interface,
but have not set command_interface attribute to 0x6060 nor 0x6061,

` vendor_id: 0x00100000 product_id: 0x000c010d assign_activate: 0x0300 # DC Synch register auto_fault_reset: false # 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: 0x1600 channels:
  • {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
  • {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan} # Target torque
  • {index: 0x6060, sub_index: 0, type: int8, default: 8} # Mode of operation tpdo: # TxPDO = transmit PDO Mapping
• index: 0x1a00 channels:
  • {index: 0x6041, sub_index: 0, type: uint16} # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort} # Torque actual value
  • {index: 0x6061, sub_index: 0, type: int8} # Mode of operation display `

[JensVanhooydonck]

modify in this way would solve the problem

Are you sure there isn't just a 0 command on the command interface? Can you give the yaml-config file? You did set default to .nan ? And do you have a

 - { index: 0x6060, sub_index: 0, type: int8, default: 8, command_interface: mode_of_operation }

and a

- { index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation, } # Mode of operation display +

Yes , I have set .nan to position command interface, but have not set command_interface attribute to 0x6060 nor 0x6061,

` vendor_id: 0x00100000 product_id: 0x000c010d assign_activate: 0x0300 # DC Synch register auto_fault_reset: false # 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: 0x1600 channels:

  • {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
  • {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan} # Target torque
  • {index: 0x6060, sub_index: 0, type: int8, default: 8} # Mode of operation tpdo: # TxPDO = transmit PDO Mapping

• index: 0x1a00 channels:

  • {index: 0x6041, sub_index: 0, type: uint16} # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort} # Torque actual value
  • {index: 0x6061, sub_index: 0, type: int8} # Mode of operation display `

You should try to add the 0x6060 and the 0x6061

Because all these features use the mode_of_operationdisplay which is set by reading from 0x6061

Here some features are set: https://github.com/ICube-Robotics/ethercat_driver_ros2/blob/52be2c2ed163bab25d46c402ddb4e7216c0a0ec3/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp#L58

I think this would also be solved if we add an extra line here: https://github.com/ICube-Robotics/ethercat_driver_ros2/blob/52be2c2ed163bab25d46c402ddb4e7216c0a0ec3/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp#L131 mode_of_operation_display_ = std::stod(paramters_["mode_of_operation"]);

[yguel]

@JensVanhooydonck Thanks for your proposal and your PR. I answer here, since I would like the feedback of @wei1224hf , as I understand, the use case is to have a proper functioning system without reading the mode_of_operationdisplay . Is there a good case for that ? Especially, the fix proposed is to set a fake mode_of_operation_display, so at initialization time, the EcSlave would potentially be in a state not reflecting the true state of the EtherCat slave. As the CiA402 state potentially depends upon its prior states, I fear that it could lead to potential problems for the first frame (but I cannot describe one yet). If that is really something that you would like to be implemented, and there is a good case (because clearly not reading the mode_of_operationdisplay is not one, if it is possible), because it happens for a lot of drives, I would prefer (from more agreed to least agreed):

1. create a new slave derived from EcCiA402Drive like NoModeOfOperationDisplayEcCiA402Drive that will have the special setup of the mode_of_operationdisplay
2. create a new parameter and read it:

   mode_of_operation_display_ = std::stod(paramters_["default_mode_of_operation_display"]);

3. check that the 0x6061 is not defined for reading in the yaml config file, issue a warning and set mode_of_operationdisplay to the value you proposed.

And to be user friendly, we should report the possible problem of initialization when mode_of_operation_display tpdo is not defined. Actually we should report (or fail to launch) if one of the following

• 0x6061 TPDO
• 0x6060 RPDO
• 0x6040 RPDO control word
• 0x6041 TPDO status word

is not defined in the config file as well as one of Position, Velocity and Torque with matching rpdo and tpdo.

My opinion is that a failure to launch would be preferable (we could however make that failure optional with an advanced parameter to ON for user wanting to experiment with exotic drive loosely following CiA402 norm).

[JensVanhooydonck]

be preferable (we could however make that failure optional with an advanced parameter to ON for user wanting to experiment with exotic drive loosely following CiA402

This is indeed the best solution. That's also the reason i asked if the 0x6061 was put in the yaml-config file. This was just a quick fix. I completely agree that splitting this to another controller, which is specific for drives which do not report the mode of operation, is the better solution.

Also failing to launch if 0x6061, 0x6060, 0x6040 and 0x6041 are missing seems like a good idea as well.

Maybe we could create NoModeOfOperationDisplayEcCiA402Drive which only overwrites the setupSlave function and inherits from EcCiA402Drive?

[yguel]

@JensVanhooydonck, your question about 0x6061 was indeed crucial and I would like to know from @wei1224hf, if this is possible for wei1224hf's system to read it. If this is not possible, I confirm that the derived class: NoModeOfOperationDisplayEcCiA402Drive solution is my preferred option.

I have another use case and solution from P. Zanne, a colleague who takes part of the development of the driver:

The use case would be a system with many motors (like many, many motors) and we would like to avoid having the mode_of_operationdisplay (as well as the mode_of_operation) polluting the frame. Indeed, mode_of_operation and mode_of_operation_display data are only necessary in case of errors and when a switch of mode is required (like at start) or when a switch of mode of control is required.

The solution P. Zanne suggested, in this case, is to use SDOs to asynchronously interrogate the drives when an error occurs or before and after a switch of mode is required. This is a very specific use case, nevertheless potentially a real one. The solution is however not trivial to implement and we should begin working on it, only if a real use case happens (because, a solution designed for an imaginary problem most often fails to solve the real problem when it occurs as it always differ slightly from what was envisioned and devils love to hide in those differences ... ).

[467480903]

be preferable (we could however make that failure optional with an advanced parameter to ON for user wanting to experiment with exotic drive loosely following CiA402

This is indeed the best solution. That's also the reason i asked if the 0x6061 was put in the yaml-config file. This was just a quick fix. I completely agree that splitting this to another controller, which is specific for drives which do not report the mode of operation, is the better solution.

Also failing to launch if 0x6061, 0x6060, 0x6040 and 0x6041 are missing seems like a good idea as well.

Maybe we could create NoModeOfOperationDisplayEcCiA402Drive which only overwrites the setupSlave function and inherits from EcCiA402Drive?

I'm wei1224hf's another account, I think the problem has nothing to do with ModeOfOperation nor 0x6061, 0x6060, 0x6040 0x6041, it's the problem is from this code: std::isnan(command_interfaceptr->at(interface_index)) if nothing has send to the trajctrry_controller , it should be nan, but this code take it as 0 , so the motor moved to position 0

[yguel]

From your code it is clear that command_interfaceptr->at(interface_index) is 0, so it means that either initialization should be nan or that the command is really 0. Can you check that your command code does not send 0 ? I will check initialization. Your fix introduces a problem: if you want to go to 0 (and your command is 0) it will send default_value.

[yguel]

I definitely confirm that the problem comes from the mode of operation_display or 0x6061 being not read and triggering the whole chain of consequences. Initialization is correct and there is a unit test that check it and passes

TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) in file test_generic_ec_cia402_drive.cpp

Can you add 0x6061 : mode_of_operation_display and 0x6060 : mode_of_operation in your config file and make a test ? Without an answer to that question, we cannot go further.

I make better documentation of that test for a next PR, you can have a look at that link: https://github.com/yguel/ethercat_driver_ros2/blob/8455edabd605218b8bf22f7c18d5c7ba3dd4095e/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp#L274

You can run the test and verify that indeed, correct setting of the target_position is made if the command is NaN. So give it a try with adding 0x6061 and 0x6060 !

I have seen that we had another problem (not related to yours though). If a default value for mode_of_operation was set in the YAML file, it was not taken into account, only if put in the URDF file as a plugin parameter, was that value used.

If you look at the branch (yguel-humble-cia402-checks) in my fork: https://github.com/yguel/ethercat_driver_ros2/tree/yguel-humble-cia402-checks, in the commit: https://github.com/ICube-Robotics/ethercat_driver_ros2/commit/8455edabd605218b8bf22f7c18d5c7ba3dd4095e I propose to use the YAML value to intialize mode_ofoperation. Now both values are used with the URDF value taking priority in order to have backward compatible.

That will go into a PR.

[467480903]

I definitely confirm that the problem comes from the mode of operation_display or 0x6061 being not read and triggering the whole chain of consequences. Initialization is correct and there is a unit test that check it and passes

TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) in file test_generic_ec_cia402_drive.cpp

Can you add 0x6061 : mode_of_operation_display and 0x6060 : mode_of_operation in your config file and make a test ? Without an answer to that question, we cannot go further.

I make better documentation of that test for a next PR, you can have a look at that link: https://github.com/yguel/ethercat_driver_ros2/blob/8455edabd605218b8bf22f7c18d5c7ba3dd4095e/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp#L274

You can run the test and verify that indeed, correct setting of the target_position is made if the command is NaN. So give it a try with adding 0x6061 and 0x6060 !

I have seen that we had another problem (not related to yours though). If a default value for mode_of_operation was set in the YAML file, it was not taken into account, only if put in the URDF file as a plugin parameter, was that value used.

If you look at the branch (yguel-humble-cia402-checks) in my fork: https://github.com/yguel/ethercat_driver_ros2/tree/yguel-humble-cia402-checks, in the commit: 8455eda I propose to use the YAML value to intialize mode_ofoperation. Now both values are used with the URDF value taking priority in order to have backward compatible.

That will go into a PR.

I tried , it did not work ,here is the yaml:

vendor_id:  0x00100000
product_id: 0x000c010d
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, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan}  # Target torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 8 ,command_interface: mode_of_operation}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}  # Mode of operation display

chage to mode_of_operation_display , not work too

vendor_id:  0x00100000
product_id: 0x000c010d
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, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan}  # Target torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 8 ,command_interface: mode_of_operation}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation_display}  # Mode of operation display

[467480903]

I'm using the main branch. my ros is ros2-humble , but I'm not using the humble branch , is this a problem?

[JensVanhooydonck]

I definitely confirm that the problem comes from the mode of operation_display or 0x6061 being not read and triggering the whole chain of consequences. Initialization is correct and there is a unit test that check it and passes

TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) in file test_generic_ec_cia402_drive.cpp

Can you add 0x6061 : mode_of_operation_display and 0x6060 : mode_of_operation in your config file and make a test ? Without an answer to that question, we cannot go further. I make better documentation of that test for a next PR, you can have a look at that link: https://github.com/yguel/ethercat_driver_ros2/blob/8455edabd605218b8bf22f7c18d5c7ba3dd4095e/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp#L274 You can run the test and verify that indeed, correct setting of the target_position is made if the command is NaN. So give it a try with adding 0x6061 and 0x6060 ! I have seen that we had another problem (not related to yours though). If a default value for mode_of_operation was set in the YAML file, it was not taken into account, only if put in the URDF file as a plugin parameter, was that value used. If you look at the branch (yguel-humble-cia402-checks) in my fork: https://github.com/yguel/ethercat_driver_ros2/tree/yguel-humble-cia402-checks, in the commit: 8455eda I propose to use the YAML value to intialize mode_ofoperation. Now both values are used with the URDF value taking priority in order to have backward compatible. That will go into a PR.

I tried , it did not work ,here is the yaml:

vendor_id:  0x00100000
product_id: 0x000c010d
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, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan}  # Target torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 8 ,command_interface: mode_of_operation}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}  # Mode of operation display

chage to mode_of_operation_display , not work too

vendor_id:  0x00100000
product_id: 0x000c010d
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, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan}  # Target torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 8 ,command_interface: mode_of_operation}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation_display}  # Mode of operation display

And your ros_control looks something like this:

<joint name="${prefix}vertical_slider">
            <command_interface name="position" />
            <state_interface name="position" />
            <state_interface name="velocity" />
            <state_interface name="effort" />
            <state_interface name="status_word"/>
            <command_interface name="control_word"/>
            <state_interface name="mode_of_operation"/>
            <command_interface name="mode_of_operation"/>
            <state_interface name="reset_fault"/>
            <command_interface name="reset_fault"/>
            <ec_module name="Inovance_sv660_fry">
                <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
                <param name="alias">1001</param>
                <param name="position">0</param>
                <param name="mode_of_operation">8</param>
                <param name="slave_config">$(find config)/ethercat/slave_config_vertical.yaml</param>
            </ec_module>
        </joint>

[467480903]

I definitely confirm that the problem comes from the mode of operation_display or 0x6061 being not read and triggering the whole chain of consequences. Initialization is correct and there is a unit test that check it and passes

TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) in file test_generic_ec_cia402_drive.cpp

Can you add 0x6061 : mode_of_operation_display and 0x6060 : mode_of_operation in your config file and make a test ? Without an answer to that question, we cannot go further. I make better documentation of that test for a next PR, you can have a look at that link: https://github.com/yguel/ethercat_driver_ros2/blob/8455edabd605218b8bf22f7c18d5c7ba3dd4095e/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp#L274 You can run the test and verify that indeed, correct setting of the target_position is made if the command is NaN. So give it a try with adding 0x6061 and 0x6060 ! I have seen that we had another problem (not related to yours though). If a default value for mode_of_operation was set in the YAML file, it was not taken into account, only if put in the URDF file as a plugin parameter, was that value used. If you look at the branch (yguel-humble-cia402-checks) in my fork: https://github.com/yguel/ethercat_driver_ros2/tree/yguel-humble-cia402-checks, in the commit: 8455eda I propose to use the YAML value to intialize mode_ofoperation. Now both values are used with the URDF value taking priority in order to have backward compatible. That will go into a PR.

I tried , it did not work ,here is the yaml:

vendor_id:  0x00100000
product_id: 0x000c010d
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, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan}  # Target torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 8 ,command_interface: mode_of_operation}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}  # Mode of operation display

chage to mode_of_operation_display , not work too

vendor_id:  0x00100000
product_id: 0x000c010d
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, default: 0}  # Control word
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, 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, command_interface: effort, default: .nan}  # Target torque
      - {index: 0x6060, sub_index: 0, type: int8, default: 8 ,command_interface: mode_of_operation}  # Mode of operation
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # 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: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation_display}  # Mode of operation display

And your ros_control looks something like this:

<joint name="${prefix}vertical_slider">
            <command_interface name="position" />
            <state_interface name="position" />
            <state_interface name="velocity" />
            <state_interface name="effort" />
            <state_interface name="status_word"/>
            <command_interface name="control_word"/>
            <state_interface name="mode_of_operation"/>
            <command_interface name="mode_of_operation"/>
            <state_interface name="reset_fault"/>
            <command_interface name="reset_fault"/>
            <ec_module name="Inovance_sv660_fry">
                <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
                <param name="alias">1001</param>
                <param name="position">0</param>
                <param name="mode_of_operation">8</param>
                <param name="slave_config">$(find config)/ethercat/slave_config_vertical.yaml</param>
            </ec_module>
        </joint>

I did not add the mode_of_operation state nor command interface , should I add these?

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

ethercat_driver/EthercatDriver 0 125 ethercat_generic_plugins/EcCiA402Drive 0 0 8 $(find ethercat_motor_drive)/config/huichuan_csp.yaml ```

[467480903]

thank you for your help , @JensVanhooydonck I tried , but it did not work:

<?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">125</param>
      </hardware>

      <joint name="joint_1">
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
        <command_interface name="position"/>
        <command_interface name="reset_fault"/>
            <state_interface name="status_word"/>
            <command_interface name="control_word"/>
            <state_interface name="mode_of_operation"/>
            <command_interface name="mode_of_operation"/>
            <state_interface name="reset_fault"/>
            <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">$(find ethercat_motor_drive)/config/huichuan_csp.yaml</param>
        </ec_module>
      </joint>

    </ros2_control>

  </xacro:macro>

</robot>

motor will go to position 0 during startup

[JensVanhooydonck]

I did have something simular. It got better after i altered this: https://github.com/JensVanhooydonck/ethercat_driver_ros2/commit/e4522114fb149097a50cc951bf714b9e27c60c8a But i should try to go back to the main branch and open PR's for fixes i have made. (My branch is altered to allow multiple joints in 1 config file). I try to set the last_value to NaN when resetting an error. (Which happens here on startup).

This still not fixes it 100% of the time, that's why i use a delay on startup (I have 32 connected & configured ethercat-slaves, so startup takes some time)

[467480903]

I did have something simular. It got better after i altered this: JensVanhooydonck@e452211 But i should try to go back to the main branch and open PR's for fixes i have made. (My branch is altered to allow multiple joints in 1 config file). I try to set the last_value to NaN when resetting an error. (Which happens here on startup).

This still not fixes it 100% of the time, that's why i use a delay on startup (I have 32 connected & configured ethercat-slaves, so startup takes some time)

32 motors, that's a lot , are you making a humnoid robot？ my robot has 12 for legs, 14 for amrs, 3 for waist, 3 for head , 34 in total ,

[yguel]

I reopen this issue, since there is obviously still a problem. Effectively 32 or 34 motors begin to make setups and handle of mode_of_operation and mode_of_operation_display via SDO a good and sound use case. If there is an error nothing particular is done for reset, and I am pretty sure it does not agree with the CIA402 lifecycle. Put the last_value to NaN is one thing that should be done to restore init state, but not only, all the interfaces (state and command) should also be set to NaN. I will conduct an audit of the CIA402 state_machine to see if something else is amiss and where we should restore an initial state.