Hotfix for PlanToOffset

[egordon]

The New PlanToOffset function (based on the original planToEndEffectorOffset) requires the body node to be part of the controlled Metaskeleton. This isn't actually a required check, and it makes it difficult to, say, select the end effector body node when the controlled metaskeleton only covers the controlled arm.

Additionally, the ConfigurationToEndEffectorOffset planner needs the true metaskeleton (not a clone) to actually function properly. This was tested on the Kinova Gen3 (ADA) robot feeding demo.

---

Before creating a pull request

• [ N/A ] Document new methods and classes
• [ N/A ] Format code with make format

Before merging a pull request

• [x] Set version target by selecting a milestone on the right side
• [ N/A ] Summarize this change in CHANGELOG.md
• [ N/A ] Add unit test(s) for this change

[codecov[bot]]

Codecov Report

Merging #615 (91b1100) into master (a8fcb58) will not change coverage. The diff coverage is n/a.

@@           Coverage Diff           @@
##           master     #615   +/-   ##
=======================================
  Coverage   76.38%   76.38%           
=======================================
  Files         200      200           
  Lines        7030     7030           
=======================================
  Hits         5370     5370           
  Misses       1660     1660