hubo/hubo-motion-rt — pull request #5 by pvieira3 (2013-02-28 21:48:13+00:00)
Title: Fix to Leg FK and IK

I fixed the Leg FK and IK functions in Hubo_Tech.cpp by making the following changes:

1. It was returning -nan for joints 3, 4 and 5 (RHP, RKN and RAP) when the goal was outside the workspace (when performing teleoperation). I changed the square roots in the calculations of the joint angles to be csqrt (complex sqrt) and changed the radical to type 'double complex'.

2. The norm between the goal position and the solution positions wasn't being calculated with the correct goal position because it was changed at the top of the function, so I changed the input goal parameter to a const and changed the transformation calculated to be performed in one line.

3. The ordering of the limits for the leg joints was wrong as well, because the hip pitch and hip yaw values were switched, so this was fixed.

p.s. Grey has changed the way the limits work, but I figured this fix should be added first and then Grey's latest changes can be merged in on top.

-Pete