fix IK and FK for IK types other than Transform6D
Merge pull request #2 from mintar/fix_ik_5d

fix IK and FK for IK types other than Transform6D