Fix orientation calculation in cost function and frame tests

[sea-bass]

In some issues that were filed recently, we discovered that the rotation cost was the biggest computational bottleneck.

Then, after talking to @stephanie-eng we realized the best path is to look at what bio_ik did -- specifically, they use the following KDL function whose implementation is here, which negates the quaternion dot product if it's less than zero because of "short vs. long path" of calculating an angle.

Anyway, this sped up my results significantly when testing locally!

[peterdavidfagan]

These changes align the pick_ik implementation with how angular distance is calculated in bio_ik and also clean up the code a little bit. Looks good from my POV.

With the above being said, I need to gain greater familiarity with both pick_ik and bio_ik, the above was a coarse review based on ensuring that the pick_ik implementation aligns with bio_ik. I would still receommend a review from someone more experienced with these repositories (alternatively I spend more time exploring the codebase of both repositories but I will need to dedicate time for this).

From qualitative tests on the UFactory Lite6 the changes committed within the pr resolve issues related to not finding ik solutions with pick_ik moveit/moveit2#1950 (reply in thread).