Find the bug why P step is not feasible. But in the P step there should be a bug in computing the torque