DiffDriveKinematics: Scale wheel velocity to stay below max speed (#285)

* 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.

* Make corrections to math + implement proportional + tests

* remove trailing comma to maybe fix clang-format

* add comma back

* Correct proportional scaling + cleanup + some work on tests

* Tests: Add tests for preferxvel diff drive kinematic scaling

* Tests: Add tests for preferthetavel in diffdrive kinematics

* Kinematics: Remove unnecessary include

* DiffDriveKinematics: Clean up + add test for thetaVel being too large

* Add missing break + make immutable values const

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));
+}