Add parameter to TrajectoryFollower stop rotation when bearing is reached

[iche033]

Signed-off-by: Ian Chen [email protected]

🎉 New feature

Summary

Added parameter <zero_vel_on_bearing_reached>. When set to true, it forces angular velocity to be zero when the target bearing is reached. This is intended to remove any residue angular velocity of the link and prevent overshooting.

Test it

Test with trajectory_follower.sdf world. box_02 now has this parameter set and uses high torque for faster rotation and should not overshoot

ign gazebo -v 4 ./src/ign-gazebo/examples/worlds/trajectory_follower.sdf

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[codecov]

Codecov Report

Merging #1349 (5a77fcd) into ign-gazebo6 (8b8b9f6) will increase coverage by 0.01%.
The diff coverage is 100.00%.

@@               Coverage Diff               @@
##           ign-gazebo6    #1349      +/-   ##
===============================================
+ Coverage        62.92%   62.93%   +0.01%     
===============================================
  Files              299      299              
  Lines            24159    24167       +8     
===============================================
+ Hits             15202    15210       +8     
  Misses            8957     8957              

Impacted Files	Coverage Δ
include/ignition/gazebo/Link.hh	100.00% <ø> (ø)
src/systems/physics/Physics.cc	71.55% <ø> (ø)
src/Link.cc	94.77% <100.00%> (+0.28%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 8b8b9f6...5a77fcd. Read the comment docs.

[caguero]

Do we need the second expression math::equal(std::abs(bearing.Degree()), 0.0, 1.0)?

We're supposed to be within the acceptable range bearing. If we need higher bearing accuracy, I think we should change the parameter bearingTolerance.

[iche033]

The behavior I found without this check is that as soon as the vehicle moves forward 1 iteration, its bearing immediately becomes outside of bearingTolerance. So it constantly switched back and forth between rotation and moving forward. The problem was that removing the AngularVelocityCmd component is found to be very expensive and this locks up simulation. With this check, I still do notice a slight pause in simulation when the component is being removed but it only happens occasionally when bearing needs to be corrected.

[iche033]

I tweaked the 2nd tol check to be a percentage of bearingTolerance. aa8cf68

[caguero]

Looks good to me.

[osrf-triage]

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-03-01-citadel-edifice-fortress/1313/1