Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move math functionality into new wpimath library (#2629)

Browse Source 
The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers
This commit is contained in:
Peter Johnson
2020-08-06 23:57:39 -07:00
committed by GitHub
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h Normal file
View File

@@ -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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "frc/geometry/Rotation2d.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
* reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
* frame of reference.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
struct ChassisSpeeds {
/**
* Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
*/
units::meters_per_second_t vx = 0_mps;
/**
* Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
*/
units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
units::radians_per_second_t omega = 0_rad_per_s;
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
* @param vx The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vy The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param omega The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away from
* your alliance station wall. Remember that this should be CCW positive.
*
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
}
};
} // namespace frc

70
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h Normal file
View File

@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "math/MathShared.h"
#include "units/angle.h"
#include "units/length.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
*
* Inverse kinematics converts a desired chassis speed into left and right
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
*/
class DifferentialDriveKinematics {
public:
/**
* Constructs a differential drive kinematics object.
*
* @param trackWidth The track width of the drivetrain. Theoretically, this is
* the distance between the left wheels and right wheels. However, the
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
explicit DifferentialDriveKinematics(units::meter_t trackWidth)
: trackWidth(trackWidth) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
}
/**
* Returns a chassis speed from left and right component velocities using
* forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
constexpr ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
}
/**
* Returns left and right component velocities from a chassis speed using
* inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that
* represent the chassis' speed.
* @return The left and right velocities.
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const {
return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
units::meter_t trackWidth;
};
} // namespace frc

87
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* 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 "DifferentialDriveKinematics.h"
#include "frc/geometry/Pose2d.h"
#include "units/length.h"
namespace frc {
/**
* Class for differential drive odometry. Odometry allows you to track the
* robot's position on the field over the course of a match using readings from
* 2 encoders and a gyroscope.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
* It is important that you reset your encoders to zero before using this class.
* Any subsequent pose resets also require the encoders to be reset to zero.
*/
class DifferentialDriveOdometry {
public:
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* 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 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) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_prevLeftDistance = 0_m;
m_prevRightDistance = 0_m;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot position on the field using distance measurements from
* encoders. This method is more numerically accurate than using velocities to
* integrate the pose and is also advantageous for teams that are using lower
* CPR encoders.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
private:
Pose2d m_pose;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
units::meter_t m_prevLeftDistance = 0_m;
units::meter_t m_prevRightDistance = 0_m;
};
} // namespace frc

39
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* 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 "units/velocity.h"
namespace frc {
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
struct DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
units::meters_per_second_t left = 0_mps;
/**
* Speed of the right side of the robot.
*/
units::meters_per_second_t right = 0_mps;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Normalize(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc

145
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h Normal file
View File

@@ -0,0 +1,145 @@
/*----------------------------------------------------------------------------*/
/* 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 <Eigen/Core>
#include <Eigen/QR>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "math/MathShared.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds.
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual wheel speeds) uses the relative locations of the wheels with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion manuevers.
*
* Forward kinematics (converting an array of wheel speeds into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
* multiply by [wheelSpeeds] to get our chassis speeds.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
class MecanumDriveKinematics {
public:
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheel The location of the front-left wheel relative to the
* physical center of the robot.
* @param frontRightWheel The location of the front-right wheel relative to
* the physical center of the robot.
* @param rearLeftWheel The location of the rear-left wheel relative to the
* physical center of the robot.
* @param rearRightWheel The location of the rear-right wheel relative to the
* physical center of the robot.
*/
explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
Translation2d frontRightWheel,
Translation2d rearLeftWheel,
Translation2d rearRightWheel)
: m_frontLeftWheel{frontLeftWheel},
m_frontRightWheel{frontRightWheel},
m_rearLeftWheel{rearLeftWheel},
m_rearRightWheel{rearRightWheel} {
SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
rearRightWheel);
m_forwardKinematics = m_inverseKinematics.householderQr();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_MecanumDrive, 1);
}
MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* manuevers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and
* provide a chassis speed that only has a dtheta
* component, the robot will rotate around that
* corner.
*
* @return The wheel speeds. Use caution because they are not normalized.
* Sometimes, a user input may cause one of the wheel speeds to go
* above the attainable max velocity. Use the
* MecanumDriveWheelSpeeds::Normalize() function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the wheel speeds to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
* @endcode
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d()) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
* given wheel speeds. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed of
* each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
private:
mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
Translation2d m_frontLeftWheel;
Translation2d m_frontRightWheel;
Translation2d m_rearLeftWheel;
Translation2d m_rearRightWheel;
mutable Translation2d m_previousCoR;
/**
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical
* center of the robot.
* @param fr The location of the front-right wheel relative to the physical
* center of the robot.
* @param rl The location of the rear-left wheel relative to the physical
* center of the robot.
* @param rr The location of the rear-right wheel relative to the physical
* center of the robot.
*/
void SetInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr) const;
};
} // namespace frc

107
wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h Normal file
View File

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/timestamp.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* mecanum wheel encoders.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
class MecanumDriveOdometry {
public:
/**
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* 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 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) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
* the current time to calculate period (difference between two timestamps).
* The period is used to calculate the change in distance from a velocity.
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
}
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
} // namespace frc

49
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h Normal file
View File

@@ -0,0 +1,49 @@
/*----------------------------------------------------------------------------*/
/* 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 "units/velocity.h"
namespace frc {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
*/
struct MecanumDriveWheelSpeeds {
/**
* Speed of the front-left wheel.
*/
units::meters_per_second_t frontLeft = 0_mps;
/**
* Speed of the front-right wheel.
*/
units::meters_per_second_t frontRight = 0_mps;
/**
* Speed of the rear-left wheel.
*/
units::meters_per_second_t rearLeft = 0_mps;
/**
* Speed of the rear-right wheel.
*/
units::meters_per_second_t rearRight = 0_mps;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Normalize(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc

170
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h Normal file
View File

@@ -0,0 +1,170 @@
/*----------------------------------------------------------------------------*/
/* 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 <array>
#include <cstddef>
#include <Eigen/Core>
#include <Eigen/QR>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "math/MathShared.h"
#include "units/velocity.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual module states) uses the relative locations of the modules with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion manuevers.
*
* Forward kinematics (converting an array of module states into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
* multiply by [moduleStates] to get our chassis speeds.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
template <size_t NumModules>
class SwerveDriveKinematics {
public:
/**
* Constructs a swerve drive kinematics object. This takes in a variable
* number of wheel locations as Translation2ds. The order in which you pass in
* the wheel locations is the same order that you will recieve the module
* states when performing inverse kinematics. It is also expected that you
* pass in the module states in the same order when calling the forward
* kinematics methods.
*
* @param wheels The locations of the wheels relative to the physical center
* of the robot.
*/
template <typename... Wheels>
explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
: m_modules{wheel, wheels...} {
static_assert(sizeof...(wheels) >= 1,
"A swerve drive requires at least two modules");
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<double>(),
0, 1, (+m_modules[i].X()).template to<double>();
// clang-format on
}
m_forwardKinematics = m_inverseKinematics.householderQr();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
}
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* manuevers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis speed
* that only has a dtheta component, the robot will rotate around that corner.
*
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* <NormalizeWheelSpeeds> function to rectify this issue. In addition, you can
* leverage the power of C++17 to directly assign the module states to
* variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
* @endcode
*/
std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d()) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
*
* @param wheelStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
*
* @return The resulting chassis speed.
*/
template <typename... ModuleStates>
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
*
* @param moduleStates The state of the modules as an std::array of type
* SwerveModuleState, NumModules long as measured from respective encoders
* and gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates) const;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param attainableMaxSpeed The absolute max speed that a module can reach.
*/
static void NormalizeWheelSpeeds(
std::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);
private:
mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
m_forwardKinematics;
std::array<Translation2d, NumModules> m_modules;
mutable Translation2d m_previousCoR;
};
} // namespace frc
#include "SwerveDriveKinematics.inc"

113
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc Normal file
View File

@@ -0,0 +1,113 @@
/*----------------------------------------------------------------------------*/
/* 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 <algorithm>
#include "units/math.h"
namespace frc {
template <class... Wheels>
SwerveDriveKinematics(Translation2d, Wheels...)
-> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
template <size_t NumModules>
std::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation != m_previousCoR) {
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() + centerOfRotation.Y()).template to<double>(),
0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
// clang-format on
}
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector;
chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
m_inverseKinematics * chassisSpeedsVector;
std::array<SwerveModuleState, NumModules> moduleStates;
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x =
units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
units::meters_per_second_t y =
units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.to<double>(), y.to<double>()};
moduleStates[i] = {speed, rotation};
}
return moduleStates;
}
template <size_t NumModules>
template <typename... ModuleStates>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
ModuleStates&&... wheelStates) const {
static_assert(sizeof...(wheelStates) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
return this->ToChassisSpeeds(moduleStates);
}
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
for (size_t i = 0; i < NumModules; i++) {
SwerveModuleState module = moduleStates[i];
moduleStatesMatrix.row(i * 2)
<< module.speed.to<double>() * module.angle.Cos();
moduleStatesMatrix.row(i * 2 + 1)
<< module.speed.to<double>() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStatesMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
std::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed;
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
}
}
}
} // namespace frc

120
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* 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 <chrono>
#include <cstddef>
#include <ctime>
#include <wpi/timestamp.h>
#include "SwerveDriveKinematics.h"
#include "SwerveModuleState.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
namespace frc {
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* swerve drive encoders and swerve azimuth encoders.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
template <size_t NumModules>
class SwerveDriveOdometry {
public:
/**
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* 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 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) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
* the current time to calculate period (difference between two timestamps).
* The period is used to calculate the change in distance from a velocity.
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
}
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
} // namespace frc
#include "SwerveDriveOdometry.inc"

46
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc Normal file
View File

@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* 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 "math/MathShared.h"
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
auto angle = gyroAngle + m_gyroOffset;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
static_cast<void>(dtheta);
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}
} // namespace frc

28
wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h Normal file
View File

@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/geometry/Rotation2d.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents the state of one swerve module.
*/
struct SwerveModuleState {
/**
* Speed of the wheel of the module.
*/
units::meters_per_second_t speed = 0_mps;
/**
* Angle of the module.
*/
Rotation2d angle;
};
} // namespace frc