diff --git a/src/kinematics/DiffDriveKinematics.cpp b/src/kinematics/DiffDriveKinematics.cpp index afdd84855..7b2f4032b 100644 --- a/src/kinematics/DiffDriveKinematics.cpp +++ b/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 diff --git a/src/kinematics/DiffDriveKinematics.h b/src/kinematics/DiffDriveKinematics.h index 79ccc933d..b8143cd76 100644 --- a/src/kinematics/DiffDriveKinematics.h +++ b/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; }; diff --git a/tests/kinematics/DiffDriveKinematicsTest.cpp b/tests/kinematics/DiffDriveKinematicsTest.cpp index c791423cf..d54ad31f6 100644 --- a/tests/kinematics/DiffDriveKinematicsTest.cpp +++ b/tests/kinematics/DiffDriveKinematicsTest.cpp @@ -1,4 +1,5 @@ #include "../../src/kinematics/DiffDriveKinematics.h" + #include "../../src/navtypes.h" #include @@ -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)); -} \ No newline at end of file + + 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)); +}