diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java index 4e74dd9954..15b4119aa5 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java @@ -206,6 +206,52 @@ public class Trajectory { .collect(Collectors.toList())); } + /** + * Concatenates another trajectory to the current trajectory. The user is responsible for making + * sure that the end pose of this trajectory and the start pose of the other trajectory match (if + * that is the desired behavior). + * + * @param other The trajectory to concatenate. + * @return The concatenated trajectory. + */ + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + public Trajectory concatenate(Trajectory other) { + // If this is a default constructed trajectory with no states, then we can + // simply return the rhs trajectory. + if (m_states.isEmpty()) { + return other; + } + + // Deep copy the current states. + List states = + m_states.stream() + .map( + state -> + new State( + state.timeSeconds, + state.velocityMetersPerSecond, + state.accelerationMetersPerSecondSq, + state.poseMeters, + state.curvatureRadPerMeter)) + .collect(Collectors.toList()); + + // 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. + for (int i = 1; i < other.getStates().size(); ++i) { + var s = other.getStates().get(i); + states.add( + new State( + s.timeSeconds + m_totalTimeSeconds, + s.velocityMetersPerSecond, + s.accelerationMetersPerSecondSq, + s.poseMeters, + s.curvatureRadPerMeter)); + } + return new Trajectory(states); + } + /** * Represents a time-parameterized trajectory. The trajectory contains of various States that * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp index 9e08fb38f0..ec2eff011f 100644 --- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp @@ -124,6 +124,27 @@ Trajectory Trajectory::RelativeTo(const Pose2d& 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.to()}, {"velocity", state.velocity.to()}, diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h index d6b5c80ea4..580f806af7 100644 --- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h @@ -119,6 +119,16 @@ class Trajectory { */ Trajectory RelativeTo(const Pose2d& pose); + /** + * Concatenates another trajectory to the current trajectory. The user is + * responsible for making sure that the end pose of this trajectory and the + * start pose of the other trajectory match (if that is the desired behavior). + * + * @param other The trajectory to concatenate. + * @return The concatenated trajectory. + */ + Trajectory operator+(const Trajectory& other) const; + /** * Returns the initial pose of the trajectory. * diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConcatenateTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConcatenateTest.java new file mode 100644 index 0000000000..d8ff29f400 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConcatenateTest.java @@ -0,0 +1,52 @@ +// 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.wpilibj.trajectory; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import java.util.List; +import org.junit.jupiter.api.Test; + +class TrajectoryConcatenateTest { + @Test + void testStates() { + var t1 = + TrajectoryGenerator.generateTrajectory( + new Pose2d(), + List.of(), + new Pose2d(1, 1, new Rotation2d()), + new TrajectoryConfig(2, 2)); + + var t2 = + TrajectoryGenerator.generateTrajectory( + new Pose2d(1, 1, new Rotation2d()), + List.of(), + new Pose2d(2, 2, Rotation2d.fromDegrees(45)), + new TrajectoryConfig(2, 2)); + + var t = t1.concatenate(t2); + + double time = -1.0; + for (int i = 0; i < t.getStates().size(); ++i) { + var state = t.getStates().get(i); + + // Make sure that the timestamps are strictly increasing. + assertTrue(state.timeSeconds > time); + time = state.timeSeconds; + + // Ensure that the states in t are the same as those in t1 and t2. + if (i < t1.getStates().size()) { + assertEquals(state, t1.getStates().get(i)); + } else { + var st = t2.getStates().get(i - t1.getStates().size() + 1); + st.timeSeconds += t1.getTotalTimeSeconds(); + assertEquals(state, st); + } + } + } +} diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp new file mode 100644 index 0000000000..6c915f1374 --- /dev/null +++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp @@ -0,0 +1,34 @@ +// 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/TrajectoryConfig.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" + +TEST(TrajectoryConcatenate, States) { + auto t1 = frc::TrajectoryGenerator::GenerateTrajectory( + {}, {}, {1_m, 1_m, 0_deg}, {2_mps, 2_mps_sq}); + auto t2 = frc::TrajectoryGenerator::GenerateTrajectory( + {1_m, 1_m, 0_deg}, {}, {2_m, 2_m, 45_deg}, {2_mps, 2_mps_sq}); + + auto t = t1 + t2; + + double time = -1.0; + for (size_t i = 0; i < t.States().size(); ++i) { + const auto& state = t.States()[i]; + + // Make sure that the timestamps are strictly increasing. + EXPECT_GT(state.t.to(), time); + time = state.t.to(); + + // Ensure that the states in t are the same as those in t1 and t2. + if (i < t1.States().size()) { + EXPECT_EQ(state, t1.States()[i]); + } else { + auto st = t2.States()[i - t1.States().size() + 1]; + st.t += t1.TotalTime(); + EXPECT_EQ(state, st); + } + } +}