[Servo] Make applyJointUpdate() a free function

[ibrahiminfinite]

Description

Fixes #2120
Should be merged after #2106

This PR makes the applyJointUpdate() method of ServoCalcs a free function and moves it to utilities.
This is done in order to keep the core servo code cleaner and enable better testing.

Checklist

• Required by CI: Code is auto formatted using clang-format

[codecov]

Codecov Report

Patch coverage: 70.59% and project coverage change: -0.03 ⚠️

Comparison is base (d343ed5) 50.84% compared to head (cbdc532) 50.81%.

❗ Current head cbdc532 differs from pull request most recent head 77a181d. Consider uploading reports for the commit 77a181d to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2121      +/-   ##
==========================================
- Coverage   50.84%   50.81%   -0.03%     
==========================================
  Files         391      391              
  Lines       32165    32167       +2     
==========================================
- Hits        16352    16343       -9     
- Misses      15813    15824      +11     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/utilities.cpp	66.67% <66.67%> (ø)
moveit_ros/moveit_servo/src/servo_calcs.cpp	66.73% <100.00%> (-0.21%)	⬇️

... and 2 files with indirect coverage changes

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.