Fix RobotState::getRigidlyConnectedParentLinkModel() (backport #2985)

[mergify]

Simplify function using getFrameInfo() to yield the link corresponding to the given frame.

The order of frame resolution was wrong here:
If a link frame containing a slash was given, the code was expecting an attached body with a subframe. As these didn't exist, the function falsely returned NULL.

@sjahr: Maybe this is the error you recently observed? Note, that the function was already fixed in MoveIt1.

---

This is an automatic backport of pull request #2985 done by Mergify.

[mergify]

Cherry-pick of 1f23344 has failed:

On branch mergify/bp/humble/pr-2985
Your branch is up to date with 'origin/humble'.

You are currently cherry-picking commit 1f23344de.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   moveit_core/robot_state/src/robot_state.cpp

no changes added to commit (use "git add" and/or "git commit -a")

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally