Added handler for inverse kinematics special case joint request

src/Constants.h

@@ -131,14 +131,9 @@ namespace video {
 constexpr int H264_RF_CONSTANT = 40;
 } // namespace video
 
-// Arm inverse kinematics
-namespace arm {
-
-constexpr double MAX_EE_VEL = 0.3; // m/s
-constexpr double IK_SOLVER_THRESH = 0.001;
-
-constexpr int IK_SOLVER_MAX_ITER = 50;
-
+/**
+ * A map that pairs each of the joints to its corresponding motor.
+ */
 constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t, 7>
 	JOINT_MOTOR_MAP{{robot::types::jointid_t::armBase, robot::types::motorid_t::armBase},
 					{robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder},
@@ -149,21 +144,35 @@ constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t
 					{robot::types::jointid_t::activeSuspension,
 					 robot::types::motorid_t::activeSuspension}};
 
+// Arm inverse kinematics
+namespace arm {
+
+/**
+ * Maximum commanded end-effector velocity, in m/s
+ */
+constexpr double MAX_EE_VEL = 0.3;
+constexpr double IK_SOLVER_THRESH = 0.001;
+
+constexpr int IK_SOLVER_MAX_ITER = 50;
+
+/**
+ * The joints corresponding to the motors used for IK in the arm. The ordering in this array is
+ * the canonical ordering of these joints for IK purposes.
+ */
 constexpr std::array<robot::types::jointid_t, 2> IK_MOTOR_JOINTS = {
 	robot::types::jointid_t::shoulder, robot::types::jointid_t::elbow};
 
 /**
- * The motors used in IK. The ordering in this array is the canonical ordering of these motors
- * for IK purposes.
+ * The motors used in IK. The i-th element in this array corresponds to the joint in the i-th
+ * element of `IK_MOTOR_JOINTS`
  */
-constexpr std::array<robot::types::motorid_t, 2> IK_MOTORS = []() constexpr {
+constexpr std::array<robot::types::motorid_t, 2> IK_MOTORS = ([]() constexpr {
 	std::array<robot::types::motorid_t, IK_MOTOR_JOINTS.size()> ret{};
 	for (size_t i = 0; i < IK_MOTOR_JOINTS.size(); i++) {
 		ret[i] = JOINT_MOTOR_MAP.at(IK_MOTOR_JOINTS[i]);
 	}
 	return ret;
-}
-();
+})();
 
 /**
  * Map from motor ids to min and max joint limits in millidegrees

src/network/MissionControlProtocol.cpp

@@ -129,7 +129,6 @@ void MissionControlProtocol::handleDriveRequest(const json& j) {
 
 void MissionControlProtocol::handleSetArmIKEnabled(const json& j) {
 	bool enabled = j["enabled"];
-	Globals::armIKEnabled = enabled;
 	if (enabled) {
 		Globals::armIKEnabled = false;
 		if (_arm_ik_repeat_thread.joinable()) {
@@ -148,6 +147,7 @@ void MissionControlProtocol::handleSetArmIKEnabled(const json& j) {
 		_arm_ik_repeat_thread =
 			std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this);
 	} else {
+		Globals::armIKEnabled = false;
 		if (_arm_ik_repeat_thread.joinable()) {
 			_arm_ik_repeat_thread.join();
 		}

src/world_interface/kinematic_common_interface.cpp

@@ -98,8 +98,8 @@ void setJointPower(robot::types::jointid_t joint, double power) {
 
 void setJointPos(robot::types::jointid_t joint, int32_t targetPos) {
 	using robot::types::jointid_t;
-	if (Constants::arm::JOINT_MOTOR_MAP.find(joint) != Constants::arm::JOINT_MOTOR_MAP.end()) {
-		setMotorPos(Constants::arm::JOINT_MOTOR_MAP.at(joint), targetPos);
+	if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
+		setMotorPos(Constants::JOINT_MOTOR_MAP.at(joint), targetPos);
 	}
 	// FIXME: need to do some extra control (probably implementing our own PID control) for the
 	// differential position, since the potentiometers are giving us joint position instead of
@@ -126,8 +126,8 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) {
 	}
 }
 types::DataPoint<int32_t> getJointPos(robot::types::jointid_t joint) {
-	if (Constants::arm::JOINT_MOTOR_MAP.find(joint) != Constants::arm::JOINT_MOTOR_MAP.end()) {
-		return getMotorPos(Constants::arm::JOINT_MOTOR_MAP.at(joint));
+	if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
+		return getMotorPos(Constants::JOINT_MOTOR_MAP.at(joint));
 	}
 	// FIXME: need to do some extra work for differential - we will have to figure out which
 	// motor boards the potentiometers are plugged into and query those for "motor position"
@@ -162,12 +162,11 @@ double getJointPowerValue(types::jointid_t joint) {
 
 void setJointMotorPower(robot::types::jointid_t joint, double power) {
 	using robot::types::jointid_t;
-	if (Constants::arm::JOINT_MOTOR_MAP.find(joint) != Constants::arm::JOINT_MOTOR_MAP.end()) {
-		// TODO: check if associated motor is in IK_MOTORS, don't check specifically these ones
-		if (!Globals::armIKEnabled ||
-			!std::find(Constants::arm::IK_MOTOR_JOINTS.begin(),
-					   Constants::arm::IK_MOTOR_JOINTS.end(), joint)) {
-			setMotorPower(Constants::arm::JOINT_MOTOR_MAP.at(joint), power);
+	if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
+		bool isIKMotor = std::find(Constants::arm::IK_MOTOR_JOINTS.begin(),
+								   Constants::arm::IK_MOTOR_JOINTS.end(), joint);
+		if (!Globals::armIKEnabled || !isIKMotor) {
+			setMotorPower(Constants::JOINT_MOTOR_MAP.at(joint), power);
 		}
 	} else if (joint == jointid_t::ikForward || joint == jointid_t::ikUp) {
 		if (Globals::armIKEnabled) {