diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/PathPlanner.java b/pathplannerlib/src/main/java/com/pathplanner/lib/PathPlanner.java index 403afc239..f8d7cb828 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/PathPlanner.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/PathPlanner.java @@ -271,16 +271,19 @@ public static PathPlannerTrajectory generatePath( double thirdDistance = p1.position.getDistance(p2.position) / 3.0; + double p1NextDistance = p1.nextControlLength <= 0 ? thirdDistance : p1.nextControlLength; + double p2PrevDistance = p2.prevControlLength <= 0 ? thirdDistance : p2.prevControlLength; + Translation2d p1Next = p1.position.plus( new Translation2d( - p1.heading.getCos() * thirdDistance, p1.heading.getSin() * thirdDistance)); + p1.heading.getCos() * p1NextDistance, p1.heading.getSin() * p1NextDistance)); waypoints.get(i - 1).nextControl = p1Next; Translation2d p2Prev = p2.position.minus( new Translation2d( - p2.heading.getCos() * thirdDistance, p2.heading.getSin() * thirdDistance)); + p2.heading.getCos() * p2PrevDistance, p2.heading.getSin() * p2PrevDistance)); waypoints.add( new Waypoint( p2.position, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/PathPoint.java b/pathplannerlib/src/main/java/com/pathplanner/lib/PathPoint.java index 865fdc9f3..5d3bcf112 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/PathPoint.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/PathPoint.java @@ -11,6 +11,9 @@ public class PathPoint { protected final Rotation2d holonomicRotation; protected final double velocityOverride; + protected double prevControlLength = -1; + protected double nextControlLength = -1; + public PathPoint( Translation2d position, Rotation2d heading, @@ -34,6 +37,35 @@ public PathPoint(Translation2d position, Rotation2d heading) { this(position, heading, Rotation2d.fromDegrees(0)); } + public PathPoint withPrevControlLength(double lengthMeters) { + if (lengthMeters <= 0) { + throw new IllegalArgumentException("Control point lengths must be > 0"); + } + + prevControlLength = lengthMeters; + return this; + } + + public PathPoint withNextControlLength(double lengthMeters) { + if (lengthMeters <= 0) { + throw new IllegalArgumentException("Control point lengths must be > 0"); + } + + nextControlLength = lengthMeters; + return this; + } + + public PathPoint withControlLengths( + double prevControlLengthMeters, double nextControlLengthMeters) { + if (prevControlLengthMeters <= 0 || nextControlLengthMeters <= 0) { + throw new IllegalArgumentException("Control point lengths must be > 0"); + } + + prevControlLength = prevControlLengthMeters; + nextControlLength = nextControlLengthMeters; + return this; + } + public static PathPoint fromCurrentHolonomicState( Pose2d currentPose, ChassisSpeeds currentSpeeds) { double linearVel = diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/PathPlanner.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/PathPlanner.cpp index 69af28738..36fdaaf1b 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/PathPlanner.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/PathPlanner.cpp @@ -146,14 +146,21 @@ PathPlannerTrajectory PathPlanner::generatePath( units::meter_t thirdDistance = p1.m_position.Distance(p2.m_position) / 3.0; + units::meter_t p1NextDistance = + p1.m_nextControlLength <= 0_m ? + thirdDistance : p1.m_nextControlLength; + units::meter_t p2PrevDistance = + p2.m_prevControlLength <= 0_m ? + thirdDistance : p2.m_prevControlLength; + frc::Translation2d p1Next = p1.m_position - + frc::Translation2d(p1.m_heading.Cos() * thirdDistance, - p1.m_heading.Sin() * thirdDistance); + + frc::Translation2d(p1.m_heading.Cos() * p1NextDistance, + p1.m_heading.Sin() * p1NextDistance); waypoints[i - 1].nextControl = p1Next; frc::Translation2d p2Prev = p2.m_position - - frc::Translation2d(p2.m_heading.Cos() * thirdDistance, - p2.m_heading.Sin() * thirdDistance); + - frc::Translation2d(p2.m_heading.Cos() * p2PrevDistance, + p2.m_heading.Sin() * p2PrevDistance); waypoints.emplace_back(p2.m_position, p2Prev, frc::Translation2d(), p2.m_velocityOverride, p2.m_holonomicRotation, false, false, PathPlannerTrajectory::StopEvent()); diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/PathPoint.h b/pathplannerlib/src/main/native/include/pathplanner/lib/PathPoint.h index 7435a4a9e..b07ce072a 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/PathPoint.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/PathPoint.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include namespace pathplanner { class PathPoint { @@ -14,6 +16,9 @@ class PathPoint { frc::Rotation2d m_holonomicRotation; units::meters_per_second_t m_velocityOverride; + units::meter_t m_prevControlLength = -1_m; + units::meter_t m_nextControlLength = -1_m; + constexpr PathPoint(frc::Translation2d const position, frc::Rotation2d const heading, frc::Rotation2d holonomicRotation, units::meters_per_second_t velocityOverride) : m_position(position), m_heading( @@ -37,6 +42,38 @@ class PathPoint { frc::Rotation2d(), -1_mps) { } + constexpr PathPoint withPrevControlLength(units::meter_t const length) { + if (length <= 0_m) { + throw FRC_MakeError(frc::err::InvalidParameter, + "Control point lengths must be > 0"); + } + + m_prevControlLength = length; + return *this; + } + + constexpr PathPoint withNextControlLength(units::meter_t const length) { + if (length <= 0_m) { + throw FRC_MakeError(frc::err::InvalidParameter, + "Control point lengths must be > 0"); + } + + m_nextControlLength = length; + return *this; + } + + constexpr PathPoint withControlLengths(units::meter_t const prevLength, + units::meter_t const nextLength) { + if (prevLength <= 0_m || nextLength <= 0_m) { + throw FRC_MakeError(frc::err::InvalidParameter, + "Control point lengths must be > 0"); + } + + m_prevControlLength = prevLength; + m_nextControlLength = nextLength; + return *this; + } + static PathPoint fromCurrentHolonomicState(frc::Pose2d const currentPose, frc::ChassisSpeeds const currentSpeeds) { units::meters_per_second_t const linearVel = units::math::sqrt(