DiffDriveKinematics: Scale wheel velocity to stay below max speed

Sometimes we want to scale the wheel velocity so that we focus more on
rotational velocity or linear velocity. There are cases where the given
theta velocity and linear velocity result in a wheel speed too great for
the rover.

src/kinematics/DiffDriveKinematics.cpp

@@ -1,6 +1,8 @@
 #include "DiffDriveKinematics.h"
 
 #include "../utils/transform.h"
+
+#include <loguru.hpp>
 using namespace navtypes;
 using util::toTransformRotateFirst;
 
@@ -49,4 +51,34 @@ pose_t DiffDriveKinematics::getNextPose(const wheelvel_t& wheelVel, const pose_t
 										double dt) const {
 	return pose + getPoseUpdate(wheelVel, pose(2), dt);
 }
+
+pose_t DiffDriveKinematics::ensureWithinWheelSpeedLimit(
+	PreferredVelPreservation preferredVelPreservation, double xVel, double thetaVel,
+	double maxWheelSpeed) const {
+	auto wheelVels = robotVelToWheelVel(xVel, thetaVel);
+	double calculatedMaxWheelVel = std::max(wheelVels.lVel, wheelVels.rVel);
+	if (calculatedMaxWheelVel > maxWheelSpeed) {
+		double scaleFactor = maxWheelSpeed / calculatedMaxWheelVel;
+		switch (preferredVelPreservation) {
+			case PreferredVelPreservation::Proportional:
+				xVel *= scaleFactor;
+				thetaVel *= scaleFactor;
+				break;
+			case PreferredVelPreservation::PreferXVel:
+				thetaVel *= (0.5 * scaleFactor);
+				break;
+			case PreferredVelPreservation::PreferThetaVel:
+				xVel *= (0.5 * scaleFactor);
+				break;
+		}
+		// wheelVels = robotVelToWheelVel(xVel, thetaVel);
+		// double oldMaxWheelVel = calculatedMaxWheelVel;
+		// calculatedMaxWheelVel = std::max(wheelVels.lVel, wheelVels.rVel);
+		// LOG_F(INFO, "Scale Factor: %lf MaxWheelSpeed: %lf OldMaxWheelVel: %lf
+		// NewMaxWheelVel: %lf\n", scaleFactor, maxWheelSpeed, oldMaxWheelVel,
+		// calculatedMaxWheelVel);
+	}
+	return {xVel, 0, thetaVel};
+}
+
 } // namespace kinematics

src/kinematics/DiffDriveKinematics.h

@@ -78,6 +78,16 @@ class DiffDriveKinematics {
 	navtypes::pose_t getNextPose(const wheelvel_t& wheelVel, const navtypes::pose_t& pose,
 								 double dt) const;
 
+	enum PreferredVelPreservation {
+		Proportional,
+		PreferXVel,
+		PreferThetaVel,
+	};
+
+	navtypes::pose_t ensureWithinWheelSpeedLimit(PreferredVelPreservation preferred,
+												 double xVel, double thetaVel,
+												 double maxWheelSpeed) const;
+
 private:
 	double wheelBaseWidth;
 };

src/world_interface/kinematic_common_interface.cpp

@@ -59,6 +59,11 @@ double setCmdVel(double dtheta, double dx) {
 		return 0;
 	}
 
+	navtypes::pose_t scaledVelocities = driveKinematics().ensureWithinWheelSpeedLimit(
+		DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, dx, dtheta,
+		Constants::MAX_WHEEL_VEL);
+	dx = scaledVelocities.x();
+	dtheta = scaledVelocities.z();
 	wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta);
 	double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL;
 	double rPWM = wheelVels.rVel / Constants::MAX_WHEEL_VEL;