Reject enabling IK outside safety radius

src/Globals.cpp

@@ -41,7 +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(planarArmKinematics,
 													Constants::arm::SAFETY_FACTOR);
 std::atomic<bool> armIKEnabled = false;
 } // namespace Globals

src/control/PlanarArmController.h

@@ -30,19 +30,74 @@ class PlanarArmController {
 	/**
 	 * @brief Construct a new controller object.
 	 *
-	 * @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).
+	 *                arm joints).
 	 * @param safetyFactor the percentage factor to scale maximum arm extension radius by to
-	 * prevent singularity lock.
+	 *                      prevent singularity lock. Must be in range (0, 1).
 	 */
-	PlanarArmController(const navtypes::Vectord<N>& currJointPos,
-						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);
+	PlanarArmController(kinematics::PlanarArmKinematics<N> kin_obj, const double safetyFactor)
+		: kin(kin_obj), safetyFactor(safetyFactor) {
 		CHECK_F(safetyFactor > 0.0 && safetyFactor < 1.0);
+
+		for (unsigned int i = 0; i < kin_obj.getNumSegments(); i++) {
+			CHECK_F(kin_obj.getSegLens()[i] > 0.0);
+		}
+	}
+
+	/**
+	 * @brief Constructs a copy of an existing PlanarArmController object.
+	 *
+	 * @param other The existing PlanarArmController to copy.
+	 */
+	PlanarArmController(PlanarArmController&& other)
+		: kin(std::move(other.kin)), safetyFactor(other.safetyFactor) {
+		std::lock_guard<std::mutex> lock(other.mutex);
+		mutableFields = std::move(other.mutableFields);
+	}
+
+	/**
+	 * @brief Instantiates the PlanarArmController with the current joint positions,
+	 * 		  returning true if the joint positions are valid. If PlanarArmController is
+	 * already initialized, the PlanarArmController is reinitialized with the supplied
+	 * positions and this function returns true. Otherwise, controller gets uninitialized and
+	 * function returns false.
+	 *
+	 * @param currJointPos The current joint positions of the arm.
+	 * @return true iff the joint positions are within the robot's maximum arm extension
+	 * 		   radius and the controller was successfully initialized in the function call.
+	 */
+	bool tryInitController(const navtypes::Vectord<N>& currJointPos) {
+		std::lock_guard<std::mutex> lock(mutex);
+
+		if (is_setpoint_valid(currJointPos, kin, safetyFactor)) {
+			if (!mutableFields.has_value()) {
+				mutableFields.emplace();
+			}
+
+			Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(currJointPos);
+			mutableFields->setpoint = normalizeEEWithinRadius(newSetPoint);
+			return true;
+		}
+
+		mutableFields.reset();
+		return false;
+	}
+
+	/**
+	 * @brief Returns whether the target joint positions are within the arm controller's radius
+	 * limit.
+	 *
+	 * @param targetJointPos The target joint positions.
+	 * @return whether the target joint positions are within the arm controller's radius limit.
+	 */
+	static bool is_setpoint_valid(const navtypes::Vectord<N>& targetJointPos,
+								  kinematics::PlanarArmKinematics<N> kin_obj,
+								  double safetyFactor) {
+		// Compute the new EE position to determine if it is within
+		// safety factor * length of fully extended arm.
+		double eeRadius = kin_obj.jointPosToEEPos(targetJointPos).norm();
+		double maxRadius = kin_obj.getSegLens().sum() * safetyFactor;
+		return eeRadius <= maxRadius;
 	}
 
 	/**
@@ -53,7 +108,7 @@ class PlanarArmController {
 	void set_setpoint(const navtypes::Vectord<N>& targetJointPos) {
 		Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos);
 		std::lock_guard<std::mutex> lock(mutex);
-		setpoint = normalizeEEWithinRadius(newSetPoint);
+		mutableFields->setpoint = normalizeEEWithinRadius(newSetPoint);
 	}
 
 	/**
@@ -67,10 +122,11 @@ class PlanarArmController {
 		{
 			std::lock_guard<std::mutex> lock(mutex);
 			// calculate current EE setpoint
-			pos = setpoint;
-			if (velTimestamp.has_value()) {
-				double dt = util::durationToSec(currTime - velTimestamp.value());
-				pos += velocity * dt;
+			pos = mutableFields->setpoint;
+			if (mutableFields->velTimestamp.has_value()) {
+				double dt =
+					util::durationToSec(currTime - mutableFields->velTimestamp.value());
+				pos += mutableFields->velocity * dt;
 			}
 		}
 
@@ -87,14 +143,15 @@ class PlanarArmController {
 	 */
 	void set_x_vel(robot::types::datatime_t currTime, double targetVel) {
 		std::lock_guard<std::mutex> lock(mutex);
-		if (velTimestamp.has_value()) {
-			double dt = util::durationToSec(currTime - velTimestamp.value());
+		if (mutableFields->velTimestamp.has_value()) {
+			double dt = util::durationToSec(currTime - mutableFields->velTimestamp.value());
 			// bounds check (new pos + vel vector <= sum of joint lengths)
-			setpoint = normalizeEEWithinRadius(setpoint + velocity * dt);
+			mutableFields->setpoint = normalizeEEWithinRadius(mutableFields->setpoint +
+															  mutableFields->velocity * dt);
 		}
 
-		velocity(0) = targetVel;
-		velTimestamp = currTime;
+		mutableFields->velocity(0) = targetVel;
+		mutableFields->velTimestamp = currTime;
 	}
 
 	/**
@@ -106,14 +163,15 @@ class PlanarArmController {
 	 */
 	void set_y_vel(robot::types::datatime_t currTime, double targetVel) {
 		std::lock_guard<std::mutex> lock(mutex);
-		if (velTimestamp.has_value()) {
-			double dt = util::durationToSec(currTime - velTimestamp.value());
+		if (mutableFields->velTimestamp.has_value()) {
+			double dt = util::durationToSec(currTime - mutableFields->velTimestamp.value());
 			// bounds check (new pos + vel vector <= sum of joint lengths)
-			setpoint = normalizeEEWithinRadius(setpoint + velocity * dt);
+			mutableFields->setpoint = normalizeEEWithinRadius(mutableFields->setpoint +
+															  mutableFields->velocity * dt);
 		}
 
-		velocity(1) = targetVel;
-		velTimestamp = currTime;
+		mutableFields->velocity(1) = targetVel;
+		mutableFields->velTimestamp = currTime;
 	}
 
 	/**
@@ -133,22 +191,26 @@ class PlanarArmController {
 		// get new joint positions for target EE
 		bool success = false;
 		navtypes::Vectord<N> jp = kin.eePosToJointPos(newPos, currJointPos, success);
-		velTimestamp = currTime;
+		mutableFields->velTimestamp = currTime;
 		if (!success) {
 			LOG_F(WARNING, "IK Failure!");
-			velocity.setZero();
+			mutableFields->velocity.setZero();
 		} else {
-			setpoint = newPos;
+			mutableFields->setpoint = newPos;
 		}
 		return jp;
 	}
 
 private:
+	struct MutableFields {
+		Eigen::Vector2d setpoint;
+		Eigen::Vector2d velocity;
+		std::optional<robot::types::datatime_t> velTimestamp;
+	};
+
+	std::optional<MutableFields> mutableFields;
 	std::mutex mutex;
 	const kinematics::PlanarArmKinematics<N> kin;
-	Eigen::Vector2d setpoint;
-	Eigen::Vector2d velocity;
-	std::optional<robot::types::datatime_t> velTimestamp;
 	const double safetyFactor;
 
 	/**
@@ -165,14 +227,15 @@ class PlanarArmController {
 			// setpoint = setpoint inside circle
 			// eePos = new setpoint outside circle
 			// radius = ||setpoint + a*(eePos - setpoint)||  solve for a
-			double diffDotProd = (eePos - setpoint).dot(setpoint);
-			double differenceNorm = (eePos - setpoint).squaredNorm();
+			double diffDotProd =
+				(eePos - mutableFields->setpoint).dot(mutableFields->setpoint);
+			double differenceNorm = (eePos - mutableFields->setpoint).squaredNorm();
 			double discriminant =
 				std::pow(diffDotProd, 2) -
-				differenceNorm * (setpoint.squaredNorm() - std::pow(radius, 2));
+				differenceNorm * (mutableFields->setpoint.squaredNorm() - std::pow(radius, 2));
 			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;
+			eePos = (1 - a) * mutableFields->setpoint + a * eePos;
 		}
 		return eePos;
 	}

src/network/MissionControlProtocol.cpp

@@ -106,9 +106,11 @@ void MissionControlProtocol::handleRequestArmIKEnabled(const json& j) {
 		if (!Globals::armIKEnabled) {
 			DataPoint<navtypes::Vectord<Constants::arm::IK_MOTORS.size()>> armJointPositions =
 				robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
-			// Rover responds with armIKEnabledReport after requestArmIKEnable is processed
-			if (armJointPositions.isValid()) {
-				Globals::planarArmController.set_setpoint(armJointPositions.getData());
+
+			bool success =
+				Globals::planarArmController.tryInitController(armJointPositions.getData());
+
+			if (success) {
 				this->setArmIKEnabled(true);
 			} else {
 				// unable to enable IK

tests/control/PlanarArmControllerTest.cpp

@@ -16,7 +16,7 @@ std::string toString(const Eigen::Vector2d& pose) {
 	return ss.str();
 }
 
-void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-5,
+void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-4,
 				  double angle = 1e-5) {
 	std::stringstream ss;
 	ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2);
@@ -26,10 +26,30 @@ void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double d
 	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, Constants::arm::SAFETY_FACTOR);
+TEST_CASE("Test Planar Arm Controller Double Init", "[control][planararmcontroller]") {
+	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);
+	PlanarArmController<2> foo(kin_obj, Constants::arm::SAFETY_FACTOR);
+	REQUIRE(foo.tryInitController({0, M_PI_2}));
+	REQUIRE(foo.tryInitController({0, M_PI_2}));
+	REQUIRE(foo.tryInitController({M_PI_4, M_PI_2}));
+
+	// 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()));
+}
+
+TEST_CASE("Test Planar Arm Safety Factor Violation", "[control][planararmcontroller]") {
+	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);
+	PlanarArmController<2> foo(kin_obj, Constants::arm::SAFETY_FACTOR);
+	REQUIRE_FALSE(foo.tryInitController({0, 0}));
 }
 
 TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") {
@@ -40,7 +60,8 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") {
 	kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0);
 
 	// Instantiate PlanarArmController.
-	PlanarArmController<2> foo({0, M_PI_2}, kin_obj, Constants::arm::SAFETY_FACTOR);
+	PlanarArmController<2> foo(kin_obj, Constants::arm::SAFETY_FACTOR);
+	REQUIRE(foo.tryInitController({0, M_PI_2}));
 
 	// 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.