1. Add PostureError to penalize deviation to the nominal posture
2. InverseKin works in most cases. It now supports cost other than PostureError. It is 10% slower than the previous inverseKin, and I got one test case failure because the return posture is not the same as the previous inverseKin solution.