Different behavior between Foxy and Humble

[ygoumaz]

I am not really sure if my request is a bug or not, but I believe the behavior is intended and I can't find why.

In webots_ros2_universal_robot package, we use the joint_trajectory_controller package to send goal positions to multiple joints of an ABB and a UR5e robot. In Foxy, the trajectory is very smooth and it seems like multiple points are interpolated between two given trajectory points. By listening to the /robot-name/state topic, I can see that the publish rate is high (16 hz).

But on Humble, it seems like there is only the given trajectory points that are used, without any interpolation or any similar algorithm. In Webots, the robot has more rough movements and is really less smooth than in Foxy. When listening to the /state topic, the rate is very low (less than 1 hz). It seems that only the given goal positions are used. I don't know if the publishing rate has anything to do with the actual trajectory, but it really gives the impression that on Foxy there are more points taken in account.

All files, including the parameters file (shown below), are exactly the same for both distributions (Foxy and Humble):

/**:
  ros__parameters:
    # controller_manager
    update_rate: 20

    abb_joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    abb_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    # abb_joint_trajectory_controller
    joints:
      - A motor
      - B motor
      - C motor
      - D motor
      - E motor
      - F motor
      - finger_1_joint_1
      - finger_2_joint_1
      - finger_middle_joint_1
    command_interfaces:
      - position
    state_interfaces:
      - position
    allow_partial_joints_goal: True`

Is there anything that has changed in the trajectory following algorithm? Is there any parameter that can be set to get the same behavior on Humble as on Foxy?

For information the positions are sent from this file to the FollowJointTrajectory client with the send_goal_async() function.

Thanks a lot for your time!

To Reproduce Steps to reproduce the behavior:

1. Install webots_ros2
2. Start the UR example with ros2 launch webots_ros2_universal_robot multirobot_launch.py
3. See a different behavior on Humble and Foxy.
4. Optional: Check the /state topic rate of each robot.

Environments

• OS: Ubuntu 20.04
• Version: Foxy &&
  • OS: Ubunut 22.04
  • Version: Humble

[ygoumaz]

Any idea on what change could cause the different behavior between both distributions?

[bmagyar]

The algorithm hasn't changed, some handling and controller-level interfaces may have that result in what you are experiencing. That being said, smooth interpolation is the point of this controller so if it isn't happening that's an issue.

The state topic is published at a set rate, the speed of it is not indicative of issues with the controller.

How fast are commands sent to the JTC? And which interface (action/topic) is used?

[ygoumaz]

The update rate is set to 50, to match the simulation timestep (20ms). We use the FollowJointTrajectory action.

The interpolation seems fine but the difference is really in the speed at which joints are getting to the goal positions. Something may have changed in gains, maximal velocity or similar.

Here are videos to compare both behaviors. It should help understanding what is happening. We can clearly observe that movements are slower and smoother on Foxy than on Humble.

FOXY (version 0.9.0)

https://user-images.githubusercontent.com/61198661/237043032-96ce0f98-1011-4ac4-9742-1a7b251d405f.mp4

HUMBLE (version 2.19.0)

https://user-images.githubusercontent.com/61198661/237043047-2f21daaa-4471-4fc6-8498-ca341b37591e.mp4

[ygoumaz]

Is there a way to limit the speed when using position control (Position State Interface and Position Command Interface)? It seems that the controller is trying to reach the target position as fast as possible. I have tried increasing the time_from_start value but all it does is add several intermediate positions for all the joints. It really feels like the interpolation is working, but the speed at the target points is not continuous (it goes down to zero).

[bmagyar]

I think there was an issue with the JTC not doing interpolation properly (or at all) recently. Have you figured out more @christophfroehlich ?

[christophfroehlich]

The issue I experienced is described in #574, but I think it is not related. I haven't used JTC with distros older than humble, so I can't say if something changed.

What I see that you use position command interface only, as far as I remember it interpolates with constant velocity between the given points.

Since #520 the state publish rate was removed in rolling, in humble you can set the state publish rate, defaulting to 50Hz: https://github.com/ros-controls/ros2_controllers/blob/602b153f4cd87818204be035d85127188c319f75/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml#L53-L60 If you don't receive it with that rate, something is broken.

If you get that running, could you plot the reference value from the state topic?

[ygoumaz]

I have version 3.5.0 of JTC, and I'm currently investigating the rate of the /state topic. However, I'm encountering a low publish rate (1 Hz), even after adding the parameter to my YAML file (setting it to 50 Hz).

I'm wondering if this low publish rate could be the reason why the joints are attempting to move to the next position as quickly as possible? Could this be a bug in JTC, or could it be related to my ROS node that publishes the trajectory to the ActionClient?

It's worth noting that my node responsible for sending the trajectory to the ActionClient has a subscriber to the /state topic. This subscriber helps detect the first received message because the wait_for_server() method reports that the joint_trajectory_controller node is ready, even though it may require additional time to fully prepare to receive commands. I tried to remove it but it doesn't seem to fix the issue.

These are the average rates I get:

average rate: 0.754
        min: 1.318s max: 1.334s std dev: 0.00805s window: 2
average rate: 0.756
        min: 1.314s max: 1.334s std dev: 0.00859s window: 3
average rate: 0.758
        min: 1.307s max: 1.334s std dev: 0.00982s window: 4
average rate: 0.758
        min: 1.307s max: 1.334s std dev: 0.00882s window: 5
average rate: 0.759
        min: 1.307s max: 1.334s std dev: 0.00837s window: 6
average rate: 0.758
        min: 1.307s max: 1.334s std dev: 0.00925s window: 7
average rate: 0.759
        min: 1.305s max: 1.334s std dev: 0.01002s window: 8
average rate: 0.758
        min: 1.305s max: 1.334s std dev: 0.00957s window: 9
average rate: 0.759
        min: 1.305s max: 1.334s std dev: 0.00918s window: 10

These are two consecutive values received on the topic:

header:
  stamp:
    sec: 19
    nanosec: 184000000
  frame_id: ''
joint_names:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- finger_1_joint_1
- finger_2_joint_1
- finger_middle_joint_1
desired:
  positions:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 2.882412219079178e-06
  - -1.0969772281654855e-06
  - 0.72192
  - 0.72192
  - 0.51192
  velocities:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.8005
  - 0.8005
  - 0.5505
  accelerations:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0
actual:
  positions:
  - 6.120233943435912e-05
  - 7.277729287158881e-05
  - 2.4387298034567835e-05
  - -3.648944554441707e-06
  - 3.646310778793991e-06
  - -1.5298545381625672e-06
  - 0.04949999991944498
  - 0.04949999991933929
  - 0.04950000006001609
  velocities: []
  accelerations: []
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0
error:
  positions:
  - -6.120233943435912e-05
  - -7.277729287158881e-05
  - -2.4387298034567835e-05
  - 3.648944554441707e-06
  - -7.638985597148128e-07
  - 4.3287730999708174e-07
  - 0.672420000080555
  - 0.6724200000806607
  - 0.46241999993998395
  velocities: []
  accelerations: []
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0
multi_dof_joint_names: []
multi_dof_desired:
  transforms: []
  velocities: []
  accelerations: []
  time_from_start:
    sec: 0
    nanosec: 0
multi_dof_actual:
  transforms: []
  velocities: []
  accelerations: []
  time_from_start:
    sec: 0
    nanosec: 0
multi_dof_error:
  transforms: []
  velocities: []
  accelerations: []
  time_from_start:
    sec: 0
    nanosec: 0
---
header:
  stamp:
    sec: 20
    nanosec: 464000000
  frame_id: ''
joint_names:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- finger_1_joint_1
- finger_2_joint_1
- finger_middle_joint_1
desired:
  positions:
  - 0.63
  - -2.26
  - -1.88
  - -2.14
  - 2.882412219079178e-06
  - -1.0969772281654855e-06
  - 0.7539399999999999
  - 0.7539399999999999
  - 0.53394
  velocities:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - -0.8005
  - -0.8005
  - -0.5505
  accelerations:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0
actual:
  positions:
  - 6.112594192863029e-05
  - 7.26636621065952e-05
  - 2.4273701843258718e-05
  - 3.6884437683741206e-06
  - 3.671550280804174e-06
  - -1.55922856581725e-06
  - 0.7219194107290161
  - 0.7219194107294526
  - 0.5119195950589255
  velocities: []
  accelerations: []
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0
error:
  positions:
  - 0.6299388740580714
  - -2.2600726636621062
  - -1.880024273701843
  - -2.1400036884437683
  - -7.891380617249959e-07
  - 4.622513376517645e-07
  - 0.032020589270983835
  - 0.032020589270547295
  - 0.02202040494107449
  velocities: []
  accelerations: []
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0
multi_dof_joint_names: []
multi_dof_desired:
  transforms: []
  velocities: []
  accelerations: []
  time_from_start:
    sec: 0
    nanosec: 0
multi_dof_actual:
  transforms: []
  velocities: []
  accelerations: []
  time_from_start:
    sec: 0
    nanosec: 0
multi_dof_error:
  transforms: []
  velocities: []
  accelerations: []
  time_from_start:
    sec: 0
    nanosec: 0

[christophfroehlich]

the topic should be published even if no action was sent to JTC?!

You could try to adapt the demo JTC action client to eliminate effects coming from your client.

I'll try to make a minimum example based on the RRbot, if it works there we can look for the differences

[ygoumaz]

I haven't gone as far as updating the demo JTC client to match our project but I already came across something weird.

I have modified the RRbot launch file to start the joint_trajectory_position_controller instead of the forward_position_controller.

In the original config file the update_rate is set to 10 and the state_publish_rate to 200. When I check the /state topic, the rate is at 10, which is probably due to the update rate limiting the state publish rate. But if I increase the update_rate to 400 (or any value above 200.0), the /state topic has a rate of 400. Shouldn't it be limited to 200 (or the value of the state_publish_rate parameter)?

I also tried to remove the state_publish_rate parameter, so that it defaults to 50 Hz. But it is always publishing at the rate given by the update_rate parameter.

Is this normal behavior?

[ygoumaz]

In the original config file the update_rate is set to 10 and the state_publish_rate to 200. When I check the /state topic, the rate is at 10, which is probably due to the update rate limiting the state publish rate. But if I increase the update_rate to 400 (or any value above 200.0), the /state topic has a rate of 400. Shouldn't it be limited to 200 (or the value of the state_publish_rate parameter)?

I also tried to remove the state_publish_rate parameter, so that it defaults to 50 Hz. But it is always publishing at the rate given by the update_rate parameter.

Note that this behavior is only present on Iron and Rolling (3.5.0). It seems to work correctly at the rate of state_publish_rate in Humble (version 2.21.0). Given that in our project, the state topic is published at 1Hz on Humble, Iron and Rolling, it may not be related to this.

[christophfroehlich]

Starting from 3.3.0 of joint_trajectory_controller (released to iron and rolling), the state_publish_rate parameter was removed. I removed the remnant today also from the demos with https://github.com/ros-controls/ros2_control_demos/pull/297.

When I now launch ros2 launch ros2_control_demo_example_1 rrbot.launch.py with the joint_trajectory_position_controller, I get the correct rate

ros2 topic hz /joint_trajectory_position_controller/controller_state
average rate: 10.002
        min: 0.099s max: 0.101s std dev: 0.00053s window: 11

and it also changes with

controller_manager:
  ros__parameters:
    update_rate: 25  # Hz

to 25Hz.

With the older releases there is an issue that it publishes this topic only with the rate of the controller_manager, if state_publish_rate is way higher then the update_rate of the controller_manager. If a lower state_publish_rate is given, the topic is published at a fraction of the update_rate only.

[christophfroehlich]

I tried now the example_1 from the demo repository with ros2 launch ros2_control_demo_example_1 test_joint_trajectory_controller.launch.py

Rolling (3.10.0)

Humble (2.21.0)

This is the intention, the interpolation of the position to the next point for 4s before the next point is sent 2s later.

If you set open_loop_control to false, you get this funny curves: it starts interpolating from the current position.

Can you verify this? So what is the difference to your project?

[christophfroehlich]

One suggestion: You could use the MockComponent instead of your simulator, and see if you observe the same behavior.

[ygoumaz]

Thanks a lot for the references and tips, including the use of PlotJuggler. After hours of debugging, I finally found the origin of the issue! I have used the ros2_control_demo package to publish joint goal positions through the /joint_trajectory topic and the issue on the state publish rate was still present. So it was definitely not a problem from our controllers publishing the goal positions on the action server. I then looked carefully at the ros2_control config files, which have the following syntax:

/**:
  ros__parameters:
    # controller_manager
    update_rate: 20

    abb_joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    abb_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    # abb_joint_trajectory_controller
    joints:
      - A motor
      - B motor
      - C motor
      - D motor
      - E motor
      - F motor
      - finger_1_joint_1
      - finger_2_joint_1
      - finger_middle_joint_1
    command_interfaces:
      - position
    state_interfaces:
      - position
    allow_partial_joints_goal: True` 

This is the source of the problem. For a reason, this was working in Foxy but not in later distributions. I am not at the origin of this file structure but I find it really unclear. So I converted it to:

abb/controller_manager:
  ros__parameters:
    update_rate: 50

    abb_joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    abb_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

abb/abb_joint_trajectory_controller:
  ros__parameters:
    joints:
      - A motor
      - B motor
      - C motor
      - D motor
      - E motor
      - F motor
      - finger_1_joint_1
      - finger_2_joint_1
      - finger_middle_joint_1
    command_interfaces:
      - position
    state_interfaces:
      - position

    allow_partial_joints_goal: true

This works perfectly and the interpolation is now completely smooth. Something in the previous syntax was completely messing the rate at which topics and nodes were running. If you have any idea about the difference between both config files structures and if it has been deprecated in Humble, any explanations is welcome.

Thanks again for your help!

[christophfroehlich]

Ah, I see the difference now. With the earlier yaml, you set

    joints:
    command_interfaces:
    state_interfaces:
    allow_partial_joints_goal: True` 

for all nodes in your system. But not sure how this explains your behavior, but it seems to be related with the missing namespaces.

@bmagyar or @destogl do you have an idea what the source of the problem was, and how we could be more robust here?

[destogl]

If parameters are not set properly, everything is possible. That is m understanding. How knows which parameters and why controller were using.