DiffDriveKinematics: Scale wheel velocity to stay below max speed

src/kinematics/DiffDriveKinematics.cpp

@@ -1,6 +1,7 @@
 #include "DiffDriveKinematics.h"
 
 #include "../utils/transform.h"
+
 using namespace navtypes;
 using util::toTransformRotateFirst;
 
@@ -49,4 +50,56 @@ 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 {
+	const auto [lVel, rVel] = robotVelToWheelVel(xVel, thetaVel);
+	const double calculatedMaxWheelVel = std::max(std::abs(lVel), std::abs(rVel));
+	if (calculatedMaxWheelVel > maxWheelSpeed) {
+		switch (preferredVelPreservation) {
+			case PreferredVelPreservation::Proportional: {
+				xVel *= maxWheelSpeed / calculatedMaxWheelVel;
+				thetaVel *= maxWheelSpeed / calculatedMaxWheelVel;
+				break;
+			}
+			case PreferredVelPreservation::PreferXVel: {
+				if (std::abs(xVel) > maxWheelSpeed) {
+					return {std::copysign(maxWheelSpeed, xVel), 0, 0};
+				}
+				// Bring lVel up to -maxWheelSpeed
+				if (lVel < -maxWheelSpeed) {
+					thetaVel = (-maxWheelSpeed - xVel) * -2.0 / wheelBaseWidth;
+				}
+				// Bring lVel down to maxWheelSpeed
+				if (lVel > maxWheelSpeed) {
+					thetaVel = (maxWheelSpeed - xVel) * -2.0 / wheelBaseWidth;
+				}
+				// Bring rVel up to -maxWheelSpeed
+				if (rVel < -maxWheelSpeed) {
+					thetaVel = (-maxWheelSpeed - xVel) * 2.0 / wheelBaseWidth;
+				}
+				// Bring rVel down to maxWheelSpeed
+				if (rVel > maxWheelSpeed) {
+					thetaVel = (maxWheelSpeed - xVel) * 2.0 / wheelBaseWidth;
+				}
+				break;
+			}
+			case PreferredVelPreservation::PreferThetaVel: {
+				if (std::abs(wheelBaseWidth / 2 * thetaVel) > maxWheelSpeed) {
+					return {0, 0, std::copysign(maxWheelSpeed * 2 / wheelBaseWidth, thetaVel)};
+				}
+				if (std::max(lVel, rVel) > maxWheelSpeed) {
+					xVel -= std::max(lVel, rVel) - maxWheelSpeed;
+				}
+				if (std::min(lVel, rVel) < -maxWheelSpeed) {
+					xVel += -maxWheelSpeed - std::min(lVel, rVel);
+				}
+				break;
+			}
+		}
+	}
+	return {xVel, 0, thetaVel};
+}
+
 } // namespace kinematics

src/kinematics/DiffDriveKinematics.h

@@ -13,6 +13,24 @@ struct wheelvel_t {
  */
 class DiffDriveKinematics {
 public:
+	/**
+	 * Proportional: The xVel and thetaVel are scaled an equal amount. This keeps the linear
+	 * and theta velocity at equal proportions to their original values but their output wheel
+	 * speed is below the max allowed.
+	 *
+	 * PreferXVel: The theta velocity is scaled and the x velocity is maintained. This
+	 * preserves linear velocity over theta velocity meaning we lose turning ability.
+	 *
+	 * PreferThetaVel: The x velocity is scaled and the theta velocity is maintained. This
+	 * preserves the ability to rotate the rover by scaling down the linear velocity so that
+	 * the calculated max wheel speed is below the set max wheel speed allowed.
+	 */
+	enum class PreferredVelPreservation {
+		Proportional,
+		PreferXVel,
+		PreferThetaVel,
+	};
+
 	/**
 	 * Create a new DiffDriveKinematics with the given wheel base width.
 	 *
@@ -78,6 +96,22 @@ class DiffDriveKinematics {
 	navtypes::pose_t getNextPose(const wheelvel_t& wheelVel, const navtypes::pose_t& pose,
 								 double dt) const;
 
+	/**
+	 * Ensure that the given xVel and thetaVel translate to a wheel speed less than or equal
+	 * to the max wheel speed. Scales them if they do not.
+	 *
+	 * @param preferred Choose proportional if x velocity and theta velocity should be
+	 *                  scaled proportionally to each other, choose PreferXVel if linear
+	 * velocity is preferred, or choose PreferThetaVel if turning is preferred.
+	 * @param xVel The xVel used to calculate the wheel speed
+	 * @param thetaVel The theta veloicty used to calculate the wheel speed.
+	 * @param maxWheelSpeed The current max possible wheel speed. This is the value
+	 *                      that the calculated velocity will be checked against.
+	 */
+	navtypes::pose_t ensureWithinWheelSpeedLimit(PreferredVelPreservation preferred,
+												 double xVel, double thetaVel,
+												 double maxWheelSpeed) const;
+
 private:
 	double wheelBaseWidth;
 };

tests/kinematics/DiffDriveKinematicsTest.cpp

@@ -1,4 +1,5 @@
 #include "../../src/kinematics/DiffDriveKinematics.h"
+
 #include "../../src/navtypes.h"
 
 #include <iostream>
@@ -8,19 +9,17 @@
 using namespace navtypes;
 using namespace kinematics;
 
-namespace
-{
+namespace {
 constexpr double PI = M_PI;
 
-std::string toString(const pose_t &pose)
-{
+std::string toString(const pose_t& pose) {
 	std::stringstream ss;
 	ss << "(" << pose(0) << ", " << pose(1) << ", " << pose(2) << ")";
 	return ss.str();
 }
 
-void assertApprox(const pose_t &p1, const pose_t &p2, double dist = 0.01, double angle = 0.01)
-{
+void assertApprox(const pose_t& p1, const pose_t& p2, double dist = 0.01,
+				  double angle = 0.01) {
 	pose_t diff = p1 - p2;
 	bool distEqual = diff.topRows<2>().norm() <= dist;
 	double thetaDiff = std::fmod(abs(diff(2)), 2 * PI);
@@ -29,25 +28,20 @@ void assertApprox(const pose_t &p1, const pose_t &p2, double dist = 0.01, double
 		thetaDiff -= 2 * PI;
 	}
 	bool angleEqual = abs(thetaDiff) <= angle;
-	if (distEqual && angleEqual)
-	{
+	if (distEqual && angleEqual) {
 		SUCCEED();
-	}
-	else
-	{
+	} else {
 		std::cout << "Expected: " << toString(p1) << ", Actual: " << toString(p2) << std::endl;
 		FAIL();
 	}
 }
 
-wheelvel_t wheelVel(double lVel, double rVel)
-{
+wheelvel_t wheelVel(double lVel, double rVel) {
 	return (wheelvel_t){lVel, rVel};
 }
 } // namespace
 
-TEST_CASE("Differential Drive Kinematics Test")
-{
+TEST_CASE("Differential Drive Kinematics Test") {
 	DiffDriveKinematics kinematics(1);
 
 	// go straight forward 1m
@@ -60,4 +54,55 @@ TEST_CASE("Differential Drive Kinematics Test")
 	// drive CW in a full circle with radius 1
 	assertApprox({0, 0, 0},
 				 kinematics.getNextPose(wheelVel(3 * PI / 4, PI / 4), {0, 0, 0}, 4));
-}
+
+	constexpr double max_wheel_vel = 0.75;
+
+	// Proportional Preservation Tests
+	assertApprox({0.5, 0, 0.5},
+				 kinematics.ensureWithinWheelSpeedLimit(
+					 DiffDriveKinematics::PreferredVelPreservation::Proportional, 1.0, 1.0,
+					 max_wheel_vel));
+
+	// xVel Preservation Tests
+	// Test #1: Test the xVel alone being too fast, therefore xVel should become the max and
+	// theta should be zeroed
+	assertApprox({-max_wheel_vel, 0, 0},
+				 kinematics.ensureWithinWheelSpeedLimit(
+					 DiffDriveKinematics::PreferredVelPreservation::PreferXVel, -1.0, 0.5,
+					 max_wheel_vel));
+	// Test #2: lVel < -max_wheel_vel
+	assertApprox({-0.4, 0, 0.7}, kinematics.ensureWithinWheelSpeedLimit(
+									 DiffDriveKinematics::PreferredVelPreservation::PreferXVel,
+									 -0.4, 1.6, max_wheel_vel));
+	// Test #3: lVel > max_wheel_vel: lVel = 1.0 rVel = 0.2
+	assertApprox({0.6, 0, -0.3}, kinematics.ensureWithinWheelSpeedLimit(
+									 DiffDriveKinematics::PreferredVelPreservation::PreferXVel,
+									 0.6, -0.5714, max_wheel_vel));
+	// Test #4: rVel < -max_wheel_vel: lVel = 0.2 rVel = -0.9
+	assertApprox({-0.35, 0, -0.8},
+				 kinematics.ensureWithinWheelSpeedLimit(
+					 DiffDriveKinematics::PreferredVelPreservation::PreferXVel, -0.35, -1.1,
+					 max_wheel_vel));
+	// Test #5: rVel > max_wheel_vel: lVel = 0.2 rVel = 0.9
+	assertApprox({0.55, 0, 0.4}, kinematics.ensureWithinWheelSpeedLimit(
+									 DiffDriveKinematics::PreferredVelPreservation::PreferXVel,
+									 0.55, 0.7, max_wheel_vel));
+
+	// thetaVel Preservation Tests
+	// Test #1: std::abs(wheelBaseWidth / 2 * thetaVel) > maxWheelSpeed - ie. thetaVel too big
+	// by itself
+	assertApprox({0, 0, 1.5},
+				 kinematics.ensureWithinWheelSpeedLimit(
+					 DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, 0, 1.6,
+					 max_wheel_vel));
+	// Test #2: std::max(lVel, rVel) > maxWheelSpeed: lVel = 0.9 r_vel = 0.35
+	assertApprox({0.475, 0, -0.55},
+				 kinematics.ensureWithinWheelSpeedLimit(
+					 DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, 0.625,
+					 -0.55, max_wheel_vel));
+	// Test #3: std::min(lVel, rVel) < -maxWheelSpeed: lVel = -0.9 rVel = -0.2
+	assertApprox({-0.4, 0, 0.7},
+				 kinematics.ensureWithinWheelSpeedLimit(
+					 DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, -0.55, 0.7,
+					 max_wheel_vel));
+}