Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Replace .to<double>() and .template to<double>() with .value() (#3667)

Browse Source 
It's a less verbose way to do the same thing.
This commit is contained in:
Tyler Veness
2021-10-25 08:58:12 -07:00
committed by GitHub
parent 6bc1db44bc
commit 181723e573
134 changed files with 782 additions and 826 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -7,14 +7,14 @@
namespace frc {
Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector<double, 3>{pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>()};
return Eigen::Vector<double, 3>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()};
}
Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector<double, 4>{pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
return Eigen::Vector<double, 4>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Cos(), pose.Rotation().Sin()};
}
@@ -31,8 +31,8 @@ bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
}
Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
return Eigen::Vector<double, 3>{pose.X().to<double>(), pose.Y().to<double>(),
pose.Rotation().Radians().to<double>()};
return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};
}
} // namespace frc

8
wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
View File

@@ -56,10 +56,10 @@ ChassisSpeeds HolonomicDriveController::Calculate(
}
// Calculate feedback velocities (based on position error).
auto xFeedback = units::meters_per_second_t(m_xController.Calculate(
currentPose.X().to<double>(), poseRef.X().to<double>()));
auto yFeedback = units::meters_per_second_t(m_yController.Calculate(
currentPose.Y().to<double>(), poseRef.Y().to<double>()));
auto xFeedback = units::meters_per_second_t(
m_xController.Calculate(currentPose.X().value(), poseRef.X().value()));
auto yFeedback = units::meters_per_second_t(
m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value()));
// Return next output.
return ChassisSpeeds::FromFieldRelativeSpeeds(

8
wpimath/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -21,7 +21,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd,
if (period <= 0_s) {
wpi::math::MathSharedStore::ReportError(
"Controller period must be a non-zero positive number, got {}!",
period.to<double>());
period.value());
m_period = 20_ms;
wpi::math::MathSharedStore::ReportWarning(
"{}", "Controller period defaulted to 20ms.");
@@ -86,7 +86,7 @@ bool PIDController::AtSetpoint() const {
positionError = m_setpoint - m_measurement;
}
double velocityError = (positionError - m_prevError) / m_period.to<double>();
double velocityError = (positionError - m_prevError) / m_period.value();
return std::abs(positionError) < m_positionTolerance &&
std::abs(velocityError) < m_velocityTolerance;
@@ -139,11 +139,11 @@ double PIDController::Calculate(double measurement) {
m_positionError = m_setpoint - measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
m_velocityError = (m_positionError - m_prevError) / m_period.value();
if (m_Ki != 0) {
m_totalError =
std::clamp(m_totalError + m_positionError * m_period.to<double>(),
std::clamp(m_totalError + m_positionError * m_period.value(),
m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
}

10
wpimath/src/main/native/cpp/controller/RamseteController.cpp
View File

@@ -51,11 +51,11 @@ ChassisSpeeds RamseteController::Calculate(
m_poseError = poseRef.RelativeTo(currentPose);
// Aliases for equation readability
double eX = m_poseError.X().to<double>();
double eY = m_poseError.Y().to<double>();
double eTheta = m_poseError.Rotation().Radians().to<double>();
double vRef = linearVelocityRef.to<double>();
double omegaRef = angularVelocityRef.to<double>();
double eX = m_poseError.X().value();
double eY = m_poseError.Y().value();
double eTheta = m_poseError.Rotation().Radians().value();
double vRef = linearVelocityRef.value();
double omegaRef = angularVelocityRef.value();
double k =
2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));

16
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -96,14 +96,12 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
auto u = Eigen::Vector<double, 3>{
(wheelSpeeds.left + wheelSpeeds.right).to<double>() / 2.0, 0.0,
omega.to<double>()};
(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
m_previousAngle = angle;
auto localY = Eigen::Vector<double, 3>{leftDistance.to<double>(),
rightDistance.to<double>(),
angle.Radians().to<double>()};
auto localY = Eigen::Vector<double, 3>{
leftDistance.value(), rightDistance.value(), angle.Radians().value()};
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
m_observer.Predict(u, dt);
@@ -140,8 +138,8 @@ wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
const Pose2d& pose, units::meter_t leftDistance,
units::meter_t rightDistance) {
return Eigen::Vector<double, 5>{
pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
rightDistance.to<double>()};
return Eigen::Vector<double, 5>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value(),
leftDistance.value(), rightDistance.value()};
}

8
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -100,11 +100,11 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().to<double>(),
fieldRelativeVelocities.Y().to<double>(),
omega.to<double>()};
Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(),
omega.value()};
Eigen::Vector<double, 1> localY{angle.Radians().template to<double>()};
Eigen::Vector<double, 1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);

4
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -46,7 +46,7 @@ Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
Pose2d Pose2d::Exp(const Twist2d& twist) const {
const auto dx = twist.dx;
const auto dy = twist.dy;
const auto dtheta = twist.dtheta.to<double>();
const auto dtheta = twist.dtheta.value();
const auto sinTheta = std::sin(dtheta);
const auto cosTheta = std::cos(dtheta);
@@ -68,7 +68,7 @@ Pose2d Pose2d::Exp(const Twist2d& twist) const {
Twist2d Pose2d::Log(const Pose2d& end) const {
const auto transform = end.RelativeTo(*this);
const auto dtheta = transform.Rotation().Radians().to<double>();
const auto dtheta = transform.Rotation().Radians().value();
const auto halfDtheta = dtheta / 2.0;
const auto cosMinusOne = transform.Rotation().Cos() - 1;

2
wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -64,7 +64,7 @@ Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
}
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
json = wpi::json{{"radians", rotation.Radians().to<double>()}};
json = wpi::json{{"radians", rotation.Radians().value()}};
}
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {

4
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -59,8 +59,8 @@ bool Translation2d::operator!=(const Translation2d& other) const {
}
void frc::to_json(wpi::json& json, const Translation2d& translation) {
json = wpi::json{{"x", translation.X().to<double>()},
{"y", translation.Y().to<double>()}};
json =
wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
}
void frc::from_json(const wpi::json& json, Translation2d& translation) {

14
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -50,13 +50,13 @@ std::vector<double> GetElementsFromTrajectory(
elements.reserve(trajectory.States().size() * 7);
for (auto&& state : trajectory.States()) {
elements.push_back(state.t.to<double>());
elements.push_back(state.velocity.to<double>());
elements.push_back(state.acceleration.to<double>());
elements.push_back(state.pose.X().to<double>());
elements.push_back(state.pose.Y().to<double>());
elements.push_back(state.pose.Rotation().Radians().to<double>());
elements.push_back(state.curvature.to<double>());
elements.push_back(state.t.value());
elements.push_back(state.velocity.value());
elements.push_back(state.acceleration.value());
elements.push_back(state.pose.X().value());
elements.push_back(state.pose.Y().value());
elements.push_back(state.pose.Rotation().Radians().value());
elements.push_back(state.curvature.value());
}
return elements;

20
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -21,9 +21,9 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(),
chassisSpeeds.omega.to<double>()};
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Vector<double, 4> wheelsVector =
m_inverseKinematics * chassisSpeedsVector;
@@ -39,8 +39,8 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Eigen::Vector<double, 4> wheelSpeedsVector{
wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>()};
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(wheelSpeedsVector);
@@ -54,9 +54,9 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) const {
m_inverseKinematics = Eigen::Matrix<double, 4, 3>{
{1, -1, (-(fl.X() + fl.Y())).template to<double>()},
{1, 1, (fr.X() - fr.Y()).template to<double>()},
{1, 1, (rl.X() - rl.Y()).template to<double>()},
{1, -1, (-(rr.X() + rr.Y())).template to<double>()}};
m_inverseKinematics =
Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
{1, 1, (fr.X() - fr.Y()).value()},
{1, 1, (rl.X() - rl.Y()).value()},
{1, -1, (-(rr.X() + rr.Y())).value()}};
}

50
wpimath/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -52,29 +52,27 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
c.emplace_back(0);
// populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
xInitial[1]);
dy.emplace_back(
3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
yInitial[1]);
dx.emplace_back(3 * (waypoints[2].X().value() - waypoints[0].X().value()) -
xInitial[1]);
dy.emplace_back(3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) -
yInitial[1]);
if (waypoints.size() > 4) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
// dx and dy represent the derivatives of the internal waypoints. The
// derivative of the second internal waypoint should involve the third
// and first internal waypoint, which have indices of 1 and 3 in the
// waypoints list (which contains ALL waypoints).
dx.emplace_back(3 * (waypoints[i + 2].X().to<double>() -
waypoints[i].X().to<double>()));
dy.emplace_back(3 * (waypoints[i + 2].Y().to<double>() -
waypoints[i].Y().to<double>()));
dx.emplace_back(
3 * (waypoints[i + 2].X().value() - waypoints[i].X().value()));
dy.emplace_back(
3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value()));
}
}
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
waypoints[waypoints.size() - 3].X().to<double>()) -
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() -
waypoints[waypoints.size() - 3].X().value()) -
xFinal[1]);
dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
waypoints[waypoints.size() - 3].Y().to<double>()) -
dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() -
waypoints[waypoints.size() - 3].Y().value()) -
yFinal[1]);
// Compute solution to tridiagonal system
@@ -89,10 +87,10 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
for (size_t i = 0; i < fx.size() - 1; ++i) {
// Create the spline.
const CubicHermiteSpline spline{
{waypoints[i].X().to<double>(), fx[i]},
{waypoints[i + 1].X().to<double>(), fx[i + 1]},
{waypoints[i].Y().to<double>(), fy[i]},
{waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
{waypoints[i].X().value(), fx[i]},
{waypoints[i + 1].X().value(), fx[i + 1]},
{waypoints[i].Y().value(), fy[i]},
{waypoints[i + 1].Y().value(), fy[i + 1]}};
splines.push_back(spline);
}
@@ -102,10 +100,8 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
wpi::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
xDeriv};
wpi::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
yDeriv};
wpi::array<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
wpi::array<double, 2> midYControlVector{waypoints[0].Y().value(), yDeriv};
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
@@ -140,16 +136,14 @@ SplineHelper::CubicControlVectorsFromWaypoints(
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
scalar = 1.2 * start.Translation().Distance(end.Translation()).value();
} else {
scalar =
1.2 *
start.Translation().Distance(interiorWaypoints.front()).to<double>();
1.2 * start.Translation().Distance(interiorWaypoints.front()).value();
}
const auto initialCV = CubicControlVector(scalar, start);
if (!interiorWaypoints.empty()) {
scalar =
1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
scalar = 1.2 * end.Translation().Distance(interiorWaypoints.back()).value();
}
const auto finalCV = CubicControlVector(scalar, end);
return {initialCV, finalCV};
@@ -165,7 +159,7 @@ std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
// This just makes the splines look better.
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
1.2 * p0.Translation().Distance(p1.Translation()).value();
auto controlVectorA = QuinticControlVector(scalar, p0);
auto controlVectorB = QuinticControlVector(scalar, p1);

8
wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -146,11 +146,11 @@ Trajectory Trajectory::operator+(const Trajectory& other) const {
}
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
json = wpi::json{{"time", state.t.to<double>()},
{"velocity", state.velocity.to<double>()},
{"acceleration", state.acceleration.to<double>()},
json = wpi::json{{"time", state.t.value()},
{"velocity", state.velocity.value()},
{"acceleration", state.acceleration.value()},
{"pose", state.pose},
{"curvature", state.curvature.to<double>()}};
{"curvature", state.curvature.value()}};
}
void frc::from_json(const wpi::json& json, Trajectory::State& state) {

4
wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
View File

@@ -85,7 +85,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// Now enforce all acceleration limits.
EnforceAccelerationLimits(reversed, constraints, &constrainedState);
if (ds.to<double>() < kEpsilon) {
if (ds.value() < kEpsilon) {
break;
}
@@ -141,7 +141,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// Check all acceleration constraints with the new max velocity.
EnforceAccelerationLimits(reversed, constraints, &constrainedState);
if (ds.to<double>() > -kEpsilon) {
if (ds.value() > -kEpsilon) {
break;
}

2
wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
View File

@@ -166,7 +166,7 @@ class ControlAffinePlantInversionFeedforward {
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.to<double>();
Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
m_uff = m_B.householderQr().solve(
rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));

14
wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -193,8 +193,8 @@ class ProfiledPIDController
* @param maximumInput The maximum value expected from the input.
*/
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
m_controller.EnableContinuousInput(minimumInput.template to<double>(),
maximumInput.template to<double>());
m_controller.EnableContinuousInput(minimumInput.value(),
maximumInput.value());
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
@@ -227,8 +227,8 @@ class ProfiledPIDController
void SetTolerance(
Distance_t positionTolerance,
Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
m_controller.SetTolerance(positionTolerance.template to<double>(),
velocityTolerance.template to<double>());
m_controller.SetTolerance(positionTolerance.value(),
velocityTolerance.value());
}
/**
@@ -273,8 +273,8 @@ class ProfiledPIDController
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
m_setpoint = profile.Calculate(GetPeriod());
return m_controller.Calculate(measurement.template to<double>(),
m_setpoint.position.template to<double>());
return m_controller.Calculate(measurement.value(),
m_setpoint.position.value());
}
/**
@@ -351,7 +351,7 @@ class ProfiledPIDController
builder.AddDoubleProperty(
"d", [this] { return GetD(); }, [this](double value) { SetD(value); });
builder.AddDoubleProperty(
"goal", [this] { return GetGoal().position.template to<double>(); },
"goal", [this] { return GetGoal().position.value(); },
[this](double value) { SetGoal(Distance_t{value}); });
}

6
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -70,10 +70,10 @@ class SimpleMotorFeedforward {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Eigen::Vector<double, 1> r{currentVelocity.template to<double>()};
Eigen::Vector<double, 1> nextR{nextVelocity.template to<double>()};
Eigen::Vector<double, 1> r{currentVelocity.value()};
Eigen::Vector<double, 1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.template to<double>()) +
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};
}

2
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

@@ -25,7 +25,7 @@ Eigen::Vector<double, States> AngleResidual(
const Eigen::Vector<double, States>& b, int angleStateIdx) {
Eigen::Vector<double, States> ret = a - b;
ret[angleStateIdx] =
AngleModulus(units::radian_t{ret[angleStateIdx]}).to<double>();
AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
return ret;
}

7
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -269,11 +269,10 @@ class SwerveDrivePoseEstimator {
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().template to<double>(),
fieldRelativeSpeeds.Y().template to<double>(),
omega.template to<double>()};
Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
fieldRelativeSpeeds.Y().value(), omega.value()};
Eigen::Vector<double, 1> localY{angle.Radians().template to<double>()};
Eigen::Vector<double, 1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);

6
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -125,7 +125,7 @@ class LinearFilter {
*/
static LinearFilter<T> SinglePoleIIR(double timeConstant,
units::second_t period) {
double gain = std::exp(-period.to<double>() / timeConstant);
double gain = std::exp(-period.value() / timeConstant);
return LinearFilter({1.0 - gain}, {-gain});
}
@@ -144,7 +144,7 @@ class LinearFilter {
* user.
*/
static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
double gain = std::exp(-period.to<double>() / timeConstant);
double gain = std::exp(-period.value() / timeConstant);
return LinearFilter({gain, -gain}, {-gain});
}
@@ -225,7 +225,7 @@ class LinearFilter {
}
Eigen::Vector<double, Samples> a =
S.householderQr().solve(d) / std::pow(period.to<double>(), Derivative);
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
// Reverse gains list
std::vector<double> gains;

8
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -65,8 +65,8 @@ class SwerveDriveKinematics {
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>();
1, 0, (-m_modules[i].Y()).value(),
0, 1, (+m_modules[i].X()).value();
// clang-format on
}
@@ -82,8 +82,8 @@ class SwerveDriveKinematics {
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>();
1, 0, (-m_modules[i].Y()).value(),
0, 1, (+m_modules[i].X()).value();
// clang-format on
}

17
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -26,16 +26,16 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
Eigen::Matrix<double, 2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>()}};
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
}
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(),
chassisSpeeds.omega.to<double>()};
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
m_inverseKinematics * chassisSpeedsVector;
@@ -46,7 +46,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.to<double>(), y.to<double>()};
Rotation2d rotation{x.value(), y.value()};
moduleStates[i] = {speed, rotation};
}
@@ -74,10 +74,9 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStatesMatrix(i * 2, 0) =
module.speed.to<double>() * module.angle.Cos();
moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStatesMatrix(i * 2 + 1, 0) =
module.speed.to<double>() * module.angle.Sin();
module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =

3
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -109,8 +109,7 @@ class Spline {
* @return The vector.
*/
static Eigen::Vector2d ToVector(const Translation2d& translation) {
return Eigen::Vector2d{translation.X().to<double>(),
translation.Y().to<double>()};
return Eigen::Vector2d{translation.X().value(), translation.Y().value()};
}
/**

8
wpimath/src/main/native/include/frc/spline/SplineHelper.h
View File

@@ -80,14 +80,14 @@ class WPILIB_DLLEXPORT SplineHelper {
private:
static Spline<3>::ControlVector CubicControlVector(double scalar,
const Pose2d& point) {
return {{point.X().to<double>(), scalar * point.Rotation().Cos()},
{point.Y().to<double>(), scalar * point.Rotation().Sin()}};
return {{point.X().value(), scalar * point.Rotation().Cos()},
{point.Y().value(), scalar * point.Rotation().Sin()}};
}
static Spline<5>::ControlVector QuinticControlVector(double scalar,
const Pose2d& point) {
return {{point.X().to<double>(), scalar * point.Rotation().Cos(), 0.0},
{point.Y().to<double>(), scalar * point.Rotation().Sin(), 0.0}};
return {{point.X().value(), scalar * point.Rotation().Cos(), 0.0},
{point.Y().value(), scalar * point.Rotation().Sin(), 0.0}};
}
/**

15
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -21,7 +21,7 @@ template <int States>
void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA) {
*discA = (contA * dt.to<double>()).exp();
*discA = (contA * dt.value()).exp();
}
/**
@@ -42,8 +42,8 @@ void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
// Matrices are blocked here to minimize matrix exponentiation calculations
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.to<double>();
Mcont.template block<States, Inputs>(0, States) = contB * dt.to<double>();
Mcont.template block<States, States>(0, 0) = contA * dt.value();
Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
// Discretize A and B with the given timestep
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
@@ -76,8 +76,7 @@ void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
Eigen::Matrix<double, 2 * States, 2 * States> phi =
(M * dt.to<double>()).exp();
Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
@@ -122,7 +121,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Eigen::Matrix<double, States, States> lastTerm = Q;
double lastCoeff = dt.to<double>();
double lastCoeff = dt.value();
// Aᵀⁿ
Eigen::Matrix<double, States, States> Atn = contA.transpose();
@@ -132,7 +131,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
lastTerm = -contA * lastTerm + Q * Atn;
lastCoeff *= dt.to<double>() / static_cast<double>(i);
lastCoeff *= dt.value() / static_cast<double>(i);
phi12 += lastTerm * lastCoeff;
@@ -156,7 +155,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
template <int Outputs>
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
return R / dt.to<double>();
return R / dt.value();
}
} // namespace frc

5
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -50,8 +50,7 @@ class LinearSystemLoop {
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
return frc::NormalizeInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -97,7 +96,7 @@ class LinearSystemLoop {
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
u, maxVoltage.value());
}) {}
/**

16
wpimath/src/main/native/include/frc/system/NumericalIntegration.h
View File

@@ -23,7 +23,7 @@ namespace frc {
*/
template <typename F, typename T>
T RK4(F&& f, T x, units::second_t dt) {
const auto h = dt.to<double>();
const auto h = dt.value();
T k1 = f(x);
T k2 = f(x + h * 0.5 * k1);
@@ -43,7 +43,7 @@ T RK4(F&& f, T x, units::second_t dt) {
*/
template <typename F, typename T, typename U>
T RK4(F&& f, T x, U u, units::second_t dt) {
const auto h = dt.to<double>();
const auto h = dt.value();
T k1 = f(x, u);
T k2 = f(x + h * 0.5 * k1, u);
@@ -91,13 +91,13 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
double truncationError;
double dtElapsed = 0.0;
double h = dt.to<double>();
double h = dt.value();
// Loop until we've gotten to our desired dt
while (dtElapsed < dt.to<double>()) {
while (dtElapsed < dt.value()) {
do {
// Only allow us to advance up to the dt remaining
h = std::min(h, dt.to<double>() - dtElapsed);
h = std::min(h, dt.value() - dtElapsed);
// Notice how the derivative in the Wikipedia notation is dy/dx.
// That means their y is our x and their x is our t
@@ -167,13 +167,13 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
double truncationError;
double dtElapsed = 0.0;
double h = dt.to<double>();
double h = dt.value();
// Loop until we've gotten to our desired dt
while (dtElapsed < dt.to<double>()) {
while (dtElapsed < dt.value()) {
do {
// Only allow us to advance up to the dt remaining
h = std::min(h, dt.to<double>() - dtElapsed);
h = std::min(h, dt.value() - dtElapsed);
// clang-format off
T k1 = f(x, u);

55
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -47,9 +47,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.to<double>()}};
Eigen::Matrix<double, 2, 1> B{
0.0, (G * motor.Kt / (motor.R * r * m)).to<double>()};
.value()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * r * m)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -71,10 +71,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
DCMotor motor, units::kilogram_square_meter_t J, double G) {
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0,
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * J)).to<double>()};
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -107,9 +105,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
Eigen::Matrix<double, 1, 1> A{-kV.template to<double>() /
kA.template to<double>()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.template to<double>()};
Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -142,10 +139,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
static LinearSystem<2, 1, 1> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0, -kV.template to<double>() / kA.template to<double>()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.template to<double>()};
Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -170,12 +165,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
double A1 = -(kVlinear.to<double>() / kAlinear.to<double>() +
kVangular.to<double>() / kAangular.to<double>());
double A2 = -(kVlinear.to<double>() / kAlinear.to<double>() -
kVangular.to<double>() / kAangular.to<double>());
double B1 = 1.0 / kAlinear.to<double>() + 1.0 / kAangular.to<double>();
double B2 = 1.0 / kAlinear.to<double>() - 1.0 / kAangular.to<double>();
double A1 = -(kVlinear.value() / kAlinear.value() +
kVangular.value() / kAangular.value());
double A2 = -(kVlinear.value() / kAlinear.value() -
kVangular.value() / kAangular.value());
double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
Eigen::Matrix<double, 2, 2> A =
0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
@@ -239,8 +234,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
units::kilogram_square_meter_t J,
double G) {
Eigen::Matrix<double, 1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).to<double>()};
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -269,15 +264,15 @@ class WPILIB_DLLEXPORT LinearSystemId {
auto C2 = G * motor.Kt / (motor.R * r);
Eigen::Matrix<double, 2, 2> A{
{((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>()}};
{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Eigen::Matrix<double, 2, 2> B{
{((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>()}};
{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};