Revert "Removed unused codes and added separate tolerances for position and orientation."

This reverts commit 23d23246c4f0b5a06a5f9d6ab772b26335bbb63c.