Finish wrist implementation, fix bugs, move wrist kinematics to globals

src/Globals.cpp

@@ -46,6 +46,7 @@ net::websocket::SingleClientWSServer websocketServer("DefaultServer",
 													 Constants::WS_SERVER_PORT);
 std::atomic<bool> AUTONOMOUS = false;
 robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperipheral_t::none;
+const kinematics::DiffWristKinematics wristKinematics;
 control::PlanarArmController<2> planarArmController(createArmKinematics(),
 													Constants::arm::SAFETY_FACTOR);
 std::atomic<bool> armIKEnabled = false;

src/Globals.h

@@ -2,6 +2,7 @@
 
 #include "control/PlanarArmController.h"
 #include "kinematics/ArmKinematics.h"
+#include "kinematics/DiffWristKinematics.h"
 #include "network/websocket/WebSocketServer.h"
 #include "world_interface/data.h"
 
@@ -28,5 +29,6 @@ extern net::websocket::SingleClientWSServer websocketServer;
 extern std::atomic<bool> AUTONOMOUS;
 extern robot::types::mountedperipheral_t mountedPeripheral;
 extern control::PlanarArmController<2> planarArmController;
+extern const kinematics::DiffWristKinematics wristKinematics;
 extern std::atomic<bool> armIKEnabled;
 } // namespace Globals

src/control_interface.cpp

@@ -71,27 +71,7 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) {
 	using robot::types::jointid_t;
 	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
-	// motor position.
-	/*
-	else if (joint == jointid_t::differentialPitch || joint == jointid_t::differentialRoll) {
-		std::lock_guard<std::mutex> lk(wristPosMutex);
-		if (joint == jointid_t::differentialPitch){
-			commandedWristPos.pitch = targetPos;
-		} else {
-			commandedWristPos.roll = targetPos;
-		}
-		gearpos_t gearPos = wristKinematics().jointPosToGearPos(commandedWristPos);
-		setMotorPos(motorid_t::differentialLeft, gearPos.left);
-		setMotorPos(motorid_t::differentialRight, gearPos.right);
-	}
-	*/
-	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)
+	} else {
 		LOG_F(WARNING, "setJointPos called for currently unsupported joint %s",
 			  util::to_string(joint).c_str());
 	}
@@ -113,6 +93,19 @@ types::DataPoint<int32_t> getJointPos(robot::types::jointid_t joint) {
 		} else {
 			return {};
 		}
+	} else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
+		auto mdegToRad = [](int32_t mdeg) { return (mdeg / 1000.0) * (M_PI / 180.0); };
+		auto lPos = getMotorPos(motorid_t::wristDiffLeft).transform(mdegToRad);
+		auto rPos = getMotorPos(motorid_t::wristDiffRight).transform(mdegToRad);
+		if (lPos.isValid() && rPos.isValid()) {
+			kinematics::gearpos_t gearPos(lPos.getData(), rPos.getData());
+			auto jointPos = Globals::wristKinematics.gearPosToJointPos(gearPos);
+			float angle = joint == jointid_t::wristPitch ? jointPos.pitch : jointPos.roll;
+			return DataPoint<int32_t>(lPos.getTime(),
+									  static_cast<int32_t>(angle * (180.0 / M_PI) * 1000));
+		} else {
+			return {};
+		}
 	} else {
 		// This should ideally never happen, but may if we haven't implemented a joint yet.
 		LOG_F(WARNING, "getJointPos called for currently unsupported joint %s",
@@ -165,11 +158,13 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 			}
 		}
 	} else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
-		kinematics::DiffWristKinematics diffWristKinematics;
 		setJointPowerValue(joint, power);
 		kinematics::jointpos_t jointPwr(getJointPowerValue(jointid_t::wristPitch),
-										 getJointPowerValue(jointid_t::wristRoll));
-		kinematics::gearpos_t gearPwr = diffWristKinematics.jointPowerToGearPower(jointPwr);
+										getJointPowerValue(jointid_t::wristRoll));
+		kinematics::gearpos_t gearPwr =
+			Globals::wristKinematics.jointPowerToGearPower(jointPwr);
+		setMotorPower(motorid_t::wristDiffLeft, gearPwr.left);
+		setMotorPower(motorid_t::wristDiffRight, gearPwr.right);
 	} else {
 		LOG_F(WARNING, "setJointPower called for currently unsupported joint %s",
 			  util::to_string(joint).c_str());

src/world_interface/noop_world.cpp

@@ -6,7 +6,6 @@
 
 // using namespace kinematics;
 using kinematics::DiffDriveKinematics;
-using kinematics::DiffWristKinematics;
 using namespace navtypes;
 using namespace robot::types;
 
@@ -16,17 +15,12 @@ extern const WorldInterface WORLD_INTERFACE = WorldInterface::noop;
 
 namespace {
 DiffDriveKinematics drive_kinematics(1); // doesn't really matter what we set this to
-DiffWristKinematics wrist_kinematics;
 } // namespace
 
 const DiffDriveKinematics& driveKinematics() {
 	return drive_kinematics;
 }
 
-const DiffWristKinematics& wristKinematics() {
-	return wrist_kinematics;
-}
-
 void world_interface_init(
 	std::optional<std::reference_wrapper<net::websocket::SingleClientWSServer>> wsServer,
 	bool initMotorsOnly) {}

src/world_interface/real_world_interface.cpp

@@ -33,7 +33,6 @@ std::unordered_map<robot::types::motorid_t, std::shared_ptr<robot::base_motor>>
 
 namespace {
 kinematics::DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE);
-kinematics::DiffWristKinematics wrist_kinematics;
 bool is_emergency_stopped = false;
 
 void addMotorMapping(motorid_t motor, bool hasPosSensor) {
@@ -52,10 +51,6 @@ const kinematics::DiffDriveKinematics& driveKinematics() {
 	return drive_kinematics;
 }
 
-const kinematics::DiffWristKinematics& wristKinematics() {
-	return wrist_kinematics;
-}
-
 namespace {
 // map that associates camera id to the camera object
 std::unordered_map<CameraID, std::shared_ptr<cam::Camera>> cameraMap;

src/world_interface/simulator_interface.cpp

@@ -247,17 +247,12 @@ namespace robot {
 
 namespace {
 DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE);
-DiffWristKinematics wrist_kinematics;
 } // namespace
 
 const DiffDriveKinematics& driveKinematics() {
 	return drive_kinematics;
 }
 
-const DiffWristKinematics& wristKinematics() {
-	return wrist_kinematics;
-}
-
 extern const WorldInterface WORLD_INTERFACE = WorldInterface::sim3d;
 
 void world_interface_init(

src/world_interface/world_interface.h

@@ -2,7 +2,6 @@
 
 #include "../gps/gps_util.h"
 #include "../kinematics/DiffDriveKinematics.h"
-#include "../kinematics/DiffWristKinematics.h"
 #include "../navtypes.h"
 #include "../network/websocket/WebSocketServer.h"
 #include "data.h"
@@ -37,7 +36,6 @@ extern const WorldInterface WORLD_INTERFACE;
 
 // TODO: add documentation
 const kinematics::DiffDriveKinematics& driveKinematics();
-const kinematics::DiffWristKinematics& wristKinematics();
 
 /**
  * @brief Initialize the world interface.