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

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 {};
}