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