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

[wpimath] Add TimeInterpolatableBuffer (#2695)

Browse Source 
These classes are useful for storing previous robot positions to use in conjunction with the upcoming pose estimators.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: cttew <cttewari@gmail.com>
This commit is contained in:
Matt
2021-12-30 20:08:05 -07:00
committed by GitHub
parent b8d019cdb4
commit 315be873c4
14 changed files with 485 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,26 @@
// 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

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

@@ -6,6 +6,7 @@
#include <algorithm>
#include <wpi/MathExtras.h>
#include <wpi/json.h>
#include "units/math.h"
@@ -25,7 +26,7 @@ bool Trajectory::State::operator!=(const Trajectory::State& other) const {
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
const auto newT = Lerp(t, endValue.t, i);
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;
@@ -58,8 +59,8 @@ Trajectory::State Trajectory::State::Interpolate(State endValue,
newS / endValue.pose.Translation().Distance(pose.Translation());
return {newT, newV, acceleration,
Lerp(pose, endValue.pose, interpolationFrac),
Lerp(curvature, endValue.curvature, interpolationFrac)};
wpi::Lerp(pose, endValue.pose, interpolationFrac),
wpi::Lerp(curvature, endValue.curvature, interpolationFrac)};
}
Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {