Updated legIK to use RobotKin in a smarter way