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

Add kinematics suite (#1787)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Prateek Machiraju
2019-09-08 00:11:49 -04:00
committed by Peter Johnson
parent 561cbbd144
commit f405582f86
67 changed files with 5060 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,65 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include "frc/geometry/Rotation2d.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

60
wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h Normal file
View File

@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.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);
/**
* Returns a chassis speed from left and right component velocities using
* forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const;
/**
* 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.
*/
DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const;
private:
units::meter_t m_trackWidth;
};
} // namespace frc

95
wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h Normal file
View File

@@ -0,0 +1,95 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include "DifferentialDriveKinematics.h"
#include "frc/geometry/Pose2d.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.
*
* Note: It is important to reset both your encoders to zero before you start
* using this class. Only reset your encoders ONCE. You should not reset your
* encoders even if you want to reset your robot's pose.
*/
class DifferentialDriveOdometry {
public:
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param kinematics The differential drive kinematics for your drivetrain.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
}
/**
* 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 angle The angle of the robot.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& angle,
const DifferentialDriveWheelSpeeds& 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 angle The angle of the robot.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& angle,
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
const auto now = std::chrono::system_clock::now().time_since_epoch();
units::second_t time{now};
return UpdateWithTime(time, angle, wheelSpeeds);
}
private:
DifferentialDriveKinematics m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
};
} // namespace frc

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

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.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

141
wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h Normal file
View File

@@ -0,0 +1,141 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <Eigen/Core>
#include <Eigen/QR>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.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();
}
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());
/**
* 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);
private:
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;
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);
};
} // namespace frc

94
wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h Normal file
View File

@@ -0,0 +1,94 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.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 initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
}
/**
* 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 angle The angle of the robot.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& angle,
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 angle The angle of the robot.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& angle,
MecanumDriveWheelSpeeds wheelSpeeds) {
const auto now = std::chrono::system_clock::now().time_since_epoch();
units::second_t time{now};
return UpdateWithTime(time, angle, wheelSpeeds);
}
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
};
} // namespace frc

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

@@ -0,0 +1,49 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.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

150
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h Normal file
View File

@@ -0,0 +1,150 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include <cstddef>
#include <Eigen/Core>
#include <Eigen/QR>
#include <units/units.h>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/SwerveModuleState.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();
}
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());
/**
* 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);
/**
* 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:
Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
m_forwardKinematics;
std::array<Translation2d, NumModules> m_modules;
Translation2d m_previousCoR;
};
} // namespace frc
#include "SwerveDriveKinematics.inc"

104
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc Normal file
View File

@@ -0,0 +1,104 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <algorithm>
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) {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation.X() != m_previousCoR.X() ||
centerOfRotation.Y() != m_previousCoR.Y()) {
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) {
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...};
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

107
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h Normal file
View File

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <chrono>
#include <cstddef>
#include <ctime>
#include <units/units.h>
#include "SwerveDriveKinematics.h"
#include "SwerveModuleState.h"
#include "frc/geometry/Pose2d.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 initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
}
/**
* 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 angle The angle of the robot.
* @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& angle,
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 angle The angle of the robot.
* @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& angle,
ModuleStates&&... moduleStates) {
const auto now = std::chrono::system_clock::now().time_since_epoch();
units::second_t time{now};
return UpdateWithTime(time, angle, moduleStates...);
}
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
};
} // namespace frc
#include "SwerveDriveOdometry.inc"

38
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc Normal file
View File

@@ -0,0 +1,38 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
}
template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& angle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
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

29
wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include "frc/geometry/Rotation2d.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