Added handler for inverse kinematics special case joint request (#240)

* Added special case for inverse kinematics * Reformat * Added special case for inverse kinematics * Reformat * Added ik global * Renamed arm IK global. made type atomic as well * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Set armIKEnabled to false on e stop and connect * Fixed build error * Proper namespace for validateKey * Added special case for inverse kinematics * Reformat * Added ik global * Renamed arm IK global. made type atomic as well * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Set armIKEnabled to false on e stop and connect * Fixed build error * Proper namespace for validateKey * Added special case for inverse kinematics * Reformat * Added ik global * Renamed arm IK global. made type atomic as well * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Set armIKEnabled to false on e stop and connect * Fixed build error * Proper namespace for validateKey * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Proper namespace for validateKey * removed old bool for arm ik enabled * Set setpoint to current ee position on ik enable * Reformat and crash on no valid data * Reformat * Added updateArmThread * Reformat * fix more things * Added scalar to velocity set * Addressed some PR comments * Reformat * Addressed PR comments * Reformat * Addressed PR comments * fix formatting * Addressed PR comments * Reformat --------- Co-authored-by: Abhay D <[email protected]>
huskyroboticsteam · Jun 4, 2023 · 68897ae · 68897ae
1 parent bf351e2
commit 68897ae
Show file tree

Hide file tree

Showing 8 changed files with 148 additions and 28 deletions.
diff --git a/src/Constants.h b/src/Constants.h
@@ -103,6 +103,7 @@ constexpr const char* DGPS_PROTOCOL_NAME = "/dgps";
 constexpr const char* ARDUPILOT_PROTOCOL_NAME = "/ardupilot";
 
 constexpr std::chrono::milliseconds JOINT_POWER_REPEAT_PERIOD(333);
+constexpr std::chrono::milliseconds ARM_IK_UPDATE_PERIOD(50);
 
 namespace Nav {
 // Distance (m) we could have traveled forward in the time it takes to turn 1 radian
@@ -130,29 +131,58 @@ namespace video {
 constexpr int H264_RF_CONSTANT = 40;
 } // namespace video
 
+/**
+ * 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},
+					{robot::types::jointid_t::elbow, robot::types::motorid_t::elbow},
+					{robot::types::jointid_t::forearm, robot::types::motorid_t::forearm},
+					{robot::types::jointid_t::wrist, robot::types::motorid_t::wrist},
+					{robot::types::jointid_t::hand, robot::types::motorid_t::hand},
+					{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 motors used in IK. The ordering in this array is the canonical ordering of these motors
- * for IK purposes.
+ * 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 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 = {
-	robot::types::motorid_t::shoulder, robot::types::motorid_t::elbow};
+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
+ * Map from motor ids to min and max joint limits in millidegrees
  */
 constexpr frozen::unordered_map<robot::types::motorid_t, std::pair<int, int>, IK_MOTORS.size()>
 	JOINT_LIMITS{{robot::types::motorid_t::shoulder, {18200, 152500}},
 				 {robot::types::motorid_t::elbow, {-169100, 0}}};
 
 /**
- * Map from motor ids to segment length
+ * Map from motor ids to segment length in meters
  */
 constexpr frozen::unordered_map<robot::types::motorid_t, double, IK_MOTORS.size()>
 	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.3848608},

diff --git a/src/Globals.cpp b/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
diff --git a/src/Globals.h b/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
diff --git a/src/network/MissionControlProtocol.cpp b/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"];
+	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 {
+		Globals::armIKEnabled = false;
+		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,8 @@ 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 +267,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 +326,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 +352,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 +427,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();
 

diff --git a/src/network/MissionControlProtocol.h b/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();

diff --git a/src/world_interface/data.cpp b/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>";

diff --git a/src/world_interface/data.h b/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.

diff --git a/src/world_interface/kinematic_common_interface.cpp b/src/world_interface/kinematic_common_interface.cpp
@@ -32,14 +32,6 @@ void setCmdVelToIntegrate(const wheelvel_t& wheelVels) {
 }
 
 void setJointMotorPower(robot::types::jointid_t joint, double power);
-const std::unordered_map<robot::types::jointid_t, robot::types::motorid_t> jointMotorMap{
-	{robot::types::jointid_t::armBase, robot::types::motorid_t::armBase},
-	{robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder},
-	{robot::types::jointid_t::elbow, robot::types::motorid_t::elbow},
-	{robot::types::jointid_t::forearm, robot::types::motorid_t::forearm},
-	{robot::types::jointid_t::wrist, robot::types::motorid_t::wrist},
-	{robot::types::jointid_t::hand, robot::types::motorid_t::hand},
-	{robot::types::jointid_t::activeSuspension, robot::types::motorid_t::activeSuspension}};
 
 std::unordered_map<types::jointid_t, double> jointPowerValues{};
 std::mutex jointPowerValuesMutex;
@@ -106,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 (jointMotorMap.find(joint) != jointMotorMap.end()) {
-		setMotorPos(jointMotorMap.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
@@ -129,21 +121,21 @@ 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());
 	}
 }
 types::DataPoint<int32_t> getJointPos(robot::types::jointid_t joint) {
-	if (jointMotorMap.find(joint) != jointMotorMap.end()) {
-		return getMotorPos(jointMotorMap.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"
 	else {
 		// 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 +162,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);
+	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) != Constants::arm::IK_MOTOR_JOINTS.end();
+		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) {
+			if (joint == jointid_t::ikForward) {
+				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);
+			}
+		}
 	} else {
 		log(LOG_WARN, "setJointPower called for currently unsupported joint %s\n",
 			util::to_string(joint).c_str());