Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

9
wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
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. */
@@ -10,7 +10,8 @@
using namespace frc;
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
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) {
auto fl = m_frontLeftWheel - centerOfRotation;
@@ -39,7 +40,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) {
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
// clang-format off
wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
@@ -57,7 +58,7 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) {
Translation2d rr) const {
// clang-format off
m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
1, 1, (fr.X() - fr.Y()).template to<double>(),

4
wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
View File

@@ -15,7 +15,7 @@ CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
// ac = v^2 / r
// k (curvature) = 1 / r
@@ -33,7 +33,7 @@ units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
TrajectoryConstraint::MinMax
CentripetalAccelerationConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return {};

4
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
View File

@@ -15,7 +15,7 @@ DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
@@ -26,6 +26,6 @@ units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
TrajectoryConstraint::MinMax
DifferentialDriveKinematicsConstraint::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/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
View File

@@ -23,14 +23,14 @@ DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
return units::meters_per_second_t(std::numeric_limits<double>::max());
}
TrajectoryConstraint::MinMax
DifferentialDriveVoltageConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});

8
wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
View File

@@ -13,7 +13,7 @@ using namespace frc;
EllipticalRegionConstraint::EllipticalRegionConstraint(
const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
const Rotation2d& rotation, TrajectoryConstraint& constraint)
const Rotation2d& rotation, const TrajectoryConstraint& constraint)
: m_center(center),
m_radii(xWidth / 2.0, yWidth / 2.0),
m_constraint(constraint) {
@@ -22,7 +22,7 @@ EllipticalRegionConstraint::EllipticalRegionConstraint(
units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
@@ -32,7 +32,7 @@ units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
@@ -40,7 +40,7 @@ TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
}
}
bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
// The region (disk) bounded by the ellipse is given by the equation:
// ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
// If the inequality is satisfied, then it is inside the ellipse; otherwise

4
wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
View File

@@ -15,7 +15,7 @@ MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
units::meters_per_second_t MecanumDriveKinematicsConstraint::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 =
@@ -30,6 +30,6 @@ units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
TrajectoryConstraint::MinMax
MecanumDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
return {};
}

8
wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
View File

@@ -13,14 +13,14 @@ using namespace frc;
RectangularRegionConstraint::RectangularRegionConstraint(
const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
TrajectoryConstraint& constraint)
const TrajectoryConstraint& constraint)
: m_bottomLeftPoint(bottomLeftPoint),
m_topRightPoint(topRightPoint),
m_constraint(constraint) {}
units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
@@ -30,7 +30,7 @@ units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
@@ -38,7 +38,7 @@ TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
}
}
bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
return pose.Translation().X() >= m_bottomLeftPoint.X() &&
pose.Translation().X() <= m_topRightPoint.X() &&
pose.Translation().Y() >= m_bottomLeftPoint.Y() &&