From 828543d42ae2373605403859e137ef95565c897a Mon Sep 17 00:00:00 2001
From: Sarah Fredrickson <sarahf.s021@gmail.com>
Date: Mon, 2 Oct 2023 16:19:53 -0700
Subject: [PATCH] implemented arm constraint formula

---
 src/control/PlanarArmController.h | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h
index fba5fe7a6..63d240695 100644
--- a/src/control/PlanarArmController.h
+++ b/src/control/PlanarArmController.h
@@ -8,6 +8,7 @@
 #include <initializer_list>
 #include <mutex>
 #include <numeric>
+#include <cmath>
 #include <optional>
 
 #include <Eigen/Core>
@@ -65,13 +66,20 @@ template <unsigned int N> class PlanarArmController {
 
 		// bounds check (new pos + vel vector <= sum of joint lengths)
 		double radius = kin.getSegLens().sum();
+		Eigen::Vector2d constrainedPos = pos;
 		if (pos.norm() > radius) {
-			// new position is outside of bounds
-			// TODO: will need to eventually shrink velocity vector until it is within radius
-			// instead of just normalizing it
-			pos.normalize();
+			// new position is outside of bounds. Shrink the velocity vector until it is within
+			// radius setpoint = setpoint inside circle pos = new setpoint outside circle
+			double diffDotProd = (pos - setpoint).dot(setpoint);
+			double differenceNorm = (pos - setpoint).squaredNorm();
+			double a =
+				-1 * diffDotProd +
+				std::sqrt(std::pow(diffDotProd, 2) -
+						  differenceNorm * (setpoint.squaredNorm() - std::pow(radius, 2))) /
+					differenceNorm;
+			constrainedPos = (1 - a) * setpoint + a * pos;
 		}
-		return pos;
+		return constrainedPos;
 	}
 
 	/**