Forward kinecmatic (recursive): getFK
Fixed some bugs