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