[JTC] Jerky motion with Franka Emika Panda - Effort interface

[tingelst]

Describe the bug
I have a Franka Emika Panda setup working with Humble. With the original joint_effort_trajectory_controller I get smooth motions in MoveIt and when using rqt_joint_trajectory_controller. However, when using the joint_trajectory_controller here on the humble branch I get jerky motions.

To Reproduce

1. Run this PR on your system: Humble port frankaemika/franka_ros2#10
2. Start MoveIt or rqt_joint_trajectory_controller and move the robot
3. Robot moves with jerky motions

Expected behavior
The robot should move smoothly.

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version: Humble

[tingelst]

Seems like I am not the only one having this issue: frankaemika/franka_ros2#10 (comment)

[JafarAbdi]

Hi @tingelst, I had a similar issue a while back, I think the culprit commit is 8e6697b, do you mind testing reverting that commit?

[tingelst]

Thank you, @JafarAbdi! I can confirm that I got the expected behavior, that is, the robot motion was smooth, when I checked out the parent commit: ce0aeb2. I will continue testing tomorrow.

[tingelst]

@JafarAbdi, I have found the source of the jerky/wobbly motion. The bug was introduced in 8e6697b as you expected. The velocity error term is not computed when using an effort command interface, only for velocity command interfaces. This can be fixed by changing this line from

    if (has_velocity_state_interface_ && has_velocity_command_interface_)

to

    if (has_velocity_state_interface_ && (has_velocity_command_interface_ || has_effort_command_interface_))

I'll submit at PR for this.

I also have a problem with non-zero effort commands commanded after execution as mentioned in issue #671 (comment), but I'll take that discussion there.

[JafarAbdi]

@tingelst This should be closed since the bug is fixed in the main branch, right?

[tingelst]

@JafarAbdi, yes indeed. Thank you for the help in tracking this down.