Add planar arm safety factor (#267)

* Added safetyFactor check

* Move safety factor constant into Constants

* Add safetyFactor range limit assertion

* Update planar arm tests to use safety factor

* Set setpoint in safety factor test

* Test overreaching arm is normalized

* Bound resolved joint pos within safety factor

* Added within factor test

* Try shorter extension

* Improved formatting

* Remove now-obsolete todo

* More formatting fixes

* Use std abs over fabs in test

* Move vector normalization code to own func

* Add more logic to normalizeVectorWithinRadius

* Fix return type

* Add normalize functionality to set_setpoint

* Actually compute unnormalized setpoint instead of magic num

* Make normalizeVector private

* Rename input vector to better reflect end effector

* Make normalize func name more descriptive

* Add actual vector comparison

* Refactored redundant code

* Remove unnecessary checks

* Minor cleanup

* Fixed tests

* More cleanup

* Improve formatting

* More formatting improvements

* Rm unnecessary INFO wrapping

* Remove unused constant

* Improve REQUIRE logging

* Add safety factor logic in PlanarArmController constructor

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,17 @@ 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), velocity({0.0, 0.0}), safetyFactor(safetyFactor) {
+		// NOTE: currJointPos could extend beyond the safetyFactor, so safety factor
+		//       normalization logic is performed.
+		set_setpoint(currJointPos);
+		assert(safetyFactor > 0.0 && safetyFactor < 1.0);
+	}
 
 	/**
 	 * @brief Sets the end effector setpoint / target position.
@@ -42,7 +49,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 +71,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 +135,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,55 @@
 #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;
 
+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) {
+	std::stringstream ss;
+	ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2);
+	INFO(ss.str());
+
+	Eigen::Vector2d diff = p1 - p2;
+	REQUIRE(diff.norm() <= dist);
+}
+
 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]") {
+	// Set lengths and relative orientation bounds of robot joint segments.
+	navtypes::Vectord<2> segLens({6.0, 4.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);
+
+	// Instantiate PlanarArmController.
+	PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR);
+
+	// 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()));
+
+	// Try setting the joints to be orthogonal but within max length, such that:
+	// - End effector position is (6,4), which implies that:
+	// - Max length = sqrt(6^2 + 4^2) = sqrt(36 + 16) < 7.22 < 9.5 (max length)
+	foo.set_setpoint({0.0, M_PI_2});
+	assertApprox({6.0, 4.0}, foo.get_setpoint(robot::types::dataclock::now()));
 }