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

Add transform methods to Trajectory (#2187)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-12-23 14:16:30 -05:00
committed by Peter Johnson
parent 67b59f2b31
commit 3259cffc63
5 changed files with 240 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -107,6 +107,33 @@ Trajectory::State Trajectory::Sample(units::second_t t) const {
(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);
}
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
json = wpi::json{{"time", state.t.to<double>()},
{"velocity", state.velocity.to<double>()},