Add planar arm safety factor

src/Constants.h

@@ -56,17 +56,6 @@ constexpr double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_
 const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE)
 							  .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2);
 
-// Joint limits
-// TODO: make sure these are still accurate with the new arm.
-constexpr double ARM_BASE_MIN = -M_PI / 2;
-constexpr double ARM_BASE_MAX = M_PI / 2;
-// constexpr double SHOULDER_MIN = M_PI / 2; // TODO mechanical problem with the moon gear.
-//                                             Use this value during actual rover operation.
-constexpr double SHOULDER_MIN = 0.0;
-constexpr double SHOULDER_MAX = M_PI * 5. / 6.; // Can't quite lie flat
-constexpr double ELBOW_MIN = 0.0;
-constexpr double ELBOW_MAX = M_PI * 29. / 30.; // I think this should prevent self-collisions
-
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
 // TODO: rename cameras (in MC as well) as appropriate
 constexpr const char* AR_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml";
@@ -146,6 +135,10 @@ constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t
 
 // Arm inverse kinematics
 namespace arm {
+/**
+ * Percentage of fully extended overall arm length to limit arm extension within.
+ */
+constexpr double SAFETY_FACTOR = 0.95;
 
 /**
  * Maximum commanded end-effector velocity, in m/s

src/Globals.cpp

@@ -41,6 +41,7 @@ robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperip
 const kinematics::PlanarArmKinematics<Constants::arm::IK_MOTORS.size()>
 	planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false),
 						IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER);
-control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics);
+control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics,
+													Constants::arm::SAFETY_FACTOR);
 std::atomic<bool> armIKEnabled = false;
 } // namespace Globals

src/control/PlanarArmController.h

@@ -29,10 +29,15 @@ template <unsigned int N> class PlanarArmController {
 	 * @param currJointPos The current joint positions of the arm.
 	 * @param kin_obj PlanarArmKinematics object for the arm (should have the same number of
 	 * arm joints).
+	 * @param safetyFactor the percentage factor to scale maximum arm extension radius by to
+	 * prevent singularity lock.
 	 */
 	PlanarArmController(const navtypes::Vectord<N>& currJointPos,
-						kinematics::PlanarArmKinematics<N> kin_obj)
-		: kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}) {}
+						kinematics::PlanarArmKinematics<N> kin_obj, const double safetyFactor)
+		: kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}),
+		  safetyFactor(safetyFactor) {
+		assert(safetyFactor > 0.0 && safetyFactor < 1.0);
+	}
 
 	/**
 	 * @brief Sets the end effector setpoint / target position.
@@ -42,7 +47,7 @@ template <unsigned int N> class PlanarArmController {
 	void set_setpoint(const navtypes::Vectord<N>& targetJointPos) {
 		Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos);
 		std::lock_guard<std::mutex> lock(mutex);
-		setpoint = newSetPoint;
+		setpoint = normalizeEEWithinRadius(newSetPoint);
 	}
 
 	/**
@@ -64,14 +69,7 @@ template <unsigned int N> class PlanarArmController {
 		}
 
 		// bounds check (new pos + vel vector <= sum of joint lengths)
-		double radius = kin.getSegLens().sum();
-		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();
-		}
-		return pos;
+		return normalizeEEWithinRadius(pos);
 	}
 
 	/**
@@ -135,5 +133,26 @@ template <unsigned int N> class PlanarArmController {
 	Eigen::Vector2d setpoint;
 	Eigen::Vector2d velocity;
 	std::optional<robot::types::datatime_t> velTimestamp;
+	const double safetyFactor;
+
+	/**
+	 * @brief Normalize the input vector (end-effector position) to have a set radius,
+	 *  	  while maintaining the same direction it did before if it exceeds that set radius.
+	 *
+	 * @param eePos The end-effector position to normalize.
+	 */
+	Eigen::Vector2d normalizeEEWithinRadius(Eigen::Vector2d eePos) {
+		double radius = kin.getSegLens().sum() * safetyFactor;
+
+		if (eePos.norm() > radius) {
+			// TODO: will need to eventually shrink velocity vector until it is within radius
+			// instead of just normalizing it.
+
+			eePos.normalize();
+			eePos *= radius;
+		}
+
+		return eePos;
+	}
 };
 } // namespace control

tests/control/PlanarArmControllerTest.cpp

@@ -1,13 +1,60 @@
 #include "../../src/control/PlanarArmController.h"
 
+#include "../../src/Constants.h"
+
+#include <iostream>
+
 #include <catch2/catch.hpp>
 
 using namespace Catch::literals;
 using namespace control;
 using namespace std::chrono_literals;
 
+constexpr double PI = M_PI;
+
+std::string toString(const Eigen::Vector2d& pose) {
+	std::stringstream ss;
+	ss << "(" << pose(0) << ", " << pose(1) << ")";
+	return ss.str();
+}
+
+void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-5,
+				  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();
+	}
+}
+
 TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") {
 	navtypes::Vectord<2> vec({0, 0});
 	kinematics::PlanarArmKinematics<2> kin_obj(vec, vec, vec, 0.0, 0);
-	PlanarArmController<2> foo({0, 0}, kin_obj);
+	PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR);
+}
+
+TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") {
+	navtypes::Vectord<2> segLens({4.0, 6.0});
+	navtypes::Vectord<2> minAngles({-M_PI, -M_PI});
+	navtypes::Vectord<2> maxAngles({M_PI, M_PI});
+	kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0);
+
+	PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR);
+	foo.set_setpoint({10.0, 0.0});
+	REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5);
+
+	foo.set_setpoint({2.0, 3.0});
+	Eigen::Vector2d origSetPoint = kin_obj.jointPosToEEPos({2.0, 3.0});
+	double expectedNorm = origSetPoint.norm();
+
+	assertApprox(origSetPoint, foo.get_setpoint(robot::types::dataclock::now()));
 }