Closed ygoumaz closed 1 year ago
Any idea on what change could cause the different behavior between both distributions?
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?
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)
HUMBLE (version 2.19.0)
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).
I think there was an issue with the JTC not doing interpolation properly (or at all) recently. Have you figured out more @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?
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
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
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?
In the original config file the
update_rate
is set to 10 and thestate_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 theupdate_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 thestate_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 theupdate_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.
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.
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?
One suggestion: You could use the MockComponent instead of your simulator, and see if you observe the same behavior.
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!
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?
If parameters are not set properly, everything is possible. That is m understanding. How knows which parameters and why controller were using.
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 thejoint_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):
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 thesend_goal_async()
function.Thanks a lot for your time!
To Reproduce Steps to reproduce the behavior:
ros2 launch webots_ros2_universal_robot multirobot_launch.py
Environments