Reject enabling IK outside safety radius

src/Globals.cpp

@@ -41,7 +41,6 @@ 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,
-													Constants::arm::SAFETY_FACTOR);
+std::optional<control::PlanarArmController<2>> planarArmController;
 std::atomic<bool> armIKEnabled = false;
 } // namespace Globals

src/Globals.h

@@ -28,6 +28,6 @@ extern std::atomic<bool> E_STOP;
 extern std::atomic<bool> AUTONOMOUS;
 extern robot::types::mountedperipheral_t mountedPeripheral;
 extern const kinematics::PlanarArmKinematics<2> planarArmKinematics;
-extern control::PlanarArmController<2> planarArmController;
+extern std::optional<control::PlanarArmController<2>> planarArmController;
 extern std::atomic<bool> armIKEnabled;
 } // namespace Globals

src/control/PlanarArmController.h

@@ -44,6 +44,46 @@ class PlanarArmController {
 		CHECK_F(safetyFactor > 0.0 && safetyFactor < 1.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);
+		setpoint = std::move(other.setpoint);
+		velocity = std::move(other.velocity);
+		velTimestamp = std::move(other.velTimestamp);
+	}
+
+	static std::optional<PlanarArmController>
+	makeController(const navtypes::Vectord<N>& currJointPos,
+				   kinematics::PlanarArmKinematics<N> kin_obj, double safetyFactor) {
+		if (is_setpoint_valid(currJointPos, kin_obj, safetyFactor)) {
+			return PlanarArmController(currJointPos, kin_obj, safetyFactor);
+		}
+
+		return {};
+	}
+
+	/**
+	 * @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,
+								  const 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;
+	}
+
 	/**
 	 * @brief Sets the end effector setpoint / target position.
 	 *

src/network/MissionControlProtocol.cpp

@@ -139,9 +139,16 @@ void MissionControlProtocol::handleRequestArmIKEnabled(const json& j) {
 			}
 			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());
+
+			auto o = control::PlanarArmController<2>::makeController(
+				armJointPositions, Globals::planarArmKinematics,
+				Constants::arm::SAFETY_FACTOR);
+
+			if (o) {
+				Globals::planarArmController.emplace(std::move(*o));
+			}
+
+			if (Globals::planarArmController.has_value()) {
 				this->setArmIKEnabled(true);
 				_arm_ik_repeat_thread =
 					std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this);
@@ -467,7 +474,7 @@ void MissionControlProtocol::updateArmIKRepeatTask() {
 			robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
 		if (armJointPositions.isValid()) {
 			navtypes::Vectord<Constants::arm::IK_MOTORS.size()> targetJointPositions =
-				Globals::planarArmController.getCommand(dataclock::now(), armJointPositions);
+				Globals::planarArmController->getCommand(dataclock::now(), armJointPositions);
 			targetJointPositions /=
 				M_PI / 180.0 / 1000.0; // convert from radians to millidegrees
 			for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {

src/world_interface/kinematic_common_interface.cpp

@@ -181,11 +181,11 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 	} else if (joint == jointid_t::ikForward || joint == jointid_t::ikUp) {
 		if (Globals::armIKEnabled) {
 			if (joint == jointid_t::ikForward) {
-				Globals::planarArmController.set_x_vel(dataclock::now(),
-													   power * Constants::arm::MAX_EE_VEL);
+				Globals::planarArmController->set_x_vel(dataclock::now(),
+														power * Constants::arm::MAX_EE_VEL);
 			} else {
-				Globals::planarArmController.set_y_vel(dataclock::now(),
-													   power * Constants::arm::MAX_EE_VEL);
+				Globals::planarArmController->set_y_vel(dataclock::now(),
+														power * Constants::arm::MAX_EE_VEL);
 			}
 		}
 	} else {