Helper method and path constructor for on-the-fly path creation

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java

@@ -10,10 +10,7 @@
 import java.io.BufferedReader;
 import java.io.File;
 import java.io.FileReader;
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-import java.util.Objects;
+import java.util.*;
 import java.util.stream.Collectors;
 import org.json.simple.JSONArray;
 import org.json.simple.JSONObject;
@@ -61,6 +58,47 @@ public PathPlannerPath(
     precalcValues();
   }
 
+  /**
+   * Simplified constructor to create a path with no rotation targets, constraint zones, or event
+   * markers.
+   *
+   * <p>You likely want to use bezierFromPoses to create the bezier points.
+   *
+   * @param bezierPoints List of points representing the cubic Bezier curve of the path
+   * @param constraints The global constraints of the path
+   * @param goalEndState The goal end state of the path
+   * @param reversed Should the robot follow the path reversed (differential drive only)
+   */
+  public PathPlannerPath(
+      List<Translation2d> bezierPoints,
+      PathConstraints constraints,
+      GoalEndState goalEndState,
+      boolean reversed) {
+    this(
+        bezierPoints,
+        Collections.emptyList(),
+        Collections.emptyList(),
+        Collections.emptyList(),
+        constraints,
+        goalEndState,
+        reversed);
+  }
+
+  /**
+   * Simplified constructor to create a path with no rotation targets, constraint zones, or event
+   * markers.
+   *
+   * <p>You likely want to use bezierFromPoses to create the bezier points.
+   *
+   * @param bezierPoints List of points representing the cubic Bezier curve of the path
+   * @param constraints The global constraints of the path
+   * @param goalEndState The goal end state of the path
+   */
+  public PathPlannerPath(
+      List<Translation2d> bezierPoints, PathConstraints constraints, GoalEndState goalEndState) {
+    this(bezierPoints, constraints, goalEndState, false);
+  }
+
   private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndState) {
     this.bezierPoints = new ArrayList<>();
     this.rotationTargets = new ArrayList<>();
@@ -72,6 +110,66 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS
     this.allPoints = new ArrayList<>();
   }
 
+  /**
+   * Create the bezier points necessary to create a path using a list of poses
+   *
+   * @param poses List of poses. Each pose represents one waypoint.
+   * @return Bezier points
+   */
+  public static List<Translation2d> bezierFromPoses(List<Pose2d> poses) {
+    if (poses.size() < 2) {
+      throw new IllegalArgumentException("Not enough poses");
+    }
+
+    List<Translation2d> bezierPoints = new ArrayList<>();
+
+    // First pose
+    bezierPoints.add(poses.get(0).getTranslation());
+    bezierPoints.add(
+        new Translation2d(
+            poses.get(0).getTranslation().getDistance(poses.get(1).getTranslation()) / 3.0,
+            poses.get(0).getRotation()));
+
+    // Middle poses
+    for (int i = 1; i < poses.size() - 2; i++) {
+      // Prev control
+      bezierPoints.add(
+          new Translation2d(
+              poses.get(i).getTranslation().getDistance(poses.get(i - 1).getTranslation()) / 3.0,
+              poses.get(i).getRotation().plus(Rotation2d.fromDegrees(180))));
+      // Anchor
+      bezierPoints.add(poses.get(i).getTranslation());
+      // Next control
+      bezierPoints.add(
+          new Translation2d(
+              poses.get(i).getTranslation().getDistance(poses.get(i + 1).getTranslation()) / 3.0,
+              poses.get(i).getRotation()));
+    }
+
+    // Last pose
+    bezierPoints.add(
+        new Translation2d(
+            poses
+                    .get(poses.size() - 1)
+                    .getTranslation()
+                    .getDistance(poses.get(poses.size() - 2).getTranslation())
+                / 3.0,
+            poses.get(poses.size() - 1).getRotation().plus(Rotation2d.fromDegrees(180))));
+    bezierPoints.add(poses.get(poses.size() - 1).getTranslation());
+
+    return bezierPoints;
+  }
+
+  /**
+   * Create the bezier points necessary to create a path using a list of poses
+   *
+   * @param poses List of poses. Each pose represents one waypoint.
+   * @return Bezier points
+   */
+  public static List<Translation2d> bezierFromPoses(Pose2d... poses) {
+    return bezierFromPoses(Arrays.asList(poses));
+  }
+
   /**
    * Hot reload the path. This is used internally.
    *

pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp

@@ -42,6 +42,45 @@ void PathPlannerPath::hotReload(const wpi::json &json) {
 	m_allPoints = updatedPath.m_allPoints;
 }
 
+std::vector<frc::Translation2d> PathPlannerPath::bezierFromPoses(
+		std::vector<frc::Pose2d> poses) {
+	if (poses.size() < 2) {
+		throw FRC_MakeError(frc::err::InvalidParameter,
+				"Not enough poses provided to bezierFromPoses");
+	}
+
+	std::vector < frc::Translation2d > bezierPoints;
+
+	// First pose
+	bezierPoints.emplace_back(poses[0].Translation());
+	bezierPoints.emplace_back(
+			poses[0].Translation().Distance(poses[1].Translation()) / 3.0,
+			poses[0].Rotation());
+
+	// Middle poses
+	for (size_t i = 1; i < poses.size() - 2; i++) {
+		// Prev control
+		bezierPoints.emplace_back(
+				poses[i].Translation().Distance(poses[i - 1].Translation())
+						/ 3.0, poses[i].Rotation() + frc::Rotation2d(180_deg));
+		// Anchor
+		bezierPoints.emplace_back(poses[i].Translation());
+		// Next control
+		bezierPoints.emplace_back(
+				poses[i].Translation().Distance(poses[i + 1].Translation())
+						/ 3.0, poses[i].Rotation());
+	}
+
+	// Last pose
+	bezierPoints.emplace_back(
+			poses[poses.size() - 1].Translation().Distance(
+					poses[poses.size() - 2].Translation()) / 3.0,
+			poses[poses.size() - 1].Rotation() + frc::Rotation2d(180_deg));
+	bezierPoints.emplace_back(poses[poses.size() - 1].Translation());
+
+	return bezierPoints;
+}
+
 std::shared_ptr<PathPlannerPath> PathPlannerPath::fromPathFile(
 		std::string pathName) {
 	const std::string filePath = frc::filesystem::GetDeployDirectory()

pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h

@@ -14,6 +14,7 @@
 #include <string>
 #include <units/length.h>
 #include <memory>
+#include <initializer_list>
 
 namespace pathplanner {
 class PathPlannerPath {
@@ -36,8 +37,35 @@ class PathPlannerPath {
 			PathConstraints globalConstraints, GoalEndState goalEndState,
 			bool reversed);
 
+	/**
+	 * Simplified constructor to create a path with no rotation targets, constraint zones, or event
+	 * markers.
+	 *
+	 * <p>You likely want to use bezierFromPoses to create the bezier points.
+	 *
+	 * @param bezierPoints List of points representing the cubic Bezier curve of the path
+	 * @param constraints The global constraints of the path
+	 * @param goalEndState The goal end state of the path
+	 * @param reversed Should the robot follow the path reversed (differential drive only)
+	 */
+	PathPlannerPath(std::vector<frc::Translation2d> bezierPoints,
+			PathConstraints constraints, GoalEndState goalEndState,
+			bool reversed = false) : PathPlannerPath(bezierPoints,
+			std::vector<RotationTarget>(), std::vector<ConstraintsZone>(),
+			std::vector<EventMarker>(), constraints, goalEndState, reversed) {
+	}
+
 	void hotReload(const wpi::json &json);
 
+	/**
+	 * Create the bezier points necessary to create a path using a list of poses
+	 *
+	 * @param poses List of poses. Each pose represents one waypoint.
+	 * @return Bezier points
+	 */
+	static std::vector<frc::Translation2d> bezierFromPoses(
+			std::vector<frc::Pose2d> poses);
+
 	/**
 	 * Load a path from a path file in storage
 	 *