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

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

48
wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
View File

@@ -34,15 +34,15 @@
#include "wpi/units/math.hpp"
using namespace frc;
using namespace wpi::math;
Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
const std::vector<PoseWithCurvature>& points,
const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration, bool reversed) {
wpi::units::meters_per_second_t startVelocity,
wpi::units::meters_per_second_t endVelocity,
wpi::units::meters_per_second_t maxVelocity,
wpi::units::meters_per_second_squared_t maxAcceleration, bool reversed) {
std::vector<ConstrainedState> constrainedStates(points.size());
ConstrainedState predecessor{points.front(), 0_m, startVelocity,
@@ -56,7 +56,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
constrainedState.pose = points[i];
// Begin constraining based on predecessor
units::meter_t ds = constrainedState.pose.first.Translation().Distance(
wpi::units::meter_t ds = constrainedState.pose.first.Translation().Distance(
predecessor.pose.first.Translation());
constrainedState.distance = ds + predecessor.distance;
@@ -66,9 +66,9 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. v_f = √(v_i² + 2ad).
constrainedState.maxVelocity = units::math::min(
constrainedState.maxVelocity = wpi::units::math::min(
maxVelocity,
units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
wpi::units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
predecessor.maxAcceleration * ds * 2.0));
constrainedState.minAcceleration = -maxAcceleration;
@@ -77,7 +77,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// At this point, the constrained state is fully constructed apart from
// all the custom-defined user constraints.
for (const auto& constraint : constraints) {
constrainedState.maxVelocity = units::math::min(
constrainedState.maxVelocity = wpi::units::math::min(
constrainedState.maxVelocity,
constraint->MaxVelocity(constrainedState.pose.first,
constrainedState.pose.second,
@@ -94,7 +94,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// If the actual acceleration for this state is higher than the max
// acceleration that we applied, then we need to reduce the max
// acceleration of the predecessor and try again.
units::meters_per_second_squared_t actualAcceleration =
wpi::units::meters_per_second_squared_t actualAcceleration =
(constrainedState.maxVelocity * constrainedState.maxVelocity -
predecessor.maxVelocity * predecessor.maxVelocity) /
(ds * 2.0);
@@ -123,14 +123,14 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// Backward pass
for (int i = points.size() - 1; i >= 0; i--) {
auto& constrainedState = constrainedStates[i];
units::meter_t ds =
wpi::units::meter_t ds =
constrainedState.distance - successor.distance; // negative
while (true) {
// Enforce max velocity limit (reverse)
// v_f = √(v_i² + 2ad), where v_i = successor.
units::meters_per_second_t newMaxVelocity =
units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
wpi::units::meters_per_second_t newMaxVelocity =
wpi::units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
successor.minAcceleration * ds * 2.0);
// No more limits to impose! This state can be finalized.
@@ -150,7 +150,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// If the actual acceleration for this state is lower than the min
// acceleration, then we need to lower the min acceleration of the
// successor and try again.
units::meters_per_second_squared_t actualAcceleration =
wpi::units::meters_per_second_squared_t actualAcceleration =
(constrainedState.maxVelocity * constrainedState.maxVelocity -
successor.maxVelocity * successor.maxVelocity) /
(ds * 2.0);
@@ -168,30 +168,30 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// trajectory states.
std::vector<Trajectory::State> states(points.size());
units::second_t t = 0_s;
units::meter_t s = 0_m;
units::meters_per_second_t v = 0_mps;
wpi::units::second_t t = 0_s;
wpi::units::meter_t s = 0_m;
wpi::units::meters_per_second_t v = 0_mps;
for (unsigned int i = 0; i < constrainedStates.size(); i++) {
auto state = constrainedStates[i];
// Calculate the change in position between the current state and the
// previous state.
units::meter_t ds = state.distance - s;
wpi::units::meter_t ds = state.distance - s;
// Calculate the acceleration between the current state and the previous
// state.
units::meters_per_second_squared_t accel =
wpi::units::meters_per_second_squared_t accel =
(state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
// Calculate dt.
units::second_t dt = 0_s;
wpi::units::second_t dt = 0_s;
if (i > 0) {
states.at(i - 1).acceleration = reversed ? -accel : accel;
if (units::math::abs(accel) > 1E-6_mps_sq) {
if (wpi::units::math::abs(accel) > 1E-6_mps_sq) {
// v_f = v_0 + at
dt = (state.maxVelocity - v) / accel;
} else if (units::math::abs(v) > 1E-6_mps) {
} else if (wpi::units::math::abs(v) > 1E-6_mps) {
// delta_x = vt
dt = ds / v;
} else {
@@ -230,11 +230,11 @@ void TrajectoryParameterizer::EnforceAccelerationLimits(
"back one-by-one.");
}
state->minAcceleration = units::math::max(
state->minAcceleration = wpi::units::math::max(
state->minAcceleration,
reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
state->maxAcceleration = units::math::min(
state->maxAcceleration = wpi::units::math::min(
state->maxAcceleration,
reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
}