Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31: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

43
wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java Normal file
View File

@@ -0,0 +1,43 @@
// 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.
package edu.wpi.first.math.interpolation;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import org.junit.jupiter.api.Test;
public class TimeInterpolatableBufferTest {
@Test
public void testInterpolation() {
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
buffer.addSample(0, new Rotation2d());
assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
buffer.addSample(1, new Rotation2d(1));
assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
buffer.addSample(3, new Rotation2d(2));
assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
buffer.addSample(10.5, new Rotation2d(2));
assertEquals(new Rotation2d(1), buffer.getSample(0));
}
@Test
public void testPose2d() {
TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
// We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
Pose2d sample = buffer.getSample(0.5);
assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
assertEquals(45, sample.getRotation().getDegrees(), 0.01);
}
}

38
wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp Normal file
View File

@@ -0,0 +1,38 @@
// 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 <cmath>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "gtest/gtest.h"
#include "units/time.h"
TEST(TimeInterpolatableBufferTest, Interpolation) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
buffer.AddSample(0_s, frc::Rotation2d(0_rad));
EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
buffer.AddSample(1_s, frc::Rotation2d(1_rad));
EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
buffer.AddSample(3_s, frc::Rotation2d(2_rad));
EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
}
TEST(TimeInterpolatableBufferTest, Pose2d) {
frc::TimeInterpolatableBuffer<frc::Pose2d> buffer{10_s};
// We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
frc::Pose2d sample = buffer.Sample(0.5_s);
EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
0.01);
EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
}