support "rotate fast" option in c++ pplib

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

@@ -68,8 +68,7 @@ private static List<State> generateStates(
       RotationTarget nextTarget = path.getPoint(nextRotationTargetIdx).rotationTarget;
 
       if (nextTarget.shouldRotateFast()) {
-        state.targetHolonomicRotation =
-            path.getPoint(nextRotationTargetIdx).rotationTarget.getTarget();
+        state.targetHolonomicRotation = nextTarget.getTarget();
       } else {
         double t =
             (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets;

pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp

@@ -27,7 +27,8 @@ void FollowPathCommand::Initialize() {
 							>= 0.25_mps)) {
 		replanPath(currentPose, currentSpeeds);
 	} else {
-		m_generatedTrajectory = PathPlannerTrajectory(m_path, currentSpeeds);
+		m_generatedTrajectory = PathPlannerTrajectory(m_path, currentSpeeds,
+				currentPose.Rotation());
 		PathPlannerLogging::logActivePath (m_path);
 		PPLibTelemetry::setCurrentPath(m_path);
 	}

pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp

@@ -13,7 +13,7 @@ PathfindingCommand::PathfindingCommand(
 		std::unique_ptr<PathFollowingController> controller,
 		units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig,
 		frc2::Requirements requirements) : m_targetPath(targetPath), m_targetPose(), m_goalEndState(
-		0_mps, frc::Rotation2d()), m_constraints(constraints), m_poseSupplier(
+		0_mps, frc::Rotation2d(), true), m_constraints(constraints), m_poseSupplier(
 		poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller(
 		std::move(controller)), m_rotationDelayDistance(rotationDelayDistance), m_replanningConfig(
 		replanningConfig) {
@@ -23,8 +23,8 @@ PathfindingCommand::PathfindingCommand(
 
 	frc::Rotation2d targetRotation;
 	for (PathPoint p : m_targetPath->getAllPathPoints()) {
-		if (p.holonomicRotation.has_value()) {
-			targetRotation = p.holonomicRotation.value();
+		if (p.rotationTarget.has_value()) {
+			targetRotation = p.rotationTarget.value().getTarget();
 			break;
 		}
 	}
@@ -33,7 +33,7 @@ PathfindingCommand::PathfindingCommand(
 			targetRotation);
 	m_goalEndState = GoalEndState(
 			m_targetPath->getGlobalConstraints().getMaxVelocity(),
-			targetRotation);
+			targetRotation, true);
 }
 
 PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose,
@@ -44,7 +44,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose,
 		std::unique_ptr<PathFollowingController> controller,
 		units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig,
 		frc2::Requirements requirements) : m_targetPath(), m_targetPose(
-		targetPose), m_goalEndState(goalEndVel, targetPose.Rotation()), m_constraints(
+		targetPose), m_goalEndState(goalEndVel, targetPose.Rotation(), true), m_constraints(
 		constraints), m_poseSupplier(poseSupplier), m_speedsSupplier(
 		speedsSupplier), m_output(output), m_controller(std::move(controller)), m_rotationDelayDistance(
 		rotationDelayDistance), m_replanningConfig(replanningConfig) {
@@ -105,7 +105,7 @@ void PathfindingCommand::Execute() {
 							m_currentPath->getPoint(0).position) <= 0.25_m
 							&& onHeading)) {
 				m_currentTrajectory = PathPlannerTrajectory(m_currentPath,
-						currentSpeeds);
+						currentSpeeds, currentPose.Rotation());
 
 				// Find the two closest states in front of and behind robot
 				size_t closestState1Idx = 0;
@@ -146,7 +146,7 @@ void PathfindingCommand::Execute() {
 				auto replanned = m_currentPath->replan(currentPose,
 						currentSpeeds);
 				m_currentTrajectory = PathPlannerTrajectory(replanned,
-						currentSpeeds);
+						currentSpeeds, currentPose.Rotation());
 
 				m_timeOffset = 0_s;
 

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

@@ -7,6 +7,10 @@ using namespace pathplanner;
 GoalEndState GoalEndState::fromJson(const wpi::json &json) {
 	auto vel = units::meters_per_second_t(json.at("velocity").get<double>());
 	auto rotationDeg = units::degree_t(json.at("rotation").get<double>());
+	bool rotateFast = false;
+	if (json.contains("rotateFast")) {
+		rotateFast = json.at("rotateFast").get<bool>();
+	}
 
-	return GoalEndState(vel, frc::Rotation2d(rotationDeg));
+	return GoalEndState(vel, frc::Rotation2d(rotationDeg), rotateFast);
 }

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

@@ -305,8 +305,9 @@ void PathPlannerPath::precalcValues() {
 			m.setMarkerPosition(m_allPoints[pointIndex].position);
 		}
 
-		m_allPoints[m_allPoints.size() - 1].holonomicRotation =
-				m_goalEndState.getRotation();
+		m_allPoints[m_allPoints.size() - 1].rotationTarget = RotationTarget(-1,
+				m_goalEndState.getRotation(),
+				m_goalEndState.shouldRotateFast());
 		m_allPoints[m_allPoints.size() - 1].maxV = m_goalEndState.getVelocity();
 	}
 }
@@ -436,7 +437,7 @@ std::shared_ptr<PathPlannerPath> PathPlannerPath::replan(
 					std::back_inserter(targets),
 					[](RotationTarget target) {
 						return RotationTarget(target.getPosition() + 1,
-								target.getTarget());
+								target.getTarget(), target.shouldRotateFast());
 					});
 			std::vector < ConstraintsZone > zones;
 			std::transform(m_constraintZones.begin(), m_constraintZones.end(),
@@ -573,10 +574,11 @@ std::shared_ptr<PathPlannerPath> PathPlannerPath::replan(
 	for (RotationTarget t : m_rotationTargets) {
 		if (t.getPosition() >= nextWaypointIdx) {
 			mappedTargets.emplace_back(t.getPosition() - nextWaypointIdx + 2,
-					t.getTarget());
+					t.getTarget(), t.shouldRotateFast());
 		} else if (t.getPosition() >= nextWaypointIdx - 1) {
 			double pct = t.getPosition() - (nextWaypointIdx - 1);
-			mappedTargets.emplace_back(mapPct(pct, segment1Pct), t.getTarget());
+			mappedTargets.emplace_back(mapPct(pct, segment1Pct), t.getTarget(),
+					t.shouldRotateFast());
 		}
 	}
 

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

@@ -36,7 +36,7 @@ size_t PathPlannerTrajectory::getNextRotationTargetIdx(
 	size_t idx = path->numPoints() - 1;
 
 	for (size_t i = startingIndex; i < path->numPoints() - 2; i++) {
-		if (path->getPoint(i).holonomicRotation) {
+		if (path->getPoint(i).rotationTarget) {
 			idx = i;
 			break;
 		}
@@ -47,13 +47,18 @@ size_t PathPlannerTrajectory::getNextRotationTargetIdx(
 
 std::vector<PathPlannerTrajectory::State> PathPlannerTrajectory::generateStates(
 		std::shared_ptr<PathPlannerPath> path,
-		const frc::ChassisSpeeds &startingSpeeds) {
+		const frc::ChassisSpeeds &startingSpeeds,
+		const frc::Rotation2d &startingRotation) {
 	std::vector < State > states;
 
 	units::meters_per_second_t startVel = units::math::hypot(startingSpeeds.vx,
 			startingSpeeds.vy);
 
+	units::meter_t prevRotationTargetDist = 0.0_m;
+	frc::Rotation2d prevRotationTargetRot = startingRotation;
 	size_t nextRotationTargetIdx = getNextRotationTargetIdx(path, 0);
+	units::meter_t distanceBetweenTargets = path->getPoint(
+			nextRotationTargetIdx).distanceAlongPath;
 
 	// Initial pass. Creates all states and handles linear acceleration
 	for (size_t i = 0; i < path->numPoints(); i++) {
@@ -63,11 +68,32 @@ std::vector<PathPlannerTrajectory::State> PathPlannerTrajectory::generateStates(
 		state.constraints = constraints;
 
 		if (i > nextRotationTargetIdx) {
+			prevRotationTargetDist =
+					path->getPoint(nextRotationTargetIdx).distanceAlongPath;
+			prevRotationTargetRot =
+					path->getPoint(nextRotationTargetIdx).rotationTarget.value().getTarget();
 			nextRotationTargetIdx = getNextRotationTargetIdx(path, i);
+			distanceBetweenTargets =
+					path->getPoint(nextRotationTargetIdx).distanceAlongPath
+							- prevRotationTargetDist;
 		}
 
-		state.targetHolonomicRotation =
-				path->getPoint(nextRotationTargetIdx).holonomicRotation.value();
+		RotationTarget nextTarget =
+				path->getPoint(nextRotationTargetIdx).rotationTarget.value();
+
+		if (nextTarget.shouldRotateFast()) {
+			state.targetHolonomicRotation = nextTarget.getTarget();
+		} else {
+			double t = ((path->getPoint(i).distanceAlongPath
+					- prevRotationTargetDist) / distanceBetweenTargets)();
+			t = std::min(std::max(0.0, t), 1.0);
+			if (!std::isfinite(t)) {
+				t = 0.0;
+			}
+
+			state.targetHolonomicRotation = (prevRotationTargetRot
+					+ (nextTarget.getTarget() - prevRotationTargetRot)) * t;
+		}
 
 		state.position = path->getPoint(i).position;
 		units::meter_t curveRadius = path->getPoint(i).curveRadius;

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

@@ -9,15 +9,15 @@ PathSegment::PathSegment(frc::Translation2d p1, frc::Translation2d p2,
 		std::vector<ConstraintsZone> constraintZones, bool endSegment) : m_segmentPoints() {
 
 	for (double t = 0.0; t < 1.0; t += PathSegment::RESOLUTION) {
-		std::optional < frc::Rotation2d > holonomicRotation = std::nullopt;
+		std::optional < RotationTarget > holonomicRotation = std::nullopt;
 
 		if (!targetHolonomicRotations.empty()) {
 			if (std::abs(targetHolonomicRotations[0].getPosition() - t)
 					<= std::abs(
 							targetHolonomicRotations[0].getPosition()
 									- std::min(t + PathSegment::RESOLUTION,
 											1.0))) {
-				holonomicRotation = targetHolonomicRotations[0].getTarget();
+				holonomicRotation = targetHolonomicRotations[0];
 				targetHolonomicRotations.erase(
 						targetHolonomicRotations.begin());
 			}
@@ -36,6 +36,17 @@ PathSegment::PathSegment(frc::Translation2d p1, frc::Translation2d p2,
 							holonomicRotation, std::nullopt));
 		}
 	}
+
+	if (endSegment) {
+		std::optional < RotationTarget > holonomicRotation = std::nullopt;
+		if (!targetHolonomicRotations.empty()) {
+			holonomicRotation = targetHolonomicRotations[0];
+		}
+
+		m_segmentPoints.push_back(
+				PathPoint(GeometryUtil::cubicLerp(p1, p2, p3, p4, 1.0),
+						holonomicRotation, std::nullopt));
+	}
 }
 
 std::optional<ConstraintsZone> PathSegment::findConstraintsZone(

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

@@ -6,6 +6,11 @@ using namespace pathplanner;
 RotationTarget RotationTarget::fromJson(const wpi::json &json) {
 	double pos = json.at("waypointRelativePos").get<double>();
 	auto targetDeg = units::degree_t(json.at("rotationDegrees").get<double>());
+	bool rotateFast = false;
 
-	return RotationTarget(pos, frc::Rotation2d(targetDeg));
+	if (json.contains("rotateFast")) {
+		rotateFast = json.at("rotateFast").get<bool>();
+	}
+
+	return RotationTarget(pos, frc::Rotation2d(targetDeg), rotateFast);
 }

pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp

@@ -297,8 +297,7 @@ std::vector<PathPoint> LocalADStar::extractPath(const GridPosition &sStart,
 		const frc::Translation2d &realGoalPos,
 		const std::unordered_set<GridPosition> &obstacles) {
 	if (sGoal == sStart) {
-		return std::vector<PathPoint> { PathPoint(realGoalPos, std::nullopt,
-				std::nullopt) };
+		return std::vector<PathPoint>();
 	}
 
 	std::vector < GridPosition > path;

pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h

@@ -63,7 +63,8 @@ class FollowPathCommand: public frc2::CommandHelper<frc2::Command,
 	inline void replanPath(const frc::Pose2d &currentPose,
 			const frc::ChassisSpeeds &currentSpeeds) {
 		auto replanned = m_path->replan(currentPose, currentSpeeds);
-		m_generatedTrajectory = PathPlannerTrajectory(replanned, currentSpeeds);
+		m_generatedTrajectory = PathPlannerTrajectory(replanned, currentSpeeds,
+				currentPose.Rotation());
 		PathPlannerLogging::logActivePath(replanned);
 		PPLibTelemetry::setCurrentPath(replanned);
 	}

pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h

@@ -100,7 +100,8 @@ class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
 	inline void replanPath(const frc::Pose2d &currentPose,
 			const frc::ChassisSpeeds &currentSpeeds) {
 		auto replanned = m_currentPath->replan(currentPose, currentSpeeds);
-		m_currentTrajectory = PathPlannerTrajectory(replanned, currentSpeeds);
+		m_currentTrajectory = PathPlannerTrajectory(replanned, currentSpeeds,
+				currentPose.Rotation());
 		PathPlannerLogging::logActivePath(replanned);
 		PPLibTelemetry::setCurrentPath(replanned);
 	}

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

@@ -13,10 +13,11 @@ class GoalEndState {
 	 *
 	 * @param velocity The goal end velocity (M/S)
 	 * @param rotation The goal rotation
+	 * @param rotateFast Should the robot reach the rotation as fast as possible
 	 */
 	constexpr GoalEndState(units::meters_per_second_t velocity,
-			frc::Rotation2d rotation) : m_velocity(velocity), m_rotation(
-			rotation) {
+			frc::Rotation2d rotation, bool rotateFast) : m_velocity(velocity), m_rotation(
+			rotation), m_rotateFast(rotateFast) {
 	}
 
 	/**
@@ -45,13 +46,24 @@ class GoalEndState {
 		return m_rotation;
 	}
 
+	/**
+	 * Get if the robot should reach the rotation as fast as possible
+	 *
+	 * @return True if the robot should reach the rotation as fast as possible
+	 */
+	constexpr bool shouldRotateFast() const {
+		return m_rotateFast;
+	}
+
 	inline bool operator==(const GoalEndState &other) const {
 		return std::abs(m_velocity() - other.m_velocity()) < 1E-9
-				&& m_rotation == other.m_rotation;
+				&& m_rotation == other.m_rotation
+				&& m_rotateFast == other.m_rotateFast;
 	}
 
 private:
 	units::meters_per_second_t m_velocity;
 	frc::Rotation2d m_rotation;
+	bool m_rotateFast;
 };
 }

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

@@ -128,10 +128,12 @@ class PathPlannerTrajectory {
 	 *
 	 * @param path PathPlannerPath to generate the trajectory for
 	 * @param startingSpeeds Starting speeds of the robot when starting the trajectory
+	 * @param startingRotation Starting rotation of the robot when starting the trajectory
 	 */
 	PathPlannerTrajectory(std::shared_ptr<PathPlannerPath> path,
-			const frc::ChassisSpeeds &startingSpeeds) : m_states(
-			generateStates(path, startingSpeeds)) {
+			const frc::ChassisSpeeds &startingSpeeds,
+			const frc::Rotation2d &startingRotation) : m_states(
+			generateStates(path, startingSpeeds, startingRotation)) {
 	}
 
 	/**
@@ -216,6 +218,7 @@ class PathPlannerTrajectory {
 
 	static std::vector<State> generateStates(
 			std::shared_ptr<PathPlannerPath> path,
-			const frc::ChassisSpeeds &startingSpeeds);
+			const frc::ChassisSpeeds &startingSpeeds,
+			const frc::Rotation2d &startingRotation);
 };
 }

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

@@ -1,12 +1,12 @@
 #pragma once
 
 #include <frc/geometry/Translation2d.h>
-#include <frc/geometry/Rotation2d.h>
 #include <limits>
 #include <optional>
 #include <units/length.h>
 #include <units/velocity.h>
 #include "pathplanner/lib/path/PathConstraints.h"
+#include "pathplanner/lib/path/RotationTarget.h"
 
 namespace pathplanner {
 class PathPoint {
@@ -16,12 +16,12 @@ class PathPoint {
 	units::meter_t curveRadius = 0_m;
 	units::meters_per_second_t maxV = units::meters_per_second_t {
 			std::numeric_limits<double>::infinity() };
-	std::optional<frc::Rotation2d> holonomicRotation = std::nullopt;
+	std::optional<RotationTarget> rotationTarget = std::nullopt;
 	std::optional<PathConstraints> constraints = std::nullopt;
 
 	constexpr PathPoint(frc::Translation2d pos,
-			std::optional<frc::Rotation2d> rot,
-			std::optional<PathConstraints> pathCostriaints) : position(pos), holonomicRotation(
+			std::optional<RotationTarget> rot,
+			std::optional<PathConstraints> pathCostriaints) : position(pos), rotationTarget(
 			rot), constraints(pathCostriaints) {
 	}
 };