diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..c96ec9873d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "Drivetrain.h" + +#include + +#include "ExampleGlobalMeasurementSensor.h" + +void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { + const auto leftFeedforward = m_feedforward.Calculate(speeds.left); + const auto rightFeedforward = m_feedforward.Calculate(speeds.right); + const double leftOutput = m_leftPIDController.Calculate( + m_leftEncoder.GetRate(), speeds.left.to()); + const double rightOutput = m_rightPIDController.Calculate( + m_rightEncoder.GetRate(), speeds.right.to()); + + m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); + m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot) { + SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); +} + +void Drivetrain::UpdateOdometry() { + m_poseEstimator.Update(m_gyro.GetRotation2d(), + {units::meters_per_second_t(m_leftEncoder.GetRate()), + units::meters_per_second_t(m_rightEncoder.GetRate())}, + units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())); + + // Also apply vision measurements. We use 0.3 seconds in the past as an + // example -- on a real robot, this must be calculated based either on latency + // or timestamps. + m_poseEstimator.AddVisionMeasurement( + ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose( + m_poseEstimator.GetEstimatedPosition()), + frc2::Timer::GetFPGATimestamp() - 0.3_s); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp new file mode 100644 index 0000000000..22d1cf614d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + TeleopPeriodic(); + m_drive.UpdateOdometry(); + } + + void TeleopPeriodic() override { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = -m_speedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * + Drivetrain::kMaxAngularSpeed; + + m_drive.Drive(xSpeed, rot); + } + + private: + frc::XboxController m_controller{0}; + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_speedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + + Drivetrain m_drive; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h new file mode 100644 index 0000000000..ee9f65d6b8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Represents a differential drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { + m_gyro.Reset(); + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.SetDistancePerPulse( + 2 * wpi::math::pi * kWheelRadius.to() / kEncoderResolution); + m_rightEncoder.SetDistancePerPulse( + 2 * wpi::math::pi * kWheelRadius.to() / kEncoderResolution); + + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + } + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); + void Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot); + void UpdateOdometry(); + + private: + static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr units::meter_t kWheelRadius = 0.0508_m; + static constexpr int kEncoderResolution = 4096; + + frc::PWMVictorSPX m_leftLeader{1}; + frc::PWMVictorSPX m_leftFollower{2}; + frc::PWMVictorSPX m_rightLeader{3}; + frc::PWMVictorSPX m_rightFollower{4}; + + frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower}; + frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower}; + + frc::Encoder m_leftEncoder{0, 1}; + frc::Encoder m_rightEncoder{2, 3}; + + frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::DifferentialDrivePoseEstimator m_poseEstimator{ + frc::Rotation2d(), + frc::Pose2d(), + {0.01, 0.01, 0.01, 0.01, 0.01}, + {0.1, 0.1, 0.1}, + {0.1, 0.1, 0.1}}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h new file mode 100644 index 0000000000..545c14eda1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +/** + * This dummy class represents a global measurement sensor, such as a computer + * vision solution. + */ +class ExampleGlobalMeasurementSensor { + public: + static frc::Pose2d GetEstimatedGlobalPose( + const frc::Pose2d& estimatedRobotPose) { + auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1); + return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)), + estimatedRobotPose.Y() + units::meter_t(randVec(1)), + estimatedRobotPose.Rotation() + + frc::Rotation2d(units::radian_t(randVec(3)))); + } +}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..8395a84975 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "Drivetrain.h" + +#include + +#include "ExampleGlobalMeasurementSensor.h" + +frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { + return {units::meters_per_second_t(m_frontLeftEncoder.GetRate()), + units::meters_per_second_t(m_frontRightEncoder.GetRate()), + units::meters_per_second_t(m_backLeftEncoder.GetRate()), + units::meters_per_second_t(m_backRightEncoder.GetRate())}; +} + +void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { + std::function + calcAndSetSpeeds = + [&m_feedforward = m_feedforward](units::meters_per_second_t speed, + const auto& encoder, + auto& controller, auto& motor) { + auto feedforward = m_feedforward.Calculate(speed); + double output = + controller.Calculate(encoder.GetRate(), speed.to()); + motor.SetVoltage(units::volt_t{output} + feedforward); + }; + + calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder, + m_frontLeftPIDController, m_frontLeftMotor); + calcAndSetSpeeds(wheelSpeeds.frontRight, m_frontRightEncoder, + m_frontRightPIDController, m_frontRightMotor); + calcAndSetSpeeds(wheelSpeeds.rearLeft, m_backLeftEncoder, + m_backLeftPIDController, m_backLeftMotor); + calcAndSetSpeeds(wheelSpeeds.rearRight, m_backRightEncoder, + m_backRightPIDController, m_backRightMotor); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, bool fieldRelative) { + auto wheelSpeeds = m_kinematics.ToWheelSpeeds( + fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) + : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); + wheelSpeeds.Normalize(kMaxSpeed); + SetSpeeds(wheelSpeeds); +} + +void Drivetrain::UpdateOdometry() { + m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an + // example -- on a real robot, this must be calculated based either on latency + // or timestamps. + m_poseEstimator.AddVisionMeasurement( + ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose( + m_poseEstimator.GetEstimatedPosition()), + frc2::Timer::GetFPGATimestamp() - 0.3_s); +} diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp new file mode 100644 index 0000000000..a9695c4cd4 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + DriveWithJoystick(false); + m_mecanum.UpdateOdometry(); + } + + void TeleopPeriodic() override { DriveWithJoystick(true); } + + private: + frc::XboxController m_controller{0}; + Drivetrain m_mecanum; + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_xspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_yspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + + void DriveWithJoystick(bool fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = -m_xspeedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + Drivetrain::kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + const auto ySpeed = -m_yspeedLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kLeftHand)) * + Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * + Drivetrain::kMaxAngularSpeed; + + m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h new file mode 100644 index 0000000000..6c4b485e11 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Represents a mecanum drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { m_gyro.Reset(); } + + frc::MecanumDriveWheelSpeeds GetCurrentState() const; + void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); + void Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, units::radians_per_second_t rot, + bool fieldRelative); + void UpdateOdometry(); + + static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + private: + frc::PWMVictorSPX m_frontLeftMotor{1}; + frc::PWMVictorSPX m_frontRightMotor{2}; + frc::PWMVictorSPX m_backLeftMotor{3}; + frc::PWMVictorSPX m_backRightMotor{4}; + + frc::Encoder m_frontLeftEncoder{0, 1}; + frc::Encoder m_frontRightEncoder{2, 3}; + frc::Encoder m_backLeftEncoder{4, 5}; + frc::Encoder m_backRightEncoder{6, 7}; + + frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m}; + frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m}; + frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m}; + frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m}; + + frc2::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_frontRightPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_backLeftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_backRightPIDController{1.0, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::MecanumDriveKinematics m_kinematics{ + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, + m_backRightLocation}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::MecanumDrivePoseEstimator m_poseEstimator{ + frc::Rotation2d(), frc::Pose2d(), m_kinematics, + {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h new file mode 100644 index 0000000000..545c14eda1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +/** + * This dummy class represents a global measurement sensor, such as a computer + * vision solution. + */ +class ExampleGlobalMeasurementSensor { + public: + static frc::Pose2d GetEstimatedGlobalPose( + const frc::Pose2d& estimatedRobotPose) { + auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1); + return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)), + estimatedRobotPose.Y() + units::meter_t(randVec(1)), + estimatedRobotPose.Rotation() + + frc::Rotation2d(units::radian_t(randVec(3)))); + } +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..5c8cfdda7e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "Drivetrain.h" + +#include + +#include "ExampleGlobalMeasurementSensor.h" + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, bool fieldRelative) { + auto states = m_kinematics.ToSwerveModuleStates( + fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) + : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); + + m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed); + + auto [fl, fr, bl, br] = states; + + m_frontLeft.SetDesiredState(fl); + m_frontRight.SetDesiredState(fr); + m_backLeft.SetDesiredState(bl); + m_backRight.SetDesiredState(br); +} + +void Drivetrain::UpdateOdometry() { + m_poseEstimator.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(), + m_frontRight.GetState(), m_backLeft.GetState(), + m_backRight.GetState()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an + // example -- on a real robot, this must be calculated based either on latency + // or timestamps. + m_poseEstimator.AddVisionMeasurement( + ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose( + m_poseEstimator.GetEstimatedPosition()), + frc2::Timer::GetFPGATimestamp() - 0.3_s); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp new file mode 100644 index 0000000000..10e550b09b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + DriveWithJoystick(false); + m_swerve.UpdateOdometry(); + } + + void TeleopPeriodic() override { DriveWithJoystick(true); } + + private: + frc::XboxController m_controller{0}; + Drivetrain m_swerve; + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_xspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_yspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + + void DriveWithJoystick(bool fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = -m_xspeedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + Drivetrain::kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + const auto ySpeed = -m_yspeedLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kLeftHand)) * + Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * + Drivetrain::kMaxAngularSpeed; + + m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp new file mode 100644 index 0000000000..790a54cc26 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "SwerveModule.h" + +#include +#include + +SwerveModule::SwerveModule(const int driveMotorChannel, + const int turningMotorChannel) + : m_driveMotor(driveMotorChannel), m_turningMotor(turningMotorChannel) { + // Set the distance per pulse for the drive encoder. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_driveEncoder.SetDistancePerPulse( + 2 * wpi::math::pi * kWheelRadius.to() / kEncoderResolution); + + // Set the distance (in this case, angle) per pulse for the turning encoder. + // This is the the angle through an entire rotation (2 * wpi::math::pi) + // divided by the encoder resolution. + m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution); + + // Limit the PID Controller's input range between -pi and pi and set the input + // to be continuous. + m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi), + units::radian_t(wpi::math::pi)); +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return {units::meters_per_second_t{m_driveEncoder.GetRate()}, + frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))}; +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) { + // Calculate the drive output from the drive PID controller. + const auto driveOutput = m_drivePIDController.Calculate( + m_driveEncoder.GetRate(), state.speed.to()); + + const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed); + + // Calculate the turning motor output from the turning PID controller. + const auto turnOutput = m_turningPIDController.Calculate( + units::radian_t(m_turningEncoder.Get()), state.angle.Radians()); + + const auto turnFeedforward = m_turnFeedforward.Calculate( + m_turningPIDController.GetSetpoint().velocity); + + // Set the motor outputs. + m_driveMotor.SetVoltage(units::volt_t{driveOutput} + driveFeedforward); + m_turningMotor.SetVoltage(units::volt_t{turnOutput} + turnFeedforward); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h new file mode 100644 index 0000000000..da11c0435c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include + +#include "SwerveModule.h" + +/** + * Represents a swerve drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { m_gyro.Reset(); } + + void Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, units::radians_per_second_t rot, + bool fieldRelative); + void UpdateOdometry(); + + static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + private: + frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m}; + frc::Translation2d m_frontRightLocation{+0.381_m, -0.381_m}; + frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m}; + frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m}; + + SwerveModule m_frontLeft{1, 2}; + SwerveModule m_frontRight{2, 3}; + SwerveModule m_backLeft{5, 6}; + SwerveModule m_backRight{7, 8}; + + frc::AnalogGyro m_gyro{0}; + + frc::SwerveDriveKinematics<4> m_kinematics{ + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, + m_backRightLocation}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::SwerveDrivePoseEstimator<4> m_poseEstimator{ + frc::Rotation2d(), frc::Pose2d(), m_kinematics, + {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h new file mode 100644 index 0000000000..545c14eda1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +/** + * This dummy class represents a global measurement sensor, such as a computer + * vision solution. + */ +class ExampleGlobalMeasurementSensor { + public: + static frc::Pose2d GetEstimatedGlobalPose( + const frc::Pose2d& estimatedRobotPose) { + auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1); + return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)), + estimatedRobotPose.Y() + units::meter_t(randVec(1)), + estimatedRobotPose.Rotation() + + frc::Rotation2d(units::radian_t(randVec(3)))); + } +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h new file mode 100644 index 0000000000..dab59a43d9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SwerveModule { + public: + SwerveModule(int driveMotorChannel, int turningMotorChannel); + frc::SwerveModuleState GetState() const; + void SetDesiredState(const frc::SwerveModuleState& state); + + private: + static constexpr auto kWheelRadius = 0.0508_m; + static constexpr int kEncoderResolution = 4096; + + static constexpr auto kModuleMaxAngularVelocity = + wpi::math::pi * 1_rad_per_s; // radians per second + static constexpr auto kModuleMaxAngularAcceleration = + wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2 + + frc::PWMVictorSPX m_driveMotor; + frc::PWMVictorSPX m_turningMotor; + + frc::Encoder m_driveEncoder{0, 1}; + frc::Encoder m_turningEncoder{2, 3}; + + frc2::PIDController m_drivePIDController{1.0, 0, 0}; + frc::ProfiledPIDController m_turningPIDController{ + 1.0, + 0.0, + 0.0, + {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}}; + + frc::SimpleMotorFeedforward m_driveFeedforward{1_V, + 3_V / 1_mps}; + frc::SimpleMotorFeedforward m_turnFeedforward{ + 1_V, 0.5_V / 1_rad_per_s}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 93bfad462a..138843a64a 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -628,6 +628,38 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "DifferentialDrivePoseEstimator", + "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.", + "tags": [ + "Drivetrain", + "State space", + "Vision", + "Filter", + "Odometry", + "Pose" + ], + "foldername": "DifferentialDrivePoseEstimator", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "MecanumDrivePoseEstimator", + "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum odometry.", + "tags": [ + "Drivetrain", + "State space", + "Vision", + "Filter", + "Odometry", + "Pose" + ], + "foldername": "MecanumDrivePoseEstimator", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "ArmSimulation", "description": "Demonstrates the use of physics simulation with a simple single-jointed arm.", @@ -661,5 +693,22 @@ "gradlebase": "cpp", "mainclass": "Main", "commandversion": 2 + }, + { + "name": "SwerveDrivePoseEstimator", + "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for mecanum drive odometry.", + "tags": [ + "Drivetrain", + "State space", + "Vision", + "Filter", + "Odometry", + "State", + "Swerve" + ], + "foldername": "SwerveDrivePoseEstimator", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java new file mode 100644 index 0000000000..2bf3b6fb1b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -0,0 +1,133 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.differentialdriveposeestimator; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; + +import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor; + +/** + * Represents a differential drive style drivetrain. + */ +public class Drivetrain { + public static final double kMaxSpeed = 3.0; // meters per second + public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second + + private static final double kTrackWidth = 0.381 * 2; // meters + private static final double kWheelRadius = 0.0508; // meters + private static final int kEncoderResolution = 4096; + + private final SpeedController m_leftLeader = new PWMVictorSPX(1); + private final SpeedController m_leftFollower = new PWMVictorSPX(2); + private final SpeedController m_rightLeader = new PWMVictorSPX(3); + private final SpeedController m_rightFollower = new PWMVictorSPX(4); + + private final Encoder m_leftEncoder = new Encoder(0, 1); + private final Encoder m_rightEncoder = new Encoder(2, 3); + + private final SpeedControllerGroup m_leftGroup + = new SpeedControllerGroup(m_leftLeader, m_leftFollower); + private final SpeedControllerGroup m_rightGroup + = new SpeedControllerGroup(m_rightLeader, m_rightFollower); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final PIDController m_leftPIDController = new PIDController(1, 0, 0); + private final PIDController m_rightPIDController = new PIDController(1, 0, 0); + + private final DifferentialDriveKinematics m_kinematics + = new DifferentialDriveKinematics(kTrackWidth); + + /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The + numbers used below are robot specific, and should be tuned. */ + private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator( + m_gyro.getRotation2d(), + new Pose2d(), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01), + VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); + + // Gains are for example purposes only - must be determined for your own robot! + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + + /** + * Constructs a differential drive object. + * Sets the encoder distance per pulse and resets the gyro. + */ + public Drivetrain() { + m_gyro.reset(); + + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Sets the desired wheel speeds. + * + * @param speeds The desired wheel speeds. + */ + public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + + final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), + speeds.leftMetersPerSecond); + final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), + speeds.rightMetersPerSecond); + m_leftGroup.setVoltage(leftOutput + leftFeedforward); + m_rightGroup.setVoltage(rightOutput + rightFeedforward); + } + + /** + * Drives the robot with the given linear velocity and angular velocity. + * + * @param xSpeed Linear velocity in m/s. + * @param rot Angular velocity in rad/s. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double rot) { + var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot)); + setSpeeds(wheelSpeeds); + } + + /** + * Updates the field-relative position. + */ + public void updateOdometry() { + m_poseEstimator.update(m_gyro.getRotation2d(), + new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), + m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on + // a real robot, this must be calculated based either on latency or timestamps. + m_poseEstimator.addVisionMeasurement( + ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( + m_poseEstimator.getEstimatedPosition()), + Timer.getFPGATimestamp() - 0.3); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java new file mode 100644 index 0000000000..dfb662c4d9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.differentialdriveposeestimator; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java new file mode 100644 index 0000000000..102c4d4e97 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.differentialdriveposeestimator; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + private final Drivetrain m_drive = new Drivetrain(); + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + + + @Override + public void autonomousPeriodic() { + teleopPeriodic(); + m_drive.updateOdometry(); + } + + @Override + public void teleopPeriodic() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + final var xSpeed = + -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) + * Drivetrain.kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + final var rot = + -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; + + m_drive.drive(xSpeed, rot); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 5bc23ab761..587fd6b5dd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -680,5 +680,56 @@ "gradlebase": "java", "mainclass": "Main", "commandversion": 2 + }, + { + "name": "DifferentialDrivePoseEstimator", + "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.", + "tags": [ + "Drivetrain", + "State space", + "Vision", + "Filter", + "Odometry", + "Pose", + "Differential drive" + ], + "foldername": "differentialdriveposeestimator", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "MecanumDrivePoseEstimator", + "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum drive odometry.", + "tags": [ + "Drivetrain", + "State space", + "Vision", + "Filter", + "Odometry", + "Pose", + "Mecanum" + ], + "foldername": "mecanumdriveposeestimator", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "SwerveDrivePoseEstimator", + "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for swerve drive odometry.", + "tags": [ + "Drivetrain", + "State space", + "Vision", + "Filter", + "Odometry", + "Pose", + "Swerve" + ], + "foldername": "swervesdriveposeestimator", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java new file mode 100644 index 0000000000..7a4e5e5ea8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -0,0 +1,158 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.mecanumdriveposeestimator; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.estimator.MecanumDrivePoseEstimator; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; + +import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor; + +/** + * Represents a mecanum drive style drivetrain. + */ +@SuppressWarnings("PMD.TooManyFields") +public class Drivetrain { + public static final double kMaxSpeed = 3.0; // 3 meters per second + public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second + + private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1); + private final SpeedController m_frontRightMotor = new PWMVictorSPX(2); + private final SpeedController m_backLeftMotor = new PWMVictorSPX(3); + private final SpeedController m_backRightMotor = new PWMVictorSPX(4); + + private final Encoder m_frontLeftEncoder = new Encoder(0, 1); + private final Encoder m_frontRightEncoder = new Encoder(2, 3); + private final Encoder m_backLeftEncoder = new Encoder(4, 5); + private final Encoder m_backRightEncoder = new Encoder(6, 7); + + private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0); + private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0); + private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0); + private final PIDController m_backRightPIDController = new PIDController(1, 0, 0); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + ); + + /* Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers used + below are robot specific, and should be tuned. */ + private final MecanumDrivePoseEstimator m_poseEstimator = new MecanumDrivePoseEstimator( + m_gyro.getRotation2d(), + new Pose2d(), + m_kinematics, + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(Units.degreesToRadians(0.01)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); + + // Gains are for example purposes only - must be determined for your own robot! + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + + /** + * Constructs a MecanumDrive and resets the gyro. + */ + public Drivetrain() { + m_gyro.reset(); + } + + /** + * Returns the current state of the drivetrain. + * + * @return The current state of the drivetrain. + */ + public MecanumDriveWheelSpeeds getCurrentState() { + return new MecanumDriveWheelSpeeds( + m_frontLeftEncoder.getRate(), + m_frontRightEncoder.getRate(), + m_backLeftEncoder.getRate(), + m_backRightEncoder.getRate() + ); + } + + /** + * Set the desired speeds for each wheel. + * + * @param speeds The desired wheel speeds. + */ + public void setSpeeds(MecanumDriveWheelSpeeds speeds) { + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); + + final double frontLeftOutput = m_frontLeftPIDController.calculate( + m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond + ); + final double frontRightOutput = m_frontRightPIDController.calculate( + m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond + ); + final double backLeftOutput = m_backLeftPIDController.calculate( + m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond + ); + final double backRightOutput = m_backRightPIDController.calculate( + m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond + ); + + m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward); + m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward); + m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward); + m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d() + ) : new ChassisSpeeds(xSpeed, ySpeed, rot) + ); + mecanumDriveWheelSpeeds.normalize(kMaxSpeed); + setSpeeds(mecanumDriveWheelSpeeds); + } + + /** + * Updates the field relative position of the robot. + */ + public void updateOdometry() { + m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on + // a real robot, this must be calculated based either on latency or timestamps. + m_poseEstimator.addVisionMeasurement( + ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( + m_poseEstimator.getEstimatedPosition()), + Timer.getFPGATimestamp() - 0.3); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java new file mode 100644 index 0000000000..4df8bd5a53 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.examples.mecanumdriveposeestimator; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; + +/** + * This dummy class represents a global measurement sensor, such as a computer vision solution. + */ +public final class ExampleGlobalMeasurementSensor { + private ExampleGlobalMeasurementSensor() { + // Utility class + } + + /** + * Get a "noisy" fake global pose reading. + * + * @param estimatedRobotPose The robot pose. + */ + public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) { + var rand = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, + Units.degreesToRadians(30))); + return new Pose2d(estimatedRobotPose.getX() + rand.get(0, 0), + estimatedRobotPose.getY() + rand.get(1, 0), + estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0)))); + } + +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java new file mode 100644 index 0000000000..ca33532b6b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.mecanumdriveposeestimator; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java new file mode 100644 index 0000000000..5475ceea75 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.mecanumdriveposeestimator; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + private final Drivetrain m_mecanum = new Drivetrain(); + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + + @Override + public void autonomousPeriodic() { + driveWithJoystick(false); + m_mecanum.updateOdometry(); + } + + @Override + public void teleopPeriodic() { + driveWithJoystick(true); + } + + private void driveWithJoystick(boolean fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + final var xSpeed = + -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) + * Drivetrain.kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + final var ySpeed = + -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft)) + * Drivetrain.kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + final var rot = + -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; + + m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java new file mode 100644 index 0000000000..0cd4232dc8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java @@ -0,0 +1,99 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.swervesdriveposeestimator; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; + +/** + * Represents a swerve drive style drivetrain. + */ +public class Drivetrain { + public static final double kMaxSpeed = 3.0; // 3 meters per second + public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second + + private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + private final SwerveModule m_frontLeft = new SwerveModule(1, 2); + private final SwerveModule m_frontRight = new SwerveModule(3, 4); + private final SwerveModule m_backLeft = new SwerveModule(5, 6); + private final SwerveModule m_backRight = new SwerveModule(7, 8); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + ); + + /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used + below are robot specific, and should be tuned. */ + private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( + m_gyro.getRotation2d(), + new Pose2d(), + m_kinematics, + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(Units.degreesToRadians(0.01)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)) + ); + + public Drivetrain() { + m_gyro.reset(); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + var swerveModuleStates = m_kinematics.toSwerveModuleStates( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rot) + ); + SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + m_frontRight.setDesiredState(swerveModuleStates[1]); + m_backLeft.setDesiredState(swerveModuleStates[2]); + m_backRight.setDesiredState(swerveModuleStates[3]); + } + + /** + * Updates the field relative position of the robot. + */ + public void updateOdometry() { + m_poseEstimator.update( + m_gyro.getRotation2d(), + m_frontLeft.getState(), + m_frontRight.getState(), + m_backLeft.getState(), + m_backRight.getState() + ); + + // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on + // a real robot, this must be calculated based either on latency or timestamps. + m_poseEstimator.addVisionMeasurement( + ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( + m_poseEstimator.getEstimatedPosition()), + Timer.getFPGATimestamp() - 0.3); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/ExampleGlobalMeasurementSensor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/ExampleGlobalMeasurementSensor.java new file mode 100644 index 0000000000..50a097b554 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/ExampleGlobalMeasurementSensor.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.examples.swervesdriveposeestimator; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; + +/** + * This dummy class represents a global measurement sensor, such as a computer vision solution. + */ +public final class ExampleGlobalMeasurementSensor { + private ExampleGlobalMeasurementSensor() { + // Utility class + } + + + /** + * Get a "noisy" fake global pose reading. + * + * @param estimatedRobotPose The robot pose. + */ + public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) { + var rand = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, + Units.degreesToRadians(30))); + return new Pose2d(estimatedRobotPose.getX() + rand.get(0, 0), + estimatedRobotPose.getY() + rand.get(1, 0), + estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0)))); + } + +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Main.java new file mode 100644 index 0000000000..3168ab65c1 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.swervesdriveposeestimator; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java new file mode 100644 index 0000000000..b96a397131 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.swervesdriveposeestimator; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + private final Drivetrain m_swerve = new Drivetrain(); + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + + @Override + public void autonomousPeriodic() { + driveWithJoystick(false); + m_swerve.updateOdometry(); + } + + @Override + public void teleopPeriodic() { + driveWithJoystick(true); + } + + private void driveWithJoystick(boolean fieldRelative) { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + final var xSpeed = + -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) + * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + + // Get the y speed or sideways/strafe speed. We are inverting this because + // we want a positive value when we pull to the left. Xbox controllers + // return positive values when you pull to the right by default. + final var ySpeed = + -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft)) + * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + final var rot = + -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed; + + m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java new file mode 100644 index 0000000000..62826371ed --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java @@ -0,0 +1,101 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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.examples.swervesdriveposeestimator; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +public class SwerveModule { + private static final double kWheelRadius = 0.0508; + private static final int kEncoderResolution = 4096; + + private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed; + private static final double kModuleMaxAngularAcceleration + = 2 * Math.PI; // radians per second squared + + private final SpeedController m_driveMotor; + private final SpeedController m_turningMotor; + + private final Encoder m_driveEncoder = new Encoder(0, 1); + private final Encoder m_turningEncoder = new Encoder(2, 3); + + private final PIDController m_drivePIDController = new PIDController(1, 0, 0); + + private final ProfiledPIDController m_turningPIDController + = new ProfiledPIDController(1, 0, 0, + new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration)); + + // Gains are for example purposes only - must be determined for your own robot! + private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3); + private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5); + + /** + * Constructs a SwerveModule. + * + * @param driveMotorChannel ID for the drive motor. + * @param turningMotorChannel ID for the turning motor. + */ + public SwerveModule(int driveMotorChannel, int turningMotorChannel) { + m_driveMotor = new PWMVictorSPX(driveMotorChannel); + m_turningMotor = new PWMVictorSPX(turningMotorChannel); + + // Set the distance per pulse for the drive encoder. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + + // Set the distance (in this case, angle) per pulse for the turning encoder. + // This is the the angle through an entire rotation (2 * wpi::math::pi) + // divided by the encoder resolution. + m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution); + + // Limit the PID Controller's input range between -pi and pi and set the input + // to be continuous. + m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get())); + } + + /** + * Sets the desired state for the module. + * + * @param state Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState state) { + // Calculate the drive output from the drive PID controller. + final double driveOutput = m_drivePIDController.calculate( + m_driveEncoder.getRate(), state.speedMetersPerSecond); + + final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond); + + // Calculate the turning motor output from the turning PID controller. + final double turnOutput = m_turningPIDController.calculate( + m_turningEncoder.get(), state.angle.getRadians() + ); + + final double turnFeedforward = + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); + + m_driveMotor.setVoltage(driveOutput + driveFeedforward); + m_turningMotor.setVoltage(turnOutput + turnFeedforward); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java new file mode 100644 index 0000000000..5b4cd63f51 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java @@ -0,0 +1,130 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + + +import java.util.function.BiFunction; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +public final class AngleStatistics { + private AngleStatistics() { + // Utility class + } + + /** + * Subtracts a and b while normalizing the resulting value in the selected row as if it were an + * angle. + * + * @param a A vector to subtract from. + * @param b A vector to subtract with. + * @param angleStateIdx The row containing angles to be normalized. + */ + @SuppressWarnings("checkstyle:ParameterName") + public static Matrix angleResidual(Matrix a, Matrix b, + int angleStateIdx) { + Matrix ret = a.minus(b); + ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0))); + + return ret; + } + + /** + * Returns a function that subtracts two vectors while normalizing the resulting value in the + * selected row as if it were an angle. + * + * @param angleStateIdx The row containing angles to be normalized. + */ + @SuppressWarnings("checkstyle:ParameterName") + public static BiFunction, Matrix, Matrix> + angleResidual(int angleStateIdx) { + return (a, b) -> angleResidual(a, b, angleStateIdx); + } + + /** + * Adds a and b while normalizing the resulting value in the selected row as an angle. + * + * @param a A vector to add with. + * @param b A vector to add with. + * @param angleStateIdx The row containing angles to be normalized. + */ + @SuppressWarnings("checkstyle:ParameterName") + public static Matrix angleAdd(Matrix a, Matrix b, + int angleStateIdx) { + Matrix ret = a.plus(b); + ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0))); + + return ret; + } + + /** + * Returns a function that adds two vectors while normalizing the resulting value in the selected + * row as an angle. + * + * @param angleStateIdx The row containing angles to be normalized. + */ + @SuppressWarnings("checkstyle:ParameterName") + public static BiFunction, Matrix, Matrix> + angleAdd(int angleStateIdx) { + return (a, b) -> angleAdd(a, b, angleStateIdx); + } + + static double normalizeAngle(double angle) { + final double tau = 2 * Math.PI; + + angle -= Math.floor(angle / tau) * tau; + if (angle > Math.PI) { + angle -= tau; + } + return angle; + } + + /** + * Computes the mean of sigmas with the weights Wm while computing a special angle mean for a + * select row. + * + * @param sigmas Sigma points. + * @param Wm Weights for the mean. + * @param angleStateIdx The row containing the angles. + */ + @SuppressWarnings("checkstyle:ParameterName") + public static Matrix angleMean(Matrix sigmas, Matrix Wm, + int angleStateIdx) { + double[] angleSigmas = sigmas.extractRowVector(angleStateIdx).getData(); + Matrix sinAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols())); + Matrix cosAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols())); + for (int i = 0; i < angleSigmas.length; i++) { + sinAngleSigmas.set(0, i, Math.sin(angleSigmas[i])); + cosAngleSigmas.set(0, i, Math.cos(angleSigmas[i])); + } + + double sumSin = sinAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum(); + double sumCos = cosAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum(); + + Matrix ret = sigmas.times(Matrix.changeBoundsUnchecked(Wm)); + ret.set(angleStateIdx, 0, Math.atan2(sumSin, sumCos)); + + return ret; + } + + /** + * Returns a function that computes the mean of sigmas with the weights Wm while computing a + * special angle mean for a select row. + * + * @param angleStateIdx The row containing the angles. + */ + @SuppressWarnings("checkstyle:ParameterName") + public static BiFunction, Matrix, Matrix> + angleMean(int angleStateIdx) { + return (sigmas, Wm) -> angleMean(sigmas, Wm, angleStateIdx); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java new file mode 100644 index 0000000000..7b926be18f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java @@ -0,0 +1,297 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.function.BiConsumer; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpiutil.WPIUtilJNI; +import edu.wpi.first.wpiutil.math.MatBuilder; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N5; + +/** + * This class wraps an + * {@link edu.wpi.first.wpilibj.estimator.UnscentedKalmanFilter Unscented Kalman Filter} + * to fuse latency-compensated vision + * measurements with differential drive encoder measurements. It will correct + * for noisy vision measurements and encoder drift. It is intended to be an easy + * drop-in for + * {@link edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry}; in fact, + * if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} + * and only call {@link DifferentialDrivePoseEstimator#update} then this will + * behave exactly the same as DifferentialDriveOdometry. + * + *

{@link DifferentialDrivePoseEstimator#update} should be called every robot + * loop (if your robot loops are faster than the default then you should change + * the {@link DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, + * Matrix, Matrix, Matrix, double) nominal delta time}.) + * {@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as + * infrequently as you want; if you never call it then this class will behave + * exactly like regular encoder odometry. + * + *

Our state-space system is: + * + *

x = [[x, y, theta, dist_l, dist_r]]^T + * in the field coordinate system (dist_* are wheel distances.) + * + *

u = [[vx, vy, omega]]^T (robot-relative velocities) + * -- NB: using velocities make things considerably easier, because it means that + * teams don't have to worry about getting an accurate model. + * Basically, we suspect that it's easier for teams to get good encoder data than it is for + * them to perform system identification well enough to get a good model. + * + *

y = [[x, y, theta]]^T from vision, + * or y = [[dist_l, dist_r, theta]] from encoders and gyro. + */ +public class DifferentialDrivePoseEstimator { + final UnscentedKalmanFilter m_observer; // Package-private to allow for unit testing + private final BiConsumer, Matrix> m_visionCorrect; + private final KalmanFilterLatencyCompensator m_latencyCompensator; + + private final double m_nominalDt; // Seconds + private double m_prevTimeSeconds = -1.0; + + private Rotation2d m_gyroOffset; + private Rotation2d m_previousAngle; + + /** + * Constructs a DifferentialDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of model states. Increase these numbers to + * trust your wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements. + * Increase these numbers to trust encoder distances and gyro + * angle less. + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase + * these numbers to trust vision less. + */ + public DifferentialDrivePoseEstimator( + Rotation2d gyroAngle, Pose2d initialPoseMeters, + Matrix stateStdDevs, + Matrix localMeasurementStdDevs, Matrix visionMeasurementStdDevs + ) { + this(gyroAngle, initialPoseMeters, + stateStdDevs, localMeasurementStdDevs, visionMeasurementStdDevs, 0.02); + } + + /** + * Constructs a DifferentialDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of model states. Increase these numbers to + * trust your wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements. + * Increase these numbers to trust encoder distances and gyro + * angle less. + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase + * these numbers to trust vision less. + * @param nominalDtSeconds The time in seconds between each robot loop. + */ + @SuppressWarnings("ParameterName") + public DifferentialDrivePoseEstimator( + Rotation2d gyroAngle, Pose2d initialPoseMeters, + Matrix stateStdDevs, + Matrix localMeasurementStdDevs, Matrix visionMeasurementStdDevs, + double nominalDtSeconds + ) { + m_nominalDt = nominalDtSeconds; + + m_observer = new UnscentedKalmanFilter<>( + Nat.N5(), Nat.N3(), + this::f, + (x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)), + stateStdDevs, localMeasurementStdDevs, + AngleStatistics.angleMean(2), + AngleStatistics.angleMean(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleAdd(2), + m_nominalDt + ); + m_latencyCompensator = new KalmanFilterLatencyCompensator<>(); + + var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); + var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt); + m_visionCorrect = (u, y) -> m_observer.correct( + Nat.N3(), u, y, + (x, u_) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)), + visionDiscR, + AngleStatistics.angleMean(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleAdd(2) + ); + + m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle); + m_previousAngle = initialPoseMeters.getRotation(); + m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0)); + } + + @SuppressWarnings({"ParameterName", "MethodName"}) + private Matrix f(Matrix x, Matrix u) { + // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us. + var theta = x.get(2, 0); + var toFieldRotation = new MatBuilder<>(Nat.N5(), Nat.N5()).fill( + Math.cos(theta), -Math.sin(theta), 0, 0, 0, + Math.sin(theta), Math.cos(theta), 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1 + ); + return toFieldRotation.times(VecBuilder.fill( + u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0) + )); + } + + /** + * Resets the robot's position on the field. + * + *

You NEED to reset your encoders (to zero) when calling this method. + * + *

The gyroscope angle does not need to be reset here on the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param poseMeters The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + */ + public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { + m_previousAngle = poseMeters.getRotation(); + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); + m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0)); + } + + /** + * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter. + * + * @return The estimated robot pose in meters. + */ + public Pose2d getEstimatedPosition() { + return new Pose2d( + m_observer.getXhat(0), + m_observer.getXhat(1), + new Rotation2d(m_observer.getXhat(2)) + ); + } + + /** + * Add a vision measurement to the Unscented Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are + * calling {@link DifferentialDrivePoseEstimator#update} every loop. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision + * camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if + * you don't use your own time source by calling + * {@link DifferentialDrivePoseEstimator#updateWithTime} then you + * must use a timestamp with an epoch since FPGA startup + * (i.e. the epoch of this timestamp is the same epoch as + * Timer.getFPGATimestamp.) This means that you should + * use Timer.getFPGATimestamp as your time source in + * this case. + */ + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + m_latencyCompensator.applyPastGlobalMeasurement( + Nat.N3(), + m_observer, m_nominalDt, + StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters), + m_visionCorrect, + timestampSeconds + ); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder information. + * Note that this should be called every loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second. + * @param distanceLeftMeters The total distance travelled by the left wheel in meters + * since the last time you called + * {@link DifferentialDrivePoseEstimator#resetPosition}. + * @param distanceRightMeters The total distance travelled by the right wheel in meters + * since the last time you called + * {@link DifferentialDrivePoseEstimator#resetPosition}. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update( + Rotation2d gyroAngle, + DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond, + double distanceLeftMeters, double distanceRightMeters + ) { + return updateWithTime( + WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelVelocitiesMetersPerSecond, + distanceLeftMeters, distanceRightMeters + ); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder information. + * Note that this should be called every loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second. + * @param distanceLeftMeters The total distance travelled by the left wheel in meters + * since the last time you called + * {@link DifferentialDrivePoseEstimator#resetPosition}. + * @param distanceRightMeters The total distance travelled by the right wheel in meters + * since the last time you called + * {@link DifferentialDrivePoseEstimator#resetPosition}. + * @return The estimated pose of the robot in meters. + */ + @SuppressWarnings({"LocalVariableName", "ParameterName"}) + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, + DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond, + double distanceLeftMeters, double distanceRightMeters + ) { + double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt; + m_prevTimeSeconds = currentTimeSeconds; + + var angle = gyroAngle.plus(m_gyroOffset); + // Diff drive forward kinematics: + // v_c = (v_l + v_r) / 2 + var wheelVels = wheelVelocitiesMetersPerSecond; + var u = VecBuilder.fill( + (wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2, 0, + angle.minus(m_previousAngle).getRadians() / dt + ); + m_previousAngle = angle; + + var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians()); + m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); + m_observer.predict(u, dt); + m_observer.correct(u, localY); + + return getEstimatedPosition(); + } + + private static Matrix fillStateVector(Pose2d pose, double leftDist, double rightDist) { + return VecBuilder.fill( + pose.getTranslation().getX(), + pose.getTranslation().getY(), + pose.getRotation().getRadians(), + leftDist, + rightDist + ); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java new file mode 100644 index 0000000000..0464ba8e04 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java @@ -0,0 +1,161 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import java.util.function.BiConsumer; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +public class KalmanFilterLatencyCompensator { + private static final int kMaxPastObserverStates = 300; + + private final List> m_pastObserverSnapshots; + + KalmanFilterLatencyCompensator() { + m_pastObserverSnapshots = new ArrayList<>(); + } + + /** + * Add past observer states to the observer snapshots list. + * + * @param observer The observer. + * @param u The input at the timestamp. + * @param localY The local output at the timestamp + * @param timestampSeconds The timesnap of the state. + */ + @SuppressWarnings("ParameterName") + public void addObserverState( + KalmanTypeFilter observer, Matrix u, Matrix localY, + double timestampSeconds + ) { + m_pastObserverSnapshots.add(Map.entry( + timestampSeconds, new ObserverSnapshot(observer, u, localY) + )); + + if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) { + m_pastObserverSnapshots.remove(0); + } + } + + /** + * Add past global measurements (such as from vision)to the estimator. + * + * @param The rows in the global measurement vector. + * @param rows The rows in the global measurement vector. + * @param observer The observer to apply the past global measurement. + * @param nominalDtSeconds The nominal timestep. + * @param globalMeasurement The measurement. + * @param globalMeasurementCorrect The function take calls correct() on the observer. + * @param globalMeasurementTimestampSeconds The timestamp of the measurement. + */ + @SuppressWarnings({"ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public void applyPastGlobalMeasurement( + Nat rows, + KalmanTypeFilter observer, + double nominalDtSeconds, + Matrix globalMeasurement, + BiConsumer, Matrix> globalMeasurementCorrect, + double globalMeasurementTimestampSeconds + ) { + if (m_pastObserverSnapshots.isEmpty()) { + // State map was empty, which means that we got a past measurement right at startup. The only + // thing we can really do is ignore the measurement. + return; + } + + // This index starts at one because we use the previous state later on, and we always want to + // have a "previous state". + int maxIdx = m_pastObserverSnapshots.size() - 1; + int low = 1; + int high = Math.max(maxIdx, 1); + + while (low != high) { + int mid = (low + high) / 2; + if (m_pastObserverSnapshots.get(mid).getKey() < globalMeasurementTimestampSeconds) { + // This index and everything under it are less than the requested timestamp. Therefore, we + // can discard them. + low = mid + 1; + } else { + // t is at least as large as the element at this index. This means that anything after it + // cannot be what we are looking for. + high = mid; + } + } + + // We are simply assigning this index to a new variable to avoid confusion + // with variable names. + int index = low; + double timestamp = globalMeasurementTimestampSeconds; + int indexOfClosestEntry = + Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey()) + <= Math.abs(timestamp - m_pastObserverSnapshots.get(Math.min(index, maxIdx)).getKey()) + ? index - 1 + : index; + + double lastTimestamp = + m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds; + + // We will now go back in time to the state of the system at the time when + // the measurement was captured. We will reset the observer to that state, + // and apply correction based on the measurement. Then, we will go back + // through all observer states until the present and apply past inputs to + // get the present estimated state. + for (int i = indexOfClosestEntry; i < m_pastObserverSnapshots.size(); i++) { + var key = m_pastObserverSnapshots.get(i).getKey(); + var snapshot = m_pastObserverSnapshots.get(i).getValue(); + + if (i == indexOfClosestEntry) { + observer.setP(snapshot.errorCovariances); + observer.setXhat(snapshot.xHat); + } + + observer.predict(snapshot.inputs, key - lastTimestamp); + observer.correct(snapshot.inputs, snapshot.localMeasurements); + + if (i == indexOfClosestEntry) { + // Note that the measurement is at a timestep close but probably not exactly equal to the + // timestep for which we called predict. + // This makes the assumption that the dt is small enough that the difference between the + // measurement time and the time that the inputs were captured at is very small. + globalMeasurementCorrect.accept(snapshot.inputs, globalMeasurement); + } + lastTimestamp = key; + + m_pastObserverSnapshots.set(i, Map.entry(key, + new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements))); + } + } + + /** + * This class contains all the information about our observer at a given time. + */ + @SuppressWarnings("MemberName") + public class ObserverSnapshot { + public final Matrix xHat; + public final Matrix errorCovariances; + public final Matrix inputs; + public final Matrix localMeasurements; + + @SuppressWarnings("ParameterName") + private ObserverSnapshot( + KalmanTypeFilter observer, Matrix u, Matrix localY + ) { + this.xHat = observer.getXhat(); + this.errorCovariances = observer.getP(); + + inputs = u; + localMeasurements = localY; + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java new file mode 100644 index 0000000000..322b2333b8 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java @@ -0,0 +1,250 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.function.BiConsumer; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpiutil.WPIUtilJNI; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N3; + +/** + * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse + * latency-compensated vision measurements with mecanum drive encoder velocity measurements. + * It will correct for noisy measurements and encoder drift. It is intended to be an easy + * but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry}. + * + *

{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If + * your loops are faster or slower than the default of 0.02s, then you should change + * the nominal delta time using the secondary constructor: + * {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d, Pose2d, + * MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}. + * + *

{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as + * infrequently as you want; if you never call it, then this class will behave mostly like regular + * encoder odometry. + * + *

Our state-space system is: + * + *

x = [[x, y, theta]]^T in the field-coordinate system. + * + *

u = [[vx, vy, theta]]^T in the field-coordinate system. + * + *

y = [[x, y, theta]]^T in field coords from vision, + * or y = [[theta]]^T from the gyro. + */ +public class MecanumDrivePoseEstimator { + private final UnscentedKalmanFilter m_observer; + private final MecanumDriveKinematics m_kinematics; + private final BiConsumer, Matrix> m_visionCorrect; + private final KalmanFilterLatencyCompensator m_latencyCompensator; + + private final double m_nominalDt; // Seconds + private double m_prevTimeSeconds = -1.0; + + private Rotation2d m_gyroOffset; + private Rotation2d m_previousAngle; + + /** + * Constructs a MecanumDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param stateStdDevs Standard deviations of model states. Increase these numbers to + * trust your wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the gyro measurement. Increase this + * number to trust gyro angle measurements less. + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase + * these numbers to trust vision less. + */ + public MecanumDrivePoseEstimator( + Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics, + Matrix stateStdDevs, Matrix localMeasurementStdDevs, + Matrix visionMeasurementStdDevs + ) { + this(gyroAngle, initialPoseMeters, kinematics, stateStdDevs, localMeasurementStdDevs, + visionMeasurementStdDevs, 0.02); + } + + /** + * Constructs a MecanumDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param stateStdDevs Standard deviations of model states. Increase these numbers to + * trust your wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the gyro measurement. Increase this + * number to trust gyro angle measurements less. + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase + * these numbers to trust vision less. + * @param nominalDtSeconds The time in seconds between each robot loop. + */ + @SuppressWarnings("ParameterName") + public MecanumDrivePoseEstimator( + Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics, + Matrix stateStdDevs, Matrix localMeasurementStdDevs, + Matrix visionMeasurementStdDevs, double nominalDtSeconds + ) { + m_nominalDt = nominalDtSeconds; + + m_observer = new UnscentedKalmanFilter<>( + Nat.N3(), Nat.N1(), + (x_, u) -> u, + (x, u_) -> x.extractRowVector(2), + stateStdDevs, + localMeasurementStdDevs, + AngleStatistics.angleMean(2), + AngleStatistics.angleMean(0), + AngleStatistics.angleResidual(2), + AngleStatistics.angleResidual(0), + AngleStatistics.angleAdd(2), + m_nominalDt + ); + m_kinematics = kinematics; + m_latencyCompensator = new KalmanFilterLatencyCompensator<>(); + + var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); + var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt); + + m_visionCorrect = (u, y) -> m_observer.correct( + Nat.N3(), u, y, + (x, u_) -> x, + visionDiscR, + AngleStatistics.angleMean(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleAdd(2) + ); + + m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle); + m_previousAngle = initialPoseMeters.getRotation(); + m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters)); + } + + /** + * Resets the robot's position on the field. + * + *

You NEED to reset your encoders (to zero) when calling this method. + * + *

The gyroscope angle does not need to be reset in the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param poseMeters The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + */ + public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { + m_previousAngle = poseMeters.getRotation(); + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); + m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + } + + /** + * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter. + * + * @return The estimated robot pose in meters. + */ + public Pose2d getEstimatedPosition() { + return new Pose2d( + m_observer.getXhat(0), + m_observer.getXhat(1), + new Rotation2d(m_observer.getXhat(2)) + ); + } + + /** + * Add a vision measurement to the Unscented Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are + * calling {@link MecanumDrivePoseEstimator#update} every loop. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision + * camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if + * you don't use your own time source by calling + * {@link MecanumDrivePoseEstimator#updateWithTime} then you + * must use a timestamp with an epoch since FPGA startup + * (i.e. the epoch of this timestamp is the same epoch as + * Timer.getFPGATimestamp.) This means that you should + * use Timer.getFPGATimestamp as your time source + * or sync the epochs. + */ + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + m_latencyCompensator.applyPastGlobalMeasurement( + Nat.N3(), + m_observer, m_nominalDt, + StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters), + m_visionCorrect, + timestampSeconds + ); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder information. + * This should be called every loop, and the correct loop period must be passed + * into the constructor of this class. + * + * @param gyroAngle The current gyro angle. + * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { + return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder information. + * This should be called every loop, and the correct loop period must be passed + * into the constructor of this class. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyroscope angle. + * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @return The estimated pose of the robot in meters. + */ + @SuppressWarnings("LocalVariableName") + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, + MecanumDriveWheelSpeeds wheelSpeeds) { + double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt; + m_prevTimeSeconds = currentTimeSeconds; + + var angle = gyroAngle.plus(m_gyroOffset); + var omega = angle.minus(m_previousAngle).getRadians() / dt; + + var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + var fieldRelativeVelocities = + new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) + .rotateBy(angle); + + var u = VecBuilder.fill( + fieldRelativeVelocities.getX(), + fieldRelativeVelocities.getY(), + omega + ); + m_previousAngle = angle; + + var localY = VecBuilder.fill(angle.getRadians()); + m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); + m_observer.predict(u, dt); + m_observer.correct(u, localY); + + return getEstimatedPosition(); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java new file mode 100644 index 0000000000..591337a1f1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -0,0 +1,255 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.function.BiConsumer; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpiutil.WPIUtilJNI; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N3; + +/** + * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse + * latency-compensated vision measurements with swerve drive encoder velocity measurements. + * It will correct for noisy measurements and encoder drift. It is intended to be an easy + * but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry}. + * + *

{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If + * your loops are faster or slower than the default of 0.02s, then you should change + * the nominal delta time using the secondary constructor: + * {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d, Pose2d, + * SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}. + * + *

{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as + * infrequently as you want; if you never call it, then this class will behave mostly like regular + * encoder odometry. + * + *

Our state-space system is: + * + *

x = [[x, y, theta]]^T in the field-coordinate system. + * + *

u = [[vx, vy, omega]]^T in the field-coordinate system. + * + *

y = [[x, y, theta]]^T in field coords from vision, + * or y = [[theta]]^T from the gyro. + */ +public class SwerveDrivePoseEstimator { + private final UnscentedKalmanFilter m_observer; + private final SwerveDriveKinematics m_kinematics; + private final BiConsumer, Matrix> m_visionCorrect; + private final KalmanFilterLatencyCompensator m_latencyCompensator; + + private final double m_nominalDt; // Seconds + private double m_prevTimeSeconds = -1.0; + + private Rotation2d m_gyroOffset; + private Rotation2d m_previousAngle; + + /** + * Constructs a SwerveDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param stateStdDevs Standard deviations of model states. Increase these numbers to + * trust your wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the gyro measurement. Increase this + * number to trust gyro angle measurements less. + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase + * these numbers to trust vision less. + */ + public SwerveDrivePoseEstimator( + Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics, + Matrix stateStdDevs, Matrix localMeasurementStdDevs, + Matrix visionMeasurementStdDevs + ) { + this(gyroAngle, initialPoseMeters, kinematics, stateStdDevs, localMeasurementStdDevs, + visionMeasurementStdDevs, 0.02); + } + + /** + * Constructs a SwerveDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param stateStdDevs Standard deviations of model states. Increase these numbers to + * trust your wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the gyro measurement. Increase this + * number to trust gyro angle measurements less. + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase + * these numbers to trust vision less. + * @param nominalDtSeconds The time in seconds between each robot loop. + */ + @SuppressWarnings("ParameterName") + public SwerveDrivePoseEstimator( + Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics, + Matrix stateStdDevs, Matrix localMeasurementStdDevs, + Matrix visionMeasurementStdDevs, double nominalDtSeconds + ) { + m_nominalDt = nominalDtSeconds; + + m_observer = new UnscentedKalmanFilter<>( + Nat.N3(), Nat.N1(), + (x_, u) -> u, + (x, u_) -> x.extractRowVector(2), + stateStdDevs, + localMeasurementStdDevs, + AngleStatistics.angleMean(2), + AngleStatistics.angleMean(0), + AngleStatistics.angleResidual(2), + AngleStatistics.angleResidual(0), + AngleStatistics.angleAdd(2), + m_nominalDt + ); + m_kinematics = kinematics; + m_latencyCompensator = new KalmanFilterLatencyCompensator<>(); + + var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); + var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt); + + m_visionCorrect = (u, y) -> m_observer.correct( + Nat.N3(), u, y, + (x, u_) -> x, + visionDiscR, + AngleStatistics.angleMean(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleResidual(2), + AngleStatistics.angleAdd(2) + ); + + m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle); + m_previousAngle = initialPoseMeters.getRotation(); + m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters)); + } + + /** + * Resets the robot's position on the field. + * + *

You NEED to reset your encoders (to zero) when calling this method. + * + *

The gyroscope angle does not need to be reset in the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param poseMeters The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + */ + public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { + m_previousAngle = poseMeters.getRotation(); + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); + m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + } + + /** + * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter. + * + * @return The estimated robot pose in meters. + */ + public Pose2d getEstimatedPosition() { + return new Pose2d( + m_observer.getXhat(0), + m_observer.getXhat(1), + new Rotation2d(m_observer.getXhat(2)) + ); + } + + /** + * Add a vision measurement to the Unscented Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are + * calling {@link SwerveDrivePoseEstimator#update} every loop. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision + * camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if + * you don't use your own time source by calling + * {@link SwerveDrivePoseEstimator#updateWithTime} then you + * must use a timestamp with an epoch since FPGA startup + * (i.e. the epoch of this timestamp is the same epoch as + * Timer.getFPGATimestamp.) This means that you should + * use Timer.getFPGATimestamp as your time source or + * sync the epochs. + */ + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + m_latencyCompensator.applyPastGlobalMeasurement( + Nat.N3(), + m_observer, m_nominalDt, + StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters), + m_visionCorrect, + timestampSeconds + ); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder information. + * This should be called every loop, and the correct loop period must be passed + * into the constructor of this class. + * + * @param gyroAngle The current gyro angle. + * @param moduleStates The current velocities and rotations of the swerve modules. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update( + Rotation2d gyroAngle, + SwerveModuleState... moduleStates + ) { + return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder information. + * This should be called every loop, and the correct loop period must be passed + * into the constructor of this class. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyroscope angle. + * @param moduleStates The current velocities and rotations of the swerve modules. + * @return The estimated pose of the robot in meters. + */ + @SuppressWarnings("LocalVariableName") + public Pose2d updateWithTime( + double currentTimeSeconds, + Rotation2d gyroAngle, SwerveModuleState... moduleStates + ) { + double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt; + m_prevTimeSeconds = currentTimeSeconds; + + var angle = gyroAngle.plus(m_gyroOffset); + var omega = angle.minus(m_previousAngle).getRadians() / dt; + + var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates); + var fieldRelativeVelocities = new Translation2d( + chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond + ).rotateBy(angle); + + var u = VecBuilder.fill( + fieldRelativeVelocities.getX(), + fieldRelativeVelocities.getY(), + omega + ); + m_previousAngle = angle; + + var localY = VecBuilder.fill(angle.getRadians()); + m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); + m_observer.predict(u, dt); + m_observer.correct(u, localY); + + return getEstimatedPosition(); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java index ca99153297..39529b961a 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java @@ -31,7 +31,7 @@ import edu.wpi.first.wpiutil.math.numbers.N1; * an estimate of the true covariance (as opposed to a linearized version of it). This means that * the UKF works with nonlinear systems. */ -@SuppressWarnings({"MemberName", "ClassTypeParameterName"}) +@SuppressWarnings({"MemberName", "ClassTypeParameterName", "PMD.TooManyFields"}) public class UnscentedKalmanFilter implements KalmanTypeFilter { @@ -41,6 +41,12 @@ public class UnscentedKalmanFilter, Matrix, Matrix> m_f; private final BiFunction, Matrix, Matrix> m_h; + private BiFunction, Matrix, Matrix> m_meanFuncX; + private BiFunction, Matrix, Matrix> m_meanFuncY; + private BiFunction, Matrix, Matrix> m_residualFuncX; + private BiFunction, Matrix, Matrix> m_residualFuncY; + private BiFunction, Matrix, Matrix> m_addFuncX; + private Matrix m_xHat; private Matrix m_P; private final Matrix m_contQ; @@ -61,7 +67,7 @@ public class UnscentedKalmanFilter states, Nat outputs, @@ -71,18 +77,75 @@ public class UnscentedKalmanFilter> h, Matrix stateStdDevs, Matrix measurementStdDevs, - double dtSeconds) { + double nominalDtSeconds) { + this( + states, outputs, + f, h, + stateStdDevs, measurementStdDevs, + (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)), + (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)), + Matrix::minus, + Matrix::minus, + Matrix::plus, + nominalDtSeconds + ); + } + + /** + * Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using + * custom functions for arithmetic can be useful if you have angles in the state or measurements, + * because they allow you to correctly account for the modular nature of angle arithmetic. + * + * @param states A Nat representing the number of states. + * @param outputs A Nat representing the number of outputs. + * @param f A vector-valued function of x and u that returns + * the derivative of the state vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param meanFuncX A function that computes the mean of 2 * States + 1 state vectors + * using a given set of weights. + * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement + * vectors using a given set of weights. + * @param residualFuncX A function that computes the residual of two state vectors (i.e. it + * subtracts them.) + * @param residualFuncY A function that computes the residual of two measurement vectors + * (i.e. it subtracts them.) + * @param addFuncX A function that adds two state vectors. + * @param nominalDtSeconds Nominal discretization timestep. + */ + @SuppressWarnings({"ParameterName", "PMD.ExcessiveParameterList"}) + public UnscentedKalmanFilter( + Nat states, Nat outputs, + BiFunction, Matrix, Matrix> f, + BiFunction, Matrix, Matrix> h, + Matrix stateStdDevs, + Matrix measurementStdDevs, + BiFunction, Matrix, Matrix> meanFuncX, + BiFunction, Matrix, Matrix> meanFuncY, + BiFunction, Matrix, Matrix> residualFuncX, + BiFunction, Matrix, Matrix> residualFuncY, + BiFunction, Matrix, Matrix> addFuncX, + double nominalDtSeconds + ) { this.m_states = states; this.m_outputs = outputs; m_f = f; m_h = h; + m_meanFuncX = meanFuncX; + m_meanFuncY = meanFuncY; + m_residualFuncX = residualFuncX; + m_residualFuncY = residualFuncY; + m_addFuncX = addFuncX; + + m_dtSeconds = nominalDtSeconds; + m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); - m_dtSeconds = dtSeconds; - m_pts = new MerweScaledSigmaPoints<>(states); reset(); @@ -91,7 +154,9 @@ public class UnscentedKalmanFilter Pair, Matrix> unscentedTransform( - Nat s, Nat dim, Matrix sigmas, Matrix Wm, Matrix Wc + Nat s, Nat dim, Matrix sigmas, Matrix Wm, Matrix Wc, + BiFunction, Matrix, Matrix> meanFunc, + BiFunction, Matrix, Matrix> residualFunc ) { if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) { throw new IllegalArgumentException("Sigmas must be covDim by 2 * states + 1! Got " @@ -108,15 +173,16 @@ public class UnscentedKalmanFilter x = sigmas.times(Matrix.changeBoundsUnchecked(Wm)); + Matrix x = meanFunc.apply(sigmas, Wm); // New covariance is the sum of the outer product of the residuals times the // weights Matrix y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1)); for (int i = 0; i < 2 * s.getNum() + 1; i++) { - y.setColumn(i, sigmas.extractColumnVector(i).minus(x)); + // y[:, i] = sigmas[:, i] - x + y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x)); } Matrix P = y.times(Matrix.changeBoundsUnchecked(Wc.diag())) .times(Matrix.changeBoundsUnchecked(y.transpose())); @@ -234,7 +300,7 @@ public class UnscentedKalmanFilter u, Matrix y) { - correct(m_outputs, u, y, m_h, m_contR); + correct(m_outputs, u, y, m_h, m_contR, + m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX); } /** @@ -271,7 +338,11 @@ public class UnscentedKalmanFilter rows, Matrix u, Matrix y, BiFunction, Matrix, Matrix> h, - Matrix R) { + Matrix R, + BiFunction, Matrix, Matrix> meanFuncY, + BiFunction, Matrix, Matrix> residualFuncY, + BiFunction, Matrix, Matrix> residualFuncX, + BiFunction, Matrix, Matrix> addFuncX) { final var discR = Discretization.discretizeR(R, m_dtSeconds); // Transform sigma points into measurement space @@ -287,18 +358,19 @@ public class UnscentedKalmanFilter Pxy = new Matrix<>(m_states, rows); for (int i = 0; i < m_pts.getNumSigmas(); i++) { - var temp = - m_sigmasF.extractColumnVector(i).minus(m_xHat) - .times(sigmasH.extractColumnVector(i).minus(yHat).transpose()); + // Pxy += (sigmas_f[:, i] - xHat) * (sigmas_h[:, i] - yHat)^T * W_c[i] + var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat); + var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose(); - Pxy = Pxy.plus(temp.times(m_pts.getWc(i))); + Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i))); } // K = P_{xy} Py^-1 @@ -310,7 +382,8 @@ public class UnscentedKalmanFilter poseTo4dVector(Pose2d pose) { + return VecBuilder.fill( + pose.getTranslation().getX(), + pose.getTranslation().getY(), + pose.getRotation().getCos(), + pose.getRotation().getSin() + ); + } + + /** + * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians. + * + * @param pose A pose to convert to a vector. + * @return The given pose in vector form, with the third element, theta, in radians. + */ + public static Matrix poseTo3dVector(Pose2d pose) { + return VecBuilder.fill( + pose.getTranslation().getX(), + pose.getTranslation().getY(), + pose.getRotation().getRadians() + ); + } } diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index d828f30730..c1037512df 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -9,6 +9,18 @@ namespace frc { +Eigen::Matrix PoseTo3dVector(const Pose2d& pose) { + return frc::MakeMatrix<3, 1>(pose.Translation().X().to(), + pose.Translation().Y().to(), + pose.Rotation().Radians().to()); +} + +Eigen::Matrix PoseTo4dVector(const Pose2d& pose) { + return frc::MakeMatrix<4, 1>(pose.Translation().X().to(), + pose.Translation().Y().to(), + pose.Rotation().Cos(), pose.Rotation().Sin()); +} + template <> bool IsStabilizable<1, 1>(const Eigen::Matrix& A, const Eigen::Matrix& B) { diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp new file mode 100644 index 0000000000..a9e96bf698 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -0,0 +1,143 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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/estimator/DifferentialDrivePoseEstimator.h" + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/AngleStatistics.h" + +using namespace frc; + +DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurmentStdDevs, + units::second_t nominalDt) + : m_observer( + &DifferentialDrivePoseEstimator::F, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0)); + }, + stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2), + frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2), + frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt), + m_nominalDt(nominalDt) { + // Create R (covariances) for vision measurements. + Eigen::Matrix visionContR = + frc::MakeCovMatrix(visionMeasurmentStdDevs); + m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + + // Create correction mechanism for vision measurements. + m_visionCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { + m_observer.Correct<3>( + u, y, + [](const Eigen::Matrix& x, + const Eigen::Matrix&) { return x.block<3, 1>(0, 0); }, + m_visionDiscR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2), + frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)); + }; + + m_gyroOffset = initialPose.Rotation() - gyroAngle; + m_previousAngle = initialPose.Rotation(); + m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m)); +} + +void DifferentialDrivePoseEstimator::ResetPosition( + const Pose2d& pose, const Rotation2d& gyroAngle) { + m_previousAngle = pose.Rotation(); + m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle; + m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m)); +} + +Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const { + return Pose2d(units::meter_t(m_observer.Xhat(0)), + units::meter_t(m_observer.Xhat(1)), + Rotation2d(units::radian_t(m_observer.Xhat(2)))); +} + +void DifferentialDrivePoseEstimator::AddVisionMeasurement( + const Pose2d& visionRobotPose, units::second_t timestamp) { + m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt, + PoseTo3dVector(visionRobotPose), + m_visionCorrect, timestamp); +} + +Pose2d DifferentialDrivePoseEstimator::Update( + const Rotation2d& gyroAngle, + const DifferentialDriveWheelSpeeds& wheelSpeeds, + units::meter_t leftDistance, units::meter_t rightDistance) { + return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle, + wheelSpeeds, leftDistance, rightDistance); +} + +Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( + units::second_t currentTime, const Rotation2d& gyroAngle, + const DifferentialDriveWheelSpeeds& wheelSpeeds, + units::meter_t leftDistance, units::meter_t rightDistance) { + auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt; + m_prevTime = currentTime; + + auto angle = gyroAngle + m_gyroOffset; + auto omega = (gyroAngle - m_previousAngle).Radians() / dt; + + auto u = frc::MakeMatrix<3, 1>( + (wheelSpeeds.left + wheelSpeeds.right).to() / 2.0, 0.0, + omega.to()); + + m_previousAngle = angle; + + auto localY = frc::MakeMatrix<3, 1>(leftDistance.to(), + rightDistance.to(), + angle.Radians().to()); + + m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); + m_observer.Predict(u, dt); + m_observer.Correct(u, localY); + + return GetEstimatedPosition(); +} + +Eigen::Matrix DifferentialDrivePoseEstimator::F( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { + // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does + // that for us. + auto& theta = x(2, 0); + Eigen::Matrix toFieldRotation = frc::MakeMatrix<5, 5>( + // clang-format off + std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0, + std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0); // clang-format on + return toFieldRotation * + frc::MakeMatrix<5, 1>(u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)); +} + +template +std::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( + const Eigen::Matrix& stdDevs) { + std::array array; + for (size_t i = 0; i < Dim; ++i) { + array[i] = stdDevs(i); + } + return array; +} + +Eigen::Matrix DifferentialDrivePoseEstimator::FillStateVector( + const Pose2d& pose, units::meter_t leftDistance, + units::meter_t rightDistance) { + return frc::MakeMatrix<5, 1>( + pose.Translation().X().to(), pose.Translation().Y().to(), + pose.Rotation().Radians().to(), leftDistance.to(), + rightDistance.to()); +} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp new file mode 100644 index 0000000000..1b3be996c1 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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/estimator/MecanumDrivePoseEstimator.h" + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/AngleStatistics.h" + +using namespace frc; + +frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + MecanumDriveKinematics kinematics, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, + units::second_t nominalDt) + : m_observer([](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return u; }, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return x.block<1, 1>(2, 0); + }, + stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2), + frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2), + frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt), + m_kinematics(kinematics), + m_nominalDt(nominalDt) { + // Construct R (covariances) matrix for vision measurements. + Eigen::Matrix3d visionContR = frc::MakeCovMatrix<3>(visionMeasurementStdDevs); + + // Create and store discrete covariance matrix for vision measurements. + m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + + // Create vision correction mechanism. + m_visionCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { + m_observer.Correct<3>( + u, y, + [](const Eigen::Matrix& x, + const Eigen::Matrix&) { return x; }, + m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), + frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); + }; + + // Set initial state. + m_observer.SetXhat(PoseTo3dVector(initialPose)); + + // Calculate offsets. + m_gyroOffset = initialPose.Rotation() - gyroAngle; + m_previousAngle = initialPose.Rotation(); +} + +void frc::MecanumDrivePoseEstimator::ResetPosition( + const Pose2d& pose, const Rotation2d& gyroAngle) { + // Set observer state. + m_observer.SetXhat(PoseTo3dVector(pose)); + + // Calculate offsets. + m_gyroOffset = pose.Rotation() - gyroAngle; + m_previousAngle = pose.Rotation(); +} + +Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const { + return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m, + Rotation2d(units::radian_t{m_observer.Xhat(2)})); +} + +void frc::MecanumDrivePoseEstimator::AddVisionMeasurement( + const Pose2d& visionRobotPose, units::second_t timestamp) { + m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt, + PoseTo3dVector(visionRobotPose), + m_visionCorrect, timestamp); +} + +Pose2d frc::MecanumDrivePoseEstimator::Update( + const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) { + return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle, + wheelSpeeds); +} + +Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( + units::second_t currentTime, const Rotation2d& gyroAngle, + const MecanumDriveWheelSpeeds& wheelSpeeds) { + auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt; + m_prevTime = currentTime; + + auto angle = gyroAngle + m_gyroOffset; + auto omega = (angle - m_previousAngle).Radians() / dt; + + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds); + auto fieldRelativeVelocities = + Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s) + .RotateBy(angle); + + auto u = frc::MakeMatrix<3, 1>(fieldRelativeVelocities.X().to(), + fieldRelativeVelocities.Y().to(), + omega.to()); + + auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to()); + m_previousAngle = angle; + + m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); + + m_observer.Predict(u, dt); + m_observer.Correct(u, localY); + + return GetEstimatedPosition(); +} diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index b461005085..1f333eaba5 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -232,6 +232,24 @@ Eigen::Matrix MakeWhiteNoiseVector( return result; } +/** + * Converts a Pose2d into a vector of [x, y, theta]. + * + * @param pose The pose that is being represented. + * + * @return The vector. + */ +Eigen::Matrix PoseTo3dVector(const Pose2d& pose); + +/** + * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)]. + * + * @param pose The pose that is being represented. + * + * @return The vector. + */ +Eigen::Matrix PoseTo4dVector(const Pose2d& pose); + /** * Returns true if (A, B) is a stabilizable pair. * diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h new file mode 100644 index 0000000000..b2a4e16466 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h @@ -0,0 +1,128 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 "Eigen/Core" + +namespace frc { +namespace { +double NormalizeAngle(double angle) { + static constexpr double tau = 2 * wpi::math::pi; + + angle -= std::floor(angle / tau) * tau; + if (angle > wpi::math::pi) angle -= tau; + return angle; +} +} // namespace + +/** + * Subtracts a and b while normalizing the resulting value in the selected row + * as if it were an angle. + * + * @param a A vector to subtract from. + * @param b A vector to subtract with. + * @param angleStateIdx The row containing angles to be normalized. + */ +template +Eigen::Matrix AngleResidual( + const Eigen::Matrix& a, + const Eigen::Matrix& b, int angleStateIdx) { + Eigen::Matrix ret = a - b; + ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]); + return ret; +} + +/** + * Returns a function that subtracts two vectors while normalizing the resulting + * value in the selected row as if it were an angle. + * + * @param angleStateIdx The row containing angles to be normalized. + */ +template +std::function< + Eigen::Matrix(const Eigen::Matrix&, + const Eigen::Matrix&)> +AngleResidual(int angleStateIdx) { + return [=](auto a, auto b) { + return AngleResidual(a, b, angleStateIdx); + }; +} + +/** + * Adds a and b while normalizing the resulting value in the selected row as an + * angle. + * + * @param a A vector to add with. + * @param b A vector to add with. + * @param angleStateIdx The row containing angles to be normalized. + */ +template +Eigen::Matrix AngleAdd( + const Eigen::Matrix& a, + const Eigen::Matrix& b, int angleStateIdx) { + Eigen::Matrix ret = a + b; + ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]); + return ret; +} + +/** + * Returns a function that adds two vectors while normalizing the resulting + * value in the selected row as an angle. + * + * @param angleStateIdx The row containing angles to be normalized. + */ +template +std::function< + Eigen::Matrix(const Eigen::Matrix&, + const Eigen::Matrix&)> +AngleAdd(int angleStateIdx) { + return [=](auto a, auto b) { return AngleAdd(a, b, angleStateIdx); }; +} + +/** + * Computes the mean of sigmas with the weights Wm while computing a special + * angle mean for a select row. + * + * @param sigmas Sigma points. + * @param Wm Weights for the mean. + * @param angleStateIdx The row containing the angles. + */ +template +Eigen::Matrix AngleMean( + const Eigen::Matrix& sigmas, + const Eigen::Matrix& Wm, int angleStatesIdx) { + double sumSin = sigmas.row(angleStatesIdx) + .unaryExpr([](auto it) { return std::sin(it); }) + .sum(); + double sumCos = sigmas.row(angleStatesIdx) + .unaryExpr([](auto it) { return std::cos(it); }) + .sum(); + + Eigen::Matrix ret = sigmas * Wm; + ret[angleStatesIdx] = std::atan2(sumSin, sumCos); + return ret; +} + +/** + * Returns a function that computes the mean of sigmas with the weights Wm while + * computing a special angle mean for a select row. + * + * @param angleStateIdx The row containing the angles. + */ +template +std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> +AngleMean(int angleStateIdx) { + return [=](auto sigmas, auto Wm) { + return AngleMean(sigmas, Wm, angleStateIdx); + }; +} +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h new file mode 100644 index 0000000000..9b58111338 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -0,0 +1,174 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 "Eigen/Core" +#include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "units/time.h" + +namespace frc { +/** + * This class wraps an Unscented Kalman Filter to fuse latency-compensated + * vision measurements with differential drive encoder measurements. It will + * correct for noisy vision measurements and encoder drift. It is intended to be + * an easy drop-in for DifferentialDriveOdometry. In fact, if you never call + * AddVisionMeasurement(), and only call Update(), this will behave exactly the + * same as DifferentialDriveOdometry. + * + * Update() should be called every robot loop (if your robot loops are faster or + * slower than the default, then you should change the nominal delta time via + * the constructor). + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave like regular encoder odometry. + * + * Our state-space system is: + * + * x = [[x, y, theta, dist_l, dist_r]]^T in the field + * coordinate system. + * + * u = [[d_l, d_r, dtheta]]^T (robot-relative velocities) -- + * NB: using velocities make things considerably easier, because it means that + * teams don't have to worry about getting an accurate model. Basically, we + * suspect that it's easier for teams to get good encoder data than it is for + * them to perform system identification well enough to get a good model + * + * y = [[x, y, theta]]^T from vision, + * or y = [[dist_l, dist_r, theta]] from encoders and gyro. + */ +class DifferentialDrivePoseEstimator { + public: + /** + * Constructs a DifferentialDrivePoseEstimator. + * + * @param gyroAngle The gyro angle of the robot. + * @param initialPose The estimated initial pose. + * @param stateStdDevs Standard deviations of the model states. + * Increase these to trust your wheel speeds + * less. + * @param localMeasurementStdDevs Standard deviations of the encoder + * measurements. Increase these to trust + * encoder distances less. + * @param visionMeasurementStdDevs Standard deviations of the vision + * measurements. Increase these to trust + * vision less. + * @param nominalDt The period of the loop calling Update(). + */ + DifferentialDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, + units::second_t nominalDt = 0.02_s); + + /** + * Resets the robot's position on the field. + * + * You NEED to reset your encoders to zero when calling this method. The + * gyroscope angle does not need to be reset here on the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + *@param pose The estimated pose of the robot on the field. + *@param gyroAngle The current gyro angle. + */ + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle); + + /** + * Returns the pose of the robot at the current time as estimated by the + * Unscented Kalman Filter. + * + * @return The estimated robot pose. + */ + Pose2d GetEstimatedPosition() const; + + /** + * Adds a vision measurement to the Unscented Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. + * Note that if you don't use your own time source by + * calling UpdateWithTime(), then you must use a + * timestamp with an epoch since FPGA startup (i.e. the + * epoch of this timestamp is the same epoch as + * frc2::Timer::GetFPGATimestamp(). This means that + * you should use frc2::Timer::GetFPGATimestamp() as + * your time source in this case. + */ + void AddVisionMeasurement(const Pose2d& visionRobotPose, + units::second_t timestamp); + + /** + * Updates the Unscented Kalman Filter using only wheel encoder information. + * Note that this should be called every loop iteration. + * + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * + * @return The estimated pose of the robot. + */ + Pose2d Update(const Rotation2d& gyroAngle, + const DifferentialDriveWheelSpeeds& wheelSpeeds, + units::meter_t leftDistance, units::meter_t rightDistance); + + /** + * Updates the Unscented Kalman Filter using only wheel encoder information. + * Note that this should be called every loop iteration. + * + * @param currentTime The time at which this method was called. + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * + * @return The estimated pose of the robot. + */ + Pose2d UpdateWithTime(units::second_t currentTime, + const Rotation2d& gyroAngle, + const DifferentialDriveWheelSpeeds& wheelSpeeds, + units::meter_t leftDistance, + units::meter_t rightDistance); + + private: + UnscentedKalmanFilter<5, 3, 3> m_observer; + KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>> + m_latencyCompensator; + std::function& u, + const Eigen::Matrix& y)> + m_visionCorrect; + + Eigen::Matrix m_visionDiscR; + + units::second_t m_nominalDt; + units::second_t m_prevTime = -1_s; + + Rotation2d m_gyroOffset; + Rotation2d m_previousAngle; + + template + static std::array StdDevMatrixToArray( + const Eigen::Matrix& stdDevs); + + static Eigen::Matrix F(const Eigen::Matrix& x, + const Eigen::Matrix& u); + static Eigen::Matrix FillStateVector( + const Pose2d& pose, units::meter_t leftDistance, + units::meter_t rightDistance); +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index 6f0fc85482..0b376887ba 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -25,7 +25,7 @@ template class ExtendedKalmanFilter { public: /** - * Constructs an extended Kalman filter. + * Constructs an Extended Kalman filter. * * @param f A vector-valued function of x and u that returns * the derivative of the state vector. diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h new file mode 100644 index 0000000000..4504e6d6b2 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -0,0 +1,129 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 +#include +#include +#include + +#include "Eigen/Core" +#include "units/math.h" +#include "units/time.h" + +namespace frc { + +template +class KalmanFilterLatencyCompensator { + public: + struct ObserverSnapshot { + Eigen::Matrix xHat; + Eigen::Matrix errorCovariances; + Eigen::Matrix inputs; + Eigen::Matrix localMeasurements; + + ObserverSnapshot(const KalmanFilterType& observer, + const Eigen::Matrix& u, + const Eigen::Matrix& localY) + : xHat(observer.Xhat()), + errorCovariances(observer.P()), + inputs(u), + localMeasurements(localY) {} + }; + + void AddObserverState(const KalmanFilterType& observer, + Eigen::Matrix u, + Eigen::Matrix localY, + units::second_t timestamp) { + // Add the new state into the vector. + m_pastObserverSnapshots.emplace_back(timestamp, + ObserverSnapshot{observer, u, localY}); + + // Remove the oldest snapshot if the vector exceeds our maximum size. + if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) { + m_pastObserverSnapshots.erase(m_pastObserverSnapshots.begin()); + } + } + + template + void ApplyPastMeasurement( + KalmanFilterType* observer, units::second_t nominalDt, + Eigen::Matrix y, + std::function& u, + const Eigen::Matrix& y)> + globalMeasurementCorrect, + units::second_t timestamp) { + if (m_pastObserverSnapshots.size() == 0) { + // State map was empty, which means that we got a measurement right at + // startup. The only thing we can do is ignore the measurement. + return; + } + + // We will perform a binary search to find the index of the element in the + // vector that has a timestamp that is equal to or greater than the vision + // measurement timestamp. + auto lowerBoundIter = std::lower_bound( + m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(), + timestamp, + [](const auto& entry, const auto& ts) { return entry.first < ts; }); + int index = std::distance(m_pastObserverSnapshots.cbegin(), lowerBoundIter); + + // High and Low should be the same. The sampled timestamp is greater than or + // equal to the vision pose timestamp. We will now find the entry which is + // closest in time to the requested timestamp. + + size_t indexOfClosestEntry = + units::math::abs( + timestamp - m_pastObserverSnapshots[std::max(index - 1, 0)].first) < + units::math::abs(timestamp - + m_pastObserverSnapshots[index].first) + ? index - 1 + : index; + + units::second_t lastTimestamp = + m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt; + + // We will now go back in time to the state of the system at the time when + // the measurement was captured. We will reset the observer to that state, + // and apply correction based on the measurement. Then, we will go back + // through all observer states until the present and apply past inputs to + // get the present estimated state. + for (size_t i = indexOfClosestEntry; i < m_pastObserverSnapshots.size(); + ++i) { + auto& [key, snapshot] = m_pastObserverSnapshots[i]; + + if (i == indexOfClosestEntry) { + observer->SetP(snapshot.errorCovariances); + observer->SetXhat(snapshot.xHat); + } + + observer->Predict(snapshot.inputs, key - lastTimestamp); + observer->Correct(snapshot.inputs, snapshot.localMeasurements); + + if (i == indexOfClosestEntry) { + // Note that the measurement is at a timestep close but probably not + // exactly equal to the timestep for which we called predict. This makes + // the assumption that the dt is small enough that the difference + // between the measurement time and the time that the inputs were + // captured at is very small. + globalMeasurementCorrect(snapshot.inputs, y); + } + + lastTimestamp = key; + snapshot = ObserverSnapshot{*observer, snapshot.inputs, + snapshot.localMeasurements}; + } + } + + private: + static constexpr size_t kMaxPastObserverStates = 300; + std::vector> + m_pastObserverSnapshots; +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h new file mode 100644 index 0000000000..483bac6bef --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -0,0 +1,171 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 "Eigen/Core" +#include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "units/time.h" + +namespace frc { +/** + * This class wraps an Unscented Kalman Filter to fuse latency-compensated + * vision measurements with mecanum drive encoder velocity measurements. It will + * correct for noisy measurements and encoder drift. It is intended to be an + * easy but more accurate drop-in for MecanumDriveOdometry. + * + * Update() should be called every robot loop. If your loops are faster or + * slower than the default of 0.02s, then you should change the nominal delta + * time by specifying it in the constructor. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave mostly like regular encoder + * odometry. + * + * Our state-space system is: + * + * x = [[x, y, theta]]^T in the + * field-coordinate system. + * + * u = [[vx, vy, omega]]^T in the field-coordinate system. + * + * y = [[x, y, theta]]^T in field + * coords from vision, or y = [[theta]]^T + * from the gyro. + */ +class MecanumDrivePoseEstimator { + public: + /** + * Constructs a MecanumDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param kinematics A correctly-configured kinematics object + * for your drivetrain. + * @param stateStdDevs Standard deviations of model states. + * Increase these numbers to trust your + * wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the gyro + * measurement. Increase this number + * to trust gyro angle measurements less. + * @param visionMeasurementStdDevs Standard deviations of the encoder + * measurements. Increase these numbers + * to trust vision less. + * @param nominalDt The time in seconds between each robot + * loop. + */ + MecanumDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + MecanumDriveKinematics kinematics, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, + units::second_t nominalDt = 0.02_s); + + /** + * Resets the robot's position on the field. + * + *

You NEED to reset your encoders (to zero) when calling this method. + * + *

The gyroscope angle does not need to be reset in the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param poseMeters The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + */ + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle); + + /** + * Gets the pose of the robot at the current time as estimated by the Extended + * Kalman Filter. + * + * @return The estimated robot pose in meters. + */ + Pose2d GetEstimatedPosition() const; + + /** + * Add a vision measurement to the Unscented Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. + * Note that if you don't use your own time source by + * calling UpdateWithTime() then you must use a + * timestamp with an epoch since FPGA startup + * (i.e. the epoch of this timestamp is the same + * epoch as Timer#GetFPGATimestamp.) This means + * that you should use Timer#GetFPGATimestamp as your + * time source or sync the epochs. + */ + void AddVisionMeasurement(const Pose2d& visionRobotPose, + units::second_t timestamp); + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder + * information. This should be called every loop, and the correct loop period + * must be passed into the constructor of this class. + * + * @param gyroAngle The current gyro angle. + * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @return The estimated pose of the robot in meters. + */ + Pose2d Update(const Rotation2d& gyroAngle, + const MecanumDriveWheelSpeeds& wheelSpeeds); + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder + * information. This should be called every loop, and the correct loop period + * must be passed into the constructor of this class. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyroscope angle. + * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @return The estimated pose of the robot in meters. + */ + Pose2d UpdateWithTime(units::second_t currentTime, + const Rotation2d& gyroAngle, + const MecanumDriveWheelSpeeds& wheelSpeeds); + + private: + UnscentedKalmanFilter<3, 3, 1> m_observer; + MecanumDriveKinematics m_kinematics; + KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>> + m_latencyCompensator; + std::function& u, + const Eigen::Matrix& y)> + m_visionCorrect; + + Eigen::Matrix3d m_visionDiscR; + + units::second_t m_nominalDt; + units::second_t m_prevTime = -1_s; + + Rotation2d m_gyroOffset; + Rotation2d m_previousAngle; + + template + static std::array StdDevMatrixToArray( + const Eigen::Matrix& vector) { + std::array array; + for (size_t i = 0; i < Dim; ++i) { + array[i] = vector(i); + } + return array; + } +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h new file mode 100644 index 0000000000..c8589addee --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -0,0 +1,260 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +#include + +#include "Eigen/Core" +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/AngleStatistics.h" +#include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "units/time.h" + +namespace frc { +/** + * This class wraps an Unscented Kalman Filter to fuse latency-compensated + * vision measurements with swerve drive encoder velocity measurements. It will + * correct for noisy measurements and encoder drift. It is intended to be an + * easy but more accurate drop-in for SwerveDriveOdometry. + * + * Update() should be called every robot loop. If your loops are faster or + * slower than the default of 0.02s, then you should change the nominal delta + * time by specifying it in the constructor. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave mostly like regular encoder + * odometry. + * + * Our state-space system is: + * + * x = [[x, y, theta]]^T in the + * field-coordinate system. + * + * u = [[vx, vy, omega]]^T in the field-coordinate system. + * + * y = [[x, y, std::theta]]^T in field + * coords from vision, or y = [[theta]]^T + * from the gyro. + */ +template +class SwerveDrivePoseEstimator { + public: + /** + * Constructs a SwerveDrivePoseEstimator. + * + * @param gyroAngle The current gyro angle. + * @param initialPoseMeters The starting pose estimate. + * @param kinematics A correctly-configured kinematics object + * for your drivetrain. + * @param stateStdDevs Standard deviations of model states. + * Increase these numbers to trust your + * wheel and gyro velocities less. + * @param localMeasurementStdDevs Standard deviations of the gyro + * measurement. Increase this number to + * trust gyro angle measurements less. + * @param visionMeasurementStdDevs Standard deviations of the encoder + * measurements. Increase these numbers to + * trust vision less. + * @param nominalDt The time in seconds between each robot + * loop. + */ + SwerveDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + SwerveDriveKinematics& kinematics, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, + units::second_t nominalDt = 0.02_s) + : m_observer([](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return u; }, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return x.block<1, 1>(2, 0); + }, + stateStdDevs, localMeasurementStdDevs, + frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0), + frc::AngleResidual<3>(2), frc::AngleResidual<1>(0), + frc::AngleAdd<3>(2), nominalDt), + m_kinematics(kinematics), + m_nominalDt(nominalDt) { + // Construct R (covariances) matrix for vision measurements. + Eigen::Matrix3d visionContR = + frc::MakeCovMatrix<3>(visionMeasurementStdDevs); + + // Create and store discrete covariance matrix for vision measurements. + m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + + // Create correction mechanism for vision measurements. + m_visionCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { + m_observer.Correct<3>( + u, y, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x; }, + m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), + frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); + }; + + // Set initial state. + m_observer.SetXhat(PoseTo3dVector(initialPose)); + + // Calculate offsets. + m_gyroOffset = initialPose.Rotation() - gyroAngle; + m_previousAngle = initialPose.Rotation(); + } + + /** + * Resets the robot's position on the field. + * + * You NEED to reset your encoders (to zero) when calling this method. + * + * The gyroscope angle does not need to be reset in the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param pose The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + */ + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { + // Set observer state. + m_observer.SetXhat(PoseTo3dVector(pose)); + + // Calculate offsets. + m_gyroOffset = pose.Rotation() - gyroAngle; + m_previousAngle = pose.Rotation(); + } + + /** + * Gets the pose of the robot at the current time as estimated by the Extended + * Kalman Filter. + * + * @return The estimated robot pose in meters. + */ + Pose2d GetEstimatedPosition() const { + return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m, + Rotation2d(units::radian_t{m_observer.Xhat(2)})); + } + + /** + * Add a vision measurement to the Unscented Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. + * Note that if you don't use your own time source by + * calling UpdateWithTime() then you must use a + * timestamp with an epoch since FPGA startup + * (i.e. the epoch of this timestamp is the same + * epoch as Timer#GetFPGATimestamp.) This means + * that you should use Timer#GetFPGATimestamp as your + * time source or sync the epochs. + */ + void AddVisionMeasurement(const Pose2d& visionRobotPose, + units::second_t timestamp) { + m_latencyCompensator.ApplyPastMeasurement<3>( + &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose), + m_visionCorrect, timestamp); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder + * information. This should be called every loop, and the correct loop period + * must be passed into the constructor of this class. + * + * @param gyroAngle The current gyro angle. + * @param moduleStates The current velocities and rotations of the swerve + * modules. + * @return The estimated pose of the robot in meters. + */ + template + Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) { + return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle, + moduleStates...); + } + + /** + * Updates the the Unscented Kalman Filter using only wheel encoder + * information. This should be called every loop, and the correct loop period + * must be passed into the constructor of this class. + * + * @param currentTime Time at which this method was called, in seconds. + * @param gyroAngle The current gyroscope angle. + * @param moduleStates The current velocities and rotations of the swerve + * modules. + * @return The estimated pose of the robot in meters. + */ + template + Pose2d UpdateWithTime(units::second_t currentTime, + const Rotation2d& gyroAngle, + ModuleState&&... moduleStates) { + auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt; + m_prevTime = currentTime; + + auto angle = gyroAngle + m_gyroOffset; + auto omega = (angle - m_previousAngle).Radians() / dt; + + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...); + auto fieldRelativeSpeeds = + Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s) + .RotateBy(angle); + + auto u = + frc::MakeMatrix<3, 1>(fieldRelativeSpeeds.X().template to(), + fieldRelativeSpeeds.Y().template to(), + omega.template to()); + + auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to()); + m_previousAngle = angle; + + m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); + + m_observer.Predict(u, dt); + m_observer.Correct(u, localY); + + return GetEstimatedPosition(); + } + + private: + UnscentedKalmanFilter<3, 3, 1> m_observer; + SwerveDriveKinematics& m_kinematics; + KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>> + m_latencyCompensator; + std::function& u, + const Eigen::Matrix& y)> + m_visionCorrect; + + Eigen::Matrix3d m_visionDiscR; + + units::second_t m_nominalDt; + units::second_t m_prevTime = -1_s; + + Rotation2d m_gyroOffset; + Rotation2d m_previousAngle; + + template + static std::array StdDevMatrixToArray( + const Eigen::Matrix& vector) { + std::array array; + for (size_t i = 0; i < Dim; ++i) { + array[i] = vector(i); + } + return array; + } +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 8c2c31f0e6..ef9df42670 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -50,6 +50,91 @@ class UnscentedKalmanFilter { : m_f(f), m_h(h) { m_contQ = MakeCovMatrix(stateStdDevs); m_contR = MakeCovMatrix(measurementStdDevs); + m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Matrix { + return sigmas * Wm; + }; + m_meanFuncY = [](auto sigmas, + auto Wc) -> Eigen::Matrix { + return sigmas * Wc; + }; + m_residualFuncX = [](auto a, auto b) -> Eigen::Matrix { + return a - b; + }; + m_residualFuncY = [](auto a, auto b) -> Eigen::Matrix { + return a - b; + }; + m_addFuncX = [](auto a, auto b) -> Eigen::Matrix { + return a + b; + }; + m_dt = dt; + + Reset(); + } + + /** + * Constructs an unscented Kalman filter with custom mean, residual, and + * addition functions. Using custom functions for arithmetic can be useful if + * you have angles in the state or measurements, because they allow you to + * correctly account for the modular nature of angle arithmetic. + * + * @param f A vector-valued function of x and u that returns + * the derivative of the state vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param meanFuncX A function that computes the mean of 2 * States + + * 1 state vectors using a given set of weights. + * @param meanFuncY A function that computes the mean of 2 * States + + * 1 measurement vectors using a given set of weights. + * @param residualFuncX A function that computes the residual of two + * state vectors (i.e. it subtracts them.) + * @param residualFuncY A function that computes the residual of two + * measurement vectors (i.e. it subtracts them.) + * @param addFuncX A function that adds two state vectors. + * @param dt Nominal discretization timestep. + */ + UnscentedKalmanFilter( + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + f, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + h, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + meanFuncX, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + meanFuncY, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + residualFuncX, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + residualFuncY, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + addFuncX, + units::second_t dt) + : m_f(f), + m_h(h), + m_meanFuncX(meanFuncX), + m_meanFuncY(meanFuncY), + m_residualFuncX(residualFuncX), + m_residualFuncY(residualFuncY), + m_addFuncX(addFuncX) { + m_contQ = MakeCovMatrix(stateStdDevs); + m_contR = MakeCovMatrix(measurementStdDevs); m_dt = dt; Reset(); @@ -136,8 +221,8 @@ class UnscentedKalmanFilter { m_sigmasF.template block(0, i) = RungeKutta(m_f, x, u, dt); } - auto ret = - UnscentedTransform(m_sigmasF, m_pts.Wm(), m_pts.Wc()); + auto ret = UnscentedTransform( + m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX); m_xHat = std::get<0>(ret); m_P = std::get<1>(ret); @@ -152,7 +237,8 @@ class UnscentedKalmanFilter { */ void Correct(const Eigen::Matrix& u, const Eigen::Matrix& y) { - Correct(u, y, m_h, m_contR); + Correct(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY, + m_residualFuncX, m_addFuncX); } /** @@ -175,7 +261,23 @@ class UnscentedKalmanFilter { const Eigen::Matrix&, const Eigen::Matrix&)> h, - const Eigen::Matrix& R) { + const Eigen::Matrix& R, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + meanFuncY, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + residualFuncY, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + residualFuncX, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix)> + addFuncX) { const Eigen::Matrix discR = DiscretizeR(R, m_dt); // Transform sigma points into measurement space @@ -188,17 +290,19 @@ class UnscentedKalmanFilter { } // Mean and covariance of prediction passed through UT - auto [yHat, Py] = - UnscentedTransform(sigmasH, m_pts.Wm(), m_pts.Wc()); + auto [yHat, Py] = UnscentedTransform( + sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY); Py += discR; // Compute cross covariance of the state and the measurements Eigen::Matrix Pxy; Pxy.setZero(); for (int i = 0; i < m_pts.NumSigmas(); ++i) { - Pxy += m_pts.Wc(i) * - (m_sigmasF.template block(0, i) - m_xHat) * - (sigmasH.template block(0, i) - yHat).transpose(); + Pxy += + m_pts.Wc(i) * + (residualFuncX(m_sigmasF.template block(0, i), m_xHat)) * + (residualFuncY(sigmasH.template block(0, i), yHat)) + .transpose(); } // K = P_{xy} Py^-1 @@ -209,7 +313,7 @@ class UnscentedKalmanFilter { Eigen::Matrix K = Py.transpose().ldlt().solve(Pxy.transpose()).transpose(); - m_xHat += K * (y - yHat); + m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat)); m_P -= K * Py * K.transpose(); } @@ -222,6 +326,26 @@ class UnscentedKalmanFilter { const Eigen::Matrix&, const Eigen::Matrix&)> m_h; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_meanFuncX; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_meanFuncY; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_residualFuncX; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix)> + m_residualFuncY; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix)> + m_addFuncX; Eigen::Matrix m_xHat; Eigen::Matrix m_P; Eigen::Matrix m_contQ; diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h index 22b32cedad..6c4d1bed80 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h @@ -14,8 +14,8 @@ namespace frc { /** - * Computes unscented transform of a set of sigma points and weights. CovDimurns - * the mean and covariance in a tuple. + * Computes unscented transform of a set of sigma points and weights. CovDim + * returns the mean and covariance in a tuple. * * This works in conjunction with the UnscentedKalmanFilter class. * @@ -34,17 +34,26 @@ std::tuple, Eigen::Matrix> UnscentedTransform(const Eigen::Matrix& sigmas, const Eigen::Matrix& Wm, - const Eigen::Matrix& Wc) { - // New mean is just the sum of the sigmas * weight + const Eigen::Matrix& Wc, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + meanFunc, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + residualFunc) { + // New mean is usually just the sum of the sigmas * weight: // dot = \Sigma^n_1 (W[k]*Xi[k]) - Eigen::Matrix x = sigmas * Wm; + Eigen::Matrix x = meanFunc(sigmas, Wm); // New covariance is the sum of the outer product of the residuals times the // weights Eigen::Matrix y; for (int i = 0; i < 2 * States + 1; ++i) { + // y[:, i] = sigmas[:, i] - x; y.template block(0, i) = - sigmas.template block(0, i) - x; + residualFunc(sigmas.template block(0, i), x); } Eigen::Matrix P = y * Eigen::DiagonalMatrix(Wc) * y.transpose(); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 0ed50b42d8..7ba744ef5f 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -76,6 +76,23 @@ class SwerveDriveKinematics { wpi::math::MathUsageId::kKinematics_SwerveDrive, 1); } + explicit SwerveDriveKinematics( + const std::array& wheels) + : m_modules{wheels} { + for (size_t i = 0; i < NumModules; i++) { + // clang-format off + m_inverseKinematics.template block<2, 3>(i * 2, 0) << + 1, 0, (-m_modules[i].Y()).template to(), + 0, 1, (+m_modules[i].X()).template to(); + // clang-format on + } + + m_forwardKinematics = m_inverseKinematics.householderQr(); + + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kKinematics_SwerveDrive, 1); + } + SwerveDriveKinematics(const SwerveDriveKinematics&) = default; /** diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java new file mode 100644 index 0000000000..dc8111ac6e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class AngleStatisticsTest { + @Test + public void testMean() { + // 3 states, 2 sigmas + var sigmas = Matrix.mat(Nat.N3(), Nat.N2()).fill( + 1, 1.2, + Math.toRadians(359), Math.toRadians(3), + 1, 2 + ); + // Weights need to produce the mean of the sigmas + var weights = new Matrix<>(Nat.N2(), Nat.N1()); + weights.fill(1.0 / sigmas.getNumCols()); + + assertTrue(AngleStatistics.angleMean(sigmas, weights, 1).isEqual( + VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6)); + } + + @Test + public void testResidual() { + var first = VecBuilder.fill(1, Math.toRadians(1), 2); + var second = VecBuilder.fill(1, Math.toRadians(359), 1); + assertTrue(AngleStatistics.angleResidual(first, second, 1).isEqual( + VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6)); + } + + @Test + public void testAdd() { + var first = VecBuilder.fill(1, Math.toRadians(1), 2); + var second = VecBuilder.fill(1, Math.toRadians(359), 1); + assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6)); + } + + @Test + public void testNormalize() { + assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(-2000)), Math.toRadians(160), 1e-6); + assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(358)), Math.toRadians(-2), 1e-6); + assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(360)), 0, 1e-6); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java new file mode 100644 index 0000000000..b6f347bf8f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java @@ -0,0 +1,123 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.List; +import java.util.Random; + +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.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.MatBuilder; +import edu.wpi.first.wpiutil.math.Nat; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class DifferentialDrivePoseEstimatorTest { + @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", + "PMD.AvoidInstantiatingObjectsInLoops"}) + @Test + public void testAccuracy() { + var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(), + new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02), + new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001), + new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01)); + + var traj = TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(45)) + ), + new TrajectoryConfig(10, 5)); + + + var kinematics = new DifferentialDriveKinematics(1); + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + final double visionUpdateRate = 0.1; + Pose2d lastVisionPose = null; + double lastVisionUpdateTime = Double.NEGATIVE_INFINITY; + + double distanceLeft = 0.0; + double distanceRight = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + Trajectory.State groundtruthState; + DifferentialDriveWheelSpeeds input; + while (t <= traj.getTotalTimeSeconds()) { + groundtruthState = traj.sample(t); + input = kinematics.toWheelSpeeds(new ChassisSpeeds( + groundtruthState.velocityMetersPerSecond, 0.0, + // ds/dt * dtheta/ds = dtheta/dt + groundtruthState.velocityMetersPerSecond * groundtruthState.curvatureRadPerMeter + )); + + if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) { + if (lastVisionPose != null) { + estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + var groundPose = groundtruthState.poseMeters; + lastVisionPose = new Pose2d( + new Translation2d( + groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1, + groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1 + ), + new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation()) + ); + lastVisionUpdateTime = t; + } + + input.leftMetersPerSecond += rand.nextGaussian() * 0.01; + input.rightMetersPerSecond += rand.nextGaussian() * 0.01; + + distanceLeft += input.leftMetersPerSecond * dt; + distanceRight += input.rightMetersPerSecond * dt; + + var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001); + var xHat = estimator.updateWithTime( + t, + groundtruthState.poseMeters.getRotation().plus(rotNoise), + input, + distanceLeft, distanceRight + ); + + double error = + groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, + "Incorrect mean error" + ); + assertEquals( + 0.0, maxError, 0.055, + "Incorrect max error" + ); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java new file mode 100644 index 0000000000..97374d8f5a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java @@ -0,0 +1,118 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.List; +import java.util.Random; + +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.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.VecBuilder; + + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class MecanumDrivePoseEstimatorTest { + @Test + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops", + "PMD.ExcessiveMethodLength"}) + public void testAccuracy() { + var kinematics = new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var estimator = new MecanumDrivePoseEstimator( + new Rotation2d(), new Pose2d(), kinematics, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.05), + VecBuilder.fill(0.1, 0.1, 0.1) + ); + + var trajectory = TrajectoryGenerator.generateTrajectory( + List.of(new Pose2d(), + new Pose2d(20, 20, Rotation2d.fromDegrees(0)), + new Pose2d(10, 10, Rotation2d.fromDegrees(180)), + new Pose2d(30, 30, Rotation2d.fromDegrees(0)), + new Pose2d(20, 20, Rotation2d.fromDegrees(180)), + new Pose2d(10, 10, Rotation2d.fromDegrees(0))), + new TrajectoryConfig(0.5, 2) + ); + + var rand = new Random(5190); + + final double dt = 0.02; + double t = 0.0; + + final double visionUpdateRate = 0.1; + Pose2d lastVisionPose = null; + double lastVisionUpdateTime = Double.NEGATIVE_INFINITY; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + if (lastVisionUpdateTime + visionUpdateRate < t) { + if (lastVisionPose != null) { + estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + + lastVisionPose = new Pose2d( + new Translation2d( + groundTruthState.poseMeters.getTranslation().getX() + + rand.nextGaussian() * 0.1, + groundTruthState.poseMeters.getTranslation().getY() + + rand.nextGaussian() * 0.1 + ), + new Rotation2d( + rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation()) + ); + + lastVisionUpdateTime = t; + } + + var wheelSpeeds = kinematics.toWheelSpeeds(new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond, 0, + groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter)); + + wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + + var xHat = estimator.updateWithTime(t, + groundTruthState.poseMeters.getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)), wheelSpeeds); + + double error = + groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, + "Incorrect mean error" + ); + assertEquals( + 0.0, maxError, 0.42, + "Incorrect max error" + ); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java new file mode 100644 index 0000000000..f1cab6368c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java @@ -0,0 +1,125 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.estimator; + +import java.util.List; +import java.util.Random; + +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.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.VecBuilder; + + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class SwerveDrivePoseEstimatorTest { + @Test + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops", + "PMD.ExcessiveMethodLength"}) + public void testAccuracy() { + var kinematics = new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1) + ); + var estimator = new SwerveDrivePoseEstimator( + new Rotation2d(), new Pose2d(), kinematics, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.005), + VecBuilder.fill(0.1, 0.1, 0.1) + ); + + var trajectory = TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(45)) + ), + new TrajectoryConfig(0.5, 2) + ); + + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + final double visionUpdateRate = 0.1; + Pose2d lastVisionPose = null; + double lastVisionUpdateTime = Double.NEGATIVE_INFINITY; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + if (lastVisionUpdateTime + visionUpdateRate < t) { + if (lastVisionPose != null) { + estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + + lastVisionPose = new Pose2d( + new Translation2d( + groundTruthState.poseMeters.getTranslation().getX() + + rand.nextGaussian() * 0.1, + groundTruthState.poseMeters.getTranslation().getY() + + rand.nextGaussian() * 0.1 + ), + new Rotation2d( + rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation()) + ); + + lastVisionUpdateTime = t; + + } + + var moduleStates = kinematics.toSwerveModuleStates(new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond, + 0.0, + groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter) + ); + for (var moduleState : moduleStates) { + moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); + moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + } + + var xHat = estimator.updateWithTime( + t, + groundTruthState.poseMeters.getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + moduleStates); + + double error = + groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, + "Incorrect mean error" + ); + assertEquals( + 0.0, maxError, 0.42, + "Incorrect max error" + ); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java index c4340ae471..68aead83e4 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java @@ -222,7 +222,9 @@ public class UnscentedKalmanFilterTest { var R = StateSpaceUtil.makeCostMatrix( VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5)); observer.correct(Nat.N6(), u, globalY, - UnscentedKalmanFilterTest::getGlobalMeasurementModel, R); + UnscentedKalmanFilterTest::getGlobalMeasurementModel, R, + (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)), + Matrix::minus, Matrix::minus, Matrix::plus); final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds()); @@ -371,7 +373,7 @@ public class UnscentedKalmanFilterTest { 16.66666667, 16.66666667, 16.66666667 - ) + ), (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)), Matrix::minus ); assertTrue( diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp new file mode 100644 index 0000000000..c59fa336b6 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 "Eigen/Core" +#include "frc/estimator/AngleStatistics.h" + +TEST(AngleStatisticsTest, TestMean) { + Eigen::Matrix sigmas; + sigmas << 1, 1.2, 0, 359 * wpi::math::pi / 180, 3 * wpi::math::pi / 180, 0, 1, + 2, 0; + // Weights need to produce the mean of the sigmas + Eigen::Vector3d weights{}; + weights.fill(1.0 / sigmas.cols()); + + EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1) + .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3)); +} + +TEST(AngleStatisticsTest, TestResidual) { + Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2); + Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1); + + EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox( + Eigen::Vector3d(0, 2 * wpi::math::pi / 180, 1))); +} + +TEST(AngleStatisticsTest, TestAdd) { + Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2); + Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1); + + EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3))); +} + +TEST(AngleStatisticsTest, TestNormalize) { + EXPECT_NEAR(frc::NormalizeAngle(-2000 * wpi::math::pi / 180), + 160 * wpi::math::pi / 180, 1e-6); + EXPECT_NEAR(frc::NormalizeAngle(358 * wpi::math::pi / 180), + -2 * wpi::math::pi / 180, 1e-6); + EXPECT_NEAR(frc::NormalizeAngle(360 * wpi::math::pi / 180), 0, 1e-6); +} diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp new file mode 100644 index 0000000000..fef7898799 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/DifferentialDrivePoseEstimator.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveOdometry.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" +#include "units/angle.h" +#include "units/length.h" +#include "units/time.h" + +TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { + frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(), + frc::Pose2d(), + {0.02, 0.02, 0.01, 0.02, 0.02}, + {0.01, 0.01, 0.001}, + {0.1, 0.1, 0.01}}; + + frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)), + frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)), + frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)), + frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)), + frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))}, + frc::TrajectoryConfig(10_mps, 5.0_mps_sq)); + + frc::DifferentialDriveKinematics kinematics{1.0_m}; + frc::DifferentialDriveOdometry odometry{frc::Rotation2d()}; + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 0.02_s; + units::second_t t = 0.0_s; + + units::meter_t leftDistance = 0_m; + units::meter_t rightDistance = 0_m; + + units::second_t kVisionUpdateRate = 0.1_s; + frc::Pose2d lastVisionPose; + units::second_t lastVisionUpdateTime{-std::numeric_limits::max()}; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t <= trajectory.TotalTime()) { + auto groundTruthState = trajectory.Sample(t); + auto input = kinematics.ToWheelSpeeds( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + if (lastVisionUpdateTime + kVisionUpdateRate < t) { + if (lastVisionPose != frc::Pose2d()) { + estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + lastVisionPose = + groundTruthState.pose + + frc::Transform2d( + frc::Translation2d(distribution(generator) * 0.1 * 1_m, + distribution(generator) * 0.1 * 1_m), + frc::Rotation2d(distribution(generator) * 0.01 * 1_rad)); + + lastVisionUpdateTime = t; + } + + leftDistance += input.left * distribution(generator) * 0.01 * dt; + rightDistance += input.right * distribution(generator) * 0.01 * dt; + + auto xhat = estimator.UpdateWithTime( + t, + groundTruthState.pose.Rotation() + + frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)), + input, leftDistance, rightDistance); + + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation()) + .to(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_NEAR( + 0.0, errorSum / (trajectory.TotalTime().to() / dt.to()), + 0.2); + EXPECT_NEAR(0.0, maxError, 0.4); +} diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp new file mode 100644 index 0000000000..88231c8ee4 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -0,0 +1,93 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +#include "frc/estimator/MecanumDrivePoseEstimator.h" +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveOdometry.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" + +TEST(MecanumDrivePoseEstimatorTest, TestAccuracy) { + frc::MecanumDriveKinematics kinematics{ + frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, + frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; + + frc::MecanumDrivePoseEstimator estimator{ + frc::Rotation2d(), frc::Pose2d(), kinematics, + {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; + + frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()}; + + frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)), + frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)), + frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)), + frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)), + frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))}, + frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 0.02_s; + units::second_t t = 0_s; + + units::second_t kVisionUpdateRate = 0.1_s; + frc::Pose2d lastVisionPose; + units::second_t lastVisionUpdateTime{-std::numeric_limits::max()}; + + std::vector visionPoses; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + if (lastVisionUpdateTime + kVisionUpdateRate < t) { + if (lastVisionPose != frc::Pose2d()) { + estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + lastVisionPose = + groundTruthState.pose + + frc::Transform2d( + frc::Translation2d(distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m), + frc::Rotation2d(distribution(generator) * 0.1 * 1_rad)); + visionPoses.push_back(lastVisionPose); + lastVisionUpdateTime = t; + } + + auto wheelSpeeds = kinematics.ToWheelSpeeds( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + auto xhat = estimator.UpdateWithTime( + t, + groundTruthState.pose.Rotation() + + frc::Rotation2d(distribution(generator) * 0.05_rad), + wheelSpeeds); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation()) + .to(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().to() / dt.to()), + 0.2); + EXPECT_LT(maxError, 0.4); +} diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp new file mode 100644 index 0000000000..0a028d47a9 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -0,0 +1,93 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +#include "frc/estimator/SwerveDrivePoseEstimator.h" +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/SwerveDriveOdometry.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" + +TEST(SwerveDrivePoseEstimatorTest, TestAccuracy) { + frc::SwerveDriveKinematics<4> kinematics{ + frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, + frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; + + frc::SwerveDrivePoseEstimator<4> estimator{ + frc::Rotation2d(), frc::Pose2d(), kinematics, + {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; + + frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()}; + + frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)), + frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)), + frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)), + frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)), + frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))}, + frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 0.02_s; + units::second_t t = 0_s; + + units::second_t kVisionUpdateRate = 0.1_s; + frc::Pose2d lastVisionPose; + units::second_t lastVisionUpdateTime{-std::numeric_limits::max()}; + + std::vector visionPoses; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + if (lastVisionUpdateTime + kVisionUpdateRate < t) { + if (lastVisionPose != frc::Pose2d()) { + estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + lastVisionPose = + groundTruthState.pose + + frc::Transform2d( + frc::Translation2d(distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m), + frc::Rotation2d(distribution(generator) * 0.1 * 1_rad)); + visionPoses.push_back(lastVisionPose); + lastVisionUpdateTime = t; + } + + auto moduleStates = kinematics.ToSwerveModuleStates( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + auto xhat = estimator.UpdateWithTime( + t, + groundTruthState.pose.Rotation() + + frc::Rotation2d(distribution(generator) * 0.05_rad), + moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation()) + .to(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().to() / dt.to()), + 0.2); + EXPECT_LT(maxError, 0.4); +} diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp index 0017b42cc9..76d29c8077 100644 --- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -13,6 +13,7 @@ #include "Eigen/Core" #include "Eigen/QR" #include "frc/StateSpaceUtil.h" +#include "frc/estimator/AngleStatistics.h" #include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/system/NumericalJacobian.h" #include "frc/system/RungeKutta.h" @@ -94,7 +95,9 @@ TEST(UnscentedKalmanFilterTest, Init) { auto globalY = GlobalMeasurementModel(observer.Xhat(), u); auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01); - observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); + observer.Correct<5>(u, globalY, GlobalMeasurementModel, R, + frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), + frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)); } TEST(UnscentedKalmanFilterTest, Convergence) { @@ -162,7 +165,11 @@ TEST(UnscentedKalmanFilterTest, Convergence) { auto globalY = GlobalMeasurementModel(trueXhat, u); auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5); - observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); + observer.Correct<5>(u, globalY, GlobalMeasurementModel, R, + frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), + frc::AngleResidual<5>(2), frc::AngleAdd<5>(2) + + ); auto finalPosition = trajectory.Sample(trajectory.TotalTime()); ASSERT_NEAR(finalPosition.pose.Translation().X().template to(),