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