normalize yaw interpolate planar joint for diff drive

[jsupratman13]

Description

• assume and fix PlanarJointModel DIFF_DRIVE interpolate does not normalize yaw #3665 bug
• This PR normalizes the yaw component in state inside interpolate() when the the motion_model is DIFF_DRIVE

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 46.24%. Comparing base (5a8ef4d) to head (075979e).

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3666      +/-   ##
==========================================
+ Coverage   46.23%   46.24%   +0.01%     
==========================================
  Files         726      726              
  Lines       59484    59492       +8     
  Branches     7625     7625              
==========================================
+ Hits        27495    27504       +9     
+ Misses      31822    31820       -2     
- Partials      167      168       +1     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.