ported matlab IK function
Merge branch 'master' into Queron