Actually compute unnormalized setpoint instead of magic num

huskyroboticsteam · Oct 3, 2023 · 62a352a · 62a352a
1 parent 9d0cdfe
commit 62a352a
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp
@@ -25,6 +25,9 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") {
 	REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5);
 
 	foo.set_setpoint({2.0, 3.0});
-	REQUIRE(std::abs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) <
+	Eigen::Vector2d origSetPoint = kin_obj.jointPosToEEPos({2.0, 3.0});
+	double expectedNorm = origSetPoint.norm();
+
+	REQUIRE(std::abs(foo.get_setpoint(robot::types::dataclock::now()).norm() - expectedNorm) <
 			1e-6);
 }