Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21: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;
}

19
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -4,11 +4,11 @@
#pragma once
#include <frc/EigenCore.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <Eigen/Core>
#include <units/length.h>
#include <units/moment_of_inertia.h>
#include <units/time.h>
@@ -80,7 +80,7 @@ class DifferentialDrivetrainSim {
* @param u The input vector.
* @return The normalized input.
*/
Eigen::Vector<double, 2> ClampInput(const Eigen::Vector<double, 2>& u);
Vectord<2> ClampInput(const Vectord<2>& u);
/**
* Sets the applied voltage to the drivetrain. Note that positive voltage must
@@ -178,7 +178,7 @@ class DifferentialDrivetrainSim {
*
* @param state The state.
*/
void SetState(const Eigen::Vector<double, 7>& state);
void SetState(const Vectord<7>& state);
/**
* Sets the system pose.
@@ -187,8 +187,7 @@ class DifferentialDrivetrainSim {
*/
void SetPose(const frc::Pose2d& pose);
Eigen::Vector<double, 7> Dynamics(const Eigen::Vector<double, 7>& x,
const Eigen::Vector<double, 2>& u);
Vectord<7> Dynamics(const Vectord<7>& x, const Vectord<2>& u);
class State {
public:
@@ -301,7 +300,7 @@ class DifferentialDrivetrainSim {
/**
* Returns the current output vector y.
*/
Eigen::Vector<double, 7> GetOutput() const;
Vectord<7> GetOutput() const;
/**
* Returns an element of the state vector. Note that this will not include
@@ -314,7 +313,7 @@ class DifferentialDrivetrainSim {
/**
* Returns the current state vector x. Note that this will not include noise!
*/
Eigen::Vector<double, 7> GetState() const;
Vectord<7> GetState() const;
LinearSystem<2, 2, 2> m_plant;
units::meter_t m_rb;
@@ -325,9 +324,9 @@ class DifferentialDrivetrainSim {
double m_originalGearing;
double m_currentGearing;
Eigen::Vector<double, 7> m_x;
Eigen::Vector<double, 2> m_u;
Eigen::Vector<double, 7> m_y;
Vectord<7> m_x;
Vectord<2> m_u;
Vectord<7> m_y;
std::array<double, 7> m_measurementStdDevs;
};
} // namespace frc::sim

5
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

@@ -127,9 +127,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u,
units::second_t dt) override;
Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
units::second_t dt) override;
private:
DCMotor m_gearbox;

28
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
View File

@@ -6,10 +6,10 @@
#include <array>
#include <Eigen/Core>
#include <units/current.h>
#include <units/time.h>
#include "frc/EigenCore.h"
#include "frc/RobotController.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
@@ -40,9 +40,9 @@ class LinearSystemSim {
const LinearSystem<States, Inputs, Outputs>& system,
const std::array<double, Outputs>& measurementStdDevs = {})
: m_plant(system), m_measurementStdDevs(measurementStdDevs) {
m_x = Eigen::Vector<double, States>::Zero();
m_y = Eigen::Vector<double, Outputs>::Zero();
m_u = Eigen::Vector<double, Inputs>::Zero();
m_x = Vectord<States>::Zero();
m_y = Vectord<Outputs>::Zero();
m_u = Vectord<Inputs>::Zero();
}
virtual ~LinearSystemSim() = default;
@@ -71,7 +71,7 @@ class LinearSystemSim {
*
* @return The current output of the plant.
*/
const Eigen::Vector<double, Outputs>& GetOutput() const { return m_y; }
const Vectord<Outputs>& GetOutput() const { return m_y; }
/**
* Returns an element of the current output of the plant.
@@ -86,7 +86,7 @@ class LinearSystemSim {
*
* @param u The system inputs.
*/
void SetInput(const Eigen::Vector<double, Inputs>& u) { m_u = ClampInput(u); }
void SetInput(const Vectord<Inputs>& u) { m_u = ClampInput(u); }
/*
* Sets the system inputs.
@@ -104,7 +104,7 @@ class LinearSystemSim {
*
* @param state The new state.
*/
void SetState(const Eigen::Vector<double, States>& state) { m_x = state; }
void SetState(const Vectord<States>& state) { m_x = state; }
/**
* Returns the current drawn by this simulated system. Override this method to
@@ -124,9 +124,9 @@ class LinearSystemSim {
* @param u The system inputs (usually voltage).
* @param dt The time difference between controller updates.
*/
virtual Eigen::Vector<double, States> UpdateX(
const Eigen::Vector<double, States>& currentXhat,
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
virtual Vectord<States> UpdateX(const Vectord<States>& currentXhat,
const Vectord<Inputs>& u,
units::second_t dt) {
return m_plant.CalculateX(currentXhat, u, dt);
}
@@ -137,16 +137,16 @@ class LinearSystemSim {
* @param u The input vector.
* @return The normalized input.
*/
Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
Vectord<Inputs> ClampInput(Vectord<Inputs> u) {
return frc::DesaturateInputVector<Inputs>(
u, frc::RobotController::GetInputVoltage());
}
LinearSystem<States, Inputs, Outputs> m_plant;
Eigen::Vector<double, States> m_x;
Eigen::Vector<double, Outputs> m_y;
Eigen::Vector<double, Inputs> m_u;
Vectord<States> m_x;
Vectord<Outputs> m_y;
Vectord<Inputs> m_u;
std::array<double, Outputs> m_measurementStdDevs;
};
} // namespace frc::sim

5
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -142,9 +142,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u,
units::second_t dt) override;
Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
units::second_t dt) override;
private:
units::meter_t m_r;

8
wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
View File

@@ -30,10 +30,10 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
frc::RamseteController ramsete;
feedforward.Reset(Eigen::Vector<double, 2>{0.0, 0.0});
feedforward.Reset(frc::Vectord<2>{0.0, 0.0});
// Ground truth.
Eigen::Vector<double, 7> groundTruthX = Eigen::Vector<double, 7>::Zero();
frc::Vectord<7> groundTruthX = frc::Vectord<7>::Zero();
frc::TrajectoryConfig config{1_mps, 1_mps_sq};
config.AddConstraint(
@@ -48,7 +48,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
auto voltages =
feedforward.Calculate(Eigen::Vector<double, 2>{l.value(), r.value()});
feedforward.Calculate(frc::Vectord<2>{l.value(), r.value()});
// Sim periodic code.
sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0)));
@@ -56,7 +56,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
// Update ground truth.
groundTruthX = frc::RK4(
[&sim](const auto& x, const auto& u) -> Eigen::Vector<double, 7> {
[&sim](const auto& x, const auto& u) -> frc::Vectord<7> {
return sim.Dynamics(x, u);
},
groundTruthX, voltages, 20_ms);

19
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -34,8 +34,7 @@ TEST(ElevatorSimTest, StateSpaceSim) {
auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
Eigen::Vector<double, 1> u{motor.Get() *
frc::RobotController::GetInputVoltage()};
frc::Vectord<1> u{motor.Get() * frc::RobotController::GetInputVoltage()};
sim.SetInput(u);
sim.Update(20_ms);
@@ -51,7 +50,7 @@ TEST(ElevatorSimTest, MinMax) {
units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
true, {0.01});
for (size_t i = 0; i < 100; ++i) {
sim.SetInput(Eigen::Vector<double, 1>{0.0});
sim.SetInput(frc::Vectord<1>{0.0});
sim.Update(20_ms);
auto height = sim.GetPosition();
@@ -59,7 +58,7 @@ TEST(ElevatorSimTest, MinMax) {
}
for (size_t i = 0; i < 100; ++i) {
sim.SetInput(Eigen::Vector<double, 1>{12.0});
sim.SetInput(frc::Vectord<1>{12.0});
sim.Update(20_ms);
auto height = sim.GetPosition();
@@ -71,16 +70,16 @@ TEST(ElevatorSimTest, Stability) {
frc::sim::ElevatorSim sim{
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, true};
sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
sim.SetInput(Eigen::Vector<double, 1>{12.0});
sim.SetState(frc::Vectord<2>{0.0, 0.0});
sim.SetInput(frc::Vectord<1>{12.0});
for (int i = 0; i < 50; ++i) {
sim.Update(20_ms);
}
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
frc::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100);
EXPECT_NEAR_UNITS(units::meter_t{system.CalculateX(
Eigen::Vector<double, 2>{0.0, 0.0},
Eigen::Vector<double, 1>{12.0}, 20_ms * 50)(0)},
sim.GetPosition(), 1_cm);
EXPECT_NEAR_UNITS(
units::meter_t{system.CalculateX(frc::Vectord<2>{0.0, 0.0},
frc::Vectord<1>{12.0}, 20_ms * 50)(0)},
sim.GetPosition(), 1_cm);
}

4
wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
View File

@@ -10,10 +10,10 @@
TEST(SingleJointedArmTest, Disabled) {
frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m,
9.5_in, -180_deg, 0_deg, 10_lb, true);
sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
sim.SetState(frc::Vectord<2>{0.0, 0.0});
for (size_t i = 0; i < 12 / 0.02; ++i) {
sim.SetInput(Eigen::Vector<double, 1>{0.0});
sim.SetInput(frc::Vectord<1>{0.0});
sim.Update(20_ms);
}

4
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
View File

@@ -46,8 +46,8 @@ TEST(StateSpaceSimTest, FlywheelSim) {
// Then, SimulationPeriodic runs
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
sim.SetInput(Eigen::Vector<double, 1>{
motor.Get() * frc::RobotController::GetInputVoltage()});
sim.SetInput(
frc::Vectord<1>{motor.Get() * frc::RobotController::GetInputVoltage()});
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().value());
}