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

[wpilibc] Fix const-qualification in kinematics and constraints (#2478)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-04-14 01:32:25 -04:00
committed by GitHub
parent a3a8472b82
commit e5935a4737
21 changed files with 71 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* 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. */
@@ -101,7 +101,7 @@ class MecanumDriveKinematics {
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d());
const Translation2d& centerOfRotation = Translation2d()) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -113,17 +113,18 @@ class MecanumDriveKinematics {
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds);
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
private:
Eigen::Matrix<double, 4, 3> m_inverseKinematics;
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;
Translation2d m_previousCoR;
mutable Translation2d m_previousCoR;
/**
* Construct inverse kinematics matrix from wheel locations.
@@ -138,7 +139,7 @@ class MecanumDriveKinematics {
* center of the robot.
*/
void SetInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr);
Translation2d rl, Translation2d rr) const;
};
} // namespace frc

12
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* 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. */
@@ -108,7 +108,7 @@ class SwerveDriveKinematics {
*/
std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d());
const Translation2d& centerOfRotation = Translation2d()) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -123,7 +123,7 @@ class SwerveDriveKinematics {
* @return The resulting chassis speed.
*/
template <typename... ModuleStates>
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -139,7 +139,7 @@ class SwerveDriveKinematics {
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates);
std::array<SwerveModuleState, NumModules> moduleStates) const;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
@@ -158,12 +158,12 @@ class SwerveDriveKinematics {
units::meters_per_second_t attainableMaxSpeed);
private:
Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
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;
Translation2d m_previousCoR;
mutable Translation2d m_previousCoR;
};
} // namespace frc

9
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* 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. */
@@ -18,7 +18,8 @@ SwerveDriveKinematics(Translation2d, Wheels...)
template <size_t NumModules>
std::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
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++) {
@@ -57,7 +58,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
template <size_t NumModules>
template <typename... ModuleStates>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
ModuleStates&&... wheelStates) {
ModuleStates&&... wheelStates) const {
static_assert(sizeof...(wheelStates) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
@@ -69,7 +70,7 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates) {
std::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
for (size_t i = 0; i < NumModules; i++) {