An implementation of Inverse Kinematics for Vizard

[pwsnow]

EDIT! - I just realised that I should mention that Kallmann co-ords are X axis going out, Y axis going up and Z axis going away (from the avatar). So for Vizard the X axis and Z axis are swapped. Also that Kallmann uses local frames in the arm for the joint values, which is a reason why my code gives weird output. The zero position of Kallmann's algorithm might also be causing my code problems too. Food for thought.