From fbca1801a669808bd4a5a34bc750dd9f6d4579f2 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 12 Oct 2023 18:35:33 -0700 Subject: [PATCH] Remove unnecessary checks --- tests/control/PlanarArmControllerTest.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index a0c3e1591..419532b44 100644 --- 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]") {