diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp new file mode 100644 index 0000000000..c55c2e8c0f --- /dev/null +++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/controller/RamseteController.h" + +#include + +using namespace frc; + +/** + * Returns sin(x) / x. + * + * @param x Value of which to take sinc(x). + */ +static double Sinc(double x) { + if (std::abs(x) < 1e-9) { + return 1.0 - 1.0 / 6.0 * x * x; + } else { + return std::sin(x) / x; + } +} + +RamseteController::RamseteController(double b, double zeta) + : m_b{b}, m_zeta{zeta} {} + +bool RamseteController::AtReference() const { + const auto& eTranslate = m_poseError.Translation(); + const auto& eRotate = m_poseError.Rotation(); + const auto& tolTranslate = m_poseTolerance.Translation(); + const auto& tolRotate = m_poseTolerance.Rotation(); + return units::math::abs(eTranslate.X()) < tolTranslate.X() && + units::math::abs(eTranslate.Y()) < tolTranslate.Y() && + units::math::abs(eRotate.Radians()) < tolRotate.Radians(); +} + +void RamseteController::SetTolerance(const Pose2d& poseTolerance) { + m_poseTolerance = poseTolerance; +} + +ChassisSpeeds RamseteController::Calculate( + const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + units::radians_per_second_t angularVelocityRef) { + m_poseError = poseRef.RelativeTo(currentPose); + + // Aliases for equation readability + double eX = m_poseError.Translation().X().to(); + double eY = m_poseError.Translation().Y().to(); + double eTheta = m_poseError.Rotation().Radians().to(); + double vRef = linearVelocityRef.to(); + double omegaRef = angularVelocityRef.to(); + + double k = + 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2)); + + units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX}; + units::radians_per_second_t omega{omegaRef + k * eTheta + + m_b * vRef * Sinc(eTheta) * eY}; + return ChassisSpeeds{v, 0_mps, omega}; +} + +ChassisSpeeds RamseteController::Calculate( + const Pose2d& currentPose, const Trajectory::State& desiredState) { + return Calculate(currentPose, desiredState.pose, desiredState.velocity, + desiredState.velocity * desiredState.curvature); +} diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h new file mode 100644 index 0000000000..3a564aacdd --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/trajectory/Trajectory.h" + +namespace frc { + +/** + * Ramsete is a nonlinear time-varying feedback controller for unicycle models + * that drives the model to a desired pose along a two-dimensional trajectory. + * Why would we need a nonlinear control law in addition to the linear ones we + * have used so far like PID? If we use the original approach with PID + * controllers for left and right position and velocity states, the controllers + * only deal with the local pose. If the robot deviates from the path, there is + * no way for the controllers to correct and the robot may not reach the desired + * global pose. This is due to multiple endpoints existing for the robot which + * have the same encoder path arc lengths. + * + * Instead of using wheel path arc lengths (which are in the robot's local + * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use + * global pose. The controller uses this extra information to guide a linear + * reference tracker like the PID controllers back in by adjusting the + * references of the PID controllers. + * + * The paper "Control of Wheeled Mobile Robots: An Experimental Overview" + * describes a nonlinear controller for a wheeled vehicle with unicycle-like + * kinematics; a global pose consisting of x, y, and theta; and a desired pose + * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the + * acronym for the title of the book it came from in Italian ("Robotica + * Articolata e Mobile per i SErvizi e le TEcnologie"). + * + * See section + * on Ramsete unicycle controller for a derivation and analysis. + */ +class RamseteController { + public: + /** + * Construct a Ramsete unicycle controller. + * + * @param b Tuning parameter (b > 0) for which larger values make + * convergence more aggressive like a proportional term. + * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide + * more damping in response. + */ + RamseteController(double b, double zeta); + + /** + * Returns true if the pose error is within tolerance of the reference. + */ + bool AtReference() const; + + /** + * Sets the pose error which is considered tolerable for use with + * AtReference(). + * + * @param poseTolerance Pose error which is tolerable. + */ + void SetTolerance(const Pose2d& poseTolerance); + + /** + * Returns the next output of the Ramsete controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param poseRef The desired pose. + * @param linearVelocityRef The desired linear velocity. + * @param angularVelocityRef The desired angular velocity. + */ + ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + units::radians_per_second_t angularVelocityRef); + + /** + * Returns the next output of the Ramsete controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param desiredState The desired pose, linear velocity, and angular velocity + * from a trajectory. + */ + ChassisSpeeds Calculate(const Pose2d& currentPose, + const Trajectory::State& desiredState); + + private: + double m_b; + double m_zeta; + + Pose2d m_poseError; + Pose2d m_poseTolerance; +}; + +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp new file mode 100644 index 0000000000..d54c034a3b --- /dev/null +++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/controller/RamseteController.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +static constexpr units::meter_t kTolerance{1 / 12.0}; +static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0}; + +units::radian_t boundRadians(units::radian_t value) { + while (value > units::radian_t{wpi::math::pi}) { + value -= units::radian_t{wpi::math::pi * 2}; + } + while (value <= units::radian_t{-wpi::math::pi}) { + value += units::radian_t{wpi::math::pi * 2}; + } + return value; +} + +TEST(RamseteControllerTest, ReachesReference) { + frc::RamseteController controller{2.0, 0.7}; + frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}}; + + auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, + frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {}, 0_mps, 0_mps, 8.8_mps, 0.1_mps_sq, false); + + constexpr auto kDt = 0.02_s; + auto totalTime = trajectory.TotalTime(); + for (size_t i = 0; i < (totalTime / kDt).to(); ++i) { + auto state = trajectory.Sample(kDt * i); + auto [vx, vy, omega] = controller.Calculate(robotPose, state); + static_cast(vy); + + robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt}); + } + + auto& endPose = trajectory.States().back().pose; + EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(), + kTolerance); + EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(), + kTolerance); + EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() - + robotPose.Rotation().Radians()), + 0_rad, kAngularTolerance); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java new file mode 100644 index 0000000000..77b7688e2a --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java @@ -0,0 +1,150 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +/** + * Ramsete is a nonlinear time-varying feedback controller for unicycle models + * that drives the model to a desired pose along a two-dimensional trajectory. + * Why would we need a nonlinear control law in addition to the linear ones we + * have used so far like PID? If we use the original approach with PID + * controllers for left and right position and velocity states, the controllers + * only deal with the local pose. If the robot deviates from the path, there is + * no way for the controllers to correct and the robot may not reach the desired + * global pose. This is due to multiple endpoints existing for the robot which + * have the same encoder path arc lengths. + * + *

Instead of using wheel path arc lengths (which are in the robot's local + * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use + * global pose. The controller uses this extra information to guide a linear + * reference tracker like the PID controllers back in by adjusting the + * references of the PID controllers. + * + *

The paper "Control of Wheeled Mobile Robots: An Experimental Overview" + * describes a nonlinear controller for a wheeled vehicle with unicycle-like + * kinematics; a global pose consisting of x, y, and theta; and a desired pose + * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the + * acronym for the title of the book it came from in Italian ("Robotica + * Articolata e Mobile per i SErvizi e le TEcnologie"). + * + *

See + * + * Controls Engineering in the FIRST Robotics Competition section on Ramsete + * unicycle controller for a derivation and analysis. + */ +public class RamseteController { + @SuppressWarnings("MemberName") + private final double m_b; + @SuppressWarnings("MemberName") + private final double m_zeta; + + private Pose2d m_poseError = new Pose2d(); + private Pose2d m_poseTolerance = new Pose2d(); + + /** + * Construct a Ramsete unicycle controller. + * + * @param b Tuning parameter (b > 0) for which larger values make convergence more + * aggressive like a proportional term. + * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide more damping + * in response. + */ + @SuppressWarnings("ParameterName") + public RamseteController(double b, double zeta) { + m_b = b; + m_zeta = zeta; + } + + /** + * Returns true if the pose error is within tolerance of the reference. + */ + public boolean atReference() { + final var eTranslate = m_poseError.getTranslation(); + final var eRotate = m_poseError.getRotation(); + final var tolTranslate = m_poseTolerance.getTranslation(); + final var tolRotate = m_poseTolerance.getRotation(); + return Math.abs(eTranslate.getX()) < tolTranslate.getX() + && Math.abs(eTranslate.getY()) < tolTranslate.getY() + && Math.abs(eRotate.getRadians()) < tolRotate.getRadians(); + } + + /** + * Sets the pose error which is considered tolerable for use with + * atReference(). + * + * @param poseTolerance Pose error which is tolerable. + */ + public void setTolerance(Pose2d poseTolerance) { + m_poseTolerance = poseTolerance; + } + + /** + * Returns the next output of the Ramsete controller. + * + *

The reference pose, linear velocity, and angular velocity should come + * from a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param poseRef The desired pose. + * @param linearVelocityRefMeters The desired linear velocity in meters. + * @param angularVelocityRefRadiansPerSecond The desired angular velocity in meters. + */ + @SuppressWarnings("LocalVariableName") + public ChassisSpeeds calculate(Pose2d currentPose, + Pose2d poseRef, + double linearVelocityRefMeters, + double angularVelocityRefRadiansPerSecond) { + m_poseError = poseRef.relativeTo(currentPose); + + // Aliases for equation readability + final double eX = m_poseError.getTranslation().getX(); + final double eY = m_poseError.getTranslation().getY(); + final double eTheta = m_poseError.getRotation().getRadians(); + final double vRef = linearVelocityRefMeters; + final double omegaRef = angularVelocityRefRadiansPerSecond; + + double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2)); + + return new ChassisSpeeds(vRef * m_poseError.getRotation().getCos() + k * eX, + 0.0, + omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY); + } + + /** + * Returns the next output of the Ramsete controller. + * + *

The reference pose, linear velocity, and angular velocity should come + * from a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param desiredState The desired pose, linear velocity, and angular velocity + * from a trajectory. + */ + @SuppressWarnings("LocalVariableName") + public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) { + return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, + desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter); + } + + /** + * Returns sin(x) / x. + * + * @param x Value of which to take sinc(x). + */ + @SuppressWarnings("ParameterName") + private static double sinc(double x) { + if (Math.abs(x) < 1e-9) { + return 1.0 - 1.0 / 6.0 * x * x; + } else { + return Math.sin(x) / x; + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java new file mode 100644 index 0000000000..b8447299df --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import java.util.ArrayList; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Twist2d; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class RamseteControllerTest { + private static final double kTolerance = 1 / 12.0; + private static final double kAngularTolerance = Math.toRadians(2); + + @Test + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + void testReachesReference() { + final var controller = new RamseteController(2.0, 0.7); + var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0)); + + final var waypoints = new ArrayList(); + waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0))); + waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846))); + final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, + new ArrayList(), 0, 0, 8.8, 0.1, false); + + final double kDt = 0.02; + final var totalTime = trajectory.getTotalTimeSeconds(); + for (int i = 0; i < (totalTime / kDt); ++i) { + var state = trajectory.sample(kDt * i); + + var output = controller.calculate(robotPose, state); + robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt, 0, + output.omegaRadiansPerSecond * kDt)); + } + + final var states = trajectory.getStates(); + final var endPose = states.get(states.size() - 1).poseMeters; + + // Java lambdas require local variables referenced from a lambda expression + // must be final or effectively final. + final var finalRobotPose = robotPose; + assertAll( + () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(), + kTolerance), + () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(), + kTolerance), + () -> assertEquals(0.0, + boundRadians(endPose.getRotation().getRadians() + - finalRobotPose.getRotation().getRadians()), + kAngularTolerance) + ); + } + + private static double boundRadians(double value) { + while (value > Math.PI) { + value -= Math.PI * 2; + } + while (value <= -Math.PI) { + value += Math.PI * 2; + } + return value; + } +}