Slow movement of the joints when starting up the system with ethercat_driver_ros2

[jaronski]

Hi all, maybe someone can help me out with my problem.
So i have 4 revolute joints on the table and when i ros2 launch my bringup.launch.py starts:

• robot_state_publisher
• controller_manager
• joint_broadcaster_controller
• joint_trajectory_controller

Settings for the hardware joints:
Mode of operation: 8
Command Interface: position
State Interfaces: position velocity effort

joint_trajectory_controller:

controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz
    publish_rate: 1000
    use_sim_time: False
joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController
joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint_0
      - joint_1
      - joint_2
      - joint_3

    command_interfaces:
      - position
      # - reset_fault

    state_interfaces:
      - position
      - velocity
      

    write_op_modes:
      - joint_0
      - joint_1
      - joint_2
      - joint_3

    state_publish_rate: 1000.0       # Defaults to 50
    action_monitor_rate: 1000.0      # Defaults to 20

    allow_partial_joints_goal: false              # Defaults to false
    open_loop_control: false
    allow_integration_in_goal_trajectories: true
    hardware_state_has_offset: true
    deduce_states_from_derivatives: true
    
    constraints:
      stopped_velocity_tolerance: 0.01  # Defaults to 0.01
      goal_time: 0.0                    # Defaults to 0.0 (start immediately)

the output looks good and everything starts up as expected but all 4 revolute joints start moving very slowly from their current position without me sending a joint_trajectory or starting the rqt_joint_trajectory_controller

the plotjuggler shows the joint_state of joint_0 but they move all when firing up the Software:

when i then start the rqt_joint_trajectory_controller and enable/disable it the values stay the same and nothing moves anymore.
I would like to understand why that happens as i need the drives to keep their actual position when restarting the driver. Thanks for your support

[mcbed]

Hi @jaronski,
This looks to me as an issue caused by the joint trajectory controller. What happens if you do not activate this controller at start ?

[jaronski]

Hi @mcbed
ok so i did some checks without starting the joint_trajectory_controller. This isn't changing anything in the behavior. As soon as the controller_manager starts up, the drive goes max motor speed into its software limit angle. i work with this joint in the range of 90 - 180 - 270 degrees. So when i start the motor it goes from any position to the 90 degrees and stops.

here is the setup of the drive_control.yaml:

# Configuration file for Circulo 7 drive
vendor_id: 0x000022d2
product_id: 0x00000301
# 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:
  - index: 0x1600
    channels:
      - {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
      - {index: 0x6060, sub_index: 0, type: int8, default: 8}  # Mode of operation / 8 is cyclic syncronous position mode / 9 is cyclic syncronous velocity mode
      - {index: 0x6071, sub_index: 0, type: int16, default: 0}  # Target torque
      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 83444.592790387}  # Target position # was command_interface: position, default: .nan
      - {index: 0x60ff, sub_index: 0, type: int32, default: 0}  # Target velocity command_interface: velocity, default: 100000
      - {index: 0x60b2, sub_index: 0, type: int16, default: 0}  # Torque offset
      - {index: 0x2701, sub_index: 0, type: uint32, default: 0}  # Tuning command
      - {index: 0x607d, sub_index: 1, type: int32, default: 131072} # Software position limit min
      - {index: 0x607d, sub_index: 2, type: int32, default: 393216} # Software position limit max
      - {index: 0x6080, sub_index: 0, type: uint32, default: 1000000}  # MAX Motorspeed 5000000
  - index: 0x1603
    channels:
      - {index: 0x6083, sub_index: 0, type: uint32, default: 2000} # Profile Acceleration 2000000
      - {index: 0x6084, sub_index: 0, type: uint32, default: 2000} # Profile Deceleration 1000000
      - {index: 0x6072, sub_index: 0, type: uint16, default: 3200}  # MAX Torque 3200
      - {index: 0x6086, sub_index: 0, type: uint16, default: 0 } # Profile Motion
      
tpdo:  # TxPDO = transmit PDO Mapping
  - index: 0x1a00
    channels:
      - {index: 0x6041, sub_index: 0, type: uint16}  # Status word
      - {index: 0x6061, sub_index: 0, type: int8}  # Modes of operation display
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 0.000011984}  # 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: 0x6064, sub_index: 0, type: int32, state_interface: position_raw}  # Position actual value
sm:  # Sync Manager
  - {index: 0, type: output, pdo: ~, watchdog: disable}
  - {index: 1, type: input, pdo: ~, watchdog: disable}
  - {index: 2, type: output, pdo: rpdo, watchdog: enable}
  - {index: 3, type: input, pdo: tpdo, watchdog: disable}

here is the config_controllers.yaml

controller_manager:
 ros__parameters:
   update_rate: 1000  # Hz
   publish_rate: 1000
   use_sim_time: False

   jt_controller:
     type: joint_trajectory_controller/JointTrajectoryController

   joint_state_broadcaster:
     type: joint_state_broadcaster/JointStateBroadcaster

jt_controller:
 ros__parameters:
   joints:
     - joint_0

   command_interfaces:
     - position

   state_interfaces:
     - position
     - velocity

   write_op_modes:
     - joint_0

   state_publish_rate: 1000.0       # Defaults to 50
   action_monitor_rate: 1000.0      # Defaults to 20

   allow_partial_joints_goal: false              # Defaults to false
   open_loop_control: false
   allow_integration_in_goal_trajectories: false
   hardware_state_has_offset: false
   deduce_states_from_derivatives: false
   
   constraints:
     stopped_velocity_tolerance: 0.01  # Defaults to 0.01
     goal_time: 0.0                    # Defaults to 0.0 (start immediately)

the ros2_control xacro:

<ros2_control name="real_joint" type="system">
           <hardware>
               <plugin>ethercat_driver/EthercatDriver</plugin>
               <param name="master_id">0</param>
               <param name="control_frequency">1000</param>
           </hardware>
       
           <joint name="joint_0">
               <command_interface name="position"/>
               <!-- <command_interface name="reset_fault"/> -->
               <state_interface name="position"/>
               <state_interface name="position_raw"/>
               <state_interface name="velocity"/>
               <state_interface name="effort"/>

               <ec_module name="Synapticon_CIRCULO7_1">
                   <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="velocity">0</param>
                   <param name="slave_config">$(find joint_test_description)/config/ec_config_files/debugALL_joints.yaml</param>
               </ec_module>
           </joint>
       </ros2_control>
   </xacro:macro>

sudo dmesg -T:

and the console output after launch the script without the joint_trajectory_controller:

ros2 launch joint_test_description real_joint_launch.launch.py 
[INFO] [launch]: All log files can be found below .ros/log/2023-07-26-15-14-45-730099-jaron-Precision-7560-448303
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [448304]
[INFO] [ros2_control_node-2]: process started with pid [448306]
[INFO] [spawner-3]: process started with pid [448308]
[ros2_control_node-2] [INFO] [1690377286.014159537] [resource_manager]: Loading hardware 'real_joint' 
[ros2_control_node-2] [INFO] [1690377286.018081306] [resource_manager]: Initialize hardware 'real_joint' 
[ros2_control_node-2] [INFO] [1690377286.018142228] [EthercatDriver]: joints
[ros2_control_node-2] [INFO] [1690377286.021290700] [EthercatDriver]: Got 1 modules
[ros2_control_node-2] [INFO] [1690377286.021323999] [resource_manager]: Successful initialization of hardware 'real_joint'
[ros2_control_node-2] [INFO] [1690377286.021375995] [resource_manager]: 'configure' hardware 'real_joint' 
[ros2_control_node-2] [INFO] [1690377286.021387186] [resource_manager]: Successful 'configure' of hardware 'real_joint'
[ros2_control_node-2] [INFO] [1690377286.021395507] [resource_manager]: 'activate' hardware 'real_joint' 
[ros2_control_node-2] [INFO] [1690377286.021402924] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6040, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6060, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6071, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x607a, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x60ff, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x60b2, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x2701, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x607d, 0x1}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x607d, 0x2}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6080, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6083, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6084, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6072, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6086, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6041, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6061, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6064, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x606c, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6077, 0x0}
[ros2_control_node-2] {0, 0, 0x22d2, 0x301, 0x6064, 0x0}
[ros2_control_node-2] [INFO] [1690377286.021772201] [EthercatDriver]: Activated EcMaster!
[robot_state_publisher-1] [INFO] [1690377286.051074114] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1690377286.051226522] [robot_state_publisher]: got segment link_1
[robot_state_publisher-1] [INFO] [1690377286.051240666] [robot_state_publisher]: got segment world
[ros2_control_node-2] 1 slave(s).
[ros2_control_node-2] Master AL states: 0x02.
[ros2_control_node-2] Link is up.
[ros2_control_node-2] Slave: State 0x02.
[ros2_control_node-2] Slave: online.
[ros2_control_node-2] STATE: Not Ready to Switch On with status word :0
[ros2_control_node-2] [INFO] [1690377287.022190431] [EthercatDriver]: updated!
[ros2_control_node-2] [INFO] [1690377287.022227843] [EthercatDriver]: System Successfully started!
[ros2_control_node-2] [INFO] [1690377287.022249768] [resource_manager]: Successful 'activate' of hardware 'real_joint'
[ros2_control_node-2] [INFO] [1690377287.031235363] [controller_manager]: update rate is 1000 Hz
[ros2_control_node-2] [INFO] [1690377287.136634270] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-3] [INFO] [1690377287.144220364] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-2] [INFO] [1690377287.144963558] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-2] [INFO] [1690377287.145044040] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1690377287.152647496] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ros2_control_node-2] Slave: State 0x01.
[ros2_control_node-2] Slave: State 0x02.
[ros2_control_node-2] Domain: WC 1.
[ros2_control_node-2] Domain: State 1.
[ros2_control_node-2] STATE: Switch on Disabled with status word :80
[ros2_control_node-2] Domain: WC 3.
[ros2_control_node-2] Domain: State 2.
[ros2_control_node-2] Slave: State 0x08.
[ros2_control_node-2] Slave: operational.
[ros2_control_node-2] STATE: Ready to Switch On with status word :49
[ros2_control_node-2] STATE: Switch On with status word :51
[INFO] [spawner-3]: process has finished cleanly [pid 448308]
[ros2_control_node-2] Master AL states: 0x08.
[ros2_control_node-2] STATE: Operation Enabled with status word :2103

when i start the joint_trajectory_controller and also rqt_joint_trajectory_controller i can move the drive via slider to 3.1415 (180) and then restart the driver and it moves back into the software limit 90 degrees. @mcbed maybe you have an idea why the joint starts to move without me sending a command to it??

[jaronski]

OK i think i found a solution to the Problem.
Step1:
To test my revolute joint and the sudden uncontrolled movement i decided to removed the factors - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 83444.592790387} and - {index: 0x6064, sub_index: 0, type: int32, state_interface: position , factor: 0.000011984} from the configuration file with the rpdo & tpdo.
i noticed that the joint still was rotating very slowly over time when launching the software resulting into this graph:
on the y-axis encoder increments and on the x-axis time.
then i started to read from a terminal the 0x607a sdo for Target Position and i noticed that it changes over time and doesn't hold its value. So i started to look into the generic_ec_cia402_drive.cpp and found the comment //setup current position as default position. After looking into the code i noticed that as long as i don't write to 0x607a a Target Position the value is getting updated all the time by
pdo_channels_info_[index].default_value = (pdo_channels_info_[index].factor * last_position_ + pdo_channels_info_[index].offset;

I know that there is a jitter on the encoder increments i have on the motor controller, so i guessed the "last_position_" changes in very small increments over time by this jitter and as the default position gets updated as long as i do not actively write a Target Position lets make a quick & dirty try and get the default position only once. So i declared an init_counter in the generic_ec_cia402_drive.hpp updated the part : "setup current position as default position" so the it looks now like this

// setup current position as default position
  if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) {
    if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE && this->init_counter == 0) {
      pdo_channels_info_[index].default_value =
        (pdo_channels_info_[index].factor * last_position_ +
        pdo_channels_info_[index].offset;
        this->init_counter= this->init_counter+1;
    }

so that solved my problem with the slowly moving revolute joint.

Step2:
as i have 524288 increments per revolution and i want to use moveit2 the need for conversion in radiant is necessary. So i calculated the values and put them back in the configuration file.

For rpdo command interface: position factor: 83444.592790387 
For tpdo state interface: position factor: 0.000011984

but i noticed when i put the factors in the config file my 0x607a Target Position gets a very large negativ value. so i thought hmm this might come from an overflow of last_position * factor. so i looked again into the
// setup current position as default position and divided the factor by itself

  if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) {
    if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE && this->init_counter == 0) {
      pdo_channels_info_[index].default_value =
        (pdo_channels_info_[index].factor/pdo_channels_info_[index].factor) * last_position_ +
        pdo_channels_info_[index].offset;
        this->init_counter= this->init_counter+1;
    }

With these operations the problem was solved for me as far as i can tell.

@mcbed just wanted to share that here so you can check if that also happens to your joints and you could consider bringing these changes into the ethercat_driver_ros2 or think of an alternative way.

Best regards
Jaronski

[mcbed]

Hi @jaronski,
Thank you for this very useful feedback ! I will have a look into it.

[jaronski]

@mcbed my pleasure and thank you for this fantastic driver 😉. Please keep me updated on the topic

[ZorroStardust]

I have a question about your method.
We know that in CSP mode, we need to setup current position as default position before setting the real target position.
If you changed code to setup current position for only one time, unkonown occasion may occur in the future?

[wjx1991]

OK i think i found a solution to the Problem. Step1: To test my revolute joint and the sudden uncontrolled movement i decided to removed the factors - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 83444.592790387} and - {index: 0x6064, sub_index: 0, type: int32, state_interface: position , factor: 0.000011984} from the configuration file with the rpdo & tpdo. i noticed that the joint still was rotating very slowly over time when launching the software resulting into this graph: on the y-axis encoder increments and on the x-axis time. then i started to read from a terminal the 0x607a sdo for Target Position and i noticed that it changes over time and doesn't hold its value. So i started to look into the generic_ec_cia402_drive.cpp and found the comment //setup current position as default position. After looking into the code i noticed that as long as i don't write to 0x607a a Target Position the value is getting updated all the time by pdo_channels_info_[index].default_value = (pdo_channels_info_[index].factor * last_position_ + pdo_channels_info_[index].offset;

I know that there is a jitter on the encoder increments i have on the motor controller, so i guessed the "last_position_" changes in very small increments over time by this jitter and as the default position gets updated as long as i do not actively write a Target Position lets make a quick & dirty try and get the default position only once. So i declared an init_counter in the generic_ec_cia402_drive.hpp updated the part : "setup current position as default position" so the it looks now like this

// setup current position as default position
  if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) {
    if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE && this->init_counter == 0) {
      pdo_channels_info_[index].default_value =
        (pdo_channels_info_[index].factor * last_position_ +
        pdo_channels_info_[index].offset;
        this->init_counter= this->init_counter+1;
    }

so that solved my problem with the slowly moving revolute joint.

Step2: as i have 524288 increments per revolution and i want to use moveit2 the need for conversion in radiant is necessary. So i calculated the values and put them back in the configuration file.

For rpdo command interface: position factor: 83444.592790387 
For tpdo state interface: position factor: 0.000011984

but i noticed when i put the factors in the config file my 0x607a Target Position gets a very large negativ value. so i thought hmm this might come from an overflow of last_position * factor. so i looked again into the // setup current position as default position and divided the factor by itself

  if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) {
    if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE && this->init_counter == 0) {
      pdo_channels_info_[index].default_value =
        (pdo_channels_info_[index].factor/pdo_channels_info_[index].factor) * last_position_ +
        pdo_channels_info_[index].offset;
        this->init_counter= this->init_counter+1;
    }

With these operations the problem was solved for me as far as i can tell.

@mcbed just wanted to share that here so you can check if that also happens to your joints and you could consider bringing these changes into the ethercat_driver_ros2 or think of an alternative way.

Best regards Jaronski

Hi, thanks for your solution. i have the same issues.

1. you divided the factor by itself and then (pdo_channels_info_[index].factor/pdo_channels_info_[index].factor)=1, is this right?

[jaronski]

yes, thats correct. the factor is indeed = 1

[jaronski]

I have a question about your method. We know that in CSP mode, we need to setup current position as default position before setting the real target position. If you changed code to setup current position for only one time, unkonown occasion may occur in the future?

yes i am not sure about it and maybe my solution isn't the best.