Added handler for inverse kinematics special case joint request

src/Constants.h

@@ -99,6 +99,7 @@ constexpr const char* SIM_PROTOCOL_NAME = "/simulator";
 constexpr const char* DGPS_PROTOCOL_NAME = "/dgps";
 
 constexpr std::chrono::milliseconds JOINT_POWER_REPEAT_PERIOD(333);
+constexpr std::chrono::milliseconds ARM_IK_UPDATE_PERIOD(100);
 
 namespace Nav {
 // Distance (m) we could have traveled forward in the time it takes to turn 1 radian

src/Globals.cpp

@@ -42,4 +42,5 @@ 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);
+std::atomic<bool> armIKEnabled = false;
 } // namespace Globals

src/Globals.h

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

src/network/MissionControlProtocol.cpp

@@ -36,6 +36,7 @@ const std::chrono::milliseconds TELEM_REPORT_PERIOD = 100ms;
 constexpr const char* EMERGENCY_STOP_REQ_TYPE = "emergencyStopRequest";
 constexpr const char* OPERATION_MODE_REQ_TYPE = "operationModeRequest";
 constexpr const char* DRIVE_REQ_TYPE = "driveRequest";
+constexpr const char* ARM_IK_ENABLED_TYPE = "setArmIKEnabled";
 constexpr const char* MOTOR_POWER_REQ_TYPE = "motorPowerRequest";
 constexpr const char* JOINT_POWER_REQ_TYPE = "jointPowerRequest";
 constexpr const char* MOTOR_POSITION_REQ_TYPE = "motorPositionRequest";
@@ -82,6 +83,7 @@ void MissionControlProtocol::handleEmergencyStopRequest(const json& j) {
 	}
 	// TODO: reinit motors
 	Globals::E_STOP = stop;
+	Globals::armIKEnabled = false;
 }
 
 static bool validateOperationModeRequest(const json& j) {
@@ -107,6 +109,10 @@ static bool validateDriveRequest(const json& j) {
 		   util::hasKey(j, "steer") && util::validateRange(j, "steer", -1, 1);
 }
 
+static bool validateArmIKEnable(const json& j) {
+	return util::validateKey(j, "enabled", val_t::boolean);
+}
+
 void MissionControlProtocol::handleDriveRequest(const json& j) {
 	// TODO: ignore this message if we are in autonomous mode.
 	// fit straight and steer to unit circle; i.e. if |<straight, steer>| > 1, scale each
@@ -121,6 +127,33 @@ void MissionControlProtocol::handleDriveRequest(const json& j) {
 	this->setRequestedCmdVel(dtheta, dx);
 }
 
+void MissionControlProtocol::handleSetArmIKEnabled(const json& j) {
+	bool enabled = j["enabled"];
+	Globals::armIKEnabled = enabled;
+	if (enabled) {
+		Globals::armIKEnabled = false;
+		if (_arm_ik_repeat_thread.joinable()) {
+			_arm_ik_repeat_thread.join();
+		}
+		navtypes::Vectord<Constants::arm::IK_MOTORS.size()> armJointPositions;
+		for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {
+			auto positionDataPoint = robot::getMotorPos(Constants::arm::IK_MOTORS[i]);
+			assert(positionDataPoint.isValid()); // crash if data is not valid
+			double position = static_cast<double>(positionDataPoint.getData());
+			position *= M_PI / 180.0 / 1000.0;
+			armJointPositions(i) = position;
+		}
+		Globals::planarArmController.set_setpoint(armJointPositions);
+		Globals::armIKEnabled = true;
+		_arm_ik_repeat_thread =
+			std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this);
+	} else {
+		if (_arm_ik_repeat_thread.joinable()) {
+			_arm_ik_repeat_thread.join();
+		}
+	}
+}
+
 void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) {
 	{
 		std::lock_guard<std::mutex> power_lock(this->_joint_power_mutex);
@@ -132,7 +165,7 @@ void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) {
 static bool validateJoint(const json& j) {
 	return util::validateKey(j, "joint", val_t::string) &&
 		   util::validateOneOf(j, "joint",
-							   {"armBase", "shoulder", "elbow", "forearm", "wrist", "hand"});
+							   {"armBase", "shoulder", "elbow", "forearm", "wrist", "hand", "ikUp", "ikForward"});
 }
 
 static bool validateJointPowerRequest(const json& j) {
@@ -233,6 +266,9 @@ void MissionControlProtocol::sendCameraStreamReport(
 }
 
 void MissionControlProtocol::handleConnection() {
+	// Turn off inverse kinematics on connection
+	Globals::armIKEnabled = false;
+
 	// TODO: send the actual mounted peripheral, as specified by the command-line parameter
 	json j = {{"type", MOUNTED_PERIPHERAL_REP_TYPE}};
 
@@ -289,6 +325,8 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat() {
 			this->_joint_repeat_thread.join();
 		}
 	}
+	// Turn off inverse kinematics so that IK state will be in sync with mission control
+	Globals::armIKEnabled = false;
 }
 
 MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
@@ -313,6 +351,14 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 	this->addMessageHandler(DRIVE_REQ_TYPE,
 							std::bind(&MissionControlProtocol::handleDriveRequest, this, _1),
 							validateDriveRequest);
+	this->addMessageHandler(
+		ARM_IK_ENABLED_TYPE,
+		std::bind(&MissionControlProtocol::handleSetArmIKEnabled, this, _1),
+		validateArmIKEnable);
+	this->addMessageHandler(
+		ARM_IK_ENABLED_TYPE,
+		std::bind(&MissionControlProtocol::handleSetArmIKEnabled, this, _1),
+		validateArmIKEnable);
 	this->addMessageHandler(
 		JOINT_POWER_REQ_TYPE,
 		std::bind(&MissionControlProtocol::handleJointPowerRequest, this, _1),
@@ -380,6 +426,30 @@ void MissionControlProtocol::jointPowerRepeatTask() {
 	}
 }
 
+void MissionControlProtocol::updateArmIKRepeatTask() {
+	dataclock::time_point next_update_time = dataclock::now();
+	while (Globals::armIKEnabled) {
+		navtypes::Vectord<Constants::arm::IK_MOTORS.size()> armJointPositions;
+		for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {
+			DataPoint<int32_t> positionDataPoint =
+				robot::getMotorPos(Constants::arm::IK_MOTORS[i]);
+			assert(positionDataPoint.isValid()); // crash if data is not valid
+			double position = static_cast<double>(positionDataPoint.getData());
+			position *= M_PI / 180.0 / 1000.0;
+			armJointPositions(i) = position;
+		}
+		navtypes::Vectord<Constants::arm::IK_MOTORS.size()> targetJointPositions =
+			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++) {
+			robot::setMotorPos(Constants::arm::IK_MOTORS[i],
+							   static_cast<int32_t>(targetJointPositions(i)));
+		}
+		next_update_time += Constants::ARM_IK_UPDATE_PERIOD;
+		std::this_thread::sleep_until(next_update_time);
+	}
+}
+
 void MissionControlProtocol::telemReportTask() {
 	dataclock::time_point pt = dataclock::now();
 

src/network/MissionControlProtocol.h

@@ -34,6 +34,7 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 private:
 	void videoStreamTask();
 	void jointPowerRepeatTask();
+	void updateArmIKRepeatTask();
 	void telemReportTask();
 	SingleClientWSServer& _server;
 	std::shared_mutex _stream_mutex;
@@ -55,16 +56,17 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 	std::mutex _joint_repeat_thread_mutex;
 	std::thread _joint_repeat_thread;
 	std::condition_variable _power_repeat_cv;
+	std::thread _arm_ik_repeat_thread;
 	void handleEmergencyStopRequest(const json& j);
 	void handleOperationModeRequest(const json& j);
 	void handleCameraStreamOpenRequest(const json& j);
 	void handleCameraStreamCloseRequest(const json& j);
 	void handleJointPowerRequest(const json& j);
 	void handleDriveRequest(const json& j);
+	void handleSetArmIKEnabled(const json& j);
 	void sendCameraStreamReport(const CameraID& cam,
 								const std::vector<std::basic_string<uint8_t>>& videoData);
 	void sendJointPositionReport(const std::string& jointName, int32_t position);
-	void sendCameraStreamReport(const CameraID& cam, const std::string& b64_data);
 	void sendRoverPos();
 	void handleConnection();
 	void startPowerRepeat();

src/world_interface/data.cpp

@@ -46,6 +46,10 @@ std::string to_string(robot::types::jointid_t joint) {
 			return "drillArm";
 		case jointid_t::activeSuspension:
 			return "activeSuspension";
+		case jointid_t::ikUp:
+			return "ikUp";
+		case jointid_t::ikForward:
+			return "ikForward";
 		default:
 			// should never happen
 			return "<unknown>";

src/world_interface/data.h

@@ -83,12 +83,15 @@ enum class jointid_t {
 	wrist,
 	hand,
 	drill_arm,
-	activeSuspension
+	activeSuspension,
+	ikForward,
+	ikUp
 };
 
 constexpr auto all_jointid_t = frozen::make_unordered_set<jointid_t>(
 	{jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm,
-	 jointid_t::wrist, jointid_t::hand, jointid_t::drill_arm, jointid_t::activeSuspension});
+	 jointid_t::wrist, jointid_t::hand, jointid_t::drill_arm, jointid_t::activeSuspension,
+	 jointid_t::ikForward, jointid_t::ikUp});
 
 constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
 	{{"armBase", jointid_t::armBase},
@@ -98,7 +101,9 @@ constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, join
 	 {"wrist", jointid_t::wrist},
 	 {"hand", jointid_t::hand},
 	 {"drillArm", jointid_t::drill_arm},
-	 {"activeSuspension", jointid_t::activeSuspension}});
+	 {"activeSuspension", jointid_t::activeSuspension},
+	 {"ikForward", jointid_t::ikForward},
+	 {"ikUp", jointid_t::ikUp}});
 
 /**
  * @brief Represents data measured using a sensor at a given time.

src/world_interface/kinematic_common_interface.cpp

@@ -129,7 +129,7 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) {
 		// FIXME: this should ideally never happen, but we don't have support for all joints
 		// yet because we don't know anything about the drill arm (and need to do extra work
 		// for the differential)
-		log(LOG_WARN, "setJointPower called for currently unsupported joint %s\n",
+		log(LOG_WARN, "setJointPos called for currently unsupported joint %s\n",
 			util::to_string(joint).c_str());
 	}
 }
@@ -143,7 +143,7 @@ types::DataPoint<int32_t> getJointPos(robot::types::jointid_t joint) {
 		// FIXME: this should ideally never happen, but we don't have support for all joints
 		// yet because we don't know anything about the drill arm (and need to do extra work
 		// for the differential)
-		log(LOG_WARN, "getJointPower called for currently unsupported joint %s\n",
+		log(LOG_WARN, "getJointPos called for currently unsupported joint %s\n",
 			util::to_string(joint).c_str());
 		return {};
 	}
@@ -170,9 +170,23 @@ double getJointPowerValue(types::jointid_t joint) {
 
 void setJointMotorPower(robot::types::jointid_t joint, double power) {
 	using robot::types::jointid_t;
-
 	if (jointMotorMap.find(joint) != jointMotorMap.end()) {
-		setMotorPower(jointMotorMap.at(joint), power);
+		// TODO: check if associated motor is in IK_MOTORS, don't check specifically these ones
+		if (joint == jointid_t::elbow || joint == jointid_t::shoulder) {
+			if (!Globals::armIKEnabled) {
+				setMotorPower(jointMotorMap.at(joint), power);
+			}
+		} else {
+			setMotorPower(jointMotorMap.at(joint), 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);
+			} else {
+				Globals::planarArmController.set_y_vel(dataclock::now(), power);
+			}
+		}
 	} else {
 		log(LOG_WARN, "setJointPower called for currently unsupported joint %s\n",
 			util::to_string(joint).c_str());