FloatingBase model Jacobian

[YuhanPeng1219]

Thanks for your amazing work!!

I am now learning the project using your simulator and code. I find that when I use the jacobian matrix computed in floating base model to calculate the joint value of leg rotors, the position of foot is limited to the area below the body of robot. If I plug in a position that is higher than its body, the result is obviously wrong. I am wondering how to fix this problem.