Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make controllers and some trajectory classes constexpr (#7343)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-07 13:02:11 -08:00
committed by GitHub
parent 44a45d44e2
commit a66fa339dc
71 changed files with 1512 additions and 1900 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
View File

@@ -15,20 +15,6 @@
using namespace frc;
units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
return Calculate(currentAngle, currentVelocity, nextVelocity);
}
units::volt_t ArmFeedforward::Calculate(
units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity) const {
return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) +
kV * currentVelocity;
}
units::volt_t ArmFeedforward::Calculate(
units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {

44
wpimath/src/main/native/cpp/controller/BangBangController.cpp
View File

@@ -6,52 +6,8 @@
#include <wpi/sendable/SendableBuilder.h>
#include "wpimath/MathShared.h"
using namespace frc;
BangBangController::BangBangController(double tolerance)
: m_tolerance(tolerance) {}
void BangBangController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
}
double BangBangController::GetSetpoint() const {
return m_setpoint;
}
bool BangBangController::AtSetpoint() const {
return std::abs(m_setpoint - m_measurement) < m_tolerance;
}
void BangBangController::SetTolerance(double tolerance) {
m_tolerance = tolerance;
}
double BangBangController::GetTolerance() const {
return m_tolerance;
}
double BangBangController::GetMeasurement() const {
return m_measurement;
}
double BangBangController::GetError() const {
return m_setpoint - m_measurement;
}
double BangBangController::Calculate(double measurement, double setpoint) {
m_measurement = measurement;
m_setpoint = setpoint;
return measurement < setpoint ? 1 : 0;
}
double BangBangController::Calculate(double measurement) {
return Calculate(measurement, m_setpoint);
}
void BangBangController::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("BangBangController");
builder.AddDoubleProperty(

25
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

@@ -4,35 +4,10 @@
#include "frc/controller/DifferentialDriveAccelerationLimiter.h"
#include <utility>
#include <Eigen/QR>
using namespace frc;
DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
units::meters_per_second_squared_t maxLinearAccel,
units::radians_per_second_squared_t maxAngularAccel)
: DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel,
maxLinearAccel, maxAngularAccel) {}
DifferentialDriveAccelerationLimiter::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)
: m_system{std::move(system)},
m_trackwidth{trackwidth},
m_minLinearAccel{minLinearAccel},
m_maxLinearAccel{maxLinearAccel},
m_maxAngularAccel{maxAngularAccel} {
if (minLinearAccel > maxLinearAccel) {
throw std::invalid_argument(
"maxLinearAccel must be greater than minLinearAccel");
}
}
DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,

22
wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
View File

@@ -7,31 +7,9 @@
#include <Eigen/Core>
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
DifferentialDriveFeedforward::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)
// 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))
: DifferentialDriveFeedforward{kVLinear, kALinear,
kVAngular * 2.0 / trackwidth * 1_rad,
kAAngular * 2.0 / trackwidth * 1_rad} {}
DifferentialDriveFeedforward::DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular)},
m_kVLinear{kVLinear},
m_kALinear{kALinear},
m_kVAngular{kVAngular},
m_kAAngular{kAAngular} {}
DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
units::meters_per_second_t currentLeftVelocity,
units::meters_per_second_t nextLeftVelocity,

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

@@ -1,107 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/controller/HolonomicDriveController.h"
#include <utility>
#include "units/angular_velocity.h"
using namespace frc;
HolonomicDriveController::HolonomicDriveController(
PIDController xController, PIDController yController,
ProfiledPIDController<units::radian> thetaController)
: m_xController(std::move(xController)),
m_yController(std::move(yController)),
m_thetaController(std::move(thetaController)) {
m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
}
bool HolonomicDriveController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
const auto& eRotate = m_rotationError;
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();
}
void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
m_poseTolerance = tolerance;
}
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Pose2d& trajectoryPose,
units::meters_per_second_t desiredLinearVelocity,
const Rotation2d& desiredHeading) {
// If this is the first run, then we need to reset the theta controller to the
// current pose's heading.
if (m_firstRun) {
m_thetaController.Reset(currentPose.Rotation().Radians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative)
auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
currentPose.Rotation().Radians(), desiredHeading.Radians())};
m_poseError = trajectoryPose.RelativeTo(currentPose);
m_rotationError = desiredHeading - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
currentPose.Rotation());
}
// Calculate feedback velocities (based on position error).
auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
currentPose.X().value(), trajectoryPose.X().value())};
auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
currentPose.Y().value(), trajectoryPose.Y().value())};
// Return next output.
return ChassisSpeeds::FromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
}
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Trajectory::State& desiredState,
const Rotation2d& desiredHeading) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredHeading);
}
void HolonomicDriveController::SetEnabled(bool enabled) {
m_enabled = enabled;
}
PIDController& HolonomicDriveController::getXController() {
return m_xController;
}
PIDController& HolonomicDriveController::getYController() {
return m_yController;
}
ProfiledPIDController<units::radian>&
HolonomicDriveController::getThetaController() {
return m_thetaController;
}
PIDController& HolonomicDriveController::GetXController() {
return m_xController;
}
PIDController& HolonomicDriveController::GetYController() {
return m_yController;
}
ProfiledPIDController<units::radian>&
HolonomicDriveController::GetThetaController() {
return m_thetaController;
}

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

@@ -4,232 +4,10 @@
#include "frc/controller/PIDController.h"
#include <algorithm>
#include <cmath>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/MathUtil.h"
#include "wpimath/MathShared.h"
using namespace frc;
PIDController::PIDController(double Kp, double Ki, double Kd,
units::second_t period)
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
bool invalidGains = false;
if (Kp < 0.0) {
wpi::math::MathSharedStore::ReportError(
"Kp must be a non-negative number, got {}!", Kp);
invalidGains = true;
}
if (Ki < 0.0) {
wpi::math::MathSharedStore::ReportError(
"Ki must be a non-negative number, got {}!", Ki);
invalidGains = true;
}
if (Kd < 0.0) {
wpi::math::MathSharedStore::ReportError(
"Kd must be a non-negative number, got {}!", Kd);
invalidGains = true;
}
if (invalidGains) {
m_Kp = 0.0;
m_Ki = 0.0;
m_Kd = 0.0;
wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
}
if (period <= 0_s) {
wpi::math::MathSharedStore::ReportError(
"Controller period must be a positive number, got {}!", period.value());
m_period = 20_ms;
wpi::math::MathSharedStore::ReportWarning(
"Controller period defaulted to 20ms.");
}
static int instances = 0;
instances++;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kController_PIDController2, instances);
wpi::SendableRegistry::Add(this, "PIDController", instances);
}
void PIDController::SetPID(double Kp, double Ki, double Kd) {
m_Kp = Kp;
m_Ki = Ki;
m_Kd = Kd;
}
void PIDController::SetP(double Kp) {
m_Kp = Kp;
}
void PIDController::SetI(double Ki) {
m_Ki = Ki;
}
void PIDController::SetD(double Kd) {
m_Kd = Kd;
}
void PIDController::SetIZone(double iZone) {
if (iZone < 0) {
wpi::math::MathSharedStore::ReportError(
"IZone must be a non-negative number, got {}!", iZone);
}
m_iZone = iZone;
}
double PIDController::GetP() const {
return m_Kp;
}
double PIDController::GetI() const {
return m_Ki;
}
double PIDController::GetD() const {
return m_Kd;
}
double PIDController::GetIZone() const {
return m_iZone;
}
units::second_t PIDController::GetPeriod() const {
return m_period;
}
double PIDController::GetPositionTolerance() const {
return m_errorTolerance;
}
double PIDController::GetVelocityTolerance() const {
return m_errorDerivativeTolerance;
}
double PIDController::GetAccumulatedError() const {
return m_totalError;
}
void PIDController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
m_haveSetpoint = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_error = m_setpoint - m_measurement;
}
m_errorDerivative = (m_error - m_prevError) / m_period.value();
}
double PIDController::GetSetpoint() const {
return m_setpoint;
}
bool PIDController::AtSetpoint() const {
return m_haveMeasurement && m_haveSetpoint &&
std::abs(m_error) < m_errorTolerance &&
std::abs(m_errorDerivative) < m_errorDerivativeTolerance;
}
void PIDController::EnableContinuousInput(double minimumInput,
double maximumInput) {
m_continuous = true;
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
void PIDController::DisableContinuousInput() {
m_continuous = false;
}
bool PIDController::IsContinuousInputEnabled() const {
return m_continuous;
}
void PIDController::SetIntegratorRange(double minimumIntegral,
double maximumIntegral) {
m_minimumIntegral = minimumIntegral;
m_maximumIntegral = maximumIntegral;
}
void PIDController::SetTolerance(double errorTolerance,
double errorDerivativeTolerance) {
m_errorTolerance = errorTolerance;
m_errorDerivativeTolerance = errorDerivativeTolerance;
}
double PIDController::GetErrorTolerance() const {
return m_errorTolerance;
}
double PIDController::GetErrorDerivativeTolerance() const {
return m_errorDerivativeTolerance;
}
double PIDController::GetError() const {
return m_error;
}
double PIDController::GetErrorDerivative() const {
return m_errorDerivative;
}
double PIDController::GetPositionError() const {
return m_error;
}
double PIDController::GetVelocityError() const {
return m_errorDerivative;
}
double PIDController::Calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_error;
m_haveMeasurement = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_error = m_setpoint - m_measurement;
}
m_errorDerivative = (m_error - m_prevError) / m_period.value();
// If the absolute value of the position error is outside of IZone, reset the
// total error
if (std::abs(m_error) > m_iZone) {
m_totalError = 0;
} else if (m_Ki != 0) {
m_totalError =
std::clamp(m_totalError + m_error * m_period.value(),
m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
}
return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative;
}
double PIDController::Calculate(double measurement, double setpoint) {
m_setpoint = setpoint;
m_haveSetpoint = true;
return Calculate(measurement);
}
void PIDController::Reset() {
m_error = 0;
m_prevError = 0;
m_totalError = 0;
m_errorDerivative = 0;
m_haveMeasurement = false;
}
void PIDController::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("PIDController");
builder.AddDoubleProperty(

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

@@ -1,89 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/controller/RamseteController.h"
#include <wpi/deprecated.h>
#include "units/angle.h"
#include "units/math.h"
using namespace frc;
/**
* Returns sin(x) / x.
*
* @param x Value of which to take sinc(x).
*/
static decltype(1 / 1_rad) Sinc(units::radian_t x) {
if (units::math::abs(x) < 1e-9_rad) {
return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()};
} else {
return units::math::sin(x) / x;
}
}
RamseteController::RamseteController(units::unit_t<b_unit> b,
units::unit_t<zeta_unit> zeta)
: m_b{b}, m_zeta{zeta} {}
WPI_IGNORE_DEPRECATED
RamseteController::RamseteController()
: RamseteController{units::unit_t<b_unit>{2.0},
units::unit_t<zeta_unit>{0.7}} {}
WPI_UNIGNORE_DEPRECATED
bool RamseteController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
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();
}
void RamseteController::SetTolerance(const Pose2d& poseTolerance) {
m_poseTolerance = poseTolerance;
}
ChassisSpeeds RamseteController::Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef,
units::radians_per_second_t angularVelocityRef) {
if (!m_enabled) {
return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
}
m_poseError = poseRef.RelativeTo(currentPose);
// Aliases for equation readability
const auto& eX = m_poseError.X();
const auto& eY = m_poseError.Y();
const auto& eTheta = m_poseError.Rotation().Radians();
const auto& vRef = linearVelocityRef;
const auto& omegaRef = angularVelocityRef;
// k = 2ζ√(ω_ref² + b v_ref²)
auto k = 2.0 * m_zeta *
units::math::sqrt(units::math::pow<2>(omegaRef) +
m_b * units::math::pow<2>(vRef));
// v_cmd = v_ref cos(e_θ) + k e_x
// ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps,
omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY};
}
ChassisSpeeds RamseteController::Calculate(
const Pose2d& currentPose, const Trajectory::State& desiredState) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredState.velocity * desiredState.curvature);
}
void RamseteController::SetEnabled(bool enabled) {
m_enabled = enabled;
}

26
wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp
View File

@@ -1,26 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/interpolation/TimeInterpolatableBuffer.h"
namespace frc {
// Template specialization to ensure that Pose2d uses pose exponential
template <>
TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) {
if (t < 0) {
return start;
} else if (t >= 1) {
return end;
} else {
Twist2d twist = start.Log(end);
Twist2d scaledTwist = twist * t;
return start.Exp(scaledTwist);
}
}) {}
} // namespace frc

45
wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
View File

@@ -1,45 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/CubicHermiteSpline.h"
using namespace frc;
CubicHermiteSpline::CubicHermiteSpline(
wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> yFinalControlVector)
: m_initialControlVector{xInitialControlVector, yInitialControlVector},
m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
const auto y =
ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
// Populate first two rows with coefficients.
m_coefficients.template block<1, 4>(0, 0) = hermite * x;
m_coefficients.template block<1, 4>(1, 0) = hermite * y;
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 4; i++) {
// Here, we are multiplying by (3 - i) to manually take the derivative. The
// power of the term in index 0 is 3, index 1 is 2 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (3 - i);
}
for (int i = 0; i < 3; i++) {
// Here, we are multiplying by (2 - i) to manually take the derivative. The
// power of the term in index 0 is 2, index 1 is 1 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}

44
wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
View File

@@ -1,44 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/QuinticHermiteSpline.h"
using namespace frc;
QuinticHermiteSpline::QuinticHermiteSpline(
wpi::array<double, 3> xInitialControlVector,
wpi::array<double, 3> xFinalControlVector,
wpi::array<double, 3> yInitialControlVector,
wpi::array<double, 3> yFinalControlVector)
: m_initialControlVector{xInitialControlVector, yInitialControlVector},
m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
const auto y =
ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
// Populate first two rows with coefficients.
m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 6; i++) {
// Here, we are multiplying by (5 - i) to manually take the derivative. The
// power of the term in index 0 is 5, index 1 is 4 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (5 - i);
}
for (int i = 0; i < 5; i++) {
// Here, we are multiplying by (4 - i) to manually take the derivative. The
// power of the term in index 0 is 4, index 1 is 3 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}

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

@@ -1,281 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/SplineHelper.h"
#include <cstddef>
#include <vector>
using namespace frc;
std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;
wpi::array<double, 2> xInitial = start.x;
wpi::array<double, 2> yInitial = start.y;
wpi::array<double, 2> xFinal = end.x;
wpi::array<double, 2> yFinal = end.y;
if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
Translation2d{units::meter_t{xInitial[0]},
units::meter_t{yInitial[0]}});
waypoints.emplace_back(
Translation2d{units::meter_t{xFinal[0]}, units::meter_t{yFinal[0]}});
// Populate tridiagonal system for clamped cubic
/* See:
https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
/undervisningsmateriale/chap7alecture.pdf
*/
// Above-diagonal of tridiagonal matrix, zero-padded
std::vector<double> a;
// Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
// Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
// rhs vectors
std::vector<double> dx, dy;
// solution vectors
std::vector<double> fx(waypoints.size() - 2, 0.0),
fy(waypoints.size() - 2, 0.0);
// populate above-diagonal and below-diagonal vectors
a.emplace_back(0);
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
c.emplace_back(1);
}
c.emplace_back(0);
// populate rhs vectors
dx.emplace_back(3 * (waypoints[2].X().value() - waypoints[0].X().value()) -
xInitial[1]);
dy.emplace_back(3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) -
yInitial[1]);
if (waypoints.size() > 4) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
// dx and dy represent the derivatives of the internal waypoints. The
// derivative of the second internal waypoint should involve the third
// and first internal waypoint, which have indices of 1 and 3 in the
// waypoints list (which contains ALL waypoints).
dx.emplace_back(
3 * (waypoints[i + 2].X().value() - waypoints[i].X().value()));
dy.emplace_back(
3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value()));
}
}
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() -
waypoints[waypoints.size() - 3].X().value()) -
xFinal[1]);
dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() -
waypoints[waypoints.size() - 3].Y().value()) -
yFinal[1]);
// Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);
fx.emplace(fx.begin(), xInitial[1]);
fx.emplace_back(xFinal[1]);
fy.emplace(fy.begin(), yInitial[1]);
fy.emplace_back(yFinal[1]);
for (size_t i = 0; i < fx.size() - 1; ++i) {
// Create the spline.
const CubicHermiteSpline spline{
{waypoints[i].X().value(), fx[i]},
{waypoints[i + 1].X().value(), fx[i + 1]},
{waypoints[i].Y().value(), fy[i]},
{waypoints[i + 1].Y().value(), fy[i + 1]}};
splines.push_back(spline);
}
} else if (waypoints.size() == 1) {
const double xDeriv =
(3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
wpi::array<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
wpi::array<double, 2> midYControlVector{waypoints[0].Y().value(), yDeriv};
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
} else {
// Create the spline.
const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
splines.push_back(spline);
}
return splines;
}
std::vector<QuinticHermiteSpline>
SplineHelper::QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> splines;
for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
auto& xInitial = controlVectors[i].x;
auto& yInitial = controlVectors[i].y;
auto& xFinal = controlVectors[i + 1].x;
auto& yFinal = controlVectors[i + 1].y;
splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
}
return splines;
}
wpi::array<Spline<3>::ControlVector, 2>
SplineHelper::CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation()).value();
} else {
scalar =
1.2 * start.Translation().Distance(interiorWaypoints.front()).value();
}
const auto initialCV = CubicControlVector(scalar, start);
if (!interiorWaypoints.empty()) {
scalar = 1.2 * end.Translation().Distance(interiorWaypoints.back()).value();
}
const auto finalCV = CubicControlVector(scalar, end);
return {initialCV, finalCV};
}
std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints) {
std::vector<QuinticHermiteSpline> splines;
splines.reserve(waypoints.size() - 1);
for (size_t i = 0; i < waypoints.size() - 1; ++i) {
auto& p0 = waypoints[i];
auto& p1 = waypoints[i + 1];
// This just makes the splines look better.
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).value();
auto controlVectorA = QuinticControlVector(scalar, p0);
auto controlVectorB = QuinticControlVector(scalar, p1);
splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y,
controlVectorB.y);
}
return splines;
}
std::vector<QuinticHermiteSpline> SplineHelper::OptimizeCurvature(
const std::vector<QuinticHermiteSpline>& splines) {
// If there's only one spline in the vector, we can't optimize anything so
// just return that.
if (splines.size() < 2) {
return splines;
}
// Implements Section 4.1.2 of
// http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
// Cubic splines minimize the integral of the second derivative's absolute
// value. Therefore, we can create cubic splines with the same 0th and 1st
// derivatives and the provided quintic splines, find the second derivative of
// those cubic splines and then use a weighted average for the second
// derivatives of the quintic splines.
std::vector<QuinticHermiteSpline> optimizedSplines;
optimizedSplines.reserve(splines.size());
optimizedSplines.push_back(splines[0]);
for (size_t i = 0; i < splines.size() - 1; ++i) {
const auto& a = splines[i];
const auto& b = splines[i + 1];
// Get the control vectors that created the quintic splines above.
const auto& aInitial = a.GetInitialControlVector();
const auto& aFinal = a.GetFinalControlVector();
const auto& bInitial = b.GetInitialControlVector();
const auto& bFinal = b.GetFinalControlVector();
// Create cubic splines with the same control vectors.
auto Trim = [](const wpi::array<double, 3>& a) {
return wpi::array<double, 2>{a[0], a[1]};
};
CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y),
Trim(aFinal.y)};
CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y),
Trim(bFinal.y)};
// Calculate the second derivatives at the knot points.
frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0};
frc::Vectord<6> combinedA = ca.Coefficients() * bases;
double ddxA = combinedA(4);
double ddyA = combinedA(5);
double ddxB = cb.Coefficients()(4, 1);
double ddyB = cb.Coefficients()(5, 1);
// Calculate the parameters for weighted average.
double dAB =
std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
double dBC =
std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
double alpha = dBC / (dAB + dBC);
double beta = dAB / (dAB + dBC);
// Calculate the weighted average.
double ddx = alpha * ddxA + beta * ddxB;
double ddy = alpha * ddyA + beta * ddyB;
// Create new splines.
optimizedSplines[i] = {aInitial.x,
{aFinal.x[0], aFinal.x[1], ddx},
aInitial.y,
{aFinal.y[0], aFinal.y[1], ddy}};
optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx},
bFinal.x,
{bInitial.y[0], bInitial.y[1], ddy},
bFinal.y});
}
return optimizedSplines;
}
void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
const std::vector<double>& b,
const std::vector<double>& c,
const std::vector<double>& d,
std::vector<double>* solutionVector) {
auto& f = *solutionVector;
size_t N = d.size();
// Create the temporary vectors
// Note that this is inefficient as it is possible to call
// this function many times. A better implementation would
// pass these temporary matrices by non-const reference to
// save excess allocation and deallocation
std::vector<double> c_star(N, 0.0);
std::vector<double> d_star(N, 0.0);
// This updates the coefficients in the first row
// Note that we should be checking for division by zero here
c_star[0] = c[0] / b[0];
d_star[0] = d[0] / b[0];
// Create the c_star and d_star coefficients in the forward sweep
for (size_t i = 1; i < N; ++i) {
double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
c_star[i] = c[i] * m;
d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
}
f[N - 1] = d_star[N - 1];
// This is the reverse sweep, used to update the solution vector f
for (int i = N - 2; i >= 0; i--) {
f[i] = d_star[i] - c_star[i] * f[i + 1];
}
}

196
wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
View File

@@ -1,196 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
LinearSystem<2, 1, 2> LinearSystemId::ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
if (radius <= 0_m) {
throw std::domain_error("radius must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(gearing, 2) * motor.Kt /
(motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
.value()}};
Matrixd<2, 1> B{0.0,
(gearing * motor.Kt / (motor.R * radius * mass)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 1, 2> LinearSystemId::SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0,
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
if (kVLinear <= decltype(kVLinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
if (kALinear <= decltype(kALinear){0}) {
throw std::domain_error("Ka,linear must be greater than zero.");
}
if (kVAngular <= decltype(kVAngular){0}) {
throw std::domain_error("Kv,angular must be greater than zero.");
}
if (kAAngular <= decltype(kAAngular){0}) {
throw std::domain_error("Ka,angular must be greater than zero.");
}
double A1 = -(kVLinear.value() / kALinear.value() +
kVAngular.value() / kAAngular.value());
double A2 = -(kVLinear.value() / kALinear.value() -
kVAngular.value() / kAAngular.value());
double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value();
double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value();
Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
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) {
if (kVLinear <= decltype(kVLinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
if (kALinear <= decltype(kALinear){0}) {
throw std::domain_error("Ka,linear must be greater than zero.");
}
if (kVAngular <= decltype(kVAngular){0}) {
throw std::domain_error("Kv,angular must be greater than zero.");
}
if (kAAngular <= decltype(kAAngular){0}) {
throw std::domain_error("Ka,angular must be greater than zero.");
}
if (trackwidth <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
// We want to find a factor to include in Kv,angular that will convert
// `u = Kv,angular omega` to `u = Kv,angular v`.
//
// v = omega r
// omega = v/r
// omega = 1/r v
// omega = 1/(trackwidth/2) v
// omega = 2/trackwidth v
//
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
// to V/(m/s).
return IdentifyDrivetrainSystem(kVLinear, kALinear,
kVAngular * 2.0 / trackwidth * 1_rad,
kAAngular * 2.0 / trackwidth * 1_rad);
}
LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<1, 1> A{
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0,
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
units::meter_t rb, units::kilogram_square_meter_t J, double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
if (r <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
if (rb <= 0_m) {
throw std::domain_error("rb must be greater than zero.");
}
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
auto C1 = -std::pow(gearing, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = gearing * motor.Kt / (motor.R * r);
Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / mass - units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / mass - units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}

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

@@ -4,150 +4,10 @@
#include "frc/trajectory/Trajectory.h"
#include <algorithm>
#include <stdexcept>
#include <vector>
#include <wpi/MathExtras.h>
#include <wpi/json.h>
#include "units/math.h"
using namespace frc;
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
const auto newT = wpi::Lerp(t, endValue.t, i);
// Find the delta time between the current state and the interpolated state.
const auto deltaT = newT - t;
// If delta time is negative, flip the order of interpolation.
if (deltaT < 0_s) {
return endValue.Interpolate(*this, 1.0 - i);
}
// Check whether the robot is reversing at this stage.
const auto reversing =
velocity < 0_mps ||
(units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
// Calculate the new velocity.
// v = v_0 + at
const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
// Calculate the change in position.
// delta_s = v_0 t + 0.5at²
const units::meter_t newS =
(velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
(reversing ? -1.0 : 1.0);
// Return the new state. To find the new position for the new state, we need
// to interpolate between the two endpoint poses. The fraction for
// interpolation is the change in position (delta s) divided by the total
// distance between the two endpoints.
const double interpolationFrac =
newS / endValue.pose.Translation().Distance(pose.Translation());
return {newT, newV, acceleration,
wpi::Lerp(pose, endValue.pose, interpolationFrac),
wpi::Lerp(curvature, endValue.curvature, interpolationFrac)};
}
Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
if (m_states.empty()) {
throw std::invalid_argument(
"Trajectory manually initialized with no states.");
}
m_totalTime = states.back().t;
}
Trajectory::State Trajectory::Sample(units::second_t t) const {
if (m_states.empty()) {
throw std::runtime_error(
"Trajectory cannot be sampled if it has no states.");
}
if (t <= m_states.front().t) {
return m_states.front();
}
if (t >= m_totalTime) {
return m_states.back();
}
// Use binary search to get the element with a timestamp no less than the
// requested timestamp. This starts at 1 because we use the previous state
// later on for interpolation.
auto sample =
std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t,
[](const auto& a, const auto& b) { return a.t < b; });
auto prevSample = sample - 1;
// The sample's timestamp is now greater than or equal to the requested
// timestamp. If it is greater, we need to interpolate between the
// previous state and the current state to get the exact state that we
// want.
// If the difference in states is negligible, then we are spot on!
if (units::math::abs(sample->t - prevSample->t) < 1E-9_s) {
return *sample;
}
// Interpolate between the two states for the state that we want.
return prevSample->Interpolate(
*sample, (t - prevSample->t) / (sample->t - prevSample->t));
}
Trajectory Trajectory::TransformBy(const Transform2d& transform) {
auto& firstState = m_states[0];
auto& firstPose = firstState.pose;
// Calculate the transformed first pose.
auto newFirstPose = firstPose + transform;
auto newStates = m_states;
newStates[0].pose = newFirstPose;
for (unsigned int i = 1; i < newStates.size(); i++) {
auto& state = newStates[i];
// We are transforming relative to the coordinate frame of the new initial
// pose.
state.pose = newFirstPose + (state.pose - firstPose);
}
return Trajectory(newStates);
}
Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
auto newStates = m_states;
for (auto& state : newStates) {
state.pose = state.pose.RelativeTo(pose);
}
return Trajectory(newStates);
}
Trajectory Trajectory::operator+(const Trajectory& other) const {
// If this is a default constructed trajectory with no states, then we can
// simply return the rhs trajectory.
if (m_states.empty()) {
return other;
}
auto states = m_states;
auto otherStates = other.States();
for (auto& otherState : otherStates) {
otherState.t += m_totalTime;
}
// Here we omit the first state of the other trajectory because we don't want
// two time points with different states. Sample() will automatically
// interpolate between the end of this trajectory and the second state of the
// other trajectory.
states.insert(states.end(), otherStates.begin() + 1, otherStates.end());
return Trajectory(states);
}
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
json = wpi::json{{"time", state.t.value()},
{"velocity", state.velocity.value()},

39
wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
View File

@@ -1,39 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
#include "units/math.h"
using namespace frc;
CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
units::meters_per_second_squared_t maxCentripetalAcceleration)
: m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
// ac = v²/r
// k (curvature) = 1/r
// therefore, ac = v²k
// ac/k = v²
// v = √(ac/k)
// We have to multiply by 1_rad here to get the units to cancel out nicely.
// The units library defines a unit for radians although it is technically
// unitless.
return units::math::sqrt(m_maxCentripetalAcceleration /
units::math::abs(curvature) * 1_rad);
}
TrajectoryConstraint::MinMax
CentripetalAccelerationConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return {};
}

30
wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
View File

@@ -1,30 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include <utility>
using namespace frc;
DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
: m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Desaturate(m_maxSpeed);
return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
}
TrajectoryConstraint::MinMax
DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}

100
wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
View File

@@ -1,100 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
#include <algorithm>
#include <limits>
#include <utility>
#include <wpi/MathExtras.h>
#include "units/math.h"
using namespace frc;
DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
const SimpleMotorFeedforward<units::meter>& feedforward,
DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
: m_feedforward(feedforward),
m_kinematics(std::move(kinematics)),
m_maxVoltage(maxVoltage) {}
units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
return units::meters_per_second_t{std::numeric_limits<double>::max()};
}
TrajectoryConstraint::MinMax
DifferentialDriveVoltageConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
auto maxWheelSpeed = (std::max)(wheelSpeeds.left, wheelSpeeds.right);
auto minWheelSpeed = (std::min)(wheelSpeeds.left, wheelSpeeds.right);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
auto maxWheelAcceleration =
m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
auto minWheelAcceleration =
m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
// Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
// increased by half of the trackwidth T. Inner wheel has radius decreased
// by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so
// Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
// |curvature|T/2). Inner wheel is similar.
// sgn(speed) term added to correctly account for which wheel is on
// outside of turn:
// If moving forward, max acceleration constraint corresponds to wheel on
// outside of turn If moving backward, max acceleration constraint corresponds
// to wheel on inside of turn
// When velocity is zero, then wheel velocities are uniformly zero (robot
// cannot be turning on its center) - we have to treat this as a special case,
// as it breaks the signum function. Both max and min acceleration are
// *reduced in magnitude* in this case.
units::meters_per_second_squared_t maxChassisAcceleration;
units::meters_per_second_squared_t minChassisAcceleration;
if (speed == 0_mps) {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
} else {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 - m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
}
// When turning about a point inside of the wheelbase (i.e. radius less than
// half the trackwidth), the inner wheel's direction changes, but the
// magnitude remains the same. The formula above changes sign for the inner
// wheel when this happens. We can accurately account for this by simply
// negating the inner wheel.
if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
if (speed > 0_mps) {
minChassisAcceleration = -minChassisAcceleration;
} else if (speed < 0_mps) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}
return {minChassisAcceleration, maxChassisAcceleration};
}

23
wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
using namespace frc;
MaxVelocityConstraint::MaxVelocityConstraint(
units::meters_per_second_t maxVelocity)
: m_maxVelocity(units::math::abs(maxVelocity)) {}
units::meters_per_second_t MaxVelocityConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
return m_maxVelocity;
}
TrajectoryConstraint::MinMax MaxVelocityConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}

35
wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
View File

@@ -1,35 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
#include "units/math.h"
using namespace frc;
MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
const MecanumDriveKinematics& kinematics,
units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
wheelSpeeds.Desaturate(m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
TrajectoryConstraint::MinMax
MecanumDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}