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