fix ikfast bug, increase safety margin for valid trajectory (possibly not good)