001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory;
006
007import edu.wpi.first.math.WPIMathJNI;
008import edu.wpi.first.math.geometry.Pose2d;
009import edu.wpi.first.math.geometry.Rotation2d;
010import java.io.IOException;
011import java.nio.file.Path;
012import java.util.ArrayList;
013import java.util.List;
014
015public final class TrajectoryUtil {
016  private TrajectoryUtil() {
017    throw new UnsupportedOperationException("This is a utility class!");
018  }
019
020  /**
021   * Creates a trajectory from a double[] of elements.
022   *
023   * @param elements A double[] containing the raw elements of the trajectory.
024   * @return A trajectory created from the elements.
025   */
026  private static Trajectory createTrajectoryFromElements(double[] elements) {
027    // Make sure that the elements have the correct length.
028    if (elements.length % 7 != 0) {
029      throw new TrajectorySerializationException(
030          "An error occurred when converting trajectory elements into a trajectory.");
031    }
032
033    // Create a list of states from the elements.
034    List<Trajectory.State> states = new ArrayList<>();
035    for (int i = 0; i < elements.length; i += 7) {
036      states.add(
037          new Trajectory.State(
038              elements[i],
039              elements[i + 1],
040              elements[i + 2],
041              new Pose2d(elements[i + 3], elements[i + 4], new Rotation2d(elements[i + 5])),
042              elements[i + 6]));
043    }
044    return new Trajectory(states);
045  }
046
047  /**
048   * Returns a double[] of elements from the given trajectory.
049   *
050   * @param trajectory The trajectory to retrieve raw elements from.
051   * @return A double[] of elements from the given trajectory.
052   */
053  private static double[] getElementsFromTrajectory(Trajectory trajectory) {
054    // Create a double[] of elements and fill it from the trajectory states.
055    double[] elements = new double[trajectory.getStates().size() * 7];
056
057    for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) {
058      var state = trajectory.getStates().get(i / 7);
059      elements[i] = state.timeSeconds;
060      elements[i + 1] = state.velocityMetersPerSecond;
061      elements[i + 2] = state.accelerationMetersPerSecondSq;
062      elements[i + 3] = state.poseMeters.getX();
063      elements[i + 4] = state.poseMeters.getY();
064      elements[i + 5] = state.poseMeters.getRotation().getRadians();
065      elements[i + 6] = state.curvatureRadPerMeter;
066    }
067    return elements;
068  }
069
070  /**
071   * Imports a Trajectory from a JSON file exported from PathWeaver.
072   *
073   * @param path The path of the json file to import from
074   * @return The trajectory represented by the file.
075   * @throws IOException if reading from the file fails.
076   */
077  public static Trajectory fromPathweaverJson(Path path) throws IOException {
078    return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
079  }
080
081  /**
082   * Exports a Trajectory to a PathWeaver-style JSON file.
083   *
084   * @param trajectory The trajectory to export
085   * @param path The path of the file to export to
086   * @throws IOException if writing to the file fails.
087   */
088  public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
089    WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
090  }
091
092  /**
093   * Deserializes a Trajectory from JSON exported from PathWeaver.
094   *
095   * @param json The string containing the serialized JSON
096   * @return the trajectory represented by the JSON
097   * @throws TrajectorySerializationException if deserialization of the string fails.
098   */
099  public static Trajectory deserializeTrajectory(String json) {
100    return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
101  }
102
103  /**
104   * Serializes a Trajectory to PathWeaver-style JSON.
105   *
106   * @param trajectory The trajectory to export
107   * @return The string containing the serialized JSON
108   * @throws TrajectorySerializationException if serialization of the trajectory fails.
109   */
110  public static String serializeTrajectory(Trajectory trajectory) {
111    return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
112  }
113
114  public static class TrajectorySerializationException extends RuntimeException {
115    public TrajectorySerializationException(String message) {
116      super(message);
117    }
118  }
119}