increased radius check

src/control/PlanarArmController.h

@@ -149,18 +149,18 @@ class PlanarArmController {
 	Eigen::Vector2d normalizeEEWithinRadius(Eigen::Vector2d eePos) {
 		double radius = kin.getSegLens().sum() * safetyFactor;
 
-		if (eePos.norm() > radius) {
+		if (eePos.norm() > (radius + 0.5)) {
 			double radius = kin.getSegLens().sum();
-			// new position is outside of bounds. Set new EE setpoint so it will follow the velocity
-			// vector instead of drifting along the radius.
-			// setpoint = setpoint inside circle
-			// eePos = new setpoint outside circle
+			// new position is outside of bounds. Set new EE setpoint so it will follow the
+			// velocity vector instead of drifting along the radius. setpoint = setpoint inside
+			// circle eePos = new setpoint outside circle
 			double diffDotProd = (eePos - setpoint).dot(setpoint);
 			double differenceNorm = (eePos - setpoint).squaredNorm();
 			double discriminant =
 				std::pow(diffDotProd, 2) -
 				differenceNorm * (setpoint.squaredNorm() - std::pow(radius, 2));
-			double a = -diffDotProd + std::sqrt(discriminant) / differenceNorm;
+			double a = (-diffDotProd + std::sqrt(discriminant)) / differenceNorm;
+			// new constrained eePos = (1 - a) * (ee inside circle) + a * (ee outside circle)
 			eePos = (1 - a) * setpoint + a * eePos;
 		}
 		return eePos;

tests/control/PlanarArmControllerTest.cpp

@@ -45,7 +45,7 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") {
 	// Attempt to straighten out end-effector all the way, exceeding max length.
 	// This should cause the EE to be repositioned to fit the length constraint.
 	foo.set_setpoint({0.0, 0.0});
-	assertApprox({9.5, 0.0}, foo.get_setpoint(robot::types::dataclock::now()));
+	assertApprox({10, 0.0}, foo.get_setpoint(robot::types::dataclock::now()));
 
 	// Try setting the joints to be orthogonal but within max length, such that:
 	// - End effector position is (6,4), which implies that: