[JTC] Explicitly set hold position

[christophfroehlich]

Using velocity or effort command interface: After canceling a goal (tolerances or manually by cancelling the action) the movement is not stopped, see #514. Thanks to @c-rizz for proposing a solution fixing #514

Edit:

The behavior after merging this PR is the following:

• set_hold_position is called at
  • Action interface:
    • state tolerance violation
    • goal_time_tolerance violation
    • cancel of action
    • reaching the goal successfully
  • Topic interface:
    • state tolerance violation
  • Activation: (optionally) The position which is read the first time from the hardware is actively held
• set_hold_position means
  • position command interface: set the current position as command
  • velocity (open loop), acceleration command interface: set command to zero
  • command interface velocity + effort in closed loop: position control on current position
• Deactivation:
  • position command interface: set the current position as command
  • velocity + effort command interface: set zero
• Tests ensuring this behavior

This should fix

• JTC does not stop velocity command after goal succeed #469
• JointTrajectoryController fails to hold position on effort control #514
• [JTC] set_hold_position() doesn't actually stop the robot #683

[codecov-commenter]

Codecov Report

Merging #558 (0e82010) into master (e7f9962) will decrease coverage by 4.90%.
Report is 352 commits behind head on master.
The diff coverage is 25.91%.

❗ Your organization is not using the GitHub App Integration. As a result you may experience degraded service beginning May 15th. Please install the Github App Integration for your organization. Read more.

@@            Coverage Diff             @@
##           master     #558      +/-   ##
==========================================
- Coverage   35.78%   30.88%   -4.90%     
==========================================
  Files         189        7     -182     
  Lines       17570      832   -16738     
  Branches    11592      505   -11087     
==========================================
- Hits         6287      257    -6030     
+ Misses        994      133     -861     
+ Partials    10289      442    -9847     

Flag	Coverage Δ
unittests	30.88% <25.91%> (-4.90%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files Changed	Coverage Δ
...ontroller/test/test_load_diff_drive_controller.cpp	11.11% <0.00%> (ø)
...ive_controller/test/test_diff_drive_controller.cpp	12.66% <7.81%> (ø)
diff_drive_controller/src/odometry.cpp	42.16% <11.11%> (ø)
diff_drive_controller/src/speed_limiter.cpp	46.55% <11.11%> (ø)
...iff_drive_controller/src/diff_drive_controller.cpp	47.13% <46.94%> (ø)
...de/diff_drive_controller/diff_drive_controller.hpp	100.00% <100.00%> (ø)
...troller/include/diff_drive_controller/odometry.hpp	100.00% <100.00%> (ø)

... and 189 files with indirect coverage changes

[BarisYazici]

I just had the time to test on our FR3 robot. And it still didn't solve the hold position issue. I didn't dive deep in debugging, but basically the robot successfully goes to the goal position and either free falls down or drags slowly sideways. We are only using effort commands. I will test today again.

[christophfroehlich]

To ensure that I understand you setup: Are you using effort command with active feedback gains of the PID controller?

[christophfroehlich]

I think I've found a possible issue, do you have set a goal_time_tolerance>0? If the time tolerance is exceeded, the PID controller will be deactivated even in hold position mode.

[BarisYazici]

I didn't explicitly set a goal_time tolerance. So I guess this was not checked. I used the panda_moveit_config launch file in our franka_ros2 repo to test the new JTC. Maybe it's been implicitly set there but I doubt it.

[christophfroehlich]

I checked that now, false alarm. It could happen only if the PID can't hold the position (wrong tuning, effort limit,..), then it might enter a loop setting a new hold position again and again.

[BarisYazici]

I think the problem is set_hold_position() is never get called in our case. When I add this else condition from motified_jtc, it works.

[BarisYazici]

in our case goal_time is default 0 . And the set_hold_position is never get called when the robot reaches the target.

[christophfroehlich]

Do you use the topic or action interface? In case of the latter, do you get a success action result?

[christophfroehlich]

@BarisYazici do you have a public repo with which I could try your setup?

[BarisYazici]

yes, here is the launch file I used to try JTC - https://github.com/frankaemika/franka_ros2/blob/humble/franka_moveit_config/launch/moveit.launch.py . All settings are default. I removed the old JTC in our repo and added the one in humble branch from ros2_controllers. Devcontainer in the repo should reflect my setup.

[christophfroehlich]

yes, here is the launch file I used to try JTC - https://github.com/frankaemika/franka_ros2/blob/humble/franka_moveit_config/launch/moveit.launch.py . All settings are default. I removed the old JTC in our repo and added the one in humble branch from ros2_controllers. Devcontainer in the repo should reflect my setup.

This PR hasn't been backported to humble yet, which means you were still testing the old version of JTC. Could you give it a try with this manual cherrypick please?

[BarisYazici]

This PR hasn't been backported to humble yet, which means you were still testing the old version of JTC. Could you give it a try with this manual cherrypick please?

You are right, my mistake. I tested it, and it works as expected. We will remove the old JTC from our repo in the next update.

[christophfroehlich]

thanks for the feedback