Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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

10
wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -68,7 +68,8 @@ class SimpleMotorFeedforward {
* @return The maximum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
units::volt_t maxVoltage,
units::unit_t<Acceleration> acceleration) const {
// Assume max velocity is positive
return (maxVoltage - kS - kA * acceleration) / kV;
}
@@ -85,7 +86,8 @@ class SimpleMotorFeedforward {
* @return The minimum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
units::volt_t maxVoltage,
units::unit_t<Acceleration> acceleration) const {
// Assume min velocity is positive, ks flips sign
return (-maxVoltage + kS - kA * acceleration) / kV;
}
@@ -102,7 +104,7 @@ class SimpleMotorFeedforward {
* @return The maximum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
}
@@ -118,7 +120,7 @@ class SimpleMotorFeedforward {
* @return The minimum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}

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++) {

4
wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
View File

@@ -29,10 +29,10 @@ class CentripetalAccelerationConstraint : public TrajectoryConstraint {
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
private:
units::meters_per_second_squared_t m_maxCentripetalAcceleration;

4
wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
View File

@@ -26,10 +26,10 @@ class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
private:
DifferentialDriveKinematics m_kinematics;

4
wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
View File

@@ -38,10 +38,10 @@ class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
private:
SimpleMotorFeedforward<units::meter> m_feedforward;

10
wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
View File

@@ -31,14 +31,14 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
*/
EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
units::meter_t yWidth, const Rotation2d& rotation,
TrajectoryConstraint& constraint);
const TrajectoryConstraint& constraint);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
/**
* Returns whether the specified robot pose is within the region that the
@@ -47,11 +47,11 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose);
bool IsPoseInRegion(const Pose2d& pose) const;
private:
Translation2d m_center;
Translation2d m_radii;
TrajectoryConstraint& m_constraint;
const TrajectoryConstraint& m_constraint;
};
} // namespace frc

4
wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
View File

@@ -29,12 +29,12 @@ class MaxVelocityConstraint : public TrajectoryConstraint {
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override {
units::meters_per_second_t velocity) const override {
return m_maxVelocity;
}
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override {
units::meters_per_second_t speed) const override {
return {};
}

4
wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
View File

@@ -28,10 +28,10 @@ class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
private:
MecanumDriveKinematics m_kinematics;

10
wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
View File

@@ -31,14 +31,14 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
*/
RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
const Translation2d& topRightPoint,
TrajectoryConstraint& constraint);
const TrajectoryConstraint& constraint);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
/**
* Returns whether the specified robot pose is within the region that the
@@ -47,11 +47,11 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose);
bool IsPoseInRegion(const Pose2d& pose) const;
private:
Translation2d m_bottomLeftPoint;
Translation2d m_topRightPoint;
TrajectoryConstraint& m_constraint;
const TrajectoryConstraint& m_constraint;
};
} // namespace frc

4
wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
View File

@@ -31,10 +31,10 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override;
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override;
units::meters_per_second_t speed) const override;
private:
frc::SwerveDriveKinematics<NumModules> m_kinematics;

4
wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
View File

@@ -26,7 +26,7 @@ template <size_t NumModules>
units::meters_per_second_t
SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
@@ -42,7 +42,7 @@ template <size_t NumModules>
TrajectoryConstraint::MinMax
SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
return {};
}

4
wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
View File

@@ -60,7 +60,7 @@ class TrajectoryConstraint {
*/
virtual units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) = 0;
units::meters_per_second_t velocity) const = 0;
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
@@ -74,6 +74,6 @@ class TrajectoryConstraint {
*/
virtual MinMax MinMaxAcceleration(const Pose2d& pose,
units::curvature_t curvature,
units::meters_per_second_t speed) = 0;
units::meters_per_second_t speed) const = 0;
};
} // namespace frc