Fix const correctness of StateHandle

[jslee02]

StateHandle can hold either const State pointer or non-const State pointer, which is determined by template parameter _QualifiedState. _QualifiedState can be const [X]State or [X]State. However, the current implementation of StateHandle::getState() can violates const-correctness for both of const and non-const State.

The current StateHandle is expended as:

// when `_QaulifiedState` is const (e.g., const State)
const State* getState() const; // ok
// State* getState(); // missing!

// when `_QaulifiedState` is non-const (e.g., State)
State* getState() const; // violates const-correctness!

which is not const-correct.

This PR fixes getState() to be always const-correct for ether case (const/non-const) using SFINAE (std::enable_if and std::conditional) and includes several consequential changes.

---

Before creating a pull request

• Document new methods and classes
• Format code with make format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md

[codecov]

Codecov Report

Merging #419 into master will increase coverage by 0.02%.
The diff coverage is 100%.

@@            Coverage Diff             @@
##           master     #419      +/-   ##
==========================================
+ Coverage   79.46%   79.49%   +0.02%     
==========================================
  Files         231      231              
  Lines        5952     5955       +3     
==========================================
+ Hits         4730     4734       +4     
+ Misses       1222     1221       -1

Impacted Files	Coverage Δ
include/aikido/statespace/CartesianProduct.hpp	100% <ø> (ø)	⬆️
src/statespace/GeodesicInterpolator.cpp	92.85% <100%> (ø)	⬆️
...lude/aikido/statespace/detail/StateHandle-impl.hpp	100% <100%> (ø)	⬆️
include/aikido/statespace/StateSpace.hpp	100% <100%> (ø)	⬆️
src/trajectory/Spline.cpp	93.51% <100%> (ø)	⬆️
src/planner/ConfigurationToConfiguration.cpp	100% <100%> (ø)	⬆️
...aikido/statespace/detail/CartesianProduct-impl.hpp	78.57% <100%> (ø)	⬆️
include/aikido/statespace/detail/SE2-impl.hpp	100% <100%> (ø)	⬆️
include/aikido/statespace/detail/SE3-impl.hpp	100% <100%> (ø)	⬆️
...lude/aikido/statespace/detail/ScopedState-impl.hpp	100% <100%> (ø)	⬆️
... and 1 more