Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT namespace replacements

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
wpimath/src/main/native/include/wpi/math/controller/ArmFeedforward.hpp
View File

@@ -14,22 +14,22 @@
#include "wpi/util/MathExtras.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* A helper class that computes feedforward outputs for a simple arm (modeled as
* a motor acting against the force of gravity on a beam suspended at an angle).
*/
class WPILIB_DLLEXPORT ArmFeedforward {
public:
using Angle = units::radians;
using Velocity = units::radians_per_second;
using Acceleration = units::compound_unit<units::radians_per_second,
units::inverse<units::second>>;
using Angle = wpi::units::radians;
using Velocity = wpi::units::radians_per_second;
using Acceleration = wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::second>>;
using kv_unit =
units::compound_unit<units::volts,
units::inverse<units::radians_per_second>>;
wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<wpi::units::radians_per_second>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
/**
* Creates a new ArmFeedforward with the specified gains.
@@ -44,20 +44,20 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
wpi::units::volt_t kS, wpi::units::volt_t kG, wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
this->kV = units::unit_t<kv_unit>{0};
this->kV = wpi::units::unit_t<kv_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
}
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
this->kA = units::unit_t<ka_unit>{0};
this->kA = wpi::units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
}
if (dt <= 0_ms) {
@@ -82,10 +82,10 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @return The computed feedforward, in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
constexpr units::volt_t Calculate(
units::unit_t<Angle> angle, units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Angle> angle, wpi::units::unit_t<Velocity> velocity,
wpi::units::unit_t<Acceleration> acceleration) const {
return kS * wpi::util::sgn(velocity) + kG * wpi::units::math::cos(angle) +
kV * velocity + kA * acceleration;
}
@@ -103,10 +103,10 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @return The computed feedforward in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
wpi::units::volt_t Calculate(wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
return Calculate(currentAngle, currentVelocity, nextVelocity);
}
@@ -121,11 +121,11 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param currentVelocity The current velocity.
* @return The computed feedforward in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity) const {
return kS * wpi::sgn(currentVelocity) +
kG * units::math::cos(currentAngle) + kV * currentVelocity;
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity) const {
return kS * wpi::util::sgn(currentVelocity) +
kG * wpi::units::math::cos(currentAngle) + kV * currentVelocity;
}
/**
@@ -140,9 +140,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param nextVelocity The next velocity.
* @return The computed feedforward in volts.
*/
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const;
wpi::units::volt_t Calculate(wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const;
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
@@ -163,11 +163,11 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param acceleration The acceleration of the arm.
* @return The maximum possible velocity at the given acceleration and angle.
*/
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Acceleration> acceleration) {
constexpr wpi::units::unit_t<Velocity> MaxAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Angle> angle,
wpi::units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kG * units::math::cos(angle) -
return (maxVoltage - kS - kG * wpi::units::math::cos(angle) -
kA * acceleration) /
kV;
}
@@ -188,11 +188,11 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param acceleration The acceleration of the arm.
* @return The minimum possible velocity at the given acceleration and angle.
*/
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Acceleration> acceleration) {
constexpr wpi::units::unit_t<Velocity> MinAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Angle> angle,
wpi::units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kG * units::math::cos(angle) -
return (-maxVoltage + kS - kG * wpi::units::math::cos(angle) -
kA * acceleration) /
kV;
}
@@ -213,11 +213,11 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param velocity The velocity of the arm.
* @return The maximum possible acceleration at the given velocity and angle.
*/
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) -
kG * units::math::cos(angle) - kV * velocity) /
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Angle> angle,
wpi::units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::util::sgn(velocity) -
kG * wpi::units::math::cos(angle) - kV * velocity) /
kA;
}
@@ -237,9 +237,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param velocity The velocity of the arm.
* @return The minimum possible acceleration at the given velocity and angle.
*/
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Velocity> velocity) {
constexpr wpi::units::unit_t<Acceleration> MinAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Angle> angle,
wpi::units::unit_t<Velocity> velocity) {
return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
}
@@ -248,74 +248,74 @@ class WPILIB_DLLEXPORT ArmFeedforward {
*
* @param kS The static gain.
*/
constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
constexpr void SetKs(wpi::units::volt_t kS) { this->kS = kS; }
/**
* Sets the gravity gain.
*
* @param kG The gravity gain.
*/
constexpr void SetKg(units::volt_t kG) { this->kG = kG; }
constexpr void SetKg(wpi::units::volt_t kG) { this->kG = kG; }
/**
* Sets the velocity gain.
*
* @param kV The velocity gain.
*/
constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
constexpr void SetKv(wpi::units::unit_t<kv_unit> kV) { this->kV = kV; }
/**
* Sets the acceleration gain.
*
* @param kA The acceleration gain.
*/
constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
constexpr void SetKa(wpi::units::unit_t<ka_unit> kA) { this->kA = kA; }
/**
* Returns the static gain.
*
* @return The static gain.
*/
constexpr units::volt_t GetKs() const { return kS; }
constexpr wpi::units::volt_t GetKs() const { return kS; }
/**
* Returns the gravity gain.
*
* @return The gravity gain.
*/
constexpr units::volt_t GetKg() const { return kG; }
constexpr wpi::units::volt_t GetKg() const { return kG; }
/**
* Returns the velocity gain.
*
* @return The velocity gain.
*/
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
constexpr wpi::units::unit_t<kv_unit> GetKv() const { return kV; }
/**
* Returns the acceleration gain.
*
* @return The acceleration gain.
*/
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
constexpr wpi::units::unit_t<ka_unit> GetKa() const { return kA; }
private:
/// The static gain, in volts.
units::volt_t kS;
wpi::units::volt_t kS;
/// The gravity gain, in volts.
units::volt_t kG;
wpi::units::volt_t kG;
/// The velocity gain, in V/(rad/s)volt seconds per radian.
units::unit_t<kv_unit> kV;
wpi::units::unit_t<kv_unit> kV;
/// The acceleration gain, in V/(rad/s²).
units::unit_t<ka_unit> kA;
wpi::units::unit_t<ka_unit> kA;
/** The period. */
units::second_t m_dt;
wpi::units::second_t m_dt;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/controller/proto/ArmFeedforwardProto.hpp"
#include "wpi/math/controller/struct/ArmFeedforwardStruct.hpp"

10
wpimath/src/main/native/include/wpi/math/controller/BangBangController.hpp
View File

@@ -13,7 +13,7 @@
#include "wpi/util/sendable/Sendable.hpp"
#include "wpi/util/sendable/SendableHelper.hpp"
namespace frc {
namespace wpi::math {
/**
* Implements a bang-bang controller, which outputs either 0 or 1 depending on
@@ -29,8 +29,8 @@ namespace frc {
* control them with a bang-bang controller.
*/
class WPILIB_DLLEXPORT BangBangController
: public wpi::Sendable,
public wpi::SendableHelper<BangBangController> {
: public wpi::util::Sendable,
public wpi::util::SendableHelper<BangBangController> {
public:
/**
* Creates a new bang-bang controller.
@@ -128,7 +128,7 @@ class WPILIB_DLLEXPORT BangBangController
return Calculate(measurement, m_setpoint);
}
void InitSendable(wpi::SendableBuilder& builder) override;
void InitSendable(wpi::util::SendableBuilder& builder) override;
private:
double m_tolerance;
@@ -140,4 +140,4 @@ class WPILIB_DLLEXPORT BangBangController
inline static int instances = 0;
};
} // namespace frc
} // namespace wpi::math

10
wpimath/src/main/native/include/wpi/math/controller/ControlAffinePlantInversionFeedforward.hpp
View File

@@ -13,7 +13,7 @@
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
/**
* Constructs a control-affine plant inversion model-based feedforward from
@@ -55,7 +55,7 @@ class ControlAffinePlantInversionFeedforward {
*/
ControlAffinePlantInversionFeedforward(
std::function<StateVector(const StateVector&, const InputVector&)> f,
units::second_t dt)
wpi::units::second_t dt)
: m_dt(dt), m_f(f) {
m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
InputVector::Zero());
@@ -74,7 +74,7 @@ class ControlAffinePlantInversionFeedforward {
*/
ControlAffinePlantInversionFeedforward(
std::function<StateVector(const StateVector&)> f,
const Matrixd<States, Inputs>& B, units::second_t dt)
const Matrixd<States, Inputs>& B, wpi::units::second_t dt)
: m_B(B), m_dt(dt) {
m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
return f(x);
@@ -178,7 +178,7 @@ class ControlAffinePlantInversionFeedforward {
private:
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
wpi::units::second_t m_dt;
/**
* The model dynamics.
@@ -192,4 +192,4 @@ class ControlAffinePlantInversionFeedforward {
InputVector m_uff;
};
} // namespace frc
} // namespace wpi::math

32
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/units/voltage.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Filters the provided voltages to limit a differential drive's linear and
@@ -38,9 +38,9 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
* @param maxAngularAccel The maximum angular acceleration.
*/
DifferentialDriveAccelerationLimiter(
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
units::meters_per_second_squared_t maxLinearAccel,
units::radians_per_second_squared_t maxAngularAccel)
LinearSystem<2, 2, 2> system, wpi::units::meter_t trackwidth,
wpi::units::meters_per_second_squared_t maxLinearAccel,
wpi::units::radians_per_second_squared_t maxAngularAccel)
: DifferentialDriveAccelerationLimiter(system, trackwidth,
-maxLinearAccel, maxLinearAccel,
maxAngularAccel) {}
@@ -58,10 +58,10 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
* than maximum linear acceleration
*/
DifferentialDriveAccelerationLimiter(
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
units::meters_per_second_squared_t minLinearAccel,
units::meters_per_second_squared_t maxLinearAccel,
units::radians_per_second_squared_t maxAngularAccel)
LinearSystem<2, 2, 2> system, wpi::units::meter_t trackwidth,
wpi::units::meters_per_second_squared_t minLinearAccel,
wpi::units::meters_per_second_squared_t maxLinearAccel,
wpi::units::radians_per_second_squared_t maxAngularAccel)
: m_system{std::move(system)},
m_trackwidth{trackwidth},
m_minLinearAccel{minLinearAccel},
@@ -83,16 +83,16 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
* @return The constrained wheel voltages.
*/
DifferentialDriveWheelVoltages Calculate(
units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
units::volt_t rightVoltage);
wpi::units::meters_per_second_t leftVelocity,
wpi::units::meters_per_second_t rightVelocity, wpi::units::volt_t leftVoltage,
wpi::units::volt_t rightVoltage);
private:
LinearSystem<2, 2, 2> m_system;
units::meter_t m_trackwidth;
units::meters_per_second_squared_t m_minLinearAccel;
units::meters_per_second_squared_t m_maxLinearAccel;
units::radians_per_second_squared_t m_maxAngularAccel;
wpi::units::meter_t m_trackwidth;
wpi::units::meters_per_second_squared_t m_minLinearAccel;
wpi::units::meters_per_second_squared_t m_maxLinearAccel;
wpi::units::radians_per_second_squared_t m_maxAngularAccel;
};
} // namespace frc
} // namespace wpi::math

18
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp
View File

@@ -16,13 +16,13 @@
#include "wpi/units/voltage.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* A helper class which computes the feedforward outputs for a differential
* drive drivetrain.
*/
class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
frc::LinearSystem<2, 2, 2> m_plant;
wpi::math::LinearSystem<2, 2, 2> m_plant;
public:
/**
@@ -41,7 +41,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
constexpr DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_rad_per_s) kVAngular,
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
decltype(1_V / 1_rad_per_s_sq) kAAngular, wpi::units::meter_t trackwidth)
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
// 1_rad_per_s_sq))
@@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular,
decltype(1_V / 1_mps_sq) kAAngular)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
: m_plant{wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular)},
m_kVLinear{kVLinear},
m_kALinear{kALinear},
@@ -86,17 +86,17 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
* @param dt Discretization timestep.
*/
DifferentialDriveWheelVoltages Calculate(
units::meters_per_second_t currentLeftVelocity,
units::meters_per_second_t nextLeftVelocity,
units::meters_per_second_t currentRightVelocity,
units::meters_per_second_t nextRightVelocity, units::second_t dt);
wpi::units::meters_per_second_t currentLeftVelocity,
wpi::units::meters_per_second_t nextLeftVelocity,
wpi::units::meters_per_second_t currentRightVelocity,
wpi::units::meters_per_second_t nextRightVelocity, wpi::units::second_t dt);
decltype(1_V / 1_mps) m_kVLinear;
decltype(1_V / 1_mps_sq) m_kALinear;
decltype(1_V / 1_mps) m_kVAngular;
decltype(1_V / 1_mps_sq) m_kAAngular;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/controller/proto/DifferentialDriveFeedforwardProto.hpp"
#include "wpi/math/controller/struct/DifferentialDriveFeedforwardStruct.hpp"

8
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveWheelVoltages.hpp
View File

@@ -6,20 +6,20 @@
#include "wpi/units/voltage.hpp"
namespace frc {
namespace wpi::math {
/**
* Motor voltages for a differential drive.
*/
struct DifferentialDriveWheelVoltages {
/// Left wheel voltage.
units::volt_t left = 0_V;
wpi::units::volt_t left = 0_V;
/// Right wheel voltage.
units::volt_t right = 0_V;
wpi::units::volt_t right = 0_V;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/controller/proto/DifferentialDriveWheelVoltagesProto.hpp"
#include "wpi/math/controller/struct/DifferentialDriveWheelVoltagesStruct.hpp"

102
wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp
View File

@@ -13,21 +13,21 @@
#include "wpi/units/voltage.hpp"
#include "wpi/util/MathExtras.hpp"
namespace frc {
namespace wpi::math {
/**
* A helper class that computes feedforward outputs for a simple elevator
* (modeled as a motor acting against the force of gravity).
*/
class ElevatorFeedforward {
public:
using Distance = units::meters;
using Distance = wpi::units::meters;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Velocity>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
/**
* Creates a new ElevatorFeedforward with the specified gains.
@@ -42,20 +42,20 @@ class ElevatorFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ElevatorFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
wpi::units::volt_t kS, wpi::units::volt_t kG, wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
this->kV = units::unit_t<kv_unit>{0};
this->kV = wpi::units::unit_t<kv_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
}
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
this->kA = units::unit_t<ka_unit>{0};
this->kA = wpi::units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
}
if (dt <= 0_ms) {
@@ -76,10 +76,10 @@ class ElevatorFeedforward {
* @deprecated Use the current/next velocity overload instead.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
constexpr units::volt_t Calculate(
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> velocity,
wpi::units::unit_t<Acceleration> acceleration) const {
return kS * wpi::util::sgn(velocity) + kG + kV * velocity + kA * acceleration;
}
/**
@@ -92,9 +92,9 @@ class ElevatorFeedforward {
* @return The computed feedforward, in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
@@ -102,8 +102,8 @@ class ElevatorFeedforward {
Vectord<1> r{{currentVelocity.value()}};
Vectord<1> nextR{{nextVelocity.value()}};
return kG + kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};
return kG + kS * wpi::util::sgn(currentVelocity.value()) +
wpi::units::volt_t{feedforward.Calculate(r, nextR)(0)};
}
/**
@@ -113,8 +113,8 @@ class ElevatorFeedforward {
* @param currentVelocity The velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity) const {
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> currentVelocity) const {
return Calculate(currentVelocity, currentVelocity);
}
@@ -128,19 +128,19 @@ class ElevatorFeedforward {
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (kA < decltype(kA)(1e-9)) {
return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
return kS * wpi::util::sgn(nextVelocity) + kG + kV * nextVelocity;
} else {
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
return kG + kS * wpi::sgn(currentVelocity) +
units::volt_t{
return kG + kS * wpi::util::sgn(currentVelocity) +
wpi::units::volt_t{
1.0 / B_d *
(nextVelocity.value() - A_d * currentVelocity.value())};
}
@@ -160,8 +160,8 @@ class ElevatorFeedforward {
* @param acceleration The acceleration of the elevator.
* @return The maximum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
constexpr wpi::units::unit_t<Velocity> MaxAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kG - kA * acceleration) / kV;
}
@@ -177,8 +177,8 @@ class ElevatorFeedforward {
* @param acceleration The acceleration of the elevator.
* @return The minimum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
constexpr wpi::units::unit_t<Velocity> MinAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kG - kA * acceleration) / kV;
}
@@ -194,9 +194,9 @@ class ElevatorFeedforward {
* @param velocity The velocity of the elevator.
* @return The maximum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::util::sgn(velocity) - kG - kV * velocity) / kA;
}
/**
@@ -210,8 +210,8 @@ class ElevatorFeedforward {
* @param velocity The velocity of the elevator.
* @return The minimum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
constexpr wpi::units::unit_t<Acceleration> MinAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}
@@ -220,74 +220,74 @@ class ElevatorFeedforward {
*
* @param kS The static gain.
*/
constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
constexpr void SetKs(wpi::units::volt_t kS) { this->kS = kS; }
/**
* Sets the gravity gain.
*
* @param kG The gravity gain.
*/
constexpr void SetKg(units::volt_t kG) { this->kG = kG; }
constexpr void SetKg(wpi::units::volt_t kG) { this->kG = kG; }
/**
* Sets the velocity gain.
*
* @param kV The velocity gain.
*/
constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
constexpr void SetKv(wpi::units::unit_t<kv_unit> kV) { this->kV = kV; }
/**
* Sets the acceleration gain.
*
* @param kA The acceleration gain.
*/
constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
constexpr void SetKa(wpi::units::unit_t<ka_unit> kA) { this->kA = kA; }
/**
* Returns the static gain.
*
* @return The static gain.
*/
constexpr units::volt_t GetKs() const { return kS; }
constexpr wpi::units::volt_t GetKs() const { return kS; }
/**
* Returns the gravity gain.
*
* @return The gravity gain.
*/
constexpr units::volt_t GetKg() const { return kG; }
constexpr wpi::units::volt_t GetKg() const { return kG; }
/**
* Returns the velocity gain.
*
* @return The velocity gain.
*/
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
constexpr wpi::units::unit_t<kv_unit> GetKv() const { return kV; }
/**
* Returns the acceleration gain.
*
* @return The acceleration gain.
*/
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
constexpr wpi::units::unit_t<ka_unit> GetKa() const { return kA; }
private:
/// The static gain.
units::volt_t kS;
wpi::units::volt_t kS;
/// The gravity gain.
units::volt_t kG;
wpi::units::volt_t kG;
/// The velocity gain.
units::unit_t<kv_unit> kV;
wpi::units::unit_t<kv_unit> kV;
/// The acceleration gain.
units::unit_t<ka_unit> kA;
wpi::units::unit_t<ka_unit> kA;
/** The period. */
units::second_t m_dt;
wpi::units::second_t m_dt;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/controller/proto/ElevatorFeedforwardProto.hpp"
#include "wpi/math/controller/struct/ElevatorFeedforwardStruct.hpp"

4
wpimath/src/main/native/include/wpi/math/controller/ImplicitModelFollower.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
/**
* Contains the controller coefficients and logic for an implicit model
@@ -123,4 +123,4 @@ class ImplicitModelFollower {
Matrixd<Inputs, Inputs> m_B;
};
} // namespace frc
} // namespace wpi::math

38
wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp
View File

@@ -19,7 +19,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* The linear time-varying differential drive controller has a similar form to
@@ -54,16 +54,16 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
*/
LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
units::meter_t trackwidth,
const wpi::array<double, 5>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt)
LTVDifferentialDriveController(const wpi::math::LinearSystem<2, 2, 2>& plant,
wpi::units::meter_t trackwidth,
const wpi::util::array<double, 5>& Qelems,
const wpi::util::array<double, 2>& Relems,
wpi::units::second_t dt)
: m_trackwidth{trackwidth},
m_A{plant.A()},
m_B{plant.B()},
m_Q{frc::MakeCostMatrix(Qelems)},
m_R{frc::MakeCostMatrix(Relems)},
m_Q{wpi::math::MakeCostMatrix(Qelems)},
m_R{wpi::math::MakeCostMatrix(Relems)},
m_dt{dt} {}
/**
@@ -97,8 +97,8 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
* @param rightVelocityTolerance Right velocity error which is tolerable.
*/
void SetTolerance(const Pose2d& poseTolerance,
units::meters_per_second_t leftVelocityTolerance,
units::meters_per_second_t rightVelocityTolerance) {
wpi::units::meters_per_second_t leftVelocityTolerance,
wpi::units::meters_per_second_t rightVelocityTolerance) {
m_tolerance = Eigen::Vector<double, 5>{
poseTolerance.X().value(), poseTolerance.Y().value(),
poseTolerance.Rotation().Radians().value(),
@@ -119,10 +119,10 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
* @param rightVelocityRef The desired right velocity.
*/
DifferentialDriveWheelVoltages Calculate(
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
units::meters_per_second_t leftVelocityRef,
units::meters_per_second_t rightVelocityRef);
const Pose2d& currentPose, wpi::units::meters_per_second_t leftVelocity,
wpi::units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
wpi::units::meters_per_second_t leftVelocityRef,
wpi::units::meters_per_second_t rightVelocityRef);
/**
* Returns the left and right output voltages of the LTV controller.
@@ -137,8 +137,8 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
* from a trajectory.
*/
DifferentialDriveWheelVoltages Calculate(
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity,
const Pose2d& currentPose, wpi::units::meters_per_second_t leftVelocity,
wpi::units::meters_per_second_t rightVelocity,
const Trajectory::State& desiredState) {
// v = (v_r + v_l) / 2 (1)
// w = (v_r - v_l) / (2r) (2)
@@ -160,7 +160,7 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
}
private:
units::meter_t m_trackwidth;
wpi::units::meter_t m_trackwidth;
// Continuous velocity dynamics
Eigen::Matrix<double, 2, 2> m_A;
@@ -170,10 +170,10 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
Eigen::Matrix<double, 5, 5> m_Q;
Eigen::Matrix<double, 2, 2> m_R;
units::second_t m_dt;
wpi::units::second_t m_dt;
Eigen::Vector<double, 5> m_error;
Eigen::Vector<double, 5> m_tolerance;
};
} // namespace frc
} // namespace wpi::math

26
wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* The linear time-varying unicycle controller has a similar form to the LQR,
@@ -37,7 +37,7 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
*
* @param dt Discretization timestep.
*/
explicit LTVUnicycleController(units::second_t dt)
explicit LTVUnicycleController(wpi::units::second_t dt)
: LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt} {}
/**
@@ -53,10 +53,10 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* velocity, angular velocity).
* @param dt Discretization timestep.
*/
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
const wpi::array<double, 2>& Relems, units::second_t dt)
: m_Q{frc::MakeCostMatrix(Qelems)},
m_R{frc::MakeCostMatrix(Relems)},
LTVUnicycleController(const wpi::util::array<double, 3>& Qelems,
const wpi::util::array<double, 2>& Relems, wpi::units::second_t dt)
: m_Q{wpi::math::MakeCostMatrix(Qelems)},
m_R{wpi::math::MakeCostMatrix(Relems)},
m_dt{dt} {}
/**
@@ -77,9 +77,9 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
const auto& eRotate = m_poseError.Rotation();
const auto& tolTranslate = m_poseTolerance.Translation();
const auto& tolRotate = m_poseTolerance.Rotation();
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
return wpi::units::math::abs(eTranslate.X()) < tolTranslate.X() &&
wpi::units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
wpi::units::math::abs(eRotate.Radians()) < tolRotate.Radians();
}
/**
@@ -104,8 +104,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param angularVelocityRef The desired angular velocity.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef,
units::radians_per_second_t angularVelocityRef);
wpi::units::meters_per_second_t linearVelocityRef,
wpi::units::radians_per_second_t angularVelocityRef);
/**
* Returns the linear and angular velocity outputs of the LTV controller.
@@ -135,11 +135,11 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
Eigen::Matrix<double, 3, 3> m_Q;
Eigen::Matrix<double, 2, 2> m_R;
units::second_t m_dt;
wpi::units::second_t m_dt;
Pose2d m_poseError;
Pose2d m_poseTolerance;
bool m_enabled = true;
};
} // namespace frc
} // namespace wpi::math

10
wpimath/src/main/native/include/wpi/math/controller/LinearPlantInversionFeedforward.hpp
View File

@@ -14,7 +14,7 @@
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
/**
* Constructs a plant inversion model-based feedforward from a LinearSystem.
@@ -44,7 +44,7 @@ class LinearPlantInversionFeedforward {
*/
template <int Outputs>
LinearPlantInversionFeedforward(
const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt)
const LinearSystem<States, Inputs, Outputs>& plant, wpi::units::second_t dt)
: LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
/**
@@ -56,7 +56,7 @@ class LinearPlantInversionFeedforward {
*/
LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
units::second_t dt)
wpi::units::second_t dt)
: m_dt(dt) {
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
Reset();
@@ -150,7 +150,7 @@ class LinearPlantInversionFeedforward {
Matrixd<States, States> m_A;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
wpi::units::second_t m_dt;
// Current reference
StateVector m_r;
@@ -159,4 +159,4 @@ class LinearPlantInversionFeedforward {
InputVector m_uff;
};
} // namespace frc
} // namespace wpi::math

18
wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp
View File

@@ -21,7 +21,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* Contains the controller coefficients and logic for a linear-quadratic
@@ -40,8 +40,8 @@ class LinearQuadraticRegulator {
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using StateArray = wpi::array<double, States>;
using InputArray = wpi::array<double, Inputs>;
using StateArray = wpi::util::array<double, States>;
using InputArray = wpi::util::array<double, Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
@@ -60,7 +60,7 @@ class LinearQuadraticRegulator {
template <int Outputs>
LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& Qelems, const InputArray& Relems,
units::second_t dt)
wpi::units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
/**
@@ -80,7 +80,7 @@ class LinearQuadraticRegulator {
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const StateArray& Qelems, const InputArray& Relems,
units::second_t dt)
wpi::units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
@@ -98,7 +98,7 @@ class LinearQuadraticRegulator {
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
units::second_t dt) {
wpi::units::second_t dt) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
@@ -153,7 +153,7 @@ class LinearQuadraticRegulator {
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N,
units::second_t dt) {
wpi::units::second_t dt) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
@@ -291,7 +291,7 @@ class LinearQuadraticRegulator {
*/
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
units::second_t dt, units::second_t inputDelay) {
wpi::units::second_t dt, wpi::units::second_t inputDelay) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
@@ -317,4 +317,4 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
LinearQuadraticRegulator<2, 2>;
} // namespace frc
} // namespace wpi::math

18
wpimath/src/main/native/include/wpi/math/controller/PIDController.hpp
View File

@@ -19,14 +19,14 @@
#include "wpi/util/sendable/SendableHelper.hpp"
#include "wpi/util/sendable/SendableRegistry.hpp"
namespace frc {
namespace wpi::math {
/**
* Implements a PID control loop.
*/
class WPILIB_DLLEXPORT PIDController
: public wpi::Sendable,
public wpi::SendableHelper<PIDController> {
: public wpi::util::Sendable,
public wpi::util::SendableHelper<PIDController> {
public:
/**
* Allocates a PIDController with the given constants for Kp, Ki, and Kd.
@@ -38,7 +38,7 @@ class WPILIB_DLLEXPORT PIDController
* default is 20 milliseconds. Must be positive.
*/
constexpr PIDController(double Kp, double Ki, double Kd,
units::second_t period = 20_ms)
wpi::units::second_t period = 20_ms)
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
bool invalidGains = false;
if (Kp < 0.0) {
@@ -76,7 +76,7 @@ class WPILIB_DLLEXPORT PIDController
wpi::math::MathSharedStore::ReportUsage("PIDController",
std::to_string(instances));
wpi::SendableRegistry::Add(this, "PIDController", instances);
wpi::util::SendableRegistry::Add(this, "PIDController", instances);
}
}
@@ -175,7 +175,7 @@ class WPILIB_DLLEXPORT PIDController
*
* @return The period of the controller.
*/
constexpr units::second_t GetPeriod() const { return m_period; }
constexpr wpi::units::second_t GetPeriod() const { return m_period; }
/**
* Gets the error tolerance of this controller. Defaults to 0.05.
@@ -403,7 +403,7 @@ class WPILIB_DLLEXPORT PIDController
m_haveMeasurement = false;
}
void InitSendable(wpi::SendableBuilder& builder) override;
void InitSendable(wpi::util::SendableBuilder& builder) override;
private:
// Factor for "proportional" control
@@ -419,7 +419,7 @@ class WPILIB_DLLEXPORT PIDController
double m_iZone = std::numeric_limits<double>::infinity();
// The period (in seconds) of the control loop running this controller
units::second_t m_period;
wpi::units::second_t m_period;
double m_maximumIntegral = 1.0;
@@ -457,4 +457,4 @@ class WPILIB_DLLEXPORT PIDController
inline static int instances = 0;
};
} // namespace frc
} // namespace wpi::math

38
wpimath/src/main/native/include/wpi/math/controller/ProfiledPIDController.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/util/sendable/SendableBuilder.hpp"
#include "wpi/util/sendable/SendableHelper.hpp"
namespace frc {
namespace wpi::math {
namespace detail {
WPILIB_DLLEXPORT
int IncrementAndGetProfiledPIDControllerInstances();
@@ -29,16 +29,16 @@ int IncrementAndGetProfiledPIDControllerInstances();
*/
template <class Distance>
class ProfiledPIDController
: public wpi::Sendable,
public wpi::SendableHelper<ProfiledPIDController<Distance>> {
: public wpi::util::Sendable,
public wpi::util::SendableHelper<ProfiledPIDController<Distance>> {
public:
using Distance_t = units::unit_t<Distance>;
using Distance_t = wpi::units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
using Velocity_t = wpi::units::unit_t<Velocity>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using Acceleration_t = units::unit_t<Acceleration>;
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using Acceleration_t = wpi::units::unit_t<Acceleration>;
using State = typename TrapezoidProfile<Distance>::State;
using Constraints = typename TrapezoidProfile<Distance>::Constraints;
@@ -56,7 +56,7 @@ class ProfiledPIDController
*/
constexpr ProfiledPIDController(double Kp, double Ki, double Kd,
Constraints constraints,
units::second_t period = 20_ms)
wpi::units::second_t period = 20_ms)
: m_controller{Kp, Ki, Kd, period},
m_constraints{constraints},
m_profile{m_constraints} {
@@ -64,7 +64,7 @@ class ProfiledPIDController
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
wpi::math::MathSharedStore::ReportUsage("ProfiledPIDController",
std::to_string(instances));
wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
wpi::util::SendableRegistry::Add(this, "ProfiledPIDController", instances);
}
}
@@ -156,7 +156,7 @@ class ProfiledPIDController
*
* @return The period of the controller.
*/
constexpr units::second_t GetPeriod() const {
constexpr wpi::units::second_t GetPeriod() const {
return m_controller.GetPeriod();
}
@@ -327,9 +327,9 @@ class ProfiledPIDController
if (m_controller.IsContinuousInputEnabled()) {
// Get error which is smallest distance between goal and measurement
auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
auto goalMinDistance = frc::InputModulus<Distance_t>(
auto goalMinDistance = wpi::math::InputModulus<Distance_t>(
m_goal.position - measurement, -errorBound, errorBound);
auto setpointMinDistance = frc::InputModulus<Distance_t>(
auto setpointMinDistance = wpi::math::InputModulus<Distance_t>(
m_setpoint.position - measurement, -errorBound, errorBound);
// Recompute the profile goal with the smallest error, thus giving the
@@ -377,7 +377,7 @@ class ProfiledPIDController
*/
constexpr double Calculate(
Distance_t measurement, Distance_t goal,
typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
typename wpi::math::TrapezoidProfile<Distance>::Constraints constraints) {
SetConstraints(constraints);
return Calculate(measurement, goal);
}
@@ -413,7 +413,7 @@ class ProfiledPIDController
Reset(measuredPosition, Velocity_t{0});
}
void InitSendable(wpi::SendableBuilder& builder) override {
void InitSendable(wpi::util::SendableBuilder& builder) override {
builder.SetSmartDashboardType("ProfiledPIDController");
builder.AddDoubleProperty(
"p", [this] { return GetP(); }, [this](double value) { SetP(value); });
@@ -447,10 +447,10 @@ class ProfiledPIDController
Distance_t m_minimumInput{0};
Distance_t m_maximumInput{0};
typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
typename wpi::math::TrapezoidProfile<Distance>::Constraints m_constraints;
TrapezoidProfile<Distance> m_profile;
typename frc::TrapezoidProfile<Distance>::State m_goal;
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
typename wpi::math::TrapezoidProfile<Distance>::State m_goal;
typename wpi::math::TrapezoidProfile<Distance>::State m_setpoint;
};
} // namespace frc
} // namespace wpi::math

82
wpimath/src/main/native/include/wpi/math/controller/SimpleMotorFeedforward.hpp
View File

@@ -13,23 +13,23 @@
#include "wpi/units/voltage.hpp"
#include "wpi/util/MathExtras.hpp"
namespace frc {
namespace wpi::math {
/**
* A helper class that computes feedforward voltages for a simple
* permanent-magnet DC motor.
*/
template <class Distance>
requires units::length_unit<Distance> || units::angle_unit<Distance>
requires wpi::units::length_unit<Distance> || wpi::units::angle_unit<Distance>
class SimpleMotorFeedforward {
public:
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Velocity>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
/**
* Creates a new SimpleMotorFeedforward with the specified gains.
@@ -43,20 +43,20 @@ class SimpleMotorFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
wpi::units::volt_t kS, wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kV(kV), kA(kA), m_dt(dt) {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
this->kV = units::unit_t<kv_unit>{0};
this->kV = wpi::units::unit_t<kv_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
}
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
this->kA = units::unit_t<ka_unit>{0};
this->kA = wpi::units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
}
if (dt <= 0_ms) {
@@ -75,7 +75,7 @@ class SimpleMotorFeedforward {
* @param velocity The velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity) const {
constexpr wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> velocity) const {
return Calculate(velocity, velocity);
}
@@ -89,19 +89,19 @@ class SimpleMotorFeedforward {
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (kA < decltype(kA)(1e-9)) {
return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
return kS * wpi::util::sgn(nextVelocity) + kV * nextVelocity;
} else {
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
return kS * wpi::sgn(currentVelocity) +
units::volt_t{
return kS * wpi::util::sgn(currentVelocity) +
wpi::units::volt_t{
1.0 / B_d *
(nextVelocity.value() - A_d * currentVelocity.value())};
}
@@ -121,9 +121,9 @@ class SimpleMotorFeedforward {
* @param acceleration The acceleration of the motor.
* @return The maximum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage,
units::unit_t<Acceleration> acceleration) const {
constexpr wpi::units::unit_t<Velocity> MaxAchievableVelocity(
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Acceleration> acceleration) const {
// Assume max velocity is positive
return (maxVoltage - kS - kA * acceleration) / kV;
}
@@ -139,9 +139,9 @@ class SimpleMotorFeedforward {
* @param acceleration The acceleration of the motor.
* @return The minimum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage,
units::unit_t<Acceleration> acceleration) const {
constexpr wpi::units::unit_t<Velocity> MinAchievableVelocity(
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Acceleration> acceleration) const {
// Assume min velocity is positive, ks flips sign
return (-maxVoltage + kS - kA * acceleration) / kV;
}
@@ -157,9 +157,9 @@ class SimpleMotorFeedforward {
* @param velocity The velocity of the motor.
* @return The maximum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) const {
return (maxVoltage - kS * wpi::util::sgn(velocity) - kV * velocity) / kA;
}
/**
@@ -173,8 +173,8 @@ class SimpleMotorFeedforward {
* @param velocity The velocity of the motor.
* @return The minimum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
constexpr wpi::units::unit_t<Acceleration> MinAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) const {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}
@@ -183,62 +183,62 @@ class SimpleMotorFeedforward {
*
* @param kS The static gain.
*/
constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
constexpr void SetKs(wpi::units::volt_t kS) { this->kS = kS; }
/**
* Sets the velocity gain.
*
* @param kV The velocity gain.
*/
constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
constexpr void SetKv(wpi::units::unit_t<kv_unit> kV) { this->kV = kV; }
/**
* Sets the acceleration gain.
*
* @param kA The acceleration gain.
*/
constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
constexpr void SetKa(wpi::units::unit_t<ka_unit> kA) { this->kA = kA; }
/**
* Returns the static gain.
*
* @return The static gain.
*/
constexpr units::volt_t GetKs() const { return kS; }
constexpr wpi::units::volt_t GetKs() const { return kS; }
/**
* Returns the velocity gain.
*
* @return The velocity gain.
*/
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
constexpr wpi::units::unit_t<kv_unit> GetKv() const { return kV; }
/**
* Returns the acceleration gain.
*
* @return The acceleration gain.
*/
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
constexpr wpi::units::unit_t<ka_unit> GetKa() const { return kA; }
/**
* Returns the period.
*
* @return The period.
*/
constexpr units::second_t GetDt() const { return m_dt; }
constexpr wpi::units::second_t GetDt() const { return m_dt; }
private:
/** The static gain. */
units::volt_t kS;
wpi::units::volt_t kS;
/** The velocity gain. */
units::unit_t<kv_unit> kV;
wpi::units::unit_t<kv_unit> kV;
/** The acceleration gain. */
units::unit_t<ka_unit> kA;
wpi::units::unit_t<ka_unit> kA;
/** The period. */
units::second_t m_dt;
wpi::units::second_t m_dt;
};
} // namespace frc
} // namespace wpi::math

10
wpimath/src/main/native/include/wpi/math/controller/proto/ArmFeedforwardProto.hpp
View File

@@ -11,10 +11,10 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ArmFeedforward> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ArmFeedforward> {
using MessageStruct = wpi_proto_ProtobufArmFeedforward;
using InputStream = wpi::ProtoInputStream<frc::ArmFeedforward>;
using OutputStream = wpi::ProtoOutputStream<frc::ArmFeedforward>;
static std::optional<frc::ArmFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::ArmFeedforward& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::ArmFeedforward>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ArmFeedforward>;
static std::optional<wpi::math::ArmFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ArmFeedforward& value);
};

10
wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveFeedforwardProto.hpp
View File

@@ -11,13 +11,13 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveFeedforward> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveFeedforward;
using InputStream = wpi::ProtoInputStream<frc::DifferentialDriveFeedforward>;
using InputStream = wpi::util::ProtoInputStream<wpi::math::DifferentialDriveFeedforward>;
using OutputStream =
wpi::ProtoOutputStream<frc::DifferentialDriveFeedforward>;
static std::optional<frc::DifferentialDriveFeedforward> Unpack(
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveFeedforward>;
static std::optional<wpi::math::DifferentialDriveFeedforward> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const frc::DifferentialDriveFeedforward& value);
const wpi::math::DifferentialDriveFeedforward& value);
};

10
wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveWheelVoltagesProto.hpp
View File

@@ -11,14 +11,14 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveWheelVoltages> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelVoltages;
using InputStream =
wpi::ProtoInputStream<frc::DifferentialDriveWheelVoltages>;
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelVoltages>;
using OutputStream =
wpi::ProtoOutputStream<frc::DifferentialDriveWheelVoltages>;
static std::optional<frc::DifferentialDriveWheelVoltages> Unpack(
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveWheelVoltages>;
static std::optional<wpi::math::DifferentialDriveWheelVoltages> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const frc::DifferentialDriveWheelVoltages& value);
const wpi::math::DifferentialDriveWheelVoltages& value);
};

10
wpimath/src/main/native/include/wpi/math/controller/proto/ElevatorFeedforwardProto.hpp
View File

@@ -11,10 +11,10 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ElevatorFeedforward> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ElevatorFeedforward> {
using MessageStruct = wpi_proto_ProtobufElevatorFeedforward;
using InputStream = wpi::ProtoInputStream<frc::ElevatorFeedforward>;
using OutputStream = wpi::ProtoOutputStream<frc::ElevatorFeedforward>;
static std::optional<frc::ElevatorFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::ElevatorFeedforward& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::ElevatorFeedforward>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ElevatorFeedforward>;
static std::optional<wpi::math::ElevatorFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ElevatorFeedforward& value);
};

40
wpimath/src/main/native/include/wpi/math/controller/proto/SimpleMotorFeedforwardProto.hpp
View File

@@ -11,48 +11,48 @@
#include "wpimath/protobuf/controller.npb.h"
// Everything is converted into units for
// frc::SimpleMotorFeedforward<units::meters> or
// frc::SimpleMotorFeedforward<units::radians>
// wpi::math::SimpleMotorFeedforward<wpi::units::meters> or
// wpi::math::SimpleMotorFeedforward<wpi::units::radians>
template <class Distance>
requires units::length_unit<Distance> || units::angle_unit<Distance>
struct wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>> {
requires wpi::units::length_unit<Distance> || wpi::units::angle_unit<Distance>
struct wpi::util::Protobuf<wpi::math::SimpleMotorFeedforward<Distance>> {
using MessageStruct = wpi_proto_ProtobufSimpleMotorFeedforward;
using InputStream =
wpi::ProtoInputStream<frc::SimpleMotorFeedforward<Distance>>;
wpi::util::ProtoInputStream<wpi::math::SimpleMotorFeedforward<Distance>>;
using OutputStream =
wpi::ProtoOutputStream<frc::SimpleMotorFeedforward<Distance>>;
wpi::util::ProtoOutputStream<wpi::math::SimpleMotorFeedforward<Distance>>;
static std::optional<frc::SimpleMotorFeedforward<Distance>> Unpack(
static std::optional<wpi::math::SimpleMotorFeedforward<Distance>> Unpack(
InputStream& stream) {
using BaseUnit =
units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
wpi_proto_ProtobufSimpleMotorFeedforward msg;
if (!stream.Decode(msg)) {
return {};
}
return frc::SimpleMotorFeedforward<Distance>{
units::volt_t{msg.ks},
units::unit_t<typename BaseFeedforward::kv_unit>{msg.kv},
units::unit_t<typename BaseFeedforward::ka_unit>{msg.ka},
units::second_t{msg.dt},
return wpi::math::SimpleMotorFeedforward<Distance>{
wpi::units::volt_t{msg.ks},
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{msg.kv},
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{msg.ka},
wpi::units::second_t{msg.dt},
};
}
static bool Pack(OutputStream& stream,
const frc::SimpleMotorFeedforward<Distance>& value) {
const wpi::math::SimpleMotorFeedforward<Distance>& value) {
using BaseUnit =
units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
wpi_proto_ProtobufSimpleMotorFeedforward msg{
.ks = value.GetKs().value(),
.kv = units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.kv = wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value(),
.ka = units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.ka = wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value(),
.dt = units::second_t{value.GetDt()}.value(),
.dt = wpi::units::second_t{value.GetDt()}.value(),
};
return stream.Encode(msg);
}

8
wpimath/src/main/native/include/wpi/math/controller/struct/ArmFeedforwardStruct.hpp
View File

@@ -9,15 +9,15 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::ArmFeedforward> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ArmFeedforward> {
static constexpr std::string_view GetTypeName() { return "ArmFeedforward"; }
static constexpr size_t GetSize() { return 32; }
static constexpr std::string_view GetSchema() {
return "double ks;double kg;double kv;double ka";
}
static frc::ArmFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::ArmFeedforward& value);
static wpi::math::ArmFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::ArmFeedforward& value);
};
static_assert(wpi::StructSerializable<frc::ArmFeedforward>);
static_assert(wpi::util::StructSerializable<wpi::math::ArmFeedforward>);

8
wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveFeedforwardStruct.hpp
View File

@@ -9,7 +9,7 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveFeedforward> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveFeedforward> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveFeedforward";
}
@@ -19,10 +19,10 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveFeedforward> {
"ka_angular";
}
static frc::DifferentialDriveFeedforward Unpack(
static wpi::math::DifferentialDriveFeedforward Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveFeedforward& value);
const wpi::math::DifferentialDriveFeedforward& value);
};
static_assert(wpi::StructSerializable<frc::DifferentialDriveFeedforward>);
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveFeedforward>);

8
wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveWheelVoltagesStruct.hpp
View File

@@ -9,7 +9,7 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelVoltages> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelVoltages";
}
@@ -18,10 +18,10 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelVoltages> {
return "double left;double right";
}
static frc::DifferentialDriveWheelVoltages Unpack(
static wpi::math::DifferentialDriveWheelVoltages Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveWheelVoltages& value);
const wpi::math::DifferentialDriveWheelVoltages& value);
};
static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelVoltages>);
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVoltages>);

8
wpimath/src/main/native/include/wpi/math/controller/struct/ElevatorFeedforwardStruct.hpp
View File

@@ -9,7 +9,7 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::ElevatorFeedforward> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ElevatorFeedforward> {
static constexpr std::string_view GetTypeName() {
return "ElevatorFeedforward";
}
@@ -18,9 +18,9 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::ElevatorFeedforward> {
return "double ks;double kg;double kv;double ka";
}
static frc::ElevatorFeedforward Unpack(std::span<const uint8_t> data);
static wpi::math::ElevatorFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::ElevatorFeedforward& value);
const wpi::math::ElevatorFeedforward& value);
};
static_assert(wpi::StructSerializable<frc::ElevatorFeedforward>);
static_assert(wpi::util::StructSerializable<wpi::math::ElevatorFeedforward>);

50
wpimath/src/main/native/include/wpi/math/controller/struct/SimpleMotorFeedforwardStruct.hpp
View File

@@ -9,12 +9,12 @@
#include "wpi/util/struct/Struct.hpp"
// Everything is converted into units for
// frc::SimpleMotorFeedforward<units::meters> or
// frc::SimpleMotorFeedforward<units::radians>
// wpi::math::SimpleMotorFeedforward<wpi::units::meters> or
// wpi::math::SimpleMotorFeedforward<wpi::units::radians>
template <class Distance>
requires units::length_unit<Distance> || units::angle_unit<Distance>
struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
requires wpi::units::length_unit<Distance> || wpi::units::angle_unit<Distance>
struct wpi::util::Struct<wpi::math::SimpleMotorFeedforward<Distance>> {
static constexpr std::string_view GetTypeName() {
return "SimpleMotorFeedforward";
}
@@ -23,46 +23,46 @@ struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
return "double ks;double kv;double ka;double dt";
}
static frc::SimpleMotorFeedforward<Distance> Unpack(
static wpi::math::SimpleMotorFeedforward<Distance> Unpack(
std::span<const uint8_t> data) {
using BaseUnit =
units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
return {units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
units::unit_t<typename BaseFeedforward::kv_unit>{
wpi::UnpackStruct<double, kKvOff>(data)},
units::unit_t<typename BaseFeedforward::ka_unit>{
wpi::UnpackStruct<double, kKaOff>(data)},
units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
return {wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{
wpi::util::UnpackStruct<double, kKvOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{
wpi::util::UnpackStruct<double, kKaOff>(data)},
wpi::units::second_t{wpi::util::UnpackStruct<double, kDtOff>(data)}};
}
static void Pack(std::span<uint8_t> data,
const frc::SimpleMotorFeedforward<Distance>& value) {
const wpi::math::SimpleMotorFeedforward<Distance>& value) {
using BaseUnit =
units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
wpi::PackStruct<kKsOff>(data, value.GetKs().value());
wpi::PackStruct<kKvOff>(
data, units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
wpi::util::PackStruct<kKsOff>(data, value.GetKs().value());
wpi::util::PackStruct<kKvOff>(
data, wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value());
wpi::PackStruct<kKaOff>(
data, units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
wpi::util::PackStruct<kKaOff>(
data, wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value());
wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
wpi::util::PackStruct<kDtOff>(data, wpi::units::second_t{value.GetDt()}.value());
}
};
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::meters>>);
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::feet>>);
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::feet>>);
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::radians>>);
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::radians>>);

6
wpimath/src/main/native/include/wpi/math/estimator/AngleStatistics.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/util/MathUtil.hpp"
namespace frc {
namespace wpi::math {
/**
* Subtracts a and b while normalizing the resulting value in the selected row
@@ -26,7 +26,7 @@ Vectord<States> AngleResidual(const Vectord<States>& a,
const Vectord<States>& b, int angleStateIdx) {
Vectord<States> ret = a - b;
ret[angleStateIdx] =
AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
AngleModulus(wpi::units::radian_t{ret[angleStateIdx]}).value();
return ret;
}
@@ -124,4 +124,4 @@ AngleMean(int angleStateIdx) {
};
}
} // namespace frc
} // namespace wpi::math

28
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp
View File

@@ -13,7 +13,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps Differential Drive Odometry to fuse latency-compensated
* vision measurements with differential drive encoder measurements. It will
@@ -51,8 +51,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
*/
DifferentialDrivePoseEstimator(DifferentialDriveKinematics& kinematics,
const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance,
const Pose2d& initialPose);
/**
@@ -74,9 +74,9 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
*/
DifferentialDrivePoseEstimator(
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
@@ -86,8 +86,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose) {
void ResetPosition(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose2d& pose) {
PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance},
pose);
}
@@ -102,8 +102,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
*
* @return The estimated pose of the robot.
*/
Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance) {
Pose2d Update(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return PoseEstimator::Update(gyroAngle, {leftDistance, rightDistance});
}
@@ -118,10 +118,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
*
* @return The estimated pose of the robot.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
Pose2d UpdateWithTime(wpi::units::second_t currentTime,
const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return PoseEstimator::UpdateWithTime(currentTime, gyroAngle,
{leftDistance, rightDistance});
}
@@ -130,4 +130,4 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
DifferentialDriveOdometry m_odometryImpl;
};
} // namespace frc
} // namespace wpi::math

28
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp
View File

@@ -13,7 +13,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps Differential Drive Odometry to fuse latency-compensated
* vision measurements with differential drive encoder measurements. It will
@@ -56,8 +56,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
*/
DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics& kinematics,
const Rotation3d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance,
const Pose3d& initialPose);
/**
@@ -79,9 +79,9 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
*/
DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
const wpi::array<double, 4>& visionMeasurementStdDevs);
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
@@ -91,8 +91,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose3d& pose) {
void ResetPosition(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose3d& pose) {
PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance},
pose);
}
@@ -107,8 +107,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
*
* @return The estimated pose of the robot.
*/
Pose3d Update(const Rotation3d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance) {
Pose3d Update(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return PoseEstimator3d::Update(gyroAngle, {leftDistance, rightDistance});
}
@@ -123,10 +123,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
*
* @return The estimated pose of the robot.
*/
Pose3d UpdateWithTime(units::second_t currentTime,
Pose3d UpdateWithTime(wpi::units::second_t currentTime,
const Rotation3d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return PoseEstimator3d::UpdateWithTime(currentTime, gyroAngle,
{leftDistance, rightDistance});
}
@@ -135,4 +135,4 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
DifferentialDriveOdometry3d m_odometryImpl;
};
} // namespace frc
} // namespace wpi::math

16
wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp
View File

@@ -20,7 +20,7 @@
#include "wpi/units/time.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* A Kalman filter combines predictions from a model and measurements to give an
@@ -53,8 +53,8 @@ class ExtendedKalmanFilter {
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateArray = wpi::util::array<double, States>;
using OutputArray = wpi::util::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
@@ -77,7 +77,7 @@ class ExtendedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
wpi::units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
@@ -165,7 +165,7 @@ class ExtendedKalmanFilter {
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt)
wpi::units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_residualFuncY(std::move(residualFuncY)),
@@ -284,7 +284,7 @@ class ExtendedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt) {
void Predict(const InputVector& u, wpi::units::second_t dt) {
// Find continuous A
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -423,9 +423,9 @@ class ExtendedKalmanFilter {
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
units::second_t m_dt;
wpi::units::second_t m_dt;
StateMatrix m_initP;
};
} // namespace frc
} // namespace wpi::math

14
wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp
View File

@@ -20,7 +20,7 @@
#include "wpi/units/time.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* A Kalman filter combines predictions from a model and measurements to give an
@@ -49,8 +49,8 @@ class KalmanFilter {
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateArray = wpi::util::array<double, States>;
using OutputArray = wpi::util::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
@@ -69,7 +69,7 @@ class KalmanFilter {
*/
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs, units::second_t dt) {
const OutputArray& measurementStdDevs, wpi::units::second_t dt) {
m_plant = &plant;
m_contQ = MakeCovMatrix(stateStdDevs);
@@ -181,7 +181,7 @@ class KalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt) {
void Predict(const InputVector& u, wpi::units::second_t dt) {
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
@@ -257,7 +257,7 @@ class KalmanFilter {
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
units::second_t m_dt;
wpi::units::second_t m_dt;
StateMatrix m_initP;
};
@@ -267,4 +267,4 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
KalmanFilter<2, 1, 1>;
} // namespace frc
} // namespace wpi::math

22
wpimath/src/main/native/include/wpi/math/estimator/KalmanFilterLatencyCompensator.hpp
View File

@@ -14,7 +14,7 @@
#include "wpi/units/math.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
/**
* This class incorporates time-delayed measurements into a Kalman filter's
@@ -62,7 +62,7 @@ class KalmanFilterLatencyCompensator {
* @param timestamp The timestamp of the state.
*/
void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
Vectord<Outputs> localY, units::second_t timestamp) {
Vectord<Outputs> localY, wpi::units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
@@ -86,10 +86,10 @@ class KalmanFilterLatencyCompensator {
*/
template <int Rows>
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
KalmanFilterType* observer, wpi::units::second_t nominalDt, Vectord<Rows> y,
std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
globalMeasurementCorrect,
units::second_t timestamp) {
wpi::units::second_t timestamp) {
if (m_pastObserverSnapshots.size() == 0) {
// State map was empty, which means that we got a measurement right at
// startup. The only thing we can do is ignore the measurement.
@@ -130,14 +130,14 @@ class KalmanFilterLatencyCompensator {
int prevIdx = nextIdx - 1;
// Find the snapshot closest in time to global measurement
units::second_t prevTimeDiff =
units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
units::second_t nextTimeDiff =
units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
wpi::units::second_t prevTimeDiff =
wpi::units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
wpi::units::second_t nextTimeDiff =
wpi::units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
indexOfClosestEntry = prevTimeDiff < nextTimeDiff ? prevIdx : nextIdx;
}
units::second_t lastTimestamp =
wpi::units::second_t lastTimestamp =
m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt;
// We will now go back in time to the state of the system at the time when
@@ -174,7 +174,7 @@ class KalmanFilterLatencyCompensator {
private:
static constexpr size_t kMaxPastObserverStates = 300;
std::vector<std::pair<units::second_t, ObserverSnapshot>>
std::vector<std::pair<wpi::units::second_t, ObserverSnapshot>>
m_pastObserverSnapshots;
};
} // namespace frc
} // namespace wpi::math

8
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp
View File

@@ -16,7 +16,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps Mecanum Drive Odometry to fuse latency-compensated
* vision measurements with mecanum drive encoder velocity measurements. It will
@@ -74,11 +74,11 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
MecanumDrivePoseEstimator(
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs);
private:
MecanumDriveOdometry m_odometryImpl;
};
} // namespace frc
} // namespace wpi::math

8
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp
View File

@@ -16,7 +16,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps Mecanum Drive Odometry to fuse latency-compensated
* vision measurements with mecanum drive encoder velocity measurements. It will
@@ -79,11 +79,11 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
MecanumDrivePoseEstimator3d(
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
const wpi::array<double, 4>& visionMeasurementStdDevs);
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs);
private:
MecanumDriveOdometry3d m_odometryImpl;
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/estimator/MerweScaledSigmaPoints.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
namespace frc {
namespace wpi::math {
/**
* Generates sigma points and weights according to Van der Merwe's 2004
@@ -125,4 +125,4 @@ class MerweScaledSigmaPoints {
}
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/estimator/MerweUKF.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/estimator/UnscentedKalmanFilter.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
template <int States, int Inputs, int Outputs>
using MerweUKF = UnscentedKalmanFilter<States, Inputs, Outputs,
@@ -21,4 +21,4 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
} // namespace frc
} // namespace wpi::math

48
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -21,7 +21,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps odometry to fuse latency-compensated
@@ -61,8 +61,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
*/
PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
@@ -82,8 +82,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
const wpi::util::array<double, 3>& visionMeasurementStdDevs) {
wpi::util::array<double, 3> r{wpi::util::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
@@ -169,7 +169,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @return The pose at the given timestamp (or std::nullopt if the buffer is
* empty).
*/
std::optional<Pose2d> SampleAt(units::second_t timestamp) const {
std::optional<Pose2d> SampleAt(wpi::units::second_t timestamp) const {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return std::nullopt;
@@ -178,9 +178,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
// Step 1: Make sure timestamp matches the sample from the odometry pose
// buffer. (When sampling, the buffer will always use a timestamp
// between the first and last timestamps)
units::second_t oldestOdometryTimestamp =
wpi::units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
units::second_t newestOdometryTimestamp =
wpi::units::second_t newestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().back().first;
timestamp =
std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
@@ -228,11 +228,11 @@ class WPILIB_DLLEXPORT PoseEstimator {
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetTimestamp(). This means that you should use
* frc::Timer::GetTimestamp() as your time source in this case.
* wpi::math::Timer::GetTimestamp(). This means that you should use
* wpi::math::Timer::GetTimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
wpi::units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
@@ -273,9 +273,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
twist.dtheta.value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
Twist2d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::radian_t{k_times_twist(2)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
@@ -312,16 +312,16 @@ class WPILIB_DLLEXPORT PoseEstimator {
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetTimestamp(). This means that you should use
* frc::Timer::GetTimestamp() as your time source in this case.
* wpi::math::Timer::GetTimestamp(). This means that you should use
* wpi::math::Timer::GetTimestamp() as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
const Pose2d& visionRobotPose, wpi::units::second_t timestamp,
const wpi::util::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
@@ -351,7 +351,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
*
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
Pose2d UpdateWithTime(wpi::units::second_t currentTime,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
@@ -379,7 +379,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
}
// Step 1: Find the oldest timestamp that needs a vision update.
units::second_t oldestOdometryTimestamp =
wpi::units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
// Step 2: If there are no vision updates before that timestamp, skip.
@@ -422,10 +422,10 @@ class WPILIB_DLLEXPORT PoseEstimator {
}
};
static constexpr units::second_t kBufferDuration = 1.5_s;
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry<WheelSpeeds, WheelPositions>& m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
wpi::util::array<double, 3> m_q{wpi::util::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
// Maps timestamps to odometry-only pose estimates
@@ -433,9 +433,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
std::map<units::second_t, VisionUpdate> m_visionUpdates;
std::map<wpi::units::second_t, VisionUpdate> m_visionUpdates;
Pose2d m_poseEstimate;
};
} // namespace frc
} // namespace wpi::math

54
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -23,7 +23,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps odometry to fuse latency-compensated
@@ -67,8 +67,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*/
PoseEstimator3d(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry3d<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 4>& stateStdDevs,
const wpi::array<double, 4>& visionMeasurementStdDevs)
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
: m_odometry(odometry) {
for (size_t i = 0; i < 4; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
@@ -88,8 +88,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 4>& visionMeasurementStdDevs) {
wpi::array<double, 4> r{wpi::empty_array};
const wpi::util::array<double, 4>& visionMeasurementStdDevs) {
wpi::util::array<double, 4> r{wpi::util::empty_array};
for (size_t i = 0; i < 4; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
@@ -178,7 +178,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
* @return The pose at the given timestamp (or std::nullopt if the buffer is
* empty).
*/
std::optional<Pose3d> SampleAt(units::second_t timestamp) const {
std::optional<Pose3d> SampleAt(wpi::units::second_t timestamp) const {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return std::nullopt;
@@ -187,9 +187,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// Step 1: Make sure timestamp matches the sample from the odometry pose
// buffer. (When sampling, the buffer will always use a timestamp
// between the first and last timestamps)
units::second_t oldestOdometryTimestamp =
wpi::units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
units::second_t newestOdometryTimestamp =
wpi::units::second_t newestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().back().first;
timestamp =
std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
@@ -237,11 +237,11 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
* wpi::math::Timer::GetFPGATimestamp(). This means that you should use
* wpi::math::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose3d& visionRobotPose,
units::second_t timestamp) {
wpi::units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
@@ -277,16 +277,16 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
frc::Vectord<6> k_times_twist =
m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(),
wpi::math::Vectord<6> k_times_twist =
m_visionK * wpi::math::Vectord<6>{twist.dx.value(), twist.dy.value(),
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
// Step 6: Convert back to Twist3d.
Twist3d scaledTwist{
units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)},
units::meter_t{k_times_twist(2)}, units::radian_t{k_times_twist(3)},
units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}};
wpi::units::meter_t{k_times_twist(0)}, wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)}, wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)}, wpi::units::radian_t{k_times_twist(5)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
@@ -323,16 +323,16 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
* wpi::math::Timer::GetFPGATimestamp(). This means that you should use
* wpi::math::Timer::GetFPGATimestamp() as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void AddVisionMeasurement(
const Pose3d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 4>& visionMeasurementStdDevs) {
const Pose3d& visionRobotPose, wpi::units::second_t timestamp,
const wpi::util::array<double, 4>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
@@ -362,7 +362,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*
* @return The estimated pose of the robot in meters.
*/
Pose3d UpdateWithTime(units::second_t currentTime,
Pose3d UpdateWithTime(wpi::units::second_t currentTime,
const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions) {
auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
@@ -390,7 +390,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
}
// Step 1: Find the oldest timestamp that needs a vision update.
units::second_t oldestOdometryTimestamp =
wpi::units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
// Step 2: If there are no vision updates before that timestamp, skip.
@@ -433,20 +433,20 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
}
};
static constexpr units::second_t kBufferDuration = 1.5_s;
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry3d<WheelSpeeds, WheelPositions>& m_odometry;
wpi::array<double, 4> m_q{wpi::empty_array};
frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero();
wpi::util::array<double, 4> m_q{wpi::util::empty_array};
wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero();
// Maps timestamps to odometry-only pose estimates
TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
std::map<units::second_t, VisionUpdate> m_visionUpdates;
std::map<wpi::units::second_t, VisionUpdate> m_visionUpdates;
Pose3d m_poseEstimate;
};
} // namespace frc
} // namespace wpi::math

6
wpimath/src/main/native/include/wpi/math/estimator/S3SigmaPoints.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* Generates sigma points and weights according to Papakonstantinou's paper[1]
@@ -57,7 +57,7 @@ class S3SigmaPoints {
Matrixd<States, NumSigmas> SquareRootSigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& S) const {
// table (1), equation (12)
wpi::array<double, States> q{wpi::empty_array};
wpi::util::array<double, States> q{wpi::util::empty_array};
for (size_t t = 1; t <= States; ++t) {
q[t - 1] = m_alpha * std::sqrt(static_cast<double>(t * (States + 1)) /
static_cast<double>(t + 1));
@@ -131,4 +131,4 @@ class S3SigmaPoints {
}
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/estimator/S3UKF.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/estimator/UnscentedKalmanFilter.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
template <int States, int Inputs, int Outputs>
using S3UKF =
@@ -21,4 +21,4 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/estimator/SigmaPoints.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
namespace frc {
namespace wpi::math {
template <typename T, int States>
concept SigmaPoints =
@@ -23,4 +23,4 @@ concept SigmaPoints =
{ t.Wc(i) } -> std::same_as<double>;
} && std::default_initializable<T>;
} // namespace frc
} // namespace wpi::math

12
wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp
View File

@@ -21,7 +21,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* A Kalman filter combines predictions from a model and measurements to give an
@@ -54,8 +54,8 @@ class SteadyStateKalmanFilter {
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateArray = wpi::util::array<double, States>;
using OutputArray = wpi::util::array<double, Outputs>;
/**
* Constructs a steady-state Kalman filter with the given plant.
@@ -73,7 +73,7 @@ class SteadyStateKalmanFilter {
SteadyStateKalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs,
units::second_t dt) {
wpi::units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
@@ -205,7 +205,7 @@ class SteadyStateKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt) {
void Predict(const InputVector& u, wpi::units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
@@ -242,4 +242,4 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SteadyStateKalmanFilter<2, 1, 1>;
} // namespace frc
} // namespace wpi::math

16
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp
View File

@@ -15,7 +15,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps Swerve Drive Odometry to fuse latency-compensated
@@ -30,8 +30,8 @@ namespace frc {
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator
: public PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
wpi::array<SwerveModulePosition, NumModules>> {
: public PoseEstimator<wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModulePosition, NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -52,7 +52,7 @@ class SwerveDrivePoseEstimator
SwerveDrivePoseEstimator(
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: SwerveDrivePoseEstimator{kinematics, gyroAngle,
modulePositions, initialPose,
@@ -78,9 +78,9 @@ class SwerveDrivePoseEstimator
SwerveDrivePoseEstimator(
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
: SwerveDrivePoseEstimator::PoseEstimator(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
@@ -94,4 +94,4 @@ class SwerveDrivePoseEstimator
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SwerveDrivePoseEstimator<4>;
} // namespace frc
} // namespace wpi::math

16
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp
View File

@@ -15,7 +15,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* This class wraps Swerve Drive Odometry to fuse latency-compensated
@@ -34,8 +34,8 @@ namespace frc {
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator3d
: public PoseEstimator3d<wpi::array<SwerveModuleState, NumModules>,
wpi::array<SwerveModulePosition, NumModules>> {
: public PoseEstimator3d<wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModulePosition, NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator3d with default standard deviations
@@ -57,7 +57,7 @@ class SwerveDrivePoseEstimator3d
SwerveDrivePoseEstimator3d(
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation3d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose3d& initialPose)
: SwerveDrivePoseEstimator3d{kinematics, gyroAngle,
modulePositions, initialPose,
@@ -84,9 +84,9 @@ class SwerveDrivePoseEstimator3d
SwerveDrivePoseEstimator3d(
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation3d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
const wpi::array<double, 4>& visionMeasurementStdDevs)
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
: SwerveDrivePoseEstimator3d::PoseEstimator3d(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
@@ -100,4 +100,4 @@ class SwerveDrivePoseEstimator3d
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SwerveDrivePoseEstimator3d<4>;
} // namespace frc
} // namespace wpi::math

16
wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
View File

@@ -20,7 +20,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* A Kalman filter combines predictions from a model and measurements to give an
@@ -67,8 +67,8 @@ class UnscentedKalmanFilter {
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateArray = wpi::util::array<double, States>;
using OutputArray = wpi::util::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
@@ -91,7 +91,7 @@ class UnscentedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
wpi::units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
@@ -161,7 +161,7 @@ class UnscentedKalmanFilter {
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt)
wpi::units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_meanFuncX(std::move(meanFuncX)),
@@ -251,7 +251,7 @@ class UnscentedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt) {
void Predict(const InputVector& u, wpi::units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
@@ -491,9 +491,9 @@ class UnscentedKalmanFilter {
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
Matrixd<States, NumSigmas> m_sigmasF;
units::second_t m_dt;
wpi::units::second_t m_dt;
SigmaPoints m_pts;
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/estimator/UnscentedTransform.hpp
View File

@@ -11,7 +11,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
namespace frc {
namespace wpi::math {
/**
* Computes unscented transform of a set of sigma points and weights. CovDim
@@ -103,4 +103,4 @@ SquareRootUnscentedTransform(
return std::make_tuple(x, S);
}
} // namespace frc
} // namespace wpi::math

14
wpimath/src/main/native/include/wpi/math/filter/Debouncer.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/timestamp.h"
namespace frc {
namespace wpi::math {
/**
* A simple debounce filter for boolean streams. Requires that the boolean
* change value from baseline for a specified period of time before the filtered
@@ -36,7 +36,7 @@ class WPILIB_DLLEXPORT Debouncer {
* @param type Which type of state change the debouncing will be
* performed on.
*/
explicit Debouncer(units::second_t debounceTime,
explicit Debouncer(wpi::units::second_t debounceTime,
DebounceType type = DebounceType::kRising);
/**
@@ -53,7 +53,7 @@ class WPILIB_DLLEXPORT Debouncer {
* @param time The number of seconds the value must change from baseline
* for the filtered value to change.
*/
constexpr void SetDebounceTime(units::second_t time) {
constexpr void SetDebounceTime(wpi::units::second_t time) {
m_debounceTime = time;
}
@@ -63,7 +63,7 @@ class WPILIB_DLLEXPORT Debouncer {
* @return The number of seconds the value must change from baseline
* for the filtered value to change.
*/
constexpr units::second_t GetDebounceTime() const { return m_debounceTime; }
constexpr wpi::units::second_t GetDebounceTime() const { return m_debounceTime; }
/**
* Set the debounce type.
@@ -84,14 +84,14 @@ class WPILIB_DLLEXPORT Debouncer {
constexpr DebounceType GetDebounceType() const { return m_debounceType; }
private:
units::second_t m_debounceTime;
wpi::units::second_t m_debounceTime;
bool m_baseline;
DebounceType m_debounceType;
units::second_t m_prevTime;
wpi::units::second_t m_prevTime;
void ResetTimer();
bool HasElapsed() const;
};
} // namespace frc
} // namespace wpi::math

18
wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
View File

@@ -20,7 +20,7 @@
#include "wpi/util/array.hpp"
#include "wpi/util/circular_buffer.hpp"
namespace frc {
namespace wpi::math {
/**
* This class implements a linear, digital filter. All types of FIR and IIR
@@ -128,7 +128,7 @@ class LinearFilter {
* user.
*/
static constexpr LinearFilter<T> SinglePoleIIR(double timeConstant,
units::second_t period) {
wpi::units::second_t period) {
double gain = gcem::exp(-period.value() / timeConstant);
return LinearFilter({1.0 - gain}, {-gain});
}
@@ -148,7 +148,7 @@ class LinearFilter {
* user.
*/
static constexpr LinearFilter<T> HighPass(double timeConstant,
units::second_t period) {
wpi::units::second_t period) {
double gain = gcem::exp(-period.value() / timeConstant);
return LinearFilter({gain, -gain}, {-gain});
}
@@ -191,7 +191,7 @@ class LinearFilter {
*/
template <int Derivative, int Samples>
static LinearFilter<T> FiniteDifference(
const wpi::array<int, Samples>& stencil, units::second_t period) {
const wpi::util::array<int, Samples>& stencil, wpi::units::second_t period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//
@@ -257,9 +257,9 @@ class LinearFilter {
* @param period The period in seconds between samples taken by the user.
*/
template <int Derivative, int Samples>
static LinearFilter<T> BackwardFiniteDifference(units::second_t period) {
static LinearFilter<T> BackwardFiniteDifference(wpi::units::second_t period) {
// Generate stencil points from -(samples - 1) to 0
wpi::array<int, Samples> stencil{wpi::empty_array};
wpi::util::array<int, Samples> stencil{wpi::util::empty_array};
for (int i = 0; i < Samples; ++i) {
stencil[i] = -(Samples - 1) + i;
}
@@ -384,8 +384,8 @@ class LinearFilter {
constexpr T LastValue() const { return m_lastOutput; }
private:
wpi::circular_buffer<T> m_inputs;
wpi::circular_buffer<T> m_outputs;
wpi::util::circular_buffer<T> m_inputs;
wpi::util::circular_buffer<T> m_outputs;
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
T m_lastOutput{0.0};
@@ -407,4 +407,4 @@ class LinearFilter {
}
};
} // namespace frc
} // namespace wpi::math

8
wpimath/src/main/native/include/wpi/math/filter/MedianFilter.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/util/Algorithm.hpp"
#include "wpi/util/circular_buffer.hpp"
namespace frc {
namespace wpi::math {
/**
* A class that implements a moving-window median filter. Useful for reducing
* measurement noise, especially with processes that generate occasional,
@@ -36,7 +36,7 @@ class MedianFilter {
*/
constexpr T Calculate(T next) {
// Insert next value at proper point in sorted array
wpi::insert_sorted(m_orderedValues, next);
wpi::util::insert_sorted(m_orderedValues, next);
size_t curSize = m_orderedValues.size();
@@ -78,8 +78,8 @@ class MedianFilter {
}
private:
wpi::circular_buffer<T> m_valueBuffer;
wpi::util::circular_buffer<T> m_valueBuffer;
std::vector<T> m_orderedValues;
size_t m_size;
};
} // namespace frc
} // namespace wpi::math

18
wpimath/src/main/native/include/wpi/math/filter/SlewRateLimiter.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/units/time.hpp"
#include "wpi/util/timestamp.h"
namespace frc {
namespace wpi::math {
/**
* A class that limits the rate of change of an input value. Useful for
* implementing voltage, setpoint, and/or output ramps. A slew-rate limit
@@ -23,9 +23,9 @@ namespace frc {
template <class Unit>
class SlewRateLimiter {
public:
using Unit_t = units::unit_t<Unit>;
using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
using Rate_t = units::unit_t<Rate>;
using Unit_t = wpi::units::unit_t<Unit>;
using Rate = wpi::units::compound_unit<Unit, wpi::units::inverse<wpi::units::seconds>>;
using Rate_t = wpi::units::unit_t<Rate>;
/**
* Creates a new SlewRateLimiter with the given positive and negative rate
@@ -45,7 +45,7 @@ class SlewRateLimiter {
m_negativeRateLimit{negativeRateLimit},
m_prevVal{initialValue},
m_prevTime{
units::microsecond_t{wpi::math::MathSharedStore::GetTimestamp()}} {}
wpi::units::microsecond_t{wpi::math::MathSharedStore::GetTimestamp()}} {}
/**
* Creates a new SlewRateLimiter with the given positive rate limit and
@@ -64,8 +64,8 @@ class SlewRateLimiter {
* rate.
*/
Unit_t Calculate(Unit_t input) {
units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
units::second_t elapsedTime = currentTime - m_prevTime;
wpi::units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
wpi::units::second_t elapsedTime = currentTime - m_prevTime;
m_prevVal +=
std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
m_positiveRateLimit * elapsedTime);
@@ -95,6 +95,6 @@ class SlewRateLimiter {
Rate_t m_positiveRateLimit;
Rate_t m_negativeRateLimit;
Unit_t m_prevVal;
units::second_t m_prevTime;
wpi::units::second_t m_prevTime;
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/geometry/CoordinateAxis.hpp
View File

@@ -11,7 +11,7 @@
#include "wpi/math/geometry/Rotation3d.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* A class representing a coordinate system axis within the NWU coordinate
@@ -74,4 +74,4 @@ class WPILIB_DLLEXPORT CoordinateAxis {
Eigen::Vector3d m_axis;
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/geometry/Translation3d.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* A helper class that converts Pose3d objects between different standard
@@ -139,4 +139,4 @@ class WPILIB_DLLEXPORT CoordinateSystem {
Rotation3d m_rotation;
};
} // namespace frc
} // namespace wpi::math

34
wpimath/src/main/native/include/wpi/math/geometry/Ellipse2d.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/array.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents a 2d ellipse space containing translational, rotational, and
@@ -33,8 +33,8 @@ class WPILIB_DLLEXPORT Ellipse2d {
* @param xSemiAxis The x semi-axis.
* @param ySemiAxis The y semi-axis.
*/
constexpr Ellipse2d(const Pose2d& center, units::meter_t xSemiAxis,
units::meter_t ySemiAxis)
constexpr Ellipse2d(const Pose2d& center, wpi::units::meter_t xSemiAxis,
wpi::units::meter_t ySemiAxis)
: m_center{center}, m_xSemiAxis{xSemiAxis}, m_ySemiAxis{ySemiAxis} {
if (xSemiAxis <= 0_m || ySemiAxis <= 0_m) {
throw std::invalid_argument("Ellipse2d semi-axes must be positive");
@@ -71,14 +71,14 @@ class WPILIB_DLLEXPORT Ellipse2d {
*
* @return The x semi-axis.
*/
constexpr units::meter_t XSemiAxis() const { return m_xSemiAxis; }
constexpr wpi::units::meter_t XSemiAxis() const { return m_xSemiAxis; }
/**
* Returns the y semi-axis.
*
* @return The y semi-axis.
*/
constexpr units::meter_t YSemiAxis() const { return m_ySemiAxis; }
constexpr wpi::units::meter_t YSemiAxis() const { return m_ySemiAxis; }
/**
* Returns the focal points of the ellipse. In a perfect circle, this will
@@ -86,21 +86,21 @@ class WPILIB_DLLEXPORT Ellipse2d {
*
* @return The focal points.
*/
constexpr wpi::array<Translation2d, 2> FocalPoints() const {
constexpr wpi::util::array<Translation2d, 2> FocalPoints() const {
// Major semi-axis
auto a = units::math::max(m_xSemiAxis, m_ySemiAxis);
auto a = wpi::units::math::max(m_xSemiAxis, m_ySemiAxis);
// Minor semi-axis
auto b = units::math::min(m_xSemiAxis, m_ySemiAxis);
auto b = wpi::units::math::min(m_xSemiAxis, m_ySemiAxis);
auto c = units::math::sqrt(a * a - b * b);
auto c = wpi::units::math::sqrt(a * a - b * b);
if (m_xSemiAxis > m_ySemiAxis) {
return wpi::array{
return wpi::util::array{
(m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(),
(m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()};
} else {
return wpi::array{
return wpi::util::array{
(m_center + Transform2d{0_m, -c, Rotation2d{}}).Translation(),
(m_center + Transform2d{0_m, c, Rotation2d{}}).Translation()};
}
@@ -153,7 +153,7 @@ class WPILIB_DLLEXPORT Ellipse2d {
* @param point The point to check.
* @return The distance (0, if the point is contained by the ellipse)
*/
units::meter_t Distance(const Translation2d& point) const {
wpi::units::meter_t Distance(const Translation2d& point) const {
return Nearest(point).Distance(point);
}
@@ -174,14 +174,14 @@ class WPILIB_DLLEXPORT Ellipse2d {
*/
constexpr bool operator==(const Ellipse2d& other) const {
return m_center == other.m_center &&
units::math::abs(m_xSemiAxis - other.m_xSemiAxis) < 1E-9_m &&
units::math::abs(m_ySemiAxis - other.m_ySemiAxis) < 1E-9_m;
wpi::units::math::abs(m_xSemiAxis - other.m_xSemiAxis) < 1E-9_m &&
wpi::units::math::abs(m_ySemiAxis - other.m_ySemiAxis) < 1E-9_m;
}
private:
Pose2d m_center;
units::meter_t m_xSemiAxis;
units::meter_t m_ySemiAxis;
wpi::units::meter_t m_xSemiAxis;
wpi::units::meter_t m_ySemiAxis;
/**
* Solves the equation of an ellipse from the given point. This is a helper
@@ -210,7 +210,7 @@ class WPILIB_DLLEXPORT Ellipse2d {
}
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Ellipse2dProto.hpp"
#include "wpi/math/geometry/struct/Ellipse2dStruct.hpp"

20
wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
class Transform2d;
@@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT Pose2d {
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
constexpr Pose2d(wpi::units::meter_t x, wpi::units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
@@ -110,14 +110,14 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The x component of the pose's translation.
*/
constexpr units::meter_t X() const { return m_translation.X(); }
constexpr wpi::units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
constexpr wpi::units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the underlying rotation.
@@ -265,26 +265,26 @@ class WPILIB_DLLEXPORT Pose2d {
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Pose2d& pose);
void to_json(wpi::util::json& json, const Pose2d& pose);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Pose2d& pose);
void from_json(const wpi::util::json& json, Pose2d& pose);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Pose2dProto.hpp"
#include "wpi/math/geometry/struct/Pose2dStruct.hpp"
#include "wpi/math/geometry/Transform2d.hpp"
namespace frc {
namespace wpi::math {
constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
return Transform2d{pose.Translation(), pose.Rotation()};
}
constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
constexpr Pose2d Pose2d::TransformBy(const wpi::math::Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
@@ -294,4 +294,4 @@ constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
return {transform.Translation(), transform.Rotation()};
}
} // namespace frc
} // namespace wpi::math

20
wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp
View File

@@ -20,7 +20,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
class Transform3d;
@@ -53,7 +53,7 @@ class WPILIB_DLLEXPORT Pose3d {
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
constexpr Pose3d(wpi::units::meter_t x, wpi::units::meter_t y, wpi::units::meter_t z,
Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
@@ -126,21 +126,21 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The x component of the pose's translation.
*/
constexpr units::meter_t X() const { return m_translation.X(); }
constexpr wpi::units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
constexpr wpi::units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
constexpr units::meter_t Z() const { return m_translation.Z(); }
constexpr wpi::units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the underlying rotation.
@@ -297,16 +297,16 @@ class WPILIB_DLLEXPORT Pose3d {
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Pose3d& pose);
void to_json(wpi::util::json& json, const Pose3d& pose);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Pose3d& pose);
void from_json(const wpi::util::json& json, Pose3d& pose);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/Transform3d.hpp"
namespace frc {
namespace wpi::math {
constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
const auto pose = this->RelativeTo(other);
@@ -323,7 +323,7 @@ constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
return {transform.Translation(), transform.Rotation()};
}
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Pose3dProto.hpp"
#include "wpi/math/geometry/struct/Pose3dStruct.hpp"

8
wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp
View File

@@ -12,7 +12,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents a quaternion.
@@ -328,12 +328,12 @@ class WPILIB_DLLEXPORT Quaternion {
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Quaternion& quaternion);
void to_json(wpi::util::json& json, const Quaternion& quaternion);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Quaternion& quaternion);
void from_json(const wpi::util::json& json, Quaternion& quaternion);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/QuaternionProto.hpp"
#include "wpi/math/geometry/struct/QuaternionStruct.hpp"

34
wpimath/src/main/native/include/wpi/math/geometry/Rectangle2d.hpp
View File

@@ -15,7 +15,7 @@
#include "wpi/units/math.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents a 2d rectangular space containing translational, rotational, and
@@ -33,8 +33,8 @@ class WPILIB_DLLEXPORT Rectangle2d {
* @param yWidth The y size component of the rectangle, in unrotated
* coordinate frame.
*/
constexpr Rectangle2d(const Pose2d& center, units::meter_t xWidth,
units::meter_t yWidth)
constexpr Rectangle2d(const Pose2d& center, wpi::units::meter_t xWidth,
wpi::units::meter_t yWidth)
: m_center{center}, m_xWidth{xWidth}, m_yWidth{yWidth} {
if (xWidth < 0_m || yWidth < 0_m) {
throw std::invalid_argument(
@@ -52,8 +52,8 @@ class WPILIB_DLLEXPORT Rectangle2d {
constexpr Rectangle2d(const Translation2d& cornerA,
const Translation2d& cornerB)
: m_center{(cornerA + cornerB) / 2.0, Rotation2d{}},
m_xWidth{units::math::abs(cornerA.X() - cornerB.X())},
m_yWidth{units::math::abs(cornerA.Y() - cornerB.Y())} {}
m_xWidth{wpi::units::math::abs(cornerA.X() - cornerB.X())},
m_yWidth{wpi::units::math::abs(cornerA.Y() - cornerB.Y())} {}
/**
* Returns the center of the rectangle.
@@ -74,14 +74,14 @@ class WPILIB_DLLEXPORT Rectangle2d {
*
* @return The x size component of the rectangle.
*/
constexpr units::meter_t XWidth() const { return m_xWidth; }
constexpr wpi::units::meter_t XWidth() const { return m_xWidth; }
/**
* Returns the y size component of the rectangle.
*
* @return The y size component of the rectangle.
*/
constexpr units::meter_t YWidth() const { return m_yWidth; }
constexpr wpi::units::meter_t YWidth() const { return m_yWidth; }
/**
* Transforms the center of the rectangle and returns the new rectangle.
@@ -114,14 +114,14 @@ class WPILIB_DLLEXPORT Rectangle2d {
auto pointInRect = point - m_center.Translation();
pointInRect = pointInRect.RotateBy(-m_center.Rotation());
if (units::math::abs(units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <=
if (wpi::units::math::abs(wpi::units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <=
1E-9_m) {
// Point rests on left/right perimeter
return units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0;
} else if (units::math::abs(units::math::abs(pointInRect.Y()) -
return wpi::units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0;
} else if (wpi::units::math::abs(wpi::units::math::abs(pointInRect.Y()) -
m_yWidth / 2.0) <= 1E-9_m) {
// Point rests on top/bottom perimeter
return units::math::abs(pointInRect.X()) <= m_xWidth / 2.0;
return wpi::units::math::abs(pointInRect.X()) <= m_xWidth / 2.0;
}
return false;
@@ -153,7 +153,7 @@ class WPILIB_DLLEXPORT Rectangle2d {
* @param point The point to check.
* @return The distance (0, if the point is contained by the rectangle)
*/
constexpr units::meter_t Distance(const Translation2d& point) const {
constexpr wpi::units::meter_t Distance(const Translation2d& point) const {
return Nearest(point).Distance(point);
}
@@ -193,17 +193,17 @@ class WPILIB_DLLEXPORT Rectangle2d {
*/
constexpr bool operator==(const Rectangle2d& other) const {
return m_center == other.m_center &&
units::math::abs(m_xWidth - other.m_xWidth) < 1E-9_m &&
units::math::abs(m_yWidth - other.m_yWidth) < 1E-9_m;
wpi::units::math::abs(m_xWidth - other.m_xWidth) < 1E-9_m &&
wpi::units::math::abs(m_yWidth - other.m_yWidth) < 1E-9_m;
}
private:
Pose2d m_center;
units::meter_t m_xWidth;
units::meter_t m_yWidth;
wpi::units::meter_t m_xWidth;
wpi::units::meter_t m_yWidth;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Rectangle2dProto.hpp"
#include "wpi/math/geometry/struct/Rectangle2dStruct.hpp"

26
wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
/**
* A rotation in a 2D coordinate frame represented by a point on the unit circle
@@ -35,9 +35,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @param value The value of the angle.
*/
constexpr Rotation2d(units::angle_unit auto value) // NOLINT
: m_cos{gcem::cos(value.template convert<units::radian>().value())},
m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
constexpr Rotation2d(wpi::units::angle_unit auto value) // NOLINT
: m_cos{gcem::cos(value.template convert<wpi::units::radian>().value())},
m_sin{gcem::sin(value.template convert<wpi::units::radian>().value())} {}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -57,7 +57,7 @@ class WPILIB_DLLEXPORT Rotation2d {
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",
wpi::GetStackTrace(1));
wpi::util::GetStackTrace(1));
}
}
}
@@ -106,7 +106,7 @@ class WPILIB_DLLEXPORT Rotation2d {
* π.
*
* For example, <code>Rotation2d{30_deg} + Rotation2d{60_deg}</code> equals
* <code>Rotation2d{units::radian_t{std::numbers::pi/2.0}}</code>
* <code>Rotation2d{wpi::units::radian_t{std::numbers::pi/2.0}}</code>
*
* @param other The rotation to add.
*
@@ -121,7 +121,7 @@ class WPILIB_DLLEXPORT Rotation2d {
* rotation.
*
* For example, <code>Rotation2d{10_deg} - Rotation2d{100_deg}</code> equals
* <code>Rotation2d{units::radian_t{-std::numbers::pi/2.0}}</code>
* <code>Rotation2d{wpi::units::radian_t{-std::numbers::pi/2.0}}</code>
*
* @param other The rotation to subtract.
*
@@ -203,8 +203,8 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The radian value of the rotation constrained within [-π, π].
*/
constexpr units::radian_t Radians() const {
return units::radian_t{gcem::atan2(m_sin, m_cos)};
constexpr wpi::units::radian_t Radians() const {
return wpi::units::radian_t{gcem::atan2(m_sin, m_cos)};
}
/**
@@ -212,7 +212,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The degree value of the rotation constrained within [-180, 180].
*/
constexpr units::degree_t Degrees() const { return Radians(); }
constexpr wpi::units::degree_t Degrees() const { return Radians(); }
/**
* Returns the cosine of the rotation.
@@ -241,12 +241,12 @@ class WPILIB_DLLEXPORT Rotation2d {
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Rotation2d& rotation);
void to_json(wpi::util::json& json, const Rotation2d& rotation);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Rotation2d& rotation);
void from_json(const wpi::util::json& json, Rotation2d& rotation);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Rotation2dProto.hpp"
#include "wpi/math/geometry/struct/Rotation2dStruct.hpp"

60
wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp
View File

@@ -21,7 +21,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
/**
* A rotation in a 3D coordinate frame represented by a quaternion.
@@ -54,17 +54,17 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param pitch The counterclockwise rotation angle around the Y axis (pitch).
* @param yaw The counterclockwise rotation angle around the Z axis (yaw).
*/
constexpr Rotation3d(units::radian_t roll, units::radian_t pitch,
units::radian_t yaw) {
constexpr Rotation3d(wpi::units::radian_t roll, wpi::units::radian_t pitch,
wpi::units::radian_t yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = units::math::cos(roll * 0.5);
double sr = units::math::sin(roll * 0.5);
double cr = wpi::units::math::cos(roll * 0.5);
double sr = wpi::units::math::sin(roll * 0.5);
double cp = units::math::cos(pitch * 0.5);
double sp = units::math::sin(pitch * 0.5);
double cp = wpi::units::math::cos(pitch * 0.5);
double sp = wpi::units::math::sin(pitch * 0.5);
double cy = units::math::cos(yaw * 0.5);
double sy = units::math::sin(yaw * 0.5);
double cy = wpi::units::math::cos(yaw * 0.5);
double sy = wpi::units::math::sin(yaw * 0.5);
m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
@@ -77,17 +77,17 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
constexpr Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
constexpr Rotation3d(const Eigen::Vector3d& axis, wpi::units::radian_t angle) {
double norm = ct_matrix{axis}.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Eigen::Vector3d v{{axis(0) / norm * units::math::sin(angle / 2.0),
axis(1) / norm * units::math::sin(angle / 2.0),
axis(2) / norm * units::math::sin(angle / 2.0)}};
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
Eigen::Vector3d v{{axis(0) / norm * wpi::units::math::sin(angle / 2.0),
axis(1) / norm * wpi::units::math::sin(angle / 2.0),
axis(2) / norm * wpi::units::math::sin(angle / 2.0)}};
m_q = Quaternion{wpi::units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
/**
@@ -98,7 +98,7 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param rvec The rotation vector.
*/
constexpr explicit Rotation3d(const Eigen::Vector3d& rvec)
: Rotation3d{rvec, units::radian_t{ct_matrix{rvec}.norm()}} {}
: Rotation3d{rvec, wpi::units::radian_t{ct_matrix{rvec}.norm()}} {}
/**
* Constructs a Rotation3d from a rotation matrix.
@@ -282,10 +282,10 @@ class WPILIB_DLLEXPORT Rotation3d {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
2.0 * units::radian_t{scalar * gcem::acos(m_q.W())}};
2.0 * wpi::units::radian_t{scalar * gcem::acos(m_q.W())}};
} else {
return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
2.0 * units::radian_t{scalar * gcem::acos(-m_q.W())}};
2.0 * wpi::units::radian_t{scalar * gcem::acos(-m_q.W())}};
}
}
@@ -331,7 +331,7 @@ class WPILIB_DLLEXPORT Rotation3d {
/**
* Returns the counterclockwise rotation angle around the X axis (roll).
*/
constexpr units::radian_t X() const {
constexpr wpi::units::radian_t X() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
@@ -342,7 +342,7 @@ class WPILIB_DLLEXPORT Rotation3d {
double sxcy = 2.0 * (w * x + y * z);
double cy_sq = cxcy * cxcy + sxcy * sxcy;
if (cy_sq > 1e-20) {
return units::radian_t{gcem::atan2(sxcy, cxcy)};
return wpi::units::radian_t{gcem::atan2(sxcy, cxcy)};
} else {
return 0_rad;
}
@@ -351,7 +351,7 @@ class WPILIB_DLLEXPORT Rotation3d {
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch).
*/
constexpr units::radian_t Y() const {
constexpr wpi::units::radian_t Y() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
@@ -360,16 +360,16 @@ class WPILIB_DLLEXPORT Rotation3d {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
double ratio = 2.0 * (w * y - z * x);
if (gcem::abs(ratio) >= 1.0) {
return units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)};
return wpi::units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)};
} else {
return units::radian_t{gcem::asin(ratio)};
return wpi::units::radian_t{gcem::asin(ratio)};
}
}
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw).
*/
constexpr units::radian_t Z() const {
constexpr wpi::units::radian_t Z() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
@@ -380,9 +380,9 @@ class WPILIB_DLLEXPORT Rotation3d {
double cysz = 2.0 * (w * z + x * y);
double cy_sq = cycz * cycz + cysz * cysz;
if (cy_sq > 1e-20) {
return units::radian_t{gcem::atan2(cysz, cycz)};
return wpi::units::radian_t{gcem::atan2(cysz, cycz)};
} else {
return units::radian_t{gcem::atan2(2.0 * w * z, w * w - z * z)};
return wpi::units::radian_t{gcem::atan2(2.0 * w * z, w * w - z * z)};
}
}
@@ -401,9 +401,9 @@ class WPILIB_DLLEXPORT Rotation3d {
/**
* Returns the angle in the axis-angle representation of this rotation.
*/
constexpr units::radian_t Angle() const {
constexpr wpi::units::radian_t Angle() const {
double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
return wpi::units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
}
/**
@@ -442,12 +442,12 @@ class WPILIB_DLLEXPORT Rotation3d {
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Rotation3d& rotation);
void to_json(wpi::util::json& json, const Rotation3d& rotation);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Rotation3d& rotation);
void from_json(const wpi::util::json& json, Rotation3d& rotation);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Rotation3dProto.hpp"
#include "wpi/math/geometry/struct/Rotation3dStruct.hpp"

16
wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
class Pose2d;
struct Twist2d;
@@ -46,7 +46,7 @@ class WPILIB_DLLEXPORT Transform2d {
* @param y The y component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
constexpr Transform2d(wpi::units::meter_t x, wpi::units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
@@ -81,14 +81,14 @@ class WPILIB_DLLEXPORT Transform2d {
*
* @return The x component of the transformation's translation.
*/
constexpr units::meter_t X() const { return m_translation.X(); }
constexpr wpi::units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
constexpr wpi::units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns an affine transformation matrix representation of this
@@ -168,12 +168,12 @@ class WPILIB_DLLEXPORT Transform2d {
Rotation2d m_rotation;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Twist2d.hpp"
namespace frc {
namespace wpi::math {
constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
// We are rotating the difference between the translations
@@ -207,10 +207,10 @@ constexpr Twist2d Transform2d::Log() const {
m_translation.RotateBy({halfThetaByTanOfHalfDtheta, -halfDtheta}) *
gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
return {translationPart.X(), translationPart.Y(), wpi::units::radian_t{dtheta}};
}
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Transform2dProto.hpp"
#include "wpi/math/geometry/struct/Transform2dStruct.hpp"

34
wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/geometry/Translation3d.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
class Pose3d;
struct Twist3d;
@@ -47,7 +47,7 @@ class WPILIB_DLLEXPORT Transform3d {
* @param z The z component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
constexpr Transform3d(wpi::units::meter_t x, wpi::units::meter_t y, wpi::units::meter_t z,
Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
@@ -82,9 +82,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @see Rotation3d(Rotation2d)
* @see Translation3d(Translation2d)
*/
constexpr explicit Transform3d(const frc::Transform2d& transform)
: m_translation{frc::Translation3d{transform.Translation()}},
m_rotation{frc::Rotation3d{transform.Rotation()}} {}
constexpr explicit Transform3d(const wpi::math::Transform2d& transform)
: m_translation{wpi::math::Translation3d{transform.Translation()}},
m_rotation{wpi::math::Rotation3d{transform.Rotation()}} {}
/**
* Returns the translation component of the transformation.
@@ -98,21 +98,21 @@ class WPILIB_DLLEXPORT Transform3d {
*
* @return The x component of the transformation's translation.
*/
constexpr units::meter_t X() const { return m_translation.X(); }
constexpr wpi::units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
constexpr wpi::units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
constexpr units::meter_t Z() const { return m_translation.Z(); }
constexpr wpi::units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns an affine transformation matrix representation of this
@@ -193,13 +193,13 @@ class WPILIB_DLLEXPORT Transform3d {
Rotation3d m_rotation;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/geometry/Twist3d.hpp"
#include "wpi/math/geometry/detail/RotationVectorToMatrix.hpp"
namespace frc {
namespace wpi::math {
constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
// We are rotating the difference between the translations
@@ -254,12 +254,12 @@ constexpr Twist3d Transform3d::Log() const {
Vector3d translation_component = V_inv * u;
return Twist3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)},
units::radian_t{rvec(0)},
units::radian_t{rvec(1)},
units::radian_t{rvec(2)}};
return Twist3d{wpi::units::meter_t{translation_component(0)},
wpi::units::meter_t{translation_component(1)},
wpi::units::meter_t{translation_component(2)},
wpi::units::radian_t{rvec(0)},
wpi::units::radian_t{rvec(1)},
wpi::units::radian_t{rvec(2)}};
};
if (std::is_constant_evaluated()) {
@@ -268,7 +268,7 @@ constexpr Twist3d Transform3d::Log() const {
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>();
}
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Transform3dProto.hpp"
#include "wpi/math/geometry/struct/Transform3dStruct.hpp"

46
wpimath/src/main/native/include/wpi/math/geometry/Translation2d.hpp
View File

@@ -17,7 +17,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents a translation in 2D space.
@@ -41,7 +41,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
constexpr Translation2d(units::meter_t x, units::meter_t y)
constexpr Translation2d(wpi::units::meter_t x, wpi::units::meter_t y)
: m_x{x}, m_y{y} {}
/**
@@ -51,7 +51,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle)
constexpr Translation2d(wpi::units::meter_t distance, const Rotation2d& angle)
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
/**
@@ -61,7 +61,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param vector The translation vector.
*/
constexpr explicit Translation2d(const Eigen::Vector2d& vector)
: m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
: m_x{wpi::units::meter_t{vector.x()}}, m_y{wpi::units::meter_t{vector.y()}} {}
/**
* Calculates the distance between two translations in 2D space.
@@ -72,8 +72,8 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The distance between the two translations.
*/
constexpr units::meter_t Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
constexpr wpi::units::meter_t Distance(const Translation2d& other) const {
return wpi::units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
@@ -87,10 +87,10 @@ class WPILIB_DLLEXPORT Translation2d {
* @param other The translation to compute the squared distance to.
* @return The square of the distance between the two translations.
*/
constexpr units::square_meter_t SquaredDistance(
constexpr wpi::units::square_meter_t SquaredDistance(
const Translation2d& other) const {
return units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y);
return wpi::units::math::pow<2>(other.m_x - m_x) +
wpi::units::math::pow<2>(other.m_y - m_y);
}
/**
@@ -98,14 +98,14 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The X component of the translation.
*/
constexpr units::meter_t X() const { return m_x; }
constexpr wpi::units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
constexpr units::meter_t Y() const { return m_y; }
constexpr wpi::units::meter_t Y() const { return m_y; }
/**
* Returns a 2D translation vector representation of this translation.
@@ -121,7 +121,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The norm of the translation.
*/
constexpr units::meter_t Norm() const { return units::math::hypot(m_x, m_y); }
constexpr wpi::units::meter_t Norm() const { return wpi::units::math::hypot(m_x, m_y); }
/**
* Returns the squared norm, or squared distance from the origin to the
@@ -130,8 +130,8 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The squared norm of the translation.
*/
constexpr units::square_meter_t SquaredNorm() const {
return units::math::pow<2>(m_x) + units::math::pow<2>(m_y);
constexpr wpi::units::square_meter_t SquaredNorm() const {
return wpi::units::math::pow<2>(m_x) + wpi::units::math::pow<2>(m_y);
}
/**
@@ -195,7 +195,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations.
*/
constexpr units::square_meter_t Dot(const Translation2d& other) const {
constexpr wpi::units::square_meter_t Dot(const Translation2d& other) const {
return m_x * other.X() + m_y * other.Y();
}
@@ -208,7 +208,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
constexpr units::square_meter_t Cross(const Translation2d& other) const {
constexpr wpi::units::square_meter_t Cross(const Translation2d& other) const {
return m_x * other.Y() - m_y * other.X();
}
@@ -282,8 +282,8 @@ class WPILIB_DLLEXPORT Translation2d {
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
return wpi::units::math::abs(m_x - other.m_x) < 1E-9_m &&
wpi::units::math::abs(m_y - other.m_y) < 1E-9_m;
}
/**
@@ -315,17 +315,17 @@ class WPILIB_DLLEXPORT Translation2d {
}
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
wpi::units::meter_t m_x = 0_m;
wpi::units::meter_t m_y = 0_m;
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Translation2d& state);
void to_json(wpi::util::json& json, const Translation2d& state);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Translation2d& state);
void from_json(const wpi::util::json& json, Translation2d& state);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Translation2dProto.hpp"
#include "wpi/math/geometry/struct/Translation2dStruct.hpp"

68
wpimath/src/main/native/include/wpi/math/geometry/Translation3d.hpp
View File

@@ -18,7 +18,7 @@
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/json_fwd.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents a translation in 3D space.
@@ -43,7 +43,7 @@ class WPILIB_DLLEXPORT Translation3d {
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z)
constexpr Translation3d(wpi::units::meter_t x, wpi::units::meter_t y, wpi::units::meter_t z)
: m_x{x}, m_y{y}, m_z{z} {}
/**
@@ -53,7 +53,7 @@ class WPILIB_DLLEXPORT Translation3d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
constexpr Translation3d(units::meter_t distance, const Rotation3d& angle) {
constexpr Translation3d(wpi::units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
@@ -67,9 +67,9 @@ class WPILIB_DLLEXPORT Translation3d {
* @param vector The translation vector.
*/
constexpr explicit Translation3d(const Eigen::Vector3d& vector)
: m_x{units::meter_t{vector.x()}},
m_y{units::meter_t{vector.y()}},
m_z{units::meter_t{vector.z()}} {}
: m_x{wpi::units::meter_t{vector.x()}},
m_y{wpi::units::meter_t{vector.y()}},
m_z{wpi::units::meter_t{vector.z()}} {}
/**
* Constructs a 3D translation from a 2D translation in the X-Y plane.
@@ -91,10 +91,10 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The distance between the two translations.
*/
constexpr units::meter_t Distance(const Translation3d& other) const {
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +
units::math::pow<2>(other.m_z - m_z));
constexpr wpi::units::meter_t Distance(const Translation3d& other) const {
return wpi::units::math::sqrt(wpi::units::math::pow<2>(other.m_x - m_x) +
wpi::units::math::pow<2>(other.m_y - m_y) +
wpi::units::math::pow<2>(other.m_z - m_z));
}
/**
@@ -108,11 +108,11 @@ class WPILIB_DLLEXPORT Translation3d {
* @param other The translation to compute the squared distance to.
* @return The squared distance between the two translations.
*/
constexpr units::square_meter_t SquaredDistance(
constexpr wpi::units::square_meter_t SquaredDistance(
const Translation3d& other) const {
return units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +
units::math::pow<2>(other.m_z - m_z);
return wpi::units::math::pow<2>(other.m_x - m_x) +
wpi::units::math::pow<2>(other.m_y - m_y) +
wpi::units::math::pow<2>(other.m_z - m_z);
}
/**
@@ -120,21 +120,21 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The Z component of the translation.
*/
constexpr units::meter_t X() const { return m_x; }
constexpr wpi::units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
constexpr units::meter_t Y() const { return m_y; }
constexpr wpi::units::meter_t Y() const { return m_y; }
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
constexpr units::meter_t Z() const { return m_z; }
constexpr wpi::units::meter_t Z() const { return m_z; }
/**
* Returns a 3D translation vector representation of this translation.
@@ -150,8 +150,8 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The norm of the translation.
*/
constexpr units::meter_t Norm() const {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
constexpr wpi::units::meter_t Norm() const {
return wpi::units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
@@ -161,7 +161,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The squared norm of the translation.
*/
constexpr units::square_meter_t SquaredNorm() const {
constexpr wpi::units::square_meter_t SquaredNorm() const {
return m_x * m_x + m_y * m_y + m_z * m_z;
}
@@ -178,8 +178,8 @@ class WPILIB_DLLEXPORT Translation3d {
constexpr Translation3d RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
units::meter_t{qprime.Z()}};
return Translation3d{wpi::units::meter_t{qprime.X()}, wpi::units::meter_t{qprime.Y()},
wpi::units::meter_t{qprime.Z()}};
}
/**
@@ -203,7 +203,7 @@ class WPILIB_DLLEXPORT Translation3d {
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations.
*/
constexpr units::square_meter_t Dot(const Translation3d& other) const {
constexpr wpi::units::square_meter_t Dot(const Translation3d& other) const {
return m_x * other.X() + m_y * other.Y() + m_z * other.Z();
}
@@ -218,9 +218,9 @@ class WPILIB_DLLEXPORT Translation3d {
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
constexpr Eigen::Vector<units::square_meter_t, 3> Cross(
constexpr Eigen::Vector<wpi::units::square_meter_t, 3> Cross(
const Translation3d& other) const {
return Eigen::Vector<units::square_meter_t, 3>{
return Eigen::Vector<wpi::units::square_meter_t, 3>{
{m_y * other.Z() - other.Y() * m_z},
{m_z * other.X() - other.Z() * m_x},
{m_x * other.Y() - other.X() * m_y}};
@@ -305,9 +305,9 @@ class WPILIB_DLLEXPORT Translation3d {
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&
units::math::abs(m_z - other.m_z) < 1E-9_m;
return wpi::units::math::abs(m_x - other.m_x) < 1E-9_m &&
wpi::units::math::abs(m_y - other.m_y) < 1E-9_m &&
wpi::units::math::abs(m_z - other.m_z) < 1E-9_m;
}
/**
@@ -339,18 +339,18 @@ class WPILIB_DLLEXPORT Translation3d {
}
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
units::meter_t m_z = 0_m;
wpi::units::meter_t m_x = 0_m;
wpi::units::meter_t m_y = 0_m;
wpi::units::meter_t m_z = 0_m;
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const Translation3d& state);
void to_json(wpi::util::json& json, const Translation3d& state);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Translation3d& state);
void from_json(const wpi::util::json& json, Translation3d& state);
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Translation3dProto.hpp"
#include "wpi/math/geometry/struct/Translation3dStruct.hpp"

20
wpimath/src/main/native/include/wpi/math/geometry/Twist2d.hpp
View File

@@ -9,7 +9,7 @@
#include "wpi/units/math.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
class Transform2d;
@@ -24,17 +24,17 @@ struct WPILIB_DLLEXPORT Twist2d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
wpi::units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
wpi::units::meter_t dy = 0_m;
/**
* Angular "dtheta" component (radians)
*/
units::radian_t dtheta = 0_rad;
wpi::units::radian_t dtheta = 0_rad;
/**
* Obtain a new Transform2d from a (constant curvature) velocity.
@@ -61,9 +61,9 @@ struct WPILIB_DLLEXPORT Twist2d {
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Twist2d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
return wpi::units::math::abs(dx - other.dx) < 1E-9_m &&
wpi::units::math::abs(dy - other.dy) < 1E-9_m &&
wpi::units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
}
/**
@@ -77,11 +77,11 @@ struct WPILIB_DLLEXPORT Twist2d {
}
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/Transform2d.hpp"
namespace frc {
namespace wpi::math {
constexpr Transform2d Twist2d::Exp() const {
const auto theta = dtheta.value();
@@ -101,7 +101,7 @@ constexpr Transform2d Twist2d::Exp() const {
Rotation2d{cosTheta, sinTheta});
}
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Twist2dProto.hpp"
#include "wpi/math/geometry/struct/Twist2dStruct.hpp"

38
wpimath/src/main/native/include/wpi/math/geometry/Twist3d.hpp
View File

@@ -9,7 +9,7 @@
#include "wpi/units/math.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
class Transform3d;
@@ -24,32 +24,32 @@ struct WPILIB_DLLEXPORT Twist3d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
wpi::units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
wpi::units::meter_t dy = 0_m;
/**
* Linear "dz" component
*/
units::meter_t dz = 0_m;
wpi::units::meter_t dz = 0_m;
/**
* Rotation vector x component.
*/
units::radian_t rx = 0_rad;
wpi::units::radian_t rx = 0_rad;
/**
* Rotation vector y component.
*/
units::radian_t ry = 0_rad;
wpi::units::radian_t ry = 0_rad;
/**
* Rotation vector z component.
*/
units::radian_t rz = 0_rad;
wpi::units::radian_t rz = 0_rad;
/**
* Obtain a new Transform3d from a (constant curvature) velocity.
@@ -76,12 +76,12 @@ struct WPILIB_DLLEXPORT Twist3d {
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Twist3d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dz - other.dz) < 1E-9_m &&
units::math::abs(rx - other.rx) < 1E-9_rad &&
units::math::abs(ry - other.ry) < 1E-9_rad &&
units::math::abs(rz - other.rz) < 1E-9_rad;
return wpi::units::math::abs(dx - other.dx) < 1E-9_m &&
wpi::units::math::abs(dy - other.dy) < 1E-9_m &&
wpi::units::math::abs(dz - other.dz) < 1E-9_m &&
wpi::units::math::abs(rx - other.rx) < 1E-9_rad &&
wpi::units::math::abs(ry - other.ry) < 1E-9_rad &&
wpi::units::math::abs(rz - other.rz) < 1E-9_rad;
}
/**
@@ -96,12 +96,12 @@ struct WPILIB_DLLEXPORT Twist3d {
}
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/math/geometry/detail/RotationVectorToMatrix.hpp"
namespace frc {
namespace wpi::math {
constexpr Transform3d Twist3d::Exp() const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
@@ -147,9 +147,9 @@ constexpr Transform3d Twist3d::Exp() const {
Vector3d translation_component = V * u;
const Transform3d transform{
Translation3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)}},
Translation3d{wpi::units::meter_t{translation_component(0)},
wpi::units::meter_t{translation_component(1)},
wpi::units::meter_t{translation_component(2)}},
Rotation3d{R}};
return transform;
@@ -161,7 +161,7 @@ constexpr Transform3d Twist3d::Exp() const {
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>();
}
} // namespace frc
} // namespace wpi::math
#include "wpi/math/geometry/proto/Twist3dProto.hpp"
#include "wpi/math/geometry/struct/Twist3dStruct.hpp"

4
wpimath/src/main/native/include/wpi/math/geometry/detail/RotationVectorToMatrix.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/linalg/ct_matrix.hpp"
namespace frc::detail {
namespace wpi::math::detail {
constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) {
return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
@@ -23,4 +23,4 @@ constexpr Eigen::Matrix3d RotationVectorToMatrix(
{-rotation(1), rotation(0), 0.0}};
}
} // namespace frc::detail
} // namespace wpi::math::detail

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Ellipse2dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Ellipse2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Ellipse2d> {
using MessageStruct = wpi_proto_ProtobufEllipse2d;
using InputStream = wpi::ProtoInputStream<frc::Ellipse2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Ellipse2d>;
static std::optional<frc::Ellipse2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Ellipse2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Ellipse2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Ellipse2d>;
static std::optional<wpi::math::Ellipse2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Ellipse2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Pose2dProto.hpp
View File

@@ -11,10 +11,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Pose2d> {
using MessageStruct = wpi_proto_ProtobufPose2d;
using InputStream = wpi::ProtoInputStream<frc::Pose2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Pose2d>;
static std::optional<frc::Pose2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Pose2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Pose2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Pose2d>;
static std::optional<wpi::math::Pose2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Pose2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Pose3dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry3d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Pose3d> {
using MessageStruct = wpi_proto_ProtobufPose3d;
using InputStream = wpi::ProtoInputStream<frc::Pose3d>;
using OutputStream = wpi::ProtoOutputStream<frc::Pose3d>;
static std::optional<frc::Pose3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Pose3d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Pose3d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Pose3d>;
static std::optional<wpi::math::Pose3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Pose3d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/QuaternionProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry3d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Quaternion> {
using MessageStruct = wpi_proto_ProtobufQuaternion;
using InputStream = wpi::ProtoInputStream<frc::Quaternion>;
using OutputStream = wpi::ProtoOutputStream<frc::Quaternion>;
static std::optional<frc::Quaternion> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Quaternion& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Quaternion>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Quaternion>;
static std::optional<wpi::math::Quaternion> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Quaternion& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Rectangle2dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rectangle2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Rectangle2d> {
using MessageStruct = wpi_proto_ProtobufRectangle2d;
using InputStream = wpi::ProtoInputStream<frc::Rectangle2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Rectangle2d>;
static std::optional<frc::Rectangle2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Rectangle2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Rectangle2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Rectangle2d>;
static std::optional<wpi::math::Rectangle2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Rectangle2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Rotation2dProto.hpp
View File

@@ -11,10 +11,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Rotation2d> {
using MessageStruct = wpi_proto_ProtobufRotation2d;
using InputStream = wpi::ProtoInputStream<frc::Rotation2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Rotation2d>;
static std::optional<frc::Rotation2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Rotation2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Rotation2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Rotation2d>;
static std::optional<wpi::math::Rotation2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Rotation2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Rotation3dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry3d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Rotation3d> {
using MessageStruct = wpi_proto_ProtobufRotation3d;
using InputStream = wpi::ProtoInputStream<frc::Rotation3d>;
using OutputStream = wpi::ProtoOutputStream<frc::Rotation3d>;
static std::optional<frc::Rotation3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Rotation3d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Rotation3d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Rotation3d>;
static std::optional<wpi::math::Rotation3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Rotation3d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Transform2dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Transform2d> {
using MessageStruct = wpi_proto_ProtobufTransform2d;
using InputStream = wpi::ProtoInputStream<frc::Transform2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Transform2d>;
static std::optional<frc::Transform2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Transform2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Transform2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Transform2d>;
static std::optional<wpi::math::Transform2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Transform2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Transform3dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry3d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Transform3d> {
using MessageStruct = wpi_proto_ProtobufTransform3d;
using InputStream = wpi::ProtoInputStream<frc::Transform3d>;
using OutputStream = wpi::ProtoOutputStream<frc::Transform3d>;
static std::optional<frc::Transform3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Transform3d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Transform3d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Transform3d>;
static std::optional<wpi::math::Transform3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Transform3d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Translation2dProto.hpp
View File

@@ -11,10 +11,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Translation2d> {
using MessageStruct = wpi_proto_ProtobufTranslation2d;
using InputStream = wpi::ProtoInputStream<frc::Translation2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Translation2d>;
static std::optional<frc::Translation2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Translation2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Translation2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Translation2d>;
static std::optional<wpi::math::Translation2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Translation2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Translation3dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry3d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Translation3d> {
using MessageStruct = wpi_proto_ProtobufTranslation3d;
using InputStream = wpi::ProtoInputStream<frc::Translation3d>;
using OutputStream = wpi::ProtoOutputStream<frc::Translation3d>;
static std::optional<frc::Translation3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Translation3d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Translation3d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Translation3d>;
static std::optional<wpi::math::Translation3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Translation3d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Twist2dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry2d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Twist2d> {
using MessageStruct = wpi_proto_ProtobufTwist2d;
using InputStream = wpi::ProtoInputStream<frc::Twist2d>;
using OutputStream = wpi::ProtoOutputStream<frc::Twist2d>;
static std::optional<frc::Twist2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Twist2d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Twist2d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Twist2d>;
static std::optional<wpi::math::Twist2d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Twist2d& value);
};

10
wpimath/src/main/native/include/wpi/math/geometry/proto/Twist3dProto.hpp
View File

@@ -10,10 +10,10 @@
#include "wpimath/protobuf/geometry3d.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Twist3d> {
using MessageStruct = wpi_proto_ProtobufTwist3d;
using InputStream = wpi::ProtoInputStream<frc::Twist3d>;
using OutputStream = wpi::ProtoOutputStream<frc::Twist3d>;
static std::optional<frc::Twist3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const frc::Twist3d& value);
using InputStream = wpi::util::ProtoInputStream<wpi::math::Twist3d>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Twist3d>;
static std::optional<wpi::math::Twist3d> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Twist3d& value);
};

14
wpimath/src/main/native/include/wpi/math/geometry/struct/Ellipse2dStruct.hpp
View File

@@ -9,22 +9,22 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Ellipse2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Ellipse2d> {
static constexpr std::string_view GetTypeName() { return "Ellipse2d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Pose2d>() + 16;
return wpi::util::GetStructSize<wpi::math::Pose2d>() + 16;
}
static constexpr std::string_view GetSchema() {
return "Pose2d center;double xSemiAxis;double ySemiAxis";
}
static frc::Ellipse2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Ellipse2d& value);
static wpi::math::Ellipse2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Ellipse2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Pose2d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Pose2d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Ellipse2d>);
static_assert(wpi::HasNestedStruct<frc::Ellipse2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Ellipse2d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Ellipse2d>);

18
wpimath/src/main/native/include/wpi/math/geometry/struct/Pose2dStruct.hpp
View File

@@ -9,24 +9,24 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Pose2d> {
static constexpr std::string_view GetTypeName() { return "Pose2d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation2d>() +
wpi::GetStructSize<frc::Rotation2d>();
return wpi::util::GetStructSize<wpi::math::Translation2d>() +
wpi::util::GetStructSize<wpi::math::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "Translation2d translation;Rotation2d rotation";
}
static frc::Pose2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Pose2d& value);
static wpi::math::Pose2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Pose2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Translation2d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Rotation2d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Pose2d>);
static_assert(wpi::HasNestedStruct<frc::Pose2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Pose2d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Pose2d>);

18
wpimath/src/main/native/include/wpi/math/geometry/struct/Pose3dStruct.hpp
View File

@@ -9,24 +9,24 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose3d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Pose3d> {
static constexpr std::string_view GetTypeName() { return "Pose3d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation3d>() +
wpi::GetStructSize<frc::Rotation3d>();
return wpi::util::GetStructSize<wpi::math::Translation3d>() +
wpi::util::GetStructSize<wpi::math::Rotation3d>();
}
static constexpr std::string_view GetSchema() {
return "Translation3d translation;Rotation3d rotation";
}
static frc::Pose3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Pose3d& value);
static wpi::math::Pose3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Pose3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Translation3d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Rotation3d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Pose3d>);
static_assert(wpi::HasNestedStruct<frc::Pose3d>);
static_assert(wpi::util::StructSerializable<wpi::math::Pose3d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Pose3d>);

8
wpimath/src/main/native/include/wpi/math/geometry/struct/QuaternionStruct.hpp
View File

@@ -9,15 +9,15 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Quaternion> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Quaternion> {
static constexpr std::string_view GetTypeName() { return "Quaternion"; }
static constexpr size_t GetSize() { return 32; }
static constexpr std::string_view GetSchema() {
return "double w;double x;double y;double z";
}
static frc::Quaternion Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Quaternion& value);
static wpi::math::Quaternion Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Quaternion& value);
};
static_assert(wpi::StructSerializable<frc::Quaternion>);
static_assert(wpi::util::StructSerializable<wpi::math::Quaternion>);

14
wpimath/src/main/native/include/wpi/math/geometry/struct/Rectangle2dStruct.hpp
View File

@@ -9,22 +9,22 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rectangle2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Rectangle2d> {
static constexpr std::string_view GetTypeName() { return "Rectangle2d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Pose2d>() + 16;
return wpi::util::GetStructSize<wpi::math::Pose2d>() + 16;
}
static constexpr std::string_view GetSchema() {
return "Pose2d center;double xWidth;double yWidth";
}
static frc::Rectangle2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Rectangle2d& value);
static wpi::math::Rectangle2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Rectangle2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Pose2d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Pose2d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Rectangle2d>);
static_assert(wpi::HasNestedStruct<frc::Rectangle2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Rectangle2d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Rectangle2d>);

8
wpimath/src/main/native/include/wpi/math/geometry/struct/Rotation2dStruct.hpp
View File

@@ -9,13 +9,13 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Rotation2d> {
static constexpr std::string_view GetTypeName() { return "Rotation2d"; }
static constexpr size_t GetSize() { return 8; }
static constexpr std::string_view GetSchema() { return "double value"; }
static frc::Rotation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Rotation2d& value);
static wpi::math::Rotation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Rotation2d& value);
};
static_assert(wpi::StructSerializable<frc::Rotation2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Rotation2d>);

14
wpimath/src/main/native/include/wpi/math/geometry/struct/Rotation3dStruct.hpp
View File

@@ -9,20 +9,20 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation3d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Rotation3d> {
static constexpr std::string_view GetTypeName() { return "Rotation3d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Quaternion>();
return wpi::util::GetStructSize<wpi::math::Quaternion>();
}
static constexpr std::string_view GetSchema() { return "Quaternion q"; }
static frc::Rotation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Rotation3d& value);
static wpi::math::Rotation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Rotation3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Quaternion>(fn);
wpi::util::ForEachStructSchema<wpi::math::Quaternion>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Rotation3d>);
static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
static_assert(wpi::util::StructSerializable<wpi::math::Rotation3d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Rotation3d>);

18
wpimath/src/main/native/include/wpi/math/geometry/struct/Transform2dStruct.hpp
View File

@@ -9,24 +9,24 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Transform2d> {
static constexpr std::string_view GetTypeName() { return "Transform2d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation2d>() +
wpi::GetStructSize<frc::Rotation2d>();
return wpi::util::GetStructSize<wpi::math::Translation2d>() +
wpi::util::GetStructSize<wpi::math::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "Translation2d translation;Rotation2d rotation";
}
static frc::Transform2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Transform2d& value);
static wpi::math::Transform2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Transform2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Translation2d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Rotation2d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Transform2d>);
static_assert(wpi::HasNestedStruct<frc::Transform2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Transform2d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Transform2d>);

18
wpimath/src/main/native/include/wpi/math/geometry/struct/Transform3dStruct.hpp
View File

@@ -9,24 +9,24 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform3d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Transform3d> {
static constexpr std::string_view GetTypeName() { return "Transform3d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation3d>() +
wpi::GetStructSize<frc::Rotation3d>();
return wpi::util::GetStructSize<wpi::math::Translation3d>() +
wpi::util::GetStructSize<wpi::math::Rotation3d>();
}
static constexpr std::string_view GetSchema() {
return "Translation3d translation;Rotation3d rotation";
}
static frc::Transform3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Transform3d& value);
static wpi::math::Transform3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Transform3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Translation3d>(fn);
wpi::util::ForEachStructSchema<wpi::math::Rotation3d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::Transform3d>);
static_assert(wpi::HasNestedStruct<frc::Transform3d>);
static_assert(wpi::util::StructSerializable<wpi::math::Transform3d>);
static_assert(wpi::util::HasNestedStruct<wpi::math::Transform3d>);

8
wpimath/src/main/native/include/wpi/math/geometry/struct/Translation2dStruct.hpp
View File

@@ -9,13 +9,13 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Translation2d> {
static constexpr std::string_view GetTypeName() { return "Translation2d"; }
static constexpr size_t GetSize() { return 16; }
static constexpr std::string_view GetSchema() { return "double x;double y"; }
static frc::Translation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Translation2d& value);
static wpi::math::Translation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Translation2d& value);
};
static_assert(wpi::StructSerializable<frc::Translation2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Translation2d>);

8
wpimath/src/main/native/include/wpi/math/geometry/struct/Translation3dStruct.hpp
View File

@@ -9,15 +9,15 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation3d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Translation3d> {
static constexpr std::string_view GetTypeName() { return "Translation3d"; }
static constexpr size_t GetSize() { return 24; }
static constexpr std::string_view GetSchema() {
return "double x;double y;double z";
}
static frc::Translation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Translation3d& value);
static wpi::math::Translation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Translation3d& value);
};
static_assert(wpi::StructSerializable<frc::Translation3d>);
static_assert(wpi::util::StructSerializable<wpi::math::Translation3d>);

8
wpimath/src/main/native/include/wpi/math/geometry/struct/Twist2dStruct.hpp
View File

@@ -9,15 +9,15 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist2d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Twist2d> {
static constexpr std::string_view GetTypeName() { return "Twist2d"; }
static constexpr size_t GetSize() { return 24; }
static constexpr std::string_view GetSchema() {
return "double dx;double dy;double dtheta";
}
static frc::Twist2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Twist2d& value);
static wpi::math::Twist2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Twist2d& value);
};
static_assert(wpi::StructSerializable<frc::Twist2d>);
static_assert(wpi::util::StructSerializable<wpi::math::Twist2d>);

8
wpimath/src/main/native/include/wpi/math/geometry/struct/Twist3dStruct.hpp
View File

@@ -9,15 +9,15 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist3d> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Twist3d> {
static constexpr std::string_view GetTypeName() { return "Twist3d"; }
static constexpr size_t GetSize() { return 48; }
static constexpr std::string_view GetSchema() {
return "double dx;double dy;double dz;double rx;double ry;double rz";
}
static frc::Twist3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Twist3d& value);
static wpi::math::Twist3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Twist3d& value);
};
static_assert(wpi::StructSerializable<frc::Twist3d>);
static_assert(wpi::util::StructSerializable<wpi::math::Twist3d>);

28
wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp
View File

@@ -15,7 +15,7 @@
#include "wpi/util/MathExtras.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* The TimeInterpolatableBuffer provides an easy way to estimate past
@@ -24,7 +24,7 @@ namespace frc {
* when vision or other global measurement were recorded is necessary, or for
* recording the past angles of mechanisms as measured by encoders.
*
* When sampling this buffer, a user-provided function or wpi::Lerp can be
* When sampling this buffer, a user-provided function or wpi::util::Lerp can be
* used. For Pose2ds, we use Twists.
*
* @tparam T The type stored in this buffer.
@@ -38,20 +38,20 @@ class TimeInterpolatableBuffer {
* @param historySize The history size of the buffer.
* @param func The function used to interpolate between values.
*/
TimeInterpolatableBuffer(units::second_t historySize,
TimeInterpolatableBuffer(wpi::units::second_t historySize,
std::function<T(const T&, const T&, double)> func)
: m_historySize(historySize), m_interpolatingFunc(func) {}
/**
* Create a new TimeInterpolatableBuffer. By default, the interpolation
* function is wpi::Lerp except for Pose2d, which uses the pose exponential.
* function is wpi::util::Lerp except for Pose2d, which uses the pose exponential.
*
* @param historySize The history size of the buffer.
*/
explicit TimeInterpolatableBuffer(units::second_t historySize)
explicit TimeInterpolatableBuffer(wpi::units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const T& start, const T& end, double t) {
return wpi::Lerp(start, end, t);
return wpi::util::Lerp(start, end, t);
}) {}
/**
@@ -60,7 +60,7 @@ class TimeInterpolatableBuffer {
* @param time The timestamp of the sample.
* @param sample The sample object.
*/
void AddSample(units::second_t time, T sample) {
void AddSample(wpi::units::second_t time, T sample) {
// Add the new state into the vector
if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
m_pastSnapshots.emplace_back(time, sample);
@@ -97,7 +97,7 @@ class TimeInterpolatableBuffer {
*
* @param time The time at which to sample the buffer.
*/
std::optional<T> Sample(units::second_t time) const {
std::optional<T> Sample(wpi::units::second_t time) const {
if (m_pastSnapshots.empty()) {
return {};
}
@@ -137,27 +137,27 @@ class TimeInterpolatableBuffer {
* Grant access to the internal sample buffer. Used in Pose Estimation to
* replay odometry inputs stored within this buffer.
*/
std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() {
std::vector<std::pair<wpi::units::second_t, T>>& GetInternalBuffer() {
return m_pastSnapshots;
}
/**
* Grant access to the internal sample buffer.
*/
const std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() const {
const std::vector<std::pair<wpi::units::second_t, T>>& GetInternalBuffer() const {
return m_pastSnapshots;
}
private:
units::second_t m_historySize;
std::vector<std::pair<units::second_t, T>> m_pastSnapshots;
wpi::units::second_t m_historySize;
std::vector<std::pair<wpi::units::second_t, T>> m_pastSnapshots;
std::function<T(const T&, const T&, double)> m_interpolatingFunc;
};
// Template specialization to ensure that Pose2d uses pose exponential
template <>
inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
units::second_t historySize)
wpi::units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) {
if (t < 0) {
@@ -171,4 +171,4 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
}
}) {}
} // namespace frc
} // namespace wpi::math

26
wpimath/src/main/native/include/wpi/math/kinematics/ChassisSpeeds.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
@@ -25,17 +25,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
/**
* Velocity along the x-axis. (Fwd is +)
*/
units::meters_per_second_t vx = 0_mps;
wpi::units::meters_per_second_t vx = 0_mps;
/**
* Velocity along the y-axis. (Left is +)
*/
units::meters_per_second_t vy = 0_mps;
wpi::units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
units::radians_per_second_t omega = 0_rad_per_s;
wpi::units::radians_per_second_t omega = 0_rad_per_s;
/**
* Creates a Twist2d from ChassisSpeeds.
@@ -44,7 +44,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Twist2d.
*/
constexpr Twist2d ToTwist2d(units::second_t dt) const {
constexpr Twist2d ToTwist2d(wpi::units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
@@ -67,7 +67,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
*/
constexpr ChassisSpeeds Discretize(units::second_t dt) const {
constexpr ChassisSpeeds Discretize(wpi::units::second_t dt) const {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
@@ -94,10 +94,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
Translation2d{wpi::units::meter_t{vx.value()}, wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {units::meters_per_second_t{rotated.X().value()},
units::meters_per_second_t{rotated.Y().value()}, omega};
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
@@ -114,10 +114,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
Translation2d{wpi::units::meter_t{vx.value()}, wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {units::meters_per_second_t{rotated.X().value()},
units::meters_per_second_t{rotated.Y().value()}, omega};
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
@@ -194,7 +194,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*/
constexpr bool operator==(const ChassisSpeeds& other) const = default;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/kinematics/proto/ChassisSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp"

12
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp
View File

@@ -16,7 +16,7 @@
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
@@ -37,7 +37,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
constexpr explicit DifferentialDriveKinematics(units::meter_t trackwidth)
constexpr explicit DifferentialDriveKinematics(wpi::units::meter_t trackwidth)
: trackwidth(trackwidth) {
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportUsage("DifferentialDriveKinematics",
@@ -80,8 +80,8 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
* @param rightDistance The distance measured by the right encoder.
* @return The resulting Twist2d.
*/
constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
const units::meter_t rightDistance) const {
constexpr Twist2d ToTwist2d(const wpi::units::meter_t leftDistance,
const wpi::units::meter_t rightDistance) const {
return {(leftDistance + rightDistance) / 2, 0_m,
(rightDistance - leftDistance) / trackwidth * 1_rad};
}
@@ -99,9 +99,9 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
}
/// Differential drive trackwidth.
units::meter_t trackwidth;
wpi::units::meter_t trackwidth;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveKinematicsProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveKinematicsStruct.hpp"

18
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp
View File

@@ -12,7 +12,7 @@
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Class for differential drive odometry. Odometry allows you to track the
* robot's position on the field over the course of a match using readings from
@@ -41,8 +41,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance,
const Pose2d& initialPose = Pose2d{});
/**
@@ -59,8 +59,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose) {
void ResetPosition(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose2d& pose) {
Odometry::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
@@ -75,12 +75,12 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance) {
const Pose2d& Update(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return Odometry::Update(gyroAngle, {leftDistance, rightDistance});
}
private:
DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
DifferentialDriveKinematics m_kinematicsImpl{wpi::units::meter_t{1}};
};
} // namespace frc
} // namespace wpi::math

18
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp
View File

@@ -12,7 +12,7 @@
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Class for differential drive odometry. Odometry allows you to track the
* robot's position on the field over the course of a match using readings from
@@ -41,8 +41,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry3d(const Rotation3d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance,
const Pose3d& initialPose = Pose3d{});
/**
@@ -59,8 +59,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose3d& pose) {
void ResetPosition(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose3d& pose) {
Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
@@ -75,12 +75,12 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose3d& Update(const Rotation3d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance) {
const Pose3d& Update(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance});
}
private:
DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
DifferentialDriveKinematics m_kinematicsImpl{wpi::units::meter_t{1}};
};
} // namespace frc
} // namespace wpi::math

12
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelPositions.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/util/MathExtras.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents the wheel positions for a differential drive drivetrain.
*/
@@ -16,12 +16,12 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
/**
* Distance driven by the left side.
*/
units::meter_t left = 0_m;
wpi::units::meter_t left = 0_m;
/**
* Distance driven by the right side.
*/
units::meter_t right = 0_m;
wpi::units::meter_t right = 0_m;
/**
* Checks equality between this DifferentialDriveWheelPositions and another
@@ -35,11 +35,11 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
constexpr DifferentialDriveWheelPositions Interpolate(
const DifferentialDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(left, endValue.left, t),
wpi::Lerp(right, endValue.right, t)};
return {wpi::util::Lerp(left, endValue.left, t),
wpi::util::Lerp(right, endValue.right, t)};
}
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveWheelPositionsProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.hpp"

12
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
@@ -16,12 +16,12 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
units::meters_per_second_t left = 0_mps;
wpi::units::meters_per_second_t left = 0_mps;
/**
* Speed of the right side of the robot.
*/
units::meters_per_second_t right = 0_mps;
wpi::units::meters_per_second_t right = 0_mps;
/**
* Renormalizes the wheel speeds if either side is above the specified
@@ -35,9 +35,9 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
constexpr void Desaturate(wpi::units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));
wpi::units::math::max(wpi::units::math::abs(left), wpi::units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
@@ -119,7 +119,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
return operator*(1.0 / scalar);
}
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp"

4
wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds. Robot code should not use this directly-
@@ -75,4 +75,4 @@ class WPILIB_DLLEXPORT Kinematics {
const WheelPositions& end,
double t) const = 0;
};
} // namespace frc
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp
View File

@@ -16,7 +16,7 @@
#include "wpi/math/util/MathShared.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace frc {
namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
@@ -197,7 +197,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
Translation2d rl, Translation2d rr) const;
};
} // namespace frc
} // namespace wpi::math
#include "wpi/math/kinematics/proto/MecanumDriveKinematicsProto.hpp"
#include "wpi/math/kinematics/struct/MecanumDriveKinematicsStruct.hpp"

Some files were not shown because too many files have changed in this diff Show More