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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
View File

@@ -42,5 +42,5 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
}
void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}

20
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -41,8 +41,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
const Eigen::Vector<double, 2>& u) {
Vectord<2> DifferentialDrivetrainSim::ClampInput(const Vectord<2>& u) {
return frc::DesaturateInputVector<2>(u,
frc::RobotController::GetInputVoltage());
}
@@ -66,11 +65,11 @@ double DifferentialDrivetrainSim::GetGearing() const {
return m_currentGearing;
}
Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetOutput() const {
Vectord<7> DifferentialDrivetrainSim::GetOutput() const {
return m_y;
}
Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetState() const {
Vectord<7> DifferentialDrivetrainSim::GetState() const {
return m_x;
}
@@ -116,8 +115,7 @@ units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
return GetLeftCurrentDraw() + GetRightCurrentDraw();
}
void DifferentialDrivetrainSim::SetState(
const Eigen::Vector<double, 7>& state) {
void DifferentialDrivetrainSim::SetState(const Vectord<7>& state) {
m_x = state;
}
@@ -129,19 +127,19 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
m_x(State::kRightPosition) = 0;
}
Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
const Eigen::Vector<double, 7>& x, const Eigen::Vector<double, 2>& u) {
Vectord<7> DifferentialDrivetrainSim::Dynamics(const Vectord<7>& x,
const Vectord<2>& u) {
// Because G^2 can be factored out of A, we can divide by the old ratio
// squared and multiply by the new ratio squared to get a new drivetrain
// model.
Eigen::Matrix<double, 4, 2> B;
Matrixd<4, 2> B;
B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
m_originalGearing / m_originalGearing;
B.block<2, 2>(2, 0).setZero();
// Because G can be factored out of B, we can divide by the old ratio and
// multiply by the new ratio to get a new drivetrain model.
Eigen::Matrix<double, 4, 4> A;
Matrixd<4, 4> A;
A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
A.block<2, 2>(2, 0).setIdentity();
@@ -149,7 +147,7 @@ Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
Eigen::Vector<double, 7> xdot;
Vectord<7> xdot;
xdot(0) = v * std::cos(x(State::kHeading));
xdot(1) = v * std::sin(x(State::kHeading));
xdot(2) =

18
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -80,29 +80,27 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
}
void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}
Eigen::Vector<double, 2> ElevatorSim::UpdateX(
const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u, units::second_t dt) {
Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
const Vectord<1>& u, units::second_t dt) {
auto updatedXhat = RKDP(
[&](const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
[&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> {
Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{0.0, -9.8};
xdot += Vectord<2>{0.0, -9.8};
}
return xdot;
},
currentXhat, u, dt);
// Check for collision after updating x-hat.
if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_minHeight.value(), 0.0};
return Vectord<2>{m_minHeight.value(), 0.0};
}
if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_maxHeight.value(), 0.0};
return Vectord<2>{m_maxHeight.value(), 0.0};
}
return updatedXhat;
}

2
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
View File

@@ -38,5 +38,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
}
void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}

25
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -72,12 +72,12 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
}
void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}
Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u, units::second_t dt) {
Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat,
const Vectord<1>& u,
units::second_t dt) {
// Horizontal case:
// Torque = F * r = I * alpha
// alpha = F * r / I
@@ -88,15 +88,14 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// std::cos(theta)]]
Eigen::Vector<double, 2> updatedXhat = RKDP(
[&](const auto& x, const auto& u) -> Eigen::Vector<double, 2> {
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
Vectord<2> updatedXhat = RKDP(
[&](const auto& x, const auto& u) -> Vectord<2> {
Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{
0.0, (m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) *
std::cos(x(0)))
.value()};
xdot += Vectord<2>{0.0, (m_armMass * m_r * -9.8 * 3.0 /
(m_armMass * m_r * m_r) * std::cos(x(0)))
.value()};
}
return xdot;
},
@@ -104,9 +103,9 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
// Check for collisions.
if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_minAngle.value(), 0.0};
return Vectord<2>{m_minAngle.value(), 0.0};
} else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_maxAngle.value(), 0.0};
return Vectord<2>{m_maxAngle.value(), 0.0};
}
return updatedXhat;
}