Remove unnecessary checks

huskyroboticsteam · Oct 13, 2023 · fbca180 · fbca180
1 parent 7edb33b
commit fbca180
Showing 1 changed file with 7 additions and 11 deletions.
diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp
@@ -22,18 +22,14 @@ void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double d
 				  double angle = 1e-5) {
 	Eigen::Vector2d diff = p1 - p2;
 	bool distEqual = diff.norm() <= dist;
-	double thetaDiff = std::fmod(abs(diff(1)), 2 * PI);
-	// change domain from [0, 2pi) to (-pi, pi]
-	if (thetaDiff > PI) {
-		thetaDiff -= 2 * PI;
-	}
-	bool angleEqual = abs(thetaDiff) <= angle;
-	if (distEqual && angleEqual) {
-		SUCCEED();
-	} else {
-		std::cout << "Expected: " << toString(p1) << ", Actual: " << toString(p2) << std::endl;
-		FAIL();
+
+	if (!distEqual) {
+		std::stringstream ss;
+		ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2);
+		INFO(ss.str());
 	}
+
+	REQUIRE(distEqual);
 }
 
 TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") {