From b56b0a992213927cdd20c1d9844e9c648e863a28 Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Thu, 17 Oct 2024 18:48:04 -0400 Subject: [PATCH] updated pathplanner --- src/main/cpp/RobotContainer.cpp | 3 +- .../cpp/pathplanner/lib/auto/AutoBuilder.cpp | 318 ++--- .../cpp/pathplanner/lib/auto/CommandUtil.cpp | 10 +- .../pathplanner/lib/auto/NamedCommands.cpp | 10 +- .../lib/commands/FollowPathCommand.cpp | 181 +-- .../lib/commands/PathPlannerAuto.cpp | 163 ++- .../lib/commands/PathfindingCommand.cpp | 168 +-- .../lib/config/MotorTorqueCurve.cpp | 330 ----- .../pathplanner/lib/config/RobotConfig.cpp | 167 ++- .../PPHolonomicDriveController.cpp | 66 +- .../pathplanner/lib/events/EventScheduler.cpp | 77 ++ .../cpp/pathplanner/lib/path/EventMarker.cpp | 15 +- .../cpp/pathplanner/lib/path/GoalEndState.cpp | 6 +- .../lib/path/IdealStartingState.cpp | 12 + .../pathplanner/lib/path/PathConstraints.cpp | 6 +- .../pathplanner/lib/path/PathPlannerPath.cpp | 1105 ++++++++--------- .../lib/path/PathPlannerTrajectory.cpp | 199 --- .../cpp/pathplanner/lib/path/PathSegment.cpp | 60 - .../pathplanner/lib/path/RotationTarget.cpp | 7 +- .../cpp/pathplanner/lib/path/Waypoint.cpp | 58 + .../lib/pathfinding/LocalADStar.cpp | 107 +- .../lib/pathfinding/RemoteADStar.cpp | 108 -- .../lib/trajectory/PathPlannerTrajectory.cpp | 665 ++++++++++ .../trajectory/PathPlannerTrajectoryState.cpp | 84 ++ .../cpp/pathplanner/lib/util/FlippingUtil.cpp | 8 + .../pathplanner/lib/util/PPLibTelemetry.cpp | 35 +- src/main/cpp/subsystems/SwerveSubsystem.cpp | 43 +- src/main/deploy/commit.txt | 2 +- .../deploy/pathplanner/autos/AmpSideFive.auto | 25 - .../pathplanner/autos/DriveForward.auto | 25 - src/main/deploy/pathplanner/autos/PPTest.auto | 25 + src/main/deploy/pathplanner/paths/PPTest.path | 75 ++ src/main/deploy/pathplanner/settings.json | 21 + src/main/include/Autos.h | 9 +- src/main/include/RobotContainer.h | 4 +- src/main/include/constants/SwerveConstants.h | 14 +- .../pathplanner/lib/auto/AutoBuilder.h | 239 ++-- .../pathplanner/lib/auto/NamedCommands.h | 7 +- .../lib/commands/FollowPathCommand.h | 44 +- .../lib/commands/FollowPathHolonomic.h | 74 -- .../pathplanner/lib/commands/FollowPathLTV.h | 73 -- .../lib/commands/PathPlannerAuto.h | 176 ++- .../lib/commands/PathfindHolonomic.h | 101 -- .../pathplanner/lib/commands/PathfindLTV.h | 129 -- .../lib/commands/PathfindThenFollowPath.h | 46 + .../PathfindThenFollowPathHolonomic.h | 45 - .../lib/commands/PathfindThenFollowPathLTV.h | 78 -- .../lib/commands/PathfindingCommand.h | 59 +- .../pathplanner/lib/config/ModuleConfig.h | 71 +- .../pathplanner/lib/config/MotorTorqueCurve.h | 105 -- .../pathplanner/lib/config/ReplanningConfig.h | 35 - .../pathplanner/lib/config/RobotConfig.h | 42 +- .../controllers/PPHolonomicDriveController.h | 112 +- .../lib/controllers/PPLTVController.h | 9 +- .../lib/controllers/PathFollowingController.h | 4 +- .../lib/events/CancelCommandEvent.h | 38 + .../include/pathplanner/lib/events/Event.h | 68 + .../pathplanner/lib/events/EventScheduler.h | 89 ++ .../pathplanner/lib/events/EventTrigger.h | 60 + .../lib/events/OneShotTriggerEvent.h | 47 + .../lib/events/PointTowardsZoneEvent.h | 42 + .../lib/events/PointTowardsZoneTrigger.h | 60 + .../lib/events/ScheduleCommandEvent.h | 38 + .../pathplanner/lib/events/TriggerEvent.h | 41 + .../pathplanner/lib/path/ConstraintsZone.h | 34 - .../pathplanner/lib/path/EventMarker.h | 88 +- .../pathplanner/lib/path/GoalEndState.h | 18 +- .../pathplanner/lib/path/IdealStartingState.h | 57 + .../pathplanner/lib/path/PathConstraints.h | 45 +- .../pathplanner/lib/path/PathPlannerPath.h | 267 ++-- .../lib/path/PathPlannerTrajectory.h | 259 ---- .../include/pathplanner/lib/path/PathPoint.h | 18 +- .../pathplanner/lib/path/PathSegment.h | 62 - .../pathplanner/lib/path/PointTowardsZone.h | 123 ++ .../pathplanner/lib/path/RotationTarget.h | 31 +- .../include/pathplanner/lib/path/Waypoint.h | 62 + .../pathplanner/lib/pathfinding/LocalADStar.h | 7 +- .../lib/pathfinding/RemoteADStar.h | 53 - .../lib/trajectory/PathPlannerTrajectory.h | 182 +++ .../trajectory/PathPlannerTrajectoryState.h | 68 + .../trajectory/SwerveModuleTrajectoryState.h | 18 + .../pathplanner/lib/util/DriveFeedforward.h | 37 + .../pathplanner/lib/util/FlippingUtil.h | 108 ++ .../pathplanner/lib/util/GeometryUtil.h | 67 +- .../lib/util/HolonomicPathFollowerConfig.h | 64 - .../include/pathplanner/lib/util/JSONUtil.h | 25 + .../pathplanner/lib/util/PPLibTelemetry.h | 37 +- src/main/include/subsystems/SwerveSubsystem.h | 5 + 88 files changed, 4387 insertions(+), 3491 deletions(-) delete mode 100644 src/main/cpp/pathplanner/lib/config/MotorTorqueCurve.cpp create mode 100644 src/main/cpp/pathplanner/lib/events/EventScheduler.cpp create mode 100644 src/main/cpp/pathplanner/lib/path/IdealStartingState.cpp delete mode 100644 src/main/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp delete mode 100644 src/main/cpp/pathplanner/lib/path/PathSegment.cpp create mode 100644 src/main/cpp/pathplanner/lib/path/Waypoint.cpp delete mode 100644 src/main/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp create mode 100644 src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp create mode 100644 src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp create mode 100644 src/main/cpp/pathplanner/lib/util/FlippingUtil.cpp delete mode 100644 src/main/deploy/pathplanner/autos/AmpSideFive.auto delete mode 100644 src/main/deploy/pathplanner/autos/DriveForward.auto create mode 100644 src/main/deploy/pathplanner/autos/PPTest.auto create mode 100644 src/main/deploy/pathplanner/paths/PPTest.path create mode 100644 src/main/deploy/pathplanner/settings.json delete mode 100644 src/main/include/pathplanner/lib/commands/FollowPathHolonomic.h delete mode 100644 src/main/include/pathplanner/lib/commands/FollowPathLTV.h delete mode 100644 src/main/include/pathplanner/lib/commands/PathfindHolonomic.h delete mode 100644 src/main/include/pathplanner/lib/commands/PathfindLTV.h create mode 100644 src/main/include/pathplanner/lib/commands/PathfindThenFollowPath.h delete mode 100644 src/main/include/pathplanner/lib/commands/PathfindThenFollowPathHolonomic.h delete mode 100644 src/main/include/pathplanner/lib/commands/PathfindThenFollowPathLTV.h delete mode 100644 src/main/include/pathplanner/lib/config/MotorTorqueCurve.h delete mode 100644 src/main/include/pathplanner/lib/config/ReplanningConfig.h create mode 100644 src/main/include/pathplanner/lib/events/CancelCommandEvent.h create mode 100644 src/main/include/pathplanner/lib/events/Event.h create mode 100644 src/main/include/pathplanner/lib/events/EventScheduler.h create mode 100644 src/main/include/pathplanner/lib/events/EventTrigger.h create mode 100644 src/main/include/pathplanner/lib/events/OneShotTriggerEvent.h create mode 100644 src/main/include/pathplanner/lib/events/PointTowardsZoneEvent.h create mode 100644 src/main/include/pathplanner/lib/events/PointTowardsZoneTrigger.h create mode 100644 src/main/include/pathplanner/lib/events/ScheduleCommandEvent.h create mode 100644 src/main/include/pathplanner/lib/events/TriggerEvent.h create mode 100644 src/main/include/pathplanner/lib/path/IdealStartingState.h delete mode 100644 src/main/include/pathplanner/lib/path/PathPlannerTrajectory.h delete mode 100644 src/main/include/pathplanner/lib/path/PathSegment.h create mode 100644 src/main/include/pathplanner/lib/path/PointTowardsZone.h create mode 100644 src/main/include/pathplanner/lib/path/Waypoint.h delete mode 100644 src/main/include/pathplanner/lib/pathfinding/RemoteADStar.h create mode 100644 src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectory.h create mode 100644 src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h create mode 100644 src/main/include/pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h create mode 100644 src/main/include/pathplanner/lib/util/DriveFeedforward.h create mode 100644 src/main/include/pathplanner/lib/util/FlippingUtil.h delete mode 100644 src/main/include/pathplanner/lib/util/HolonomicPathFollowerConfig.h create mode 100644 src/main/include/pathplanner/lib/util/JSONUtil.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 272dbf1..d100e80 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -129,8 +129,7 @@ frc2::CommandPtr RobotContainer::IntakeNote() { } frc2::Command* RobotContainer::GetAutonomousCommand() { - // return autos.GetSelectedCommand(); - return nullptr; + return autos.GetSelectedCommand(); } SwerveSubsystem& RobotContainer::GetSwerveSubsystem() { diff --git a/src/main/cpp/pathplanner/lib/auto/AutoBuilder.cpp b/src/main/cpp/pathplanner/lib/auto/AutoBuilder.cpp index 1622fb8..7ee9d38 100644 --- a/src/main/cpp/pathplanner/lib/auto/AutoBuilder.cpp +++ b/src/main/cpp/pathplanner/lib/auto/AutoBuilder.cpp @@ -1,16 +1,13 @@ #include "pathplanner/lib/auto/AutoBuilder.h" -#include "pathplanner/lib/commands/FollowPathHolonomic.h" -#include "pathplanner/lib/commands/FollowPathLTV.h" -#include "pathplanner/lib/commands/PathfindHolonomic.h" -#include "pathplanner/lib/commands/PathfindThenFollowPathHolonomic.h" -#include "pathplanner/lib/commands/PathfindLTV.h" -#include "pathplanner/lib/commands/PathfindThenFollowPathLTV.h" -#include "pathplanner/lib/commands/PathPlannerAuto.h" +#include "pathplanner/lib/commands/FollowPathCommand.h" +#include "pathplanner/lib/commands/PathfindingCommand.h" +#include "pathplanner/lib/commands/PathfindThenFollowPath.h" #include "pathplanner/lib/auto/CommandUtil.h" +#include "pathplanner/lib/util/FlippingUtil.h" +#include "pathplanner/lib/commands/PathPlannerAuto.h" #include #include #include -#include #include #include @@ -18,158 +15,71 @@ using namespace pathplanner; bool AutoBuilder::m_configured = false; std::function)> AutoBuilder::m_pathFollowingCommandBuilder; -std::function AutoBuilder::m_getPose; +std::function AutoBuilder::m_poseSupplier; std::function AutoBuilder::m_resetPose; std::function AutoBuilder::m_shouldFlipPath; +bool AutoBuilder::m_isHolonomic = false; -std::vector AutoBuilder::m_autoCommands; +bool AutoBuilder::m_commandRefsGeneratedForSendable = false; +frc2::CommandPtr AutoBuilder::m_noneCommand = frc2::cmd::None(); +std::map AutoBuilder::m_autoCommands; bool AutoBuilder::m_pathfindingConfigured = false; std::function< frc2::CommandPtr(frc::Pose2d, PathConstraints, - units::meters_per_second_t, units::meter_t)> AutoBuilder::m_pathfindToPoseCommandBuilder; + units::meters_per_second_t)> AutoBuilder::m_pathfindToPoseCommandBuilder; std::function< - frc2::CommandPtr(std::shared_ptr, PathConstraints, - units::meter_t)> AutoBuilder::m_pathfindThenFollowPathCommandBuilder; + frc2::CommandPtr(std::shared_ptr, PathConstraints)> AutoBuilder::m_pathfindThenFollowPathCommandBuilder; -void AutoBuilder::configureHolonomic(std::function poseSupplier, +void AutoBuilder::configure(std::function poseSupplier, std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function robotRelativeOutput, - HolonomicPathFollowerConfig config, - std::function shouldFlipPath, frc2::Subsystem *driveSubsystem) { + std::function)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, + frc2::Subsystem *driveSubsystem) { if (m_configured) { FRC_ReportError(frc::err::Error, "Auto builder has already been configured. This is likely in error."); } AutoBuilder::m_pathFollowingCommandBuilder = [poseSupplier, - robotRelativeSpeedsSupplier, robotRelativeOutput, config, + robotRelativeSpeedsSupplier, output, controller, robotConfig, shouldFlipPath, driveSubsystem]( std::shared_ptr path) { - return FollowPathHolonomic(path, poseSupplier, - robotRelativeSpeedsSupplier, robotRelativeOutput, config, + return FollowPathCommand(path, poseSupplier, + robotRelativeSpeedsSupplier, output, controller, robotConfig, shouldFlipPath, { driveSubsystem }).ToPtr(); }; - AutoBuilder::m_getPose = poseSupplier; + AutoBuilder::m_poseSupplier = poseSupplier; AutoBuilder::m_resetPose = resetPose; AutoBuilder::m_configured = true; AutoBuilder::m_shouldFlipPath = shouldFlipPath; + AutoBuilder::m_isHolonomic = robotConfig.isHolonomic; AutoBuilder::m_pathfindToPoseCommandBuilder = [poseSupplier, - robotRelativeSpeedsSupplier, robotRelativeOutput, config, + robotRelativeSpeedsSupplier, output, controller, robotConfig, driveSubsystem](frc::Pose2d pose, PathConstraints constraints, - units::meters_per_second_t goalEndVel, - units::meter_t rotationDelayDistance) { - return PathfindHolonomic(pose, constraints, goalEndVel, poseSupplier, - robotRelativeSpeedsSupplier, robotRelativeOutput, config, { - driveSubsystem }, rotationDelayDistance).ToPtr(); - }; - AutoBuilder::m_pathfindThenFollowPathCommandBuilder = - [poseSupplier, robotRelativeSpeedsSupplier, robotRelativeOutput, - config, shouldFlipPath, driveSubsystem]( - std::shared_ptr path, - PathConstraints constraints, - units::meter_t rotationDelayDistance) { - return PathfindThenFollowPathHolonomic(path, constraints, - poseSupplier, robotRelativeSpeedsSupplier, - robotRelativeOutput, config, shouldFlipPath, { - driveSubsystem }, rotationDelayDistance).ToPtr(); - }; - AutoBuilder::m_pathfindingConfigured = true; -} - -void AutoBuilder::configureLTV(std::function poseSupplier, - std::function resetPose, - std::function speedsSupplier, - std::function output, - const wpi::array &Qelms, const wpi::array &Relms, - units::second_t dt, ReplanningConfig replanningConfig, - std::function shouldFlipPath, frc2::Subsystem *driveSubsystem) { - if (m_configured) { - FRC_ReportError(frc::err::Error, - "Auto builder has already been configured. This is likely in error."); - } - - AutoBuilder::m_pathFollowingCommandBuilder = - [poseSupplier, speedsSupplier, output, Qelms, Relms, dt, - replanningConfig, shouldFlipPath, driveSubsystem]( - std::shared_ptr path) { - return FollowPathLTV(path, poseSupplier, speedsSupplier, output, - Qelms, Relms, dt, replanningConfig, shouldFlipPath, { - driveSubsystem }).ToPtr(); - }; - AutoBuilder::m_getPose = poseSupplier; - AutoBuilder::m_resetPose = resetPose; - AutoBuilder::m_configured = true; - AutoBuilder::m_shouldFlipPath = shouldFlipPath; - - AutoBuilder::m_pathfindToPoseCommandBuilder = [poseSupplier, speedsSupplier, - output, Qelms, Relms, dt, replanningConfig, driveSubsystem]( - frc::Pose2d pose, PathConstraints constraints, - units::meters_per_second_t goalEndVel, - units::meter_t rotationDelayDistance) { - return PathfindLTV(pose.Translation(), constraints, goalEndVel, - poseSupplier, speedsSupplier, output, Qelms, Relms, dt, - replanningConfig, { driveSubsystem }).ToPtr(); + units::meters_per_second_t goalEndVel) { + return PathfindingCommand(pose, constraints, goalEndVel, poseSupplier, + robotRelativeSpeedsSupplier, output, controller, robotConfig, { + driveSubsystem }).ToPtr(); }; AutoBuilder::m_pathfindThenFollowPathCommandBuilder = [poseSupplier, - speedsSupplier, output, Qelms, Relms, dt, replanningConfig, + robotRelativeSpeedsSupplier, output, controller, robotConfig, shouldFlipPath, driveSubsystem]( - std::shared_ptr path, PathConstraints constraints, - units::meter_t rotationDelayDistance) { - return PathfindThenFollowPathLTV(path, constraints, poseSupplier, - speedsSupplier, output, Qelms, Relms, dt, replanningConfig, + std::shared_ptr path, + PathConstraints constraints) { + return PathfindThenFollowPath(path, constraints, poseSupplier, + robotRelativeSpeedsSupplier, output, controller, robotConfig, shouldFlipPath, { driveSubsystem }).ToPtr(); }; AutoBuilder::m_pathfindingConfigured = true; } -void AutoBuilder::configureLTV(std::function poseSupplier, - std::function resetPose, - std::function speedsSupplier, - std::function output, units::second_t dt, - ReplanningConfig replanningConfig, std::function shouldFlipPath, - frc2::Subsystem *driveSubsystem) { - if (m_configured) { - FRC_ReportError(frc::err::Error, - "Auto builder has already been configured. This is likely in error."); - } - - AutoBuilder::m_pathFollowingCommandBuilder = [poseSupplier, speedsSupplier, - output, dt, replanningConfig, shouldFlipPath, driveSubsystem]( - std::shared_ptr path) { - return FollowPathLTV(path, poseSupplier, speedsSupplier, output, dt, - replanningConfig, shouldFlipPath, { driveSubsystem }).ToPtr(); - }; - AutoBuilder::m_getPose = poseSupplier; - AutoBuilder::m_resetPose = resetPose; - AutoBuilder::m_configured = true; - AutoBuilder::m_shouldFlipPath = shouldFlipPath; - - AutoBuilder::m_pathfindToPoseCommandBuilder = [poseSupplier, speedsSupplier, - output, dt, replanningConfig, driveSubsystem](frc::Pose2d pose, - PathConstraints constraints, units::meters_per_second_t goalEndVel, - units::meter_t rotationDelayDistance) { - return PathfindLTV(pose.Translation(), constraints, goalEndVel, - poseSupplier, speedsSupplier, output, dt, replanningConfig, { - driveSubsystem }).ToPtr(); - }; - AutoBuilder::m_pathfindThenFollowPathCommandBuilder = [poseSupplier, - speedsSupplier, output, dt, replanningConfig, shouldFlipPath, - driveSubsystem](std::shared_ptr path, - PathConstraints constraints, units::meter_t rotationDelayDistance) { - return PathfindThenFollowPathLTV(path, constraints, poseSupplier, - speedsSupplier, output, dt, replanningConfig, shouldFlipPath, { - driveSubsystem }).ToPtr(); - }; - AutoBuilder::m_pathfindingConfigured = true; -} - -void AutoBuilder::configureCustom( +void AutoBuilder::configureCustom(std::function poseSupplier, std::function)> pathFollowingCommandBuilder, - std::function poseSupplier, - std::function resetPose, + std::function resetPose, bool isHolonomic, std::function shouldFlipPose) { if (m_configured) { FRC_ReportError(frc::err::Error, @@ -177,10 +87,11 @@ void AutoBuilder::configureCustom( } AutoBuilder::m_pathFollowingCommandBuilder = pathFollowingCommandBuilder; - AutoBuilder::m_getPose = poseSupplier; + AutoBuilder::m_poseSupplier = poseSupplier; AutoBuilder::m_resetPose = resetPose; AutoBuilder::m_configured = true; AutoBuilder::m_shouldFlipPath = shouldFlipPose; + AutoBuilder::m_isHolonomic = isHolonomic; AutoBuilder::m_pathfindingConfigured = false; } @@ -196,107 +107,127 @@ frc2::CommandPtr AutoBuilder::followPath( } frc2::CommandPtr AutoBuilder::buildAuto(std::string autoName) { - const std::string filePath = frc::filesystem::GetDeployDirectory() - + "/pathplanner/autos/" + autoName + ".auto"; - - wpi::expected, std::error_code> fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); - - if (!fileBuffer.has_value()) { - throw std::runtime_error("Cannot open file: " + filePath); - } - - wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); - - return getAutoCommandFromJson(json); + return PathPlannerAuto(autoName).ToPtr(); } -frc2::CommandPtr AutoBuilder::getAutoCommandFromJson(const wpi::json &json) { - wpi::json::const_reference commandJson = json.at("command"); - bool choreoAuto = json.contains("choreoAuto") - && json.at("choreoAuto").get(); - - frc2::CommandPtr autoCommand = CommandUtil::commandFromJson(commandJson, - choreoAuto); - if (!json.at("startingPose").is_null()) { - frc::Pose2d startPose = getStartingPoseFromJson( - json.at("startingPose")); - return frc2::cmd::Sequence(frc2::cmd::RunOnce([startPose]() { - if (m_shouldFlipPath()) { - m_resetPose(GeometryUtil::flipFieldPose(startPose)); - } else { - m_resetPose(startPose); - } - }), std::move(autoCommand)); - } else { - return autoCommand; +frc2::CommandPtr AutoBuilder::resetOdom(frc::Pose2d bluePose) { + if (!m_configured) { + throw std::runtime_error( + "Auto builder was used to build a command before being configured"); } -} - -frc::Pose2d AutoBuilder::getStartingPoseFromJson(const wpi::json &json) { - wpi::json::const_reference pos = json.at("position"); - units::meter_t x = units::meter_t { pos.at("x").get() }; - units::meter_t y = units::meter_t { pos.at("y").get() }; - units::degree_t deg = units::degree_t { json.at("rotation").get() }; - return frc::Pose2d(x, y, frc::Rotation2d(deg)); + return frc2::cmd::RunOnce([bluePose]() { + if (m_shouldFlipPath()) { + m_resetPose(FlippingUtil::flipFieldPose(bluePose)); + } else { + m_resetPose(bluePose); + } + }); } frc2::CommandPtr AutoBuilder::pathfindToPose(frc::Pose2d pose, - PathConstraints constraints, units::meters_per_second_t goalEndVel, - units::meter_t rotationDelayDistance) { + PathConstraints constraints, units::meters_per_second_t goalEndVel) { if (!m_pathfindingConfigured) { throw std::runtime_error( "Auto builder was used to build a pathfinding command before being configured"); } - return m_pathfindToPoseCommandBuilder(pose, constraints, goalEndVel, - rotationDelayDistance); + return m_pathfindToPoseCommandBuilder(pose, constraints, goalEndVel); } frc2::CommandPtr AutoBuilder::pathfindThenFollowPath( std::shared_ptr goalPath, - PathConstraints pathfindingConstraints, - units::meter_t rotationDelayDistance) { + PathConstraints pathfindingConstraints) { if (!m_pathfindingConfigured) { throw std::runtime_error( "Auto builder was used to build a pathfinding command before being configured"); } return m_pathfindThenFollowPathCommandBuilder(goalPath, - pathfindingConstraints, rotationDelayDistance); + pathfindingConstraints); +} + +void AutoBuilder::regenerateSendableReferences() { + std::vector < std::filesystem::path > autoPathFilepaths = getAllAutoPaths(); + + for (std::filesystem::path path : autoPathFilepaths) { + // A command which is an auto that come from a path + m_autoCommands.insert_or_assign(path, + buildAuto(path.replace_extension("").string())); + } } frc::SendableChooser AutoBuilder::buildAutoChooser( std::string defaultAutoName) { + return buildAutoChooserFilterPath( + [](const PathPlannerAuto &autoCommand, + std::filesystem::path autoPath) { + return true; + },defaultAutoName); +} + +frc::SendableChooser AutoBuilder::buildAutoChooserFilter( + std::function filter, + std::string defaultAutoName) { + return buildAutoChooserFilterPath( + [&filter](const PathPlannerAuto &autoCommand, + std::filesystem::path autoPath) { + return filter(autoCommand); + },defaultAutoName); +} + +frc::SendableChooser AutoBuilder::buildAutoChooserFilterPath( + std::function filter, + std::string defaultAutoName) { if (!m_configured) { throw std::runtime_error( "AutoBuilder was not configured before attempting to build an auto chooser"); } - frc::SendableChooser chooser; - bool foundDefaultOption = false; + if (!m_commandRefsGeneratedForSendable) { + regenerateSendableReferences(); + m_commandRefsGeneratedForSendable = true; + } + + frc::SendableChooser sendableChooser; + bool defaultSelected = false; - for (std::string const &entry : getAllAutoNames()) { - AutoBuilder::m_autoCommands.emplace_back( - pathplanner::PathPlannerAuto(entry).ToPtr()); - if (defaultAutoName != "" && entry == defaultAutoName) { - foundDefaultOption = true; - chooser.SetDefaultOption(entry, m_autoCommands.back().get()); - } else { - chooser.AddOption(entry, m_autoCommands.back().get()); + for (const std::pair &entry : m_autoCommands) { + std::string autoName = entry.first.stem().string(); + + // Found the default for sendableChooser + if (defaultAutoName == autoName) { + sendableChooser.SetDefaultOption(autoName, entry.second.get()); + defaultSelected = true; + } else if (filter(*static_cast(entry.second.get()), + entry.first)) { + sendableChooser.AddOption(autoName, entry.second.get()); } } - if (!foundDefaultOption) { - AutoBuilder::m_autoCommands.emplace_back(frc2::cmd::None()); - chooser.SetDefaultOption("None", m_autoCommands.back().get()); + // None is the default + if (!defaultSelected || defaultAutoName == "") { + sendableChooser.SetDefaultOption("None", m_noneCommand.get()); + } + // None is just there, extra precaution for programmers + else { + sendableChooser.AddOption("None", m_noneCommand.get()); } - return chooser; + return sendableChooser; } std::vector AutoBuilder::getAllAutoNames() { + std::vector < std::string > autoNames; + + for (const std::filesystem::path &path : getAllAutoPaths()) { + autoNames.push_back(path.stem().string()); + } + + return autoNames; +} + +std::vector AutoBuilder::getAllAutoPaths() { std::filesystem::path deployPath = frc::filesystem::GetDeployDirectory(); std::filesystem::path autosPath = deployPath / "pathplanner/autos"; @@ -307,17 +238,16 @@ std::vector AutoBuilder::getAllAutoNames() { return {}; } - std::vector < std::string > autoPathNames; + std::vector < std::filesystem::path > autoPathNames; - for (std::filesystem::directory_entry const &entry : std::filesystem::directory_iterator { - autosPath }) { - if (!entry.is_regular_file()) { - continue; - } - if (entry.path().extension().string() != ".auto") { + for (std::filesystem::directory_entry const &entry : std::filesystem::recursive_directory_iterator { + autosPath, + std::filesystem::directory_options::skip_permission_denied }) { + if (!entry.is_regular_file() + || entry.path().extension().string() != ".auto") { continue; } - autoPathNames.emplace_back(entry.path().stem().string()); + autoPathNames.emplace_back(entry.path().lexically_relative(autosPath)); } return autoPathNames; diff --git a/src/main/cpp/pathplanner/lib/auto/CommandUtil.cpp b/src/main/cpp/pathplanner/lib/auto/CommandUtil.cpp index ee396a2..3f3a4e9 100644 --- a/src/main/cpp/pathplanner/lib/auto/CommandUtil.cpp +++ b/src/main/cpp/pathplanner/lib/auto/CommandUtil.cpp @@ -53,8 +53,14 @@ frc2::CommandPtr CommandUtil::commandFromJson(const wpi::json &json, } frc2::CommandPtr CommandUtil::waitCommandFromJson(const wpi::json &json) { - auto waitTime = units::second_t(json.at("waitTime").get()); - return frc2::cmd::Wait(waitTime); + auto waitJson = json.at("waitTime"); + if (waitJson.is_number()) { + return frc2::cmd::Wait(units::second_t { waitJson.get() }); + } else { + // Field is not a number, probably a choreo expression + return frc2::cmd::Wait(units::second_t { + waitJson.at("val").get() }); + } } frc2::CommandPtr CommandUtil::namedCommandFromJson(const wpi::json &json) { diff --git a/src/main/cpp/pathplanner/lib/auto/NamedCommands.cpp b/src/main/cpp/pathplanner/lib/auto/NamedCommands.cpp index 3d0f007..e3f2b28 100644 --- a/src/main/cpp/pathplanner/lib/auto/NamedCommands.cpp +++ b/src/main/cpp/pathplanner/lib/auto/NamedCommands.cpp @@ -4,15 +4,19 @@ using namespace pathplanner; -std::unordered_map> NamedCommands::namedCommands; - frc2::CommandPtr NamedCommands::getCommand(std::string name) { if (NamedCommands::hasCommand(name)) { return CommandUtil::wrappedEventCommand( - NamedCommands::namedCommands.at(name)); + NamedCommands::GetNamedCommands().at(name)); } FRC_ReportError(frc::warn::Warning, "PathPlanner attempted to create a command '{}' that has not been registered with NamedCommands::registerCommand", name); return frc2::cmd::None(); } + +std::unordered_map>& NamedCommands::GetNamedCommands() { + static std::unordered_map> *namedCommands = + new std::unordered_map>(); + return *namedCommands; +} diff --git a/src/main/cpp/pathplanner/lib/commands/FollowPathCommand.cpp b/src/main/cpp/pathplanner/lib/commands/FollowPathCommand.cpp index e785a52..c3ad22f 100644 --- a/src/main/cpp/pathplanner/lib/commands/FollowPathCommand.cpp +++ b/src/main/cpp/pathplanner/lib/commands/FollowPathCommand.cpp @@ -1,37 +1,45 @@ #include "pathplanner/lib/commands/FollowPathCommand.h" +#include "pathplanner/lib/path/PathPlannerPath.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" +#include "pathplanner/lib/commands/PathPlannerAuto.h" using namespace pathplanner; FollowPathCommand::FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, - std::function output, - std::unique_ptr controller, - ReplanningConfig replanningConfig, std::function shouldFlipPath, + std::function)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) : m_originalPath(path), m_poseSupplier( poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller( - std::move(controller)), m_replanningConfig(replanningConfig), m_shouldFlipPath( - shouldFlipPath) { + controller), m_robotConfig(robotConfig), m_shouldFlipPath( + shouldFlipPath), m_eventScheduler() { AddRequirements(requirements); - auto &&driveRequirements = GetRequirements(); + auto driveRequirements = GetRequirements(); + auto eventReqs = EventScheduler::getSchedulerRequirements(m_originalPath); - for (EventMarker &marker : m_originalPath->getEventMarkers()) { - auto reqs = marker.getCommand()->GetRequirements(); - - for (auto &&requirement : reqs) { - if (driveRequirements.find(requirement) - != driveRequirements.end()) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Events that are triggered during path following cannot require the drive subsystem"); - } + for (auto requirement : eventReqs) { + if (driveRequirements.find(requirement) != driveRequirements.end()) { + throw FRC_MakeError(frc::err::CommandIllegalUse, + "Events that are triggered during path following cannot require the drive subsystem"); } + } - AddRequirements(reqs); + AddRequirements(eventReqs); + + m_path = m_originalPath; + // Ensure the ideal trajectory is generated + auto idealTraj = m_path->getIdealTrajectory(m_robotConfig); + if (idealTraj.has_value()) { + m_trajectory = idealTraj.value(); } } void FollowPathCommand::Initialize() { + PathPlannerAuto::currentPathName = m_originalPath->name; + if (m_shouldFlipPath() && !m_originalPath->preventFlipping) { m_path = m_originalPath->flipPath(); } else { @@ -43,36 +51,38 @@ void FollowPathCommand::Initialize() { m_controller->reset(currentPose, currentSpeeds); - frc::ChassisSpeeds fieldSpeeds = - frc::ChassisSpeeds::FromRobotRelativeSpeeds(currentSpeeds, - currentPose.Rotation()); - frc::Rotation2d currentHeading = frc::Rotation2d(fieldSpeeds.vx(), - fieldSpeeds.vy()); - frc::Rotation2d targetHeading = (m_path->getPoint(1).position - - m_path->getPoint(0).position).Angle(); - frc::Rotation2d headingError = currentHeading - targetHeading; - bool onHeading = units::math::hypot(currentSpeeds.vx, currentSpeeds.vy) - < 0.25_mps || units::math::abs(headingError.Degrees()) < 30_deg; - - if (!m_path->isChoreoPath() && m_replanningConfig.enableInitialReplanning - && (currentPose.Translation().Distance(m_path->getPoint(0).position) - > 0.25_m || !onHeading)) { - replanPath(currentPose, currentSpeeds); + auto linearVel = units::math::hypot(currentSpeeds.vx, currentSpeeds.vy); + + if (m_path->getIdealStartingState().has_value()) { + // Check if we match the ideal starting state + bool idealVelocity = units::math::abs( + linearVel + - m_path->getIdealStartingState().value().getVelocity()) + <= 0.25_mps; + bool idealRotation = + !m_robotConfig.isHolonomic + || units::math::abs( + (currentPose.Rotation() + - m_path->getIdealStartingState().value().getRotation()).Degrees()) + <= 30_deg; + if (idealVelocity && idealRotation) { + // We can use the ideal trajectory + m_trajectory = m_path->getIdealTrajectory(m_robotConfig).value(); + } else { + // We need to regenerate + m_trajectory = m_path->generateTrajectory(currentSpeeds, + currentPose.Rotation(), m_robotConfig); + } } else { - m_generatedTrajectory = m_path->getTrajectory(currentSpeeds, - currentPose.Rotation()); - PathPlannerLogging::logActivePath (m_path); - PPLibTelemetry::setCurrentPath(m_path); + // No ideal starting state, generate the trajectory + m_trajectory = m_path->generateTrajectory(currentSpeeds, + currentPose.Rotation(), m_robotConfig); } - // Initialize marker stuff - m_currentEventCommands.clear(); - m_untriggeredEvents.clear(); - - const auto &eventCommands = m_generatedTrajectory.getEventCommands(); + PathPlannerLogging::logActivePath (m_path); + PPLibTelemetry::setCurrentPath(m_path); - m_untriggeredEvents.insert(m_untriggeredEvents.end(), eventCommands.begin(), - eventCommands.end()); + m_eventScheduler.initialize(m_trajectory); m_timer.Reset(); m_timer.Start(); @@ -80,31 +90,14 @@ void FollowPathCommand::Initialize() { void FollowPathCommand::Execute() { units::second_t currentTime = m_timer.Get(); - PathPlannerTrajectory::State targetState = m_generatedTrajectory.sample( - currentTime); - if (m_controller->isHolonomic()) { + PathPlannerTrajectoryState targetState = m_trajectory.sample(currentTime); + if (!m_controller->isHolonomic() && m_path->isReversed()) { targetState = targetState.reverse(); } frc::Pose2d currentPose = m_poseSupplier(); frc::ChassisSpeeds currentSpeeds = m_speedsSupplier(); - if (!m_path->isChoreoPath() && m_replanningConfig.enableDynamicReplanning) { - units::meter_t previousError = units::math::abs( - m_controller->getPositionalError()); - units::meter_t currentError = currentPose.Translation().Distance( - targetState.position); - - if (currentError - >= m_replanningConfig.dynamicReplanningTotalErrorThreshold - || currentError - previousError - >= m_replanningConfig.dynamicReplanningErrorSpikeThreshold) { - replanPath(currentPose, currentSpeeds); - m_timer.Reset(); - targetState = m_generatedTrajectory.sample(0_s); - } - } - units::meters_per_second_t currentVel = units::math::hypot(currentSpeeds.vx, currentSpeeds.vy); @@ -115,76 +108,36 @@ void FollowPathCommand::Execute() { PPLibTelemetry::setCurrentPose(currentPose); PathPlannerLogging::logCurrentPose(currentPose); - if (m_controller->isHolonomic()) { - PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose()); - PathPlannerLogging::logTargetPose(targetState.getTargetHolonomicPose()); - } else { - PPLibTelemetry::setTargetPose(targetState.getDifferentialPose()); - PathPlannerLogging::logTargetPose(targetState.getDifferentialPose()); - } + PPLibTelemetry::setTargetPose(targetState.pose); + PathPlannerLogging::logTargetPose(targetState.pose); - PPLibTelemetry::setVelocities(currentVel, targetState.velocity, + PPLibTelemetry::setVelocities(currentVel, targetState.linearVelocity, currentSpeeds.omega, targetSpeeds.omega); - PPLibTelemetry::setPathInaccuracy(m_controller->getPositionalError()); - - m_output(targetSpeeds); - - if (!m_untriggeredEvents.empty() - && m_timer.HasElapsed(m_untriggeredEvents[0].first)) { - // Time to trigger this event command - auto event = m_untriggeredEvents[0]; - for (std::pair, bool> &runningCommand : m_currentEventCommands) { - if (!runningCommand.second) { - continue; - } + m_output(targetSpeeds, targetState.feedforwards); - if (!frc2::RequirementsDisjoint(runningCommand.first.get(), - event.second.get())) { - runningCommand.first->End(true); - runningCommand.second = false; - } - } - - event.second->Initialize(); - m_currentEventCommands.emplace_back(event.second, true); - - m_untriggeredEvents.pop_front(); - } - - // Run event marker commands - for (std::pair, bool> &runningCommand : m_currentEventCommands) { - if (!runningCommand.second) { - continue; - } - - runningCommand.first->Execute(); - if (runningCommand.first->IsFinished()) { - runningCommand.first->End(false); - runningCommand.second = false; - } - } + m_eventScheduler.execute(currentTime); } bool FollowPathCommand::IsFinished() { - return m_timer.HasElapsed(m_generatedTrajectory.getTotalTime()); + return m_timer.HasElapsed(m_trajectory.getTotalTime()); } void FollowPathCommand::End(bool interrupted) { m_timer.Stop(); + PathPlannerAuto::currentPathName = ""; // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting // the command to smoothly transition into some auto-alignment routine if (!interrupted && m_path->getGoalEndState().getVelocity() < 0.1_mps) { - m_output(frc::ChassisSpeeds()); + std::vector < DriveFeedforward > ff; + for (size_t m = 0; m < m_robotConfig.numModules; m++) { + ff.emplace_back(DriveFeedforward { }); + } + m_output(frc::ChassisSpeeds(), ff); } PathPlannerLogging::logActivePath(nullptr); - // End markers - for (std::pair, bool> &runningCommand : m_currentEventCommands) { - if (runningCommand.second) { - runningCommand.first->End(true); - } - } + m_eventScheduler.end(); } diff --git a/src/main/cpp/pathplanner/lib/commands/PathPlannerAuto.cpp b/src/main/cpp/pathplanner/lib/commands/PathPlannerAuto.cpp index 57cfca4..ff28126 100644 --- a/src/main/cpp/pathplanner/lib/commands/PathPlannerAuto.cpp +++ b/src/main/cpp/pathplanner/lib/commands/PathPlannerAuto.cpp @@ -1,12 +1,15 @@ #include "pathplanner/lib/commands/PathPlannerAuto.h" #include "pathplanner/lib/auto/AutoBuilder.h" +#include "pathplanner/lib/auto/CommandUtil.h" #include "pathplanner/lib/util/PPLibTelemetry.h" #include #include #include -#include "wpi/expected" +#include + using namespace pathplanner; +std::string PathPlannerAuto::currentPathName = ""; int PathPlannerAuto::m_instances = 0; PathPlannerAuto::PathPlannerAuto(std::string autoName) { @@ -15,23 +18,126 @@ PathPlannerAuto::PathPlannerAuto(std::string autoName) { "AutoBuilder was not configured before attempting to load a PathPlannerAuto from file"); } - m_autoCommand = AutoBuilder::buildAuto(autoName).Unwrap(); + const std::string filePath = frc::filesystem::GetDeployDirectory() + + "/pathplanner/autos/" + autoName + ".auto"; + + auto fileBuffer = wpi::MemoryBuffer::GetFile(filePath); + + if (!fileBuffer) { + throw std::runtime_error("Cannot open file: " + filePath); + } + + wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); + + std::string version = "1.0"; + if (json.at("version").is_string()) { + version = json.at("version").get(); + } + + if (version != "2025.0") { + throw std::runtime_error( + "Incompatible file version for '" + autoName + + ".auto'. Actual: '" + version + + "' Expected: '2025.0'"); + } + + initFromJson(json); + AddRequirements(m_autoCommand->GetRequirements()); SetName(autoName); + m_autoLoop = std::make_unique(); + + m_instances++; + HAL_Report(HALUsageReporting::kResourceType_PathPlannerAuto, m_instances); +} + +PathPlannerAuto::PathPlannerAuto(frc2::CommandPtr &&autoCommand, + frc::Pose2d startingPose) : m_autoCommand( + std::move(autoCommand).Unwrap()), m_startingPose(startingPose) { + AddRequirements(m_autoCommand->GetRequirements()); + + m_autoLoop = std::make_unique(); + m_instances++; HAL_Report(HALUsageReporting::kResourceType_PathPlannerAuto, m_instances); } +frc2::Trigger PathPlannerAuto::nearFieldPositionAutoFlipped( + frc::Translation2d blueFieldPosition, units::meter_t tolerance) { + frc::Translation2d redFieldPosition = FlippingUtil::flipFieldPosition( + blueFieldPosition); + + return condition( + [blueFieldPosition, redFieldPosition, tolerance]() { + if (AutoBuilder::shouldFlip()) { + return AutoBuilder::getCurrentPose().Translation().Distance( + redFieldPosition) <= tolerance; + } else { + return AutoBuilder::getCurrentPose().Translation().Distance( + blueFieldPosition) <= tolerance; + } + }); +} + +frc2::Trigger PathPlannerAuto::inFieldArea(frc::Translation2d boundingBoxMin, + frc::Translation2d boundingBoxMax) { + if (boundingBoxMin.X() >= boundingBoxMax.X() + || boundingBoxMin.Y() >= boundingBoxMax.Y()) { + throw std::invalid_argument( + "Minimum bounding box position must have X and Y coordinates less than the maximum bounding box position"); + } + + return condition( + [boundingBoxMin, boundingBoxMax]() { + frc::Pose2d currentPose = AutoBuilder::getCurrentPose(); + return currentPose.X() >= boundingBoxMin.X() + && currentPose.Y() >= boundingBoxMin.Y() + && currentPose.X() <= boundingBoxMax.X() + && currentPose.Y() <= boundingBoxMax.Y(); + }); +} + +frc2::Trigger PathPlannerAuto::inFieldAreaAutoFlipped( + frc::Translation2d blueBoundingBoxMin, + frc::Translation2d blueBoundingBoxMax) { + if (blueBoundingBoxMin.X() >= blueBoundingBoxMax.X() + || blueBoundingBoxMin.Y() >= blueBoundingBoxMax.Y()) { + throw std::invalid_argument( + "Minimum bounding box position must have X and Y coordinates less than the maximum bounding box position"); + } + + frc::Translation2d redBoundingBoxMin = FlippingUtil::flipFieldPosition( + blueBoundingBoxMin); + frc::Translation2d redBoundingBoxMax = FlippingUtil::flipFieldPosition( + blueBoundingBoxMax); + + return condition( + [blueBoundingBoxMin, blueBoundingBoxMax, redBoundingBoxMin, + redBoundingBoxMax]() { + frc::Pose2d currentPose = AutoBuilder::getCurrentPose(); + if (AutoBuilder::shouldFlip()) { + return currentPose.X() >= blueBoundingBoxMin.X() + && currentPose.Y() >= blueBoundingBoxMin.Y() + && currentPose.X() <= blueBoundingBoxMax.X() + && currentPose.Y() <= blueBoundingBoxMax.Y(); + } else { + return currentPose.X() >= redBoundingBoxMin.X() + && currentPose.Y() >= redBoundingBoxMin.Y() + && currentPose.X() <= redBoundingBoxMax.X() + && currentPose.Y() <= redBoundingBoxMax.Y(); + } + }); +} + std::vector> PathPlannerAuto::getPathGroupFromAutoFile( std::string autoName) { const std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/autos/" + autoName + ".auto"; - wpi::expected, std::error_code> fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); + auto fileBuffer = wpi::MemoryBuffer::GetFile(filePath); - if (!fileBuffer.has_value()) { + if (!fileBuffer) { throw std::runtime_error("Cannot open file: " + filePath); } @@ -42,28 +148,47 @@ std::vector> PathPlannerAuto::getPathGroupFromA return pathsFromCommandJson(json.at("command"), choreoAuto); } -frc::Pose2d PathPlannerAuto::getStartingPoseFromAutoFile(std::string autoName) { - const std::string filePath = frc::filesystem::GetDeployDirectory() - + "/pathplanner/autos/" + autoName + ".auto"; - - wpi::expected, std::error_code> fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); - - if (!fileBuffer.has_value()) { - throw std::runtime_error("Cannot open file: " + filePath); +void PathPlannerAuto::initFromJson(const wpi::json &json) { + bool choreoAuto = json.contains("choreoAuto") + && json.at("choreoAuto").get(); + wpi::json::const_reference commandJson = json.at("command"); + bool resetOdom = json.contains("resetOdom") + && json.at("resetOdom").get(); + auto pathsInAuto = pathsFromCommandJson(commandJson, choreoAuto); + if (!pathsInAuto.empty()) { + if (AutoBuilder::isHolonomic()) { + m_startingPose = + frc::Pose2d(pathsInAuto[0]->getPoint(0).position, + pathsInAuto[0]->getIdealStartingState().value().getRotation()); + } else { + m_startingPose = pathsInAuto[0]->getStartingDifferentialPose(); + } + } else { + m_startingPose = frc::Pose2d(); } - wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); - - return AutoBuilder::getStartingPoseFromJson(json.at("startingPose")); + if (resetOdom) { + m_autoCommand = frc2::cmd::Sequence( + AutoBuilder::resetOdom(m_startingPose), + CommandUtil::commandFromJson(commandJson, choreoAuto)).Unwrap(); + } else { + m_autoCommand = + CommandUtil::commandFromJson(commandJson, choreoAuto).Unwrap(); + } } void PathPlannerAuto::Initialize() { m_autoCommand->Initialize(); + m_timer.Restart(); + + m_isRunning = true; + m_autoLoop->Poll(); } void PathPlannerAuto::Execute() { m_autoCommand->Execute(); + + m_autoLoop->Poll(); } bool PathPlannerAuto::IsFinished() { @@ -72,6 +197,10 @@ bool PathPlannerAuto::IsFinished() { void PathPlannerAuto::End(bool interrupted) { m_autoCommand->End(interrupted); + m_timer.Stop(); + + m_isRunning = false; + m_autoLoop->Poll(); } std::vector> PathPlannerAuto::pathsFromCommandJson( diff --git a/src/main/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/src/main/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index c05ba4b..3a7aecb 100644 --- a/src/main/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/src/main/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -1,6 +1,9 @@ #include "pathplanner/lib/commands/PathfindingCommand.h" #include "pathplanner/lib/pathfinding/Pathfinding.h" #include "pathplanner/lib/util/GeometryUtil.h" +#include "pathplanner/lib/util/FlippingUtil.h" +#include "pathplanner/lib/path/PathPlannerPath.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" #include #include @@ -12,14 +15,13 @@ PathfindingCommand::PathfindingCommand( std::shared_ptr targetPath, PathConstraints constraints, std::function poseSupplier, std::function speedsSupplier, - std::function output, - std::unique_ptr controller, - units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig, - std::function shouldFlipPath, frc2::Requirements requirements) : m_targetPath( - targetPath), m_targetPose(), m_goalEndState(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), m_shouldFlipPath( + std::function)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, + frc2::Requirements requirements) : m_targetPath(targetPath), m_targetPose(), m_goalEndState( + 0_mps, frc::Rotation2d()), m_constraints(constraints), m_poseSupplier( + poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller( + controller), m_robotConfig(robotConfig), m_shouldFlipPath( shouldFlipPath) { AddRequirements(requirements); @@ -29,12 +31,11 @@ PathfindingCommand::PathfindingCommand( units::meters_per_second_t goalEndVel = targetPath->getGlobalConstraints().getMaxVelocity(); if (targetPath->isChoreoPath()) { - // Can call getTrajectory here without proper speeds since it will just return the choreo - // trajectory - PathPlannerTrajectory choreoTraj = targetPath->getTrajectory( - frc::ChassisSpeeds(), frc::Rotation2d()); - targetRotation = choreoTraj.getInitialState().targetHolonomicRotation; - goalEndVel = choreoTraj.getInitialState().velocity; + // Can value() here without issue since all choreo trajectories have ideal trajectories + PathPlannerTrajectory choreoTraj = targetPath->getIdealTrajectory( + m_robotConfig).value(); + targetRotation = choreoTraj.getInitialState().pose.Rotation(); + goalEndVel = choreoTraj.getInitialState().linearVelocity; } else { for (PathPoint p : targetPath->getAllPathPoints()) { if (p.rotationTarget) { @@ -47,7 +48,7 @@ PathfindingCommand::PathfindingCommand( m_targetPose = frc::Pose2d(m_targetPath->getPoint(0).position, targetRotation); m_originalTargetPose = m_targetPose; - m_goalEndState = GoalEndState(goalEndVel, targetRotation, true); + m_goalEndState = GoalEndState(goalEndVel, targetRotation); m_instances++; HAL_Report(HALUsageReporting::kResourceType_PathFindingCommand, @@ -58,15 +59,13 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, PathConstraints constraints, units::meters_per_second_t goalEndVel, std::function poseSupplier, std::function speedsSupplier, - std::function output, - std::unique_ptr controller, - units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig, - frc2::Requirements requirements) : m_targetPath(), m_targetPose( + std::function)> output, + std::shared_ptr controller, + RobotConfig robotConfig, frc2::Requirements requirements) : m_targetPath(), m_targetPose( targetPose), m_originalTargetPose(targetPose), m_goalEndState( - goalEndVel, targetPose.Rotation(), true), m_constraints(constraints), m_poseSupplier( + goalEndVel, targetPose.Rotation()), m_constraints(constraints), m_poseSupplier( poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller( - std::move(controller)), m_rotationDelayDistance(rotationDelayDistance), m_replanningConfig( - replanningConfig), m_shouldFlipPath([]() { + controller), m_robotConfig(robotConfig), m_shouldFlipPath([]() { return false; }) { AddRequirements(requirements); @@ -90,22 +89,24 @@ void PathfindingCommand::Initialize() { m_originalTargetPose = frc::Pose2d(m_targetPath->getPoint(0).position, m_originalTargetPose.Rotation()); if (m_shouldFlipPath()) { - m_targetPose = GeometryUtil::flipFieldPose(m_originalTargetPose); + m_targetPose = FlippingUtil::flipFieldPose(m_originalTargetPose); m_goalEndState = GoalEndState(m_goalEndState.getVelocity(), - m_targetPose.Rotation(), true); + m_targetPose.Rotation()); } } if (currentPose.Translation().Distance(m_targetPose.Translation()) < 0.5_m) { - m_output(frc::ChassisSpeeds()); + std::vector < DriveFeedforward > ff; + for (size_t m = 0; m < m_robotConfig.numModules; m++) { + ff.emplace_back(DriveFeedforward { }); + } + m_output(frc::ChassisSpeeds(), ff); Cancel(); } else { Pathfinding::setStartPosition(currentPose.Translation()); Pathfinding::setGoalPosition(m_targetPose.Translation()); } - - m_startingPose = currentPose; } void PathfindingCommand::Execute() { @@ -117,7 +118,8 @@ void PathfindingCommand::Execute() { bool skipUpdates = !m_currentTrajectory.getStates().empty() && currentPose.Translation().Distance( - m_currentTrajectory.getEndState().position) < 2.0_m; + m_currentTrajectory.getEndState().pose.Translation()) + < 2.0_m; if (!skipUpdates && Pathfinding::isNewPathAvailable()) { m_currentPath = Pathfinding::getCurrentPath(m_constraints, @@ -125,17 +127,17 @@ void PathfindingCommand::Execute() { if (m_currentPath) { m_currentTrajectory = PathPlannerTrajectory(m_currentPath, - currentSpeeds, currentPose.Rotation()); + currentSpeeds, currentPose.Rotation(), m_robotConfig); // Find the two closest states in front of and behind robot size_t closestState1Idx = 0; size_t closestState2Idx = 1; while (closestState2Idx < m_currentTrajectory.getStates().size() - 1) { auto closest2Dist = m_currentTrajectory.getState( - closestState2Idx).position.Distance( + closestState2Idx).pose.Translation().Distance( currentPose.Translation()); auto nextDist = m_currentTrajectory.getState( - closestState2Idx + 1).position.Distance( + closestState2Idx + 1).pose.Translation().Distance( currentPose.Translation()); if (nextDist < closest2Dist) { closestState1Idx++; @@ -150,45 +152,23 @@ void PathfindingCommand::Execute() { auto closestState1 = m_currentTrajectory.getState(closestState1Idx); auto closestState2 = m_currentTrajectory.getState(closestState2Idx); - frc::ChassisSpeeds fieldRelativeSpeeds = - frc::ChassisSpeeds::FromRobotRelativeSpeeds(currentSpeeds, - currentPose.Rotation()); - frc::Rotation2d currentHeading(fieldRelativeSpeeds.vx(), - fieldRelativeSpeeds.vy()); - frc::Rotation2d headingError = currentHeading - - m_currentPath->getStartingDifferentialPose().Rotation(); - bool onHeading = units::math::hypot(currentSpeeds.vx, - currentSpeeds.vy) < 1.0_mps - || units::math::abs(headingError.Degrees()) < 45_deg; - - // Replan the path if our heading is off - if (onHeading || !m_replanningConfig.enableInitialReplanning) { - auto d = closestState1.position.Distance( - closestState2.position); - double t = ((currentPose.Translation().Distance( - closestState1.position)) / d)(); - t = units::math::min(1.0, units::math::max(0.0, t)); - - m_timeOffset = GeometryUtil::unitLerp(closestState1.time, - closestState2.time, t); - - // If the robot is stationary and at the start of the path, set the time offset to the - // next loop - // This can prevent an issue where the robot will remain stationary if new paths come in - // every loop - if (m_timeOffset <= 0.02_s - && units::math::hypot(currentSpeeds.vx, - currentSpeeds.vy) < 0.1_mps) { - m_timeOffset = 0.02_s; - } - } else { - m_currentPath = m_currentPath->replan(currentPose, - currentSpeeds); - - m_currentTrajectory = PathPlannerTrajectory(m_currentPath, - currentSpeeds, currentPose.Rotation()); - - m_timeOffset = 0_s; + auto d = closestState1.pose.Translation().Distance( + closestState2.pose.Translation()); + double t = ((currentPose.Translation().Distance( + closestState1.pose.Translation())) / d)(); + t = units::math::min(1.0, units::math::max(0.0, t)); + + m_timeOffset = GeometryUtil::unitLerp(closestState1.time, + closestState2.time, t); + + // If the robot is stationary and at the start of the path, set the time offset to the + // next loop + // This can prevent an issue where the robot will remain stationary if new paths come in + // every loop + if (m_timeOffset <= 0.02_s + && units::math::hypot(currentSpeeds.vx, currentSpeeds.vy) + < 0.1_mps) { + m_timeOffset = 0.02_s; } PathPlannerLogging::logActivePath (m_currentPath); @@ -200,33 +180,9 @@ void PathfindingCommand::Execute() { } if (m_currentTrajectory.getStates().size() > 0) { - PathPlannerTrajectory::State targetState = m_currentTrajectory.sample( + PathPlannerTrajectoryState targetState = m_currentTrajectory.sample( m_timer.Get() + m_timeOffset); - if (m_replanningConfig.enableDynamicReplanning) { - units::meter_t previousError = units::math::abs( - m_controller->getPositionalError()); - units::meter_t currentError = currentPose.Translation().Distance( - targetState.position); - - if (currentError - >= m_replanningConfig.dynamicReplanningTotalErrorThreshold - || currentError - previousError - >= m_replanningConfig.dynamicReplanningErrorSpikeThreshold) { - replanPath(currentPose, currentSpeeds); - m_timer.Reset(); - m_timeOffset = 0_s; - targetState = m_currentTrajectory.sample(0_s); - } - } - - // Set the target rotation to the starting rotation if we have not yet traveled the rotation - // delay distance - if (currentPose.Translation().Distance(m_startingPose.Translation()) - < m_rotationDelayDistance) { - targetState.targetHolonomicRotation = m_startingPose.Rotation(); - } - frc::ChassisSpeeds targetSpeeds = m_controller->calculateRobotRelativeSpeeds(currentPose, targetState); @@ -237,21 +193,13 @@ void PathfindingCommand::Execute() { PPLibTelemetry::setCurrentPose(currentPose); PathPlannerLogging::logCurrentPose(currentPose); - if (m_controller->isHolonomic()) { - PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose()); - PathPlannerLogging::logTargetPose( - targetState.getTargetHolonomicPose()); - } else { - PPLibTelemetry::setTargetPose(targetState.getDifferentialPose()); - PathPlannerLogging::logTargetPose( - targetState.getDifferentialPose()); - } + PPLibTelemetry::setTargetPose(targetState.pose); + PathPlannerLogging::logTargetPose(targetState.pose); - PPLibTelemetry::setVelocities(currentVel, targetState.velocity, + PPLibTelemetry::setVelocities(currentVel, targetState.linearVelocity, currentSpeeds.omega, targetSpeeds.omega); - PPLibTelemetry::setPathInaccuracy(m_controller->getPositionalError()); - m_output(targetSpeeds); + m_output(targetSpeeds, targetState.feedforwards); } } @@ -283,7 +231,11 @@ void PathfindingCommand::End(bool interrupted) { // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting // the command to smoothly transition into some auto-alignment routine if (!interrupted && m_goalEndState.getVelocity() < 0.1_mps) { - m_output(frc::ChassisSpeeds()); + std::vector < DriveFeedforward > ff; + for (size_t m = 0; m < m_robotConfig.numModules; m++) { + ff.emplace_back(DriveFeedforward { }); + } + m_output(frc::ChassisSpeeds(), ff); } PathPlannerLogging::logActivePath(nullptr); diff --git a/src/main/cpp/pathplanner/lib/config/MotorTorqueCurve.cpp b/src/main/cpp/pathplanner/lib/config/MotorTorqueCurve.cpp deleted file mode 100644 index ac9d964..0000000 --- a/src/main/cpp/pathplanner/lib/config/MotorTorqueCurve.cpp +++ /dev/null @@ -1,330 +0,0 @@ -#include "pathplanner/lib/config/MotorTorqueCurve.h" -#include - -using namespace pathplanner; - -MotorTorqueCurve::MotorTorqueCurve(MotorType motorType, - CurrentLimit currentLimit) { - switch (motorType) { - case krakenX60: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0194 }; - initKrakenX60(currentLimit); - break; - case krakenX60_FOC: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0194 }; - initKrakenX60FOC(currentLimit); - break; - case falcon500: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0182 }; - initFalcon500(currentLimit); - break; - case falcon500_FOC: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0192 }; - initFalcon500FOC(currentLimit); - break; - case neoVortex: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0171 }; - initNEOVortex(currentLimit); - break; - case neo: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0181 }; - initNEO(currentLimit); - break; - case cim: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0184 }; - initCIM(currentLimit); - break; - case miniCim: - m_nmPerAmp = units::newton_meter_per_amp_t { 0.0158 }; - initMiniCIM(currentLimit); - break; - default: - throw std::invalid_argument("Unknown motor type"); - } -} - -MotorTorqueCurve MotorTorqueCurve::fromSettingsString( - std::string torqueCurveName) { - size_t delimIdx = torqueCurveName.find("_"); - - if (delimIdx == std::string::npos) { - throw std::invalid_argument( - "Invalid torque curve name: " + torqueCurveName); - } - - std::string motorTypeStr = torqueCurveName.substr(0, delimIdx); - std::string currentLimitStr = torqueCurveName.substr(delimIdx + 1, - torqueCurveName.length() - (delimIdx + 1)); - - MotorType motorType = motorTypeFromSettingsString(motorTypeStr); - CurrentLimit currentLimit = currentLimitFromSettingsString(currentLimitStr); - - return MotorTorqueCurve(motorType, currentLimit); -} - -MotorType MotorTorqueCurve::motorTypeFromSettingsString(std::string name) { - // There's probably a way to switch on a string but idk - if (name == "KRAKEN") - return MotorType::krakenX60; - if (name == "KRAKENFOC") - return MotorType::krakenX60_FOC; - if (name == "FALCON") - return MotorType::falcon500; - if (name == "FALCONFOC") - return MotorType::falcon500_FOC; - if (name == "VORTEX") - return MotorType::neoVortex; - if (name == "NEO") - return MotorType::neo; - if (name == "CIM") - return MotorType::cim; - if (name == "MINICIM") - return MotorType::miniCim; - - throw std::invalid_argument("Unknown motor type string: " + name); -} - -CurrentLimit MotorTorqueCurve::currentLimitFromSettingsString( - std::string name) { - // There's probably a way to switch on a string but idk - if (name == "40A") - return CurrentLimit::k40A; - if (name == "60A") - return CurrentLimit::k60A; - if (name == "80A") - return CurrentLimit::k80A; - - throw std::invalid_argument("Unknown current limit string: " + name); -} - -void MotorTorqueCurve::initKrakenX60(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.746 }); - insert(units::revolutions_per_minute_t { 5363.0 }, - units::newton_meter_t { 0.746 }); - insert(units::revolutions_per_minute_t { 6000.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.133 }); - insert(units::revolutions_per_minute_t { 5020.0 }, - units::newton_meter_t { 1.133 }); - insert(units::revolutions_per_minute_t { 6000.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.521 }); - insert(units::revolutions_per_minute_t { 4699.0 }, - units::newton_meter_t { 1.521 }); - insert(units::revolutions_per_minute_t { 6000.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initKrakenX60FOC(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.747 }); - insert(units::revolutions_per_minute_t { 5333.0 }, - units::newton_meter_t { 0.747 }); - insert(units::revolutions_per_minute_t { 5800.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.135 }); - insert(units::revolutions_per_minute_t { 5081.0 }, - units::newton_meter_t { 1.135 }); - insert(units::revolutions_per_minute_t { 5800.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.523 }); - insert(units::revolutions_per_minute_t { 4848.0 }, - units::newton_meter_t { 1.523 }); - insert(units::revolutions_per_minute_t { 5800.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initFalcon500(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.703 }); - insert(units::revolutions_per_minute_t { 5412.0 }, - units::newton_meter_t { 0.703 }); - insert(units::revolutions_per_minute_t { 6380.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.068 }); - insert(units::revolutions_per_minute_t { 4920.0 }, - units::newton_meter_t { 1.068 }); - insert(units::revolutions_per_minute_t { 6380.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.433 }); - insert(units::revolutions_per_minute_t { 4407.0 }, - units::newton_meter_t { 1.433 }); - insert(units::revolutions_per_minute_t { 6380.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initFalcon500FOC(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.74 }); - insert(units::revolutions_per_minute_t { 5295.0 }, - units::newton_meter_t { 0.74 }); - insert(units::revolutions_per_minute_t { 6080.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.124 }); - insert(units::revolutions_per_minute_t { 4888.0 }, - units::newton_meter_t { 1.124 }); - insert(units::revolutions_per_minute_t { 6080.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.508 }); - insert(units::revolutions_per_minute_t { 4501.0 }, - units::newton_meter_t { 1.508 }); - insert(units::revolutions_per_minute_t { 6080.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initNEOVortex(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.621 }); - insert(units::revolutions_per_minute_t { 5590.0 }, - units::newton_meter_t { 0.621 }); - insert(units::revolutions_per_minute_t { 6784.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.962 }); - insert(units::revolutions_per_minute_t { 4923.0 }, - units::newton_meter_t { 0.962 }); - insert(units::revolutions_per_minute_t { 6784.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.304 }); - insert(units::revolutions_per_minute_t { 4279.0 }, - units::newton_meter_t { 1.304 }); - insert(units::revolutions_per_minute_t { 6784.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initNEO(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.686 }); - insert(units::revolutions_per_minute_t { 3773.0 }, - units::newton_meter_t { 0.686 }); - insert(units::revolutions_per_minute_t { 5330.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.054 }); - insert(units::revolutions_per_minute_t { 2939.0 }, - units::newton_meter_t { 1.054 }); - insert(units::revolutions_per_minute_t { 5330.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.422 }); - insert(units::revolutions_per_minute_t { 2104.0 }, - units::newton_meter_t { 1.422 }); - insert(units::revolutions_per_minute_t { 5330.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initCIM(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.586 }); - insert(units::revolutions_per_minute_t { 3324.0 }, - units::newton_meter_t { 0.586 }); - insert(units::revolutions_per_minute_t { 5840.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.903 }); - insert(units::revolutions_per_minute_t { 1954.0 }, - units::newton_meter_t { 0.903 }); - insert(units::revolutions_per_minute_t { 5840.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.22 }); - insert(units::revolutions_per_minute_t { 604.0 }, - units::newton_meter_t { 1.22 }); - insert(units::revolutions_per_minute_t { 5840.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} - -void MotorTorqueCurve::initMiniCIM(const CurrentLimit currentLimit) { - switch (currentLimit) { - case k40A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 0.701 }); - insert(units::revolutions_per_minute_t { 4620.0 }, - units::newton_meter_t { 0.701 }); - insert(units::revolutions_per_minute_t { 5880.0 }, - units::newton_meter_t { 0.0 }); - break; - case k60A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.064 }); - insert(units::revolutions_per_minute_t { 3948.0 }, - units::newton_meter_t { 1.064 }); - insert(units::revolutions_per_minute_t { 5880.0 }, - units::newton_meter_t { 0.0 }); - break; - case k80A: - insert(units::revolutions_per_minute_t { 0.0 }, units::newton_meter_t { - 1.426 }); - insert(units::revolutions_per_minute_t { 3297.0 }, - units::newton_meter_t { 1.426 }); - insert(units::revolutions_per_minute_t { 5880.0 }, - units::newton_meter_t { 0.0 }); - break; - } -} diff --git a/src/main/cpp/pathplanner/lib/config/RobotConfig.cpp b/src/main/cpp/pathplanner/lib/config/RobotConfig.cpp index 975234f..733711e 100644 --- a/src/main/cpp/pathplanner/lib/config/RobotConfig.cpp +++ b/src/main/cpp/pathplanner/lib/config/RobotConfig.cpp @@ -1,4 +1,8 @@ #include "pathplanner/lib/config/RobotConfig.h" +#include +#include +#include +#include using namespace pathplanner; @@ -8,35 +12,168 @@ RobotConfig::RobotConfig(units::kilogram_t mass, MOI), moduleConfig(moduleConfig), moduleLocations { frc::Translation2d( wheelbase / 2, trackwidth / 2), frc::Translation2d(wheelbase / 2, -trackwidth / 2), frc::Translation2d(-wheelbase / 2, trackwidth / 2), - frc::Translation2d(-wheelbase / 2, -trackwidth / 2) }, swerveKinematics( + frc::Translation2d(-wheelbase / 2, -trackwidth / 2) }, isHolonomic( + true), numModules(4), modulePivotDistance { moduleLocations[0].Norm(), + moduleLocations[1].Norm(), moduleLocations[2].Norm(), + moduleLocations[3].Norm() }, wheelFrictionForce { moduleConfig.wheelCOF + * ((mass() / numModules) * 9.8) }, maxTorqueFriction( + wheelFrictionForce * moduleConfig.wheelRadius), swerveKinematics( frc::Translation2d(wheelbase / 2, trackwidth / 2), frc::Translation2d(wheelbase / 2, -trackwidth / 2), frc::Translation2d(-wheelbase / 2, trackwidth / 2), frc::Translation2d(-wheelbase / 2, -trackwidth / 2)), diffKinematics( - frc::Translation2d(0_m, trackwidth / 2), - frc::Translation2d(0_m, -trackwidth / 2)), isHolonomic(true), numModules( - 4), modulePivotDistance { moduleLocations[0].Norm(), - moduleLocations[1].Norm(), moduleLocations[2].Norm(), - moduleLocations[3].Norm() }, wheelFrictionForce { moduleConfig.wheelCOF - * (mass() * 9.8) } { + trackwidth) { + for (size_t i = 0; i < numModules; i++) { + frc::Translation2d modPosReciprocal = frc::Translation2d( + units::meter_t { 1.0 / moduleLocations[i].Norm()() }, + moduleLocations[i].Angle()); + swerveForceKinematics.template block<2, 3>(i * 2, 0) << 1, 0, (-modPosReciprocal.Y()).value(), 0, 1, (modPosReciprocal.X()).value(); + } + // No need to set up diff force kinematics, it will not be used } RobotConfig::RobotConfig(units::kilogram_t mass, units::kilogram_square_meter_t MOI, ModuleConfig moduleConfig, units::meter_t trackwidth) : mass(mass), MOI(MOI), moduleConfig( moduleConfig), moduleLocations { frc::Translation2d(0_m, - trackwidth / 2), frc::Translation2d(0_m, -trackwidth / 2) }, swerveKinematics( + trackwidth / 2), frc::Translation2d(0_m, -trackwidth / 2) }, isHolonomic( + false), numModules(2), modulePivotDistance { moduleLocations[0].Norm(), + moduleLocations[1].Norm() }, wheelFrictionForce { moduleConfig.wheelCOF + * ((mass() / numModules) * 9.8) }, maxTorqueFriction( + wheelFrictionForce * moduleConfig.wheelRadius), swerveKinematics( frc::Translation2d(trackwidth / 2, trackwidth / 2), frc::Translation2d(trackwidth / 2, -trackwidth / 2), frc::Translation2d(-trackwidth / 2, trackwidth / 2), frc::Translation2d(-trackwidth / 2, -trackwidth / 2)), diffKinematics( - frc::Translation2d(0_m, trackwidth / 2), - frc::Translation2d(0_m, -trackwidth / 2)), isHolonomic(false), numModules( - 2), modulePivotDistance { moduleLocations[0].Norm(), - moduleLocations[1].Norm() }, wheelFrictionForce { moduleConfig.wheelCOF - * (mass() * 9.8) } { + trackwidth) { + for (size_t i = 0; i < numModules; i++) { + frc::Translation2d modPosReciprocal = frc::Translation2d( + units::meter_t { 1.0 / moduleLocations[i].Norm()() }, + moduleLocations[i].Angle()); + diffForceKinematics.template block<2, 3>(i * 2, 0) << 1, 0, (-modPosReciprocal.Y()).value(), 0, 1, (modPosReciprocal.X()).value(); + } + // No need to set up swerve force kinematics, it will not be used } -// RobotConfig RobotConfig::fromGUISettings() { +RobotConfig RobotConfig::fromGUISettings() { + const std::string filePath = frc::filesystem::GetDeployDirectory() + + "/pathplanner/settings.json"; + + auto fileBuffer = wpi::MemoryBuffer::GetFile(filePath); -// } + if (!fileBuffer) { + throw FRC_MakeError(frc::err::Error, + "PathPlanner settings file could not be read"); + } + + wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); + + bool isHolonomic = json.at("holonomicMode").get(); + units::kilogram_t mass { json.at("robotMass").get() }; + units::kilogram_square_meter_t MOI { json.at("robotMOI").get() }; + units::meter_t wheelbase { json.at("robotWheelbase").get() }; + units::meter_t trackwidth { json.at("robotTrackwidth").get() }; + units::meter_t wheelRadius { json.at("driveWheelRadius").get() }; + double gearing = json.at("driveGearing").get(); + units::meters_per_second_t maxDriveSpeed { json.at("maxDriveSpeed").get< + double>() }; + double wheelCOF = json.at("wheelCOF").get(); + std::string driveMotor = json.at("driveMotorType").get(); + units::ampere_t driveCurrentLimit { + json.at("driveCurrentLimit").get() }; + + int numMotors = isHolonomic ? 1 : 2; + frc::DCMotor gearbox = RobotConfig::getMotorFromSettingsString(driveMotor, + numMotors).WithReduction(gearing); + + ModuleConfig moduleConfig(wheelRadius, maxDriveSpeed, wheelCOF, gearbox, + driveCurrentLimit, numMotors); + + if (isHolonomic) { + return RobotConfig(mass, MOI, moduleConfig, trackwidth, wheelbase); + } else { + return RobotConfig(mass, MOI, moduleConfig, trackwidth); + } +} + +frc::DCMotor RobotConfig::getMotorFromSettingsString(std::string motorStr, + int numMotors) { + if (motorStr == "krakenX60") { + return frc::DCMotor::KrakenX60(numMotors); + } else if (motorStr == "krakenX60FOC") { + return frc::DCMotor::KrakenX60FOC(numMotors); + } else if (motorStr == "falcon500") { + return frc::DCMotor::Falcon500(numMotors); + } else if (motorStr == "falcon500FOC") { + return frc::DCMotor::Falcon500FOC(numMotors); + } else if (motorStr == "vortex") { + return frc::DCMotor::NeoVortex(numMotors); + } else if (motorStr == "NEO") { + return frc::DCMotor::NEO(numMotors); + } else if (motorStr == "CIM") { + return frc::DCMotor::CIM(numMotors); + } else if (motorStr == "miniCIM") { + return frc::DCMotor::MiniCIM(numMotors); + } else { + throw std::invalid_argument("Unknown motor type string: " + motorStr); + } +} + +std::vector RobotConfig::toSwerveModuleStates( + frc::ChassisSpeeds speeds) const { + if (isHolonomic) { + auto states = swerveKinematics.ToSwerveModuleStates(speeds); + return std::vector < frc::SwerveModuleState + > (states.begin(), states.end()); + } else { + auto wheelSpeeds = diffKinematics.ToWheelSpeeds(speeds); + return std::vector { frc::SwerveModuleState { + wheelSpeeds.left, frc::Rotation2d() }, frc::SwerveModuleState { + wheelSpeeds.right, frc::Rotation2d() } }; + } +} + +frc::ChassisSpeeds RobotConfig::toChassisSpeeds( + std::vector states) const { + if (isHolonomic) { + wpi::array < frc::SwerveModuleState, 4 > wpiStates { + frc::SwerveModuleState { states[0].speed, states[0].angle }, + frc::SwerveModuleState { states[1].speed, states[1].angle }, + frc::SwerveModuleState { states[2].speed, states[2].angle }, + frc::SwerveModuleState { states[3].speed, states[3].angle } }; + return swerveKinematics.ToChassisSpeeds(wpiStates); + } else { + frc::DifferentialDriveWheelSpeeds wheelSpeeds { states[0].speed, + states[1].speed }; + return diffKinematics.ToChassisSpeeds(wheelSpeeds); + } +} + +std::vector RobotConfig::chassisForcesToWheelForceVectors( + frc::ChassisSpeeds chassisForces) const { + Eigen::Vector3d chassisForceVector { chassisForces.vx.value(), + chassisForces.vy.value(), chassisForces.omega.value() }; + std::vector < frc::Translation2d > forceVectors; + + if (isHolonomic) { + frc::Matrixd < 4 * 2, 1 > moduleForceMatrix = swerveForceKinematics + * (chassisForceVector / numModules); + for (size_t i = 0; i < numModules; i++) { + units::meter_t x { moduleForceMatrix(i * 2, 0) }; + units::meter_t y { moduleForceMatrix(i * 2 + 1, 0) }; + + forceVectors.emplace_back(x, y); + } + } else { + frc::Matrixd < 2 * 2, 1 > moduleForceMatrix = diffForceKinematics + * (chassisForceVector / numModules); + for (size_t i = 0; i < numModules; i++) { + units::meter_t x { moduleForceMatrix(i * 2, 0) }; + units::meter_t y { moduleForceMatrix(i * 2 + 1, 0) }; + + forceVectors.emplace_back(x, y); + } + } + + return forceVectors; +} diff --git a/src/main/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp b/src/main/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp index 2c51356..4b9b812 100644 --- a/src/main/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp +++ b/src/main/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp @@ -3,17 +3,18 @@ using namespace pathplanner; std::function()> PPHolonomicDriveController::rotationTargetOverride; +std::function PPHolonomicDriveController::xFeedbackOverride; +std::function PPHolonomicDriveController::yFeedbackOverride; +std::function PPHolonomicDriveController::rotationFeedbackOverride; PPHolonomicDriveController::PPHolonomicDriveController( PIDConstants translationConstants, PIDConstants rotationConstants, - units::meters_per_second_t maxModuleSpeed, - units::meter_t driveBaseRadius, units::second_t period) : m_xController( + units::second_t period) : m_xController(translationConstants.kP, + translationConstants.kI, translationConstants.kD, period), m_yController( translationConstants.kP, translationConstants.kI, - translationConstants.kD, period), m_yController(translationConstants.kP, - translationConstants.kI, translationConstants.kD, period), m_rotationController( - rotationConstants.kP, rotationConstants.kI, rotationConstants.kD, { - 0_rad_per_s, 0_rad_per_s_sq }, period), m_maxModuleSpeed( - maxModuleSpeed), m_mpsToRps { 1.0 / driveBaseRadius() } { + translationConstants.kD, period), m_rotationController( + rotationConstants.kP, rotationConstants.kI, rotationConstants.kD, + period) { m_xController.SetIntegratorRange(-translationConstants.iZone, translationConstants.iZone); m_yController.SetIntegratorRange(-translationConstants.iZone, @@ -21,19 +22,17 @@ PPHolonomicDriveController::PPHolonomicDriveController( m_rotationController.SetIntegratorRange(-rotationConstants.iZone, rotationConstants.iZone); - m_rotationController.EnableContinuousInput(units::radian_t { -PI }, - units::radian_t { PI }); + m_rotationController.EnableContinuousInput(-PI, PI); } frc::ChassisSpeeds PPHolonomicDriveController::calculateRobotRelativeSpeeds( const frc::Pose2d ¤tPose, - const PathPlannerTrajectory::State &referenceState) { - units::meters_per_second_t xFF = referenceState.velocity - * referenceState.heading.Cos(); - units::meters_per_second_t yFF = referenceState.velocity - * referenceState.heading.Sin(); + const PathPlannerTrajectoryState &referenceState) { + units::meters_per_second_t xFF = referenceState.fieldSpeeds.vx; + units::meters_per_second_t yFF = referenceState.fieldSpeeds.vy; - m_translationError = currentPose.Translation() - referenceState.position; + m_translationError = currentPose.Translation() + - referenceState.pose.Translation(); if (!m_enabled) { return frc::ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, @@ -41,34 +40,29 @@ frc::ChassisSpeeds PPHolonomicDriveController::calculateRobotRelativeSpeeds( } units::meters_per_second_t xFeedback { m_xController.Calculate( - currentPose.X()(), referenceState.position.X()()) }; + currentPose.X()(), referenceState.pose.X()()) }; units::meters_per_second_t yFeedback { m_yController.Calculate( - currentPose.Y()(), referenceState.position.Y()()) }; + currentPose.Y()(), referenceState.pose.Y()()) }; - units::radians_per_second_t angVelConstraint = - referenceState.constraints.getMaxAngularVelocity(); - units::radians_per_second_t maxAngVel = angVelConstraint; - if (std::isfinite(maxAngVel())) { - // Approximation of available module speed to do rotation with - units::radians_per_second_t maxAngVelModule = units::math::max( - 0_rad_per_s, - (m_maxModuleSpeed - referenceState.velocity) * m_mpsToRps); - maxAngVel = units::math::min(angVelConstraint, maxAngVelModule); - } - - frc::Rotation2d targetRotation = referenceState.targetHolonomicRotation; + frc::Rotation2d targetRotation = referenceState.pose.Rotation(); if (rotationTargetOverride) { targetRotation = rotationTargetOverride().value_or(targetRotation); } units::radians_per_second_t rotationFeedback { - m_rotationController.Calculate(currentPose.Rotation().Radians(), - referenceState.targetHolonomicRotation.Radians(), - { maxAngVel, - referenceState.constraints.getMaxAngularAcceleration() }) }; - units::radians_per_second_t rotationFF = - referenceState.holonomicAngularVelocityRps.value_or( - m_rotationController.GetSetpoint().velocity); + m_rotationController.Calculate(currentPose.Rotation().Radians()(), + targetRotation.Radians()()) }; + units::radians_per_second_t rotationFF = referenceState.fieldSpeeds.omega; + + if (xFeedbackOverride) { + xFeedback = xFeedbackOverride(); + } + if (yFeedbackOverride) { + yFeedback = yFeedbackOverride(); + } + if (rotationFeedbackOverride) { + rotationFeedback = rotationFeedbackOverride(); + } return frc::ChassisSpeeds::FromFieldRelativeSpeeds(xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, diff --git a/src/main/cpp/pathplanner/lib/events/EventScheduler.cpp b/src/main/cpp/pathplanner/lib/events/EventScheduler.cpp new file mode 100644 index 0000000..8d5b24e --- /dev/null +++ b/src/main/cpp/pathplanner/lib/events/EventScheduler.cpp @@ -0,0 +1,77 @@ +#include "pathplanner/lib/events/EventScheduler.h" + +using namespace pathplanner; + +void EventScheduler::execute(units::second_t time) { + // Check for events that should be handled this loop + while (!m_upcomingEvents.empty() + && time >= m_upcomingEvents[0]->getTimestamp()) { + m_upcomingEvents[0]->handleEvent(this); + m_upcomingEvents.pop_front(); + } + + // Run currently running commands + for (auto entry : m_eventCommands) { + if (!entry.second) { + continue; + } + + entry.first->Execute(); + if (entry.first->IsFinished()) { + entry.first->End(false); + entry.second = false; + } + } + + getEventLoop()->Poll(); +} + +void EventScheduler::end() { + // Cancel all currently running commands + for (auto entry : m_eventCommands) { + if (!entry.second) { + continue; + } + + entry.first->End(true); + } + + // Cancel any unhandled events + for (auto e : m_upcomingEvents) { + e->cancelEvent(this); + } + + m_eventCommands.clear(); + m_upcomingEvents.clear(); +} + +void EventScheduler::scheduleCommand(std::shared_ptr command) { + // Check for commands that should be cancelled by this command + auto commandReqs = command->GetRequirements(); + if (!commandReqs.empty()) { + for (auto entry : m_eventCommands) { + if (!entry.second) { + continue; + } + + auto otherReqs = entry.first->GetRequirements(); + for (auto requirement : otherReqs) { + if (commandReqs.find(requirement) != commandReqs.end()) { + cancelCommand(command); + } + } + } + } + + command->Initialize(); + m_eventCommands.emplace_back(command, true); +} + +void EventScheduler::cancelCommand(std::shared_ptr command) { + for (auto entry : m_eventCommands) { + if (entry.first == command && entry.second) { + command->End(true); + entry.second = false; + } + } +} diff --git a/src/main/cpp/pathplanner/lib/path/EventMarker.cpp b/src/main/cpp/pathplanner/lib/path/EventMarker.cpp index 454a0c4..681aacf 100644 --- a/src/main/cpp/pathplanner/lib/path/EventMarker.cpp +++ b/src/main/cpp/pathplanner/lib/path/EventMarker.cpp @@ -4,7 +4,18 @@ using namespace pathplanner; EventMarker EventMarker::fromJson(const wpi::json &json) { + std::string name = json.at("name").get(); double pos = json.at("waypointRelativePos").get(); - return EventMarker(pos, - CommandUtil::commandFromJson(json.at("command"), false)); + double endPos = -1.0; + if (json.contains("endWaypointRelativePos") + && !json.at("endWaypointRelativePos").is_null()) { + endPos = json.at("endWaypointRelativePos").get(); + } + + if (!json.at("command").is_null()) { + return EventMarker(name, pos, endPos, + CommandUtil::commandFromJson(json.at("command"), false)); + } + + return EventMarker(name, pos, endPos, frc2::cmd::None()); } diff --git a/src/main/cpp/pathplanner/lib/path/GoalEndState.cpp b/src/main/cpp/pathplanner/lib/path/GoalEndState.cpp index 772858b..ca6ff37 100644 --- a/src/main/cpp/pathplanner/lib/path/GoalEndState.cpp +++ b/src/main/cpp/pathplanner/lib/path/GoalEndState.cpp @@ -7,10 +7,6 @@ using namespace pathplanner; GoalEndState GoalEndState::fromJson(const wpi::json &json) { auto vel = units::meters_per_second_t(json.at("velocity").get()); auto rotationDeg = units::degree_t(json.at("rotation").get()); - bool rotateFast = false; - if (json.contains("rotateFast")) { - rotateFast = json.at("rotateFast").get(); - } - return GoalEndState(vel, frc::Rotation2d(rotationDeg), rotateFast); + return GoalEndState(vel, frc::Rotation2d(rotationDeg)); } diff --git a/src/main/cpp/pathplanner/lib/path/IdealStartingState.cpp b/src/main/cpp/pathplanner/lib/path/IdealStartingState.cpp new file mode 100644 index 0000000..0825be2 --- /dev/null +++ b/src/main/cpp/pathplanner/lib/path/IdealStartingState.cpp @@ -0,0 +1,12 @@ +#include "pathplanner/lib/path/IdealStartingState.h" +#include +#include + +using namespace pathplanner; + +IdealStartingState IdealStartingState::fromJson(const wpi::json &json) { + auto vel = units::meters_per_second_t(json.at("velocity").get()); + auto rotationDeg = units::degree_t(json.at("rotation").get()); + + return IdealStartingState(vel, frc::Rotation2d(rotationDeg)); +} diff --git a/src/main/cpp/pathplanner/lib/path/PathConstraints.cpp b/src/main/cpp/pathplanner/lib/path/PathConstraints.cpp index f1fea73..2716c31 100644 --- a/src/main/cpp/pathplanner/lib/path/PathConstraints.cpp +++ b/src/main/cpp/pathplanner/lib/path/PathConstraints.cpp @@ -12,6 +12,10 @@ PathConstraints PathConstraints::fromJson(const wpi::json &json) { json.at("maxAngularVelocity").get()); auto maxAngAccel = units::degrees_per_second_squared_t( json.at("maxAngularAcceleration").get()); + auto nominalVoltage = units::volt_t( + json.at("nominalVoltage").get()); + bool unlimited = json.at("unlimited").get(); - return PathConstraints(maxVel, maxAccel, maxAngVel, maxAngAccel); + return PathConstraints(maxVel, maxAccel, maxAngVel, maxAngAccel, + nominalVoltage, unlimited); } diff --git a/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 2bc8d99..6828ce9 100644 --- a/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -2,10 +2,13 @@ #include "pathplanner/lib/util/GeometryUtil.h" #include "pathplanner/lib/util/PPLibTelemetry.h" #include "pathplanner/lib/auto/CommandUtil.h" +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/OneShotTriggerEvent.h" +#include "pathplanner/lib/events/ScheduleCommandEvent.h" +#include "pathplanner/lib/util/FlippingUtil.h" #include #include #include -#include #include #include #include @@ -14,18 +17,29 @@ using namespace pathplanner; int PathPlannerPath::m_instances = 0; -PathPlannerPath::PathPlannerPath(std::vector bezierPoints, +PathPlannerPath::PathPlannerPath(std::vector waypoints, std::vector rotationTargets, + std::vector pointTowardsZones, std::vector constraintZones, std::vector eventMarkers, - PathConstraints globalConstraints, GoalEndState goalEndState, - bool reversed, frc::Rotation2d previewStartingRotation) : m_bezierPoints( - bezierPoints), m_rotationTargets(rotationTargets), m_constraintZones( + PathConstraints globalConstraints, + std::optional idealStartingState, + GoalEndState goalEndState, bool reversed) : m_waypoints(waypoints), m_rotationTargets( + rotationTargets), m_pointTowardsZones(pointTowardsZones), m_constraintZones( constraintZones), m_eventMarkers(eventMarkers), m_globalConstraints( - globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingRotation( - previewStartingRotation), m_isChoreoPath(false), m_choreoTrajectory() { - m_allPoints = PathPlannerPath::createPath(m_bezierPoints, m_rotationTargets, - m_constraintZones); + globalConstraints), m_idealStartingState(idealStartingState), m_goalEndState( + goalEndState), m_reversed(reversed), m_isChoreoPath(false) { + std::sort(m_rotationTargets.begin(), m_rotationTargets.end(), + [](auto &left, auto &right) { + return left.getPosition() < right.getPosition(); + }); + std::sort(m_eventMarkers.begin(), m_eventMarkers.end(), + [](auto &left, auto &right) { + return left.getWaypointRelativePos() + < right.getWaypointRelativePos(); + }); + + m_allPoints = createPath(); precalcValues(); @@ -34,9 +48,9 @@ PathPlannerPath::PathPlannerPath(std::vector bezierPoints, } PathPlannerPath::PathPlannerPath(PathConstraints constraints, - GoalEndState goalEndState) : m_bezierPoints(), m_rotationTargets(), m_constraintZones(), m_eventMarkers(), m_globalConstraints( - constraints), m_goalEndState(goalEndState), m_reversed(false), m_previewStartingRotation(), m_isChoreoPath( - false), m_choreoTrajectory() { + GoalEndState goalEndState) : m_waypoints(), m_rotationTargets(), m_pointTowardsZones(), m_constraintZones(), m_eventMarkers(), m_globalConstraints( + constraints), m_idealStartingState(std::nullopt), m_goalEndState( + goalEndState), m_reversed(false), m_isChoreoPath(false) { m_instances++; HAL_Report(HALUsageReporting::kResourceType_PathPlannerPath, m_instances); } @@ -44,258 +58,292 @@ PathPlannerPath::PathPlannerPath(PathConstraints constraints, void PathPlannerPath::hotReload(const wpi::json &json) { auto updatedPath = PathPlannerPath::fromJson(json); - m_bezierPoints = updatedPath->m_bezierPoints; + m_waypoints = updatedPath->m_waypoints; m_rotationTargets = updatedPath->m_rotationTargets; + m_pointTowardsZones = updatedPath->m_pointTowardsZones; m_constraintZones = updatedPath->m_constraintZones; m_eventMarkers = updatedPath->m_eventMarkers; m_globalConstraints = updatedPath->m_globalConstraints; + m_idealStartingState = updatedPath->m_idealStartingState; m_goalEndState = updatedPath->m_goalEndState; m_reversed = updatedPath->m_reversed; m_allPoints = updatedPath->m_allPoints; - m_previewStartingRotation = updatedPath->m_previewStartingRotation; + + // Clear the ideal trajectory so it gets regenerated + m_idealTrajectory = std::nullopt; } -std::vector PathPlannerPath::bezierFromPoses( +std::vector PathPlannerPath::waypointsFromPoses( std::vector poses) { if (poses.size() < 2) { throw FRC_MakeError(frc::err::InvalidParameter, - "Not enough poses provided to bezierFromPoses"); + "Not enough poses provided to waypointsFromPoses"); } - std::vector < frc::Translation2d > bezierPoints; + std::vector < Waypoint > waypoints; // First pose - bezierPoints.emplace_back(poses[0].Translation()); - bezierPoints.emplace_back( - poses[0].Translation() - + frc::Translation2d( - poses[0].Translation().Distance( - poses[1].Translation()) / 3.0, - poses[0].Rotation())); + waypoints.emplace_back( + Waypoint::autoControlPoints(poses[0].Translation(), + poses[0].Rotation(), std::nullopt, poses[1].Translation())); // Middle poses for (size_t i = 1; i < poses.size() - 1; i++) { - frc::Translation2d anchor = poses[i].Translation(); - - // Prev control - bezierPoints.emplace_back( - anchor - + frc::Translation2d( - anchor.Distance(poses[i - 1].Translation()) - / 3.0, - poses[i].Rotation() - + frc::Rotation2d(180_deg))); - // Anchor - bezierPoints.emplace_back(anchor); - // Next control - bezierPoints.emplace_back( - anchor - + frc::Translation2d( - anchor.Distance(poses[i + 1].Translation()) - / 3.0, poses[i].Rotation())); + waypoints.emplace_back( + Waypoint::autoControlPoints(poses[i].Translation(), + poses[i].Rotation(), poses[i - 1].Translation(), + poses[i + 1].Translation())); } // Last pose - bezierPoints.emplace_back( - poses[poses.size() - 1].Translation() - + frc::Translation2d( - 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; + waypoints.emplace_back( + Waypoint::autoControlPoints(poses[poses.size() - 1].Translation(), + poses[poses.size() - 1].Rotation(), + poses[poses.size() - 2].Translation(), std::nullopt)); + + return waypoints; } std::shared_ptr PathPlannerPath::fromPathFile( std::string pathName) { + if (PathPlannerPath::getPathCache().contains(pathName)) { + return PathPlannerPath::getPathCache()[pathName]; + } + const std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + pathName + ".path"; - wpi::expected, std::error_code> fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); + auto fileBuffer = wpi::MemoryBuffer::GetFile(filePath); - if (!fileBuffer.has_value()) { + if (!fileBuffer) { throw std::runtime_error("Cannot open file: " + filePath); } wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); + std::string version = "1.0"; + if (json.at("version").is_string()) { + version = json.at("version").get(); + } + + if (version != "2025.0") { + throw std::runtime_error( + "Incompatible file version for '" + pathName + + ".path'. Actual: '" + version + + "' Expected: '2025.0'"); + } + std::shared_ptr < PathPlannerPath > path = PathPlannerPath::fromJson(json); + path->name = pathName; PPLibTelemetry::registerHotReloadPath(pathName, path); + + PathPlannerPath::getPathCache().emplace(pathName, path); + return path; } -std::shared_ptr PathPlannerPath::fromChoreoTrajectory( +void PathPlannerPath::loadChoreoTrajectoryIntoCache( std::string trajectoryName) { const std::string filePath = frc::filesystem::GetDeployDirectory() + "/choreo/" + trajectoryName + ".traj"; - wpi::expected, std::error_code>fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); + auto fileBuffer = wpi::MemoryBuffer::GetFile(filePath); - if (!fileBuffer.has_value()) { + if (!fileBuffer) { throw std::runtime_error("Cannot open file: " + filePath); } wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); - std::vector < PathPlannerTrajectory::State > trajStates; - for (wpi::json::const_reference s : json.at("samples")) { - PathPlannerTrajectory::State state; + std::string version = "1.0"; + if (json.at("version").is_string()) { + version = json.at("version").get(); + } + + if (version != "v2025.0.0") { + throw std::runtime_error( + "Incompatible file version for '" + trajectoryName + + ".traj'. Actual: '" + version + + "' Expected: 'v2025.0.0'"); + } + + auto trajJson = json.at("trajectory"); + + std::vector < PathPlannerTrajectoryState > fullTrajStates; + for (wpi::json::const_reference s : trajJson.at("samples")) { + PathPlannerTrajectoryState state; - units::second_t time { s.at("timestamp").get() }; + units::second_t time { s.at("t").get() }; units::meter_t xPos { s.at("x").get() }; units::meter_t yPos { s.at("y").get() }; units::radian_t rotationRad { s.at("heading").get() }; - units::meters_per_second_t xVel { s.at("velocityX").get() }; - units::meters_per_second_t yVel { s.at("velocityY").get() }; - units::radians_per_second_t angularVelRps { s.at("angularVelocity").get< - double>() }; + units::meters_per_second_t xVel { s.at("vx").get() }; + units::meters_per_second_t yVel { s.at("vy").get() }; + units::radians_per_second_t angularVelRps { s.at("omega").get() }; state.time = time; - state.velocity = units::math::hypot(xVel, yVel); - state.acceleration = 0_mps_sq; // Not encoded, not needed anyway - state.headingAngularVelocity = 0_rad_per_s; // Not encoded, only used for diff drive anyway - state.position = frc::Translation2d(xPos, yPos); - state.heading = frc::Rotation2d(xVel(), yVel()); - state.targetHolonomicRotation = frc::Rotation2d(rotationRad); - state.holonomicAngularVelocityRps = angularVelRps; - state.curvature = units::curvature_t { 0.0 }; - state.constraints = PathConstraints(units::meters_per_second_t { - std::numeric_limits::infinity() }, - units::meters_per_second_squared_t { - std::numeric_limits::infinity() }, - units::radians_per_second_t { - std::numeric_limits::infinity() }, - units::radians_per_second_squared_t { - std::numeric_limits::infinity() }); - - trajStates.emplace_back(state); - } + state.linearVelocity = units::math::hypot(xVel, yVel); + state.pose = frc::Pose2d(frc::Translation2d(xPos, yPos), + frc::Rotation2d(rotationRad)); + state.fieldSpeeds = frc::ChassisSpeeds { xVel, yVel, angularVelRps }; - auto path = std::make_shared < PathPlannerPath - > (PathConstraints( - units::meters_per_second_t { - std::numeric_limits::infinity() }, - units::meters_per_second_squared_t { std::numeric_limits< - double>::infinity() }, units::radians_per_second_t { - std::numeric_limits::infinity() }, - units::radians_per_second_squared_t { std::numeric_limits< - double>::infinity() }), GoalEndState( - trajStates[trajStates.size() - 1].velocity, - trajStates[trajStates.size() - 1].targetHolonomicRotation, - true)); - - std::vector < PathPoint > pathPoints; - for (auto state : trajStates) { - pathPoints.emplace_back(state.position); + fullTrajStates.emplace_back(state); } - path->m_allPoints = pathPoints; - path->m_isChoreoPath = true; + std::vector < std::shared_ptr < Event >> fullEvents; + if (json.contains("events")) { + for (wpi::json::const_reference markerJson : json.at("events")) { + std::string name = markerJson.at("name").get(); + + auto fromJson = markerJson.at("from"); + auto fromOffsetJson = markerJson.at("offset"); - std::vector < std::pair> - > eventCommands; - if (json.contains("eventMarkers")) { - for (wpi::json::const_reference m : json.at("eventMarkers")) { - units::second_t timestamp { m.at("timestamp").get() }; + units::second_t fromTargetTimestamp { + fromJson.at("targetTimestamp").get() }; + units::second_t fromOffset { fromOffsetJson.at("val").get() }; + units::second_t fromTimestamp = fromTargetTimestamp + fromOffset; - EventMarker eventMarker = EventMarker(timestamp(), - CommandUtil::commandFromJson(m.at("command"), false)); + fullEvents.emplace_back( + std::make_shared < OneShotTriggerEvent + > (fromTimestamp, name)); - path->m_eventMarkers.emplace_back(eventMarker); - eventCommands.emplace_back(timestamp, eventMarker.getCommand()); + frc2::CommandPtr eventCommand = frc2::cmd::None(); + if (!markerJson.at("event").is_null()) { + eventCommand = CommandUtil::commandFromJson( + markerJson.at("event"), true); + } + fullEvents.emplace_back( + std::make_shared < ScheduleCommandEvent + > (fromTimestamp, std::shared_ptr < frc2::Command + > (std::move(eventCommand).Unwrap()))); } } - - std::sort(eventCommands.begin(), eventCommands.end(), + std::sort(fullEvents.begin(), fullEvents.end(), [](auto &left, auto &right) { - return left.first < right.first; + return left->getTimestamp() < right->getTimestamp(); }); - path->m_choreoTrajectory = PathPlannerTrajectory(trajStates, eventCommands); + // Add the full path to the cache + auto fullPath = std::make_shared < PathPlannerPath + > (PathConstraints::unlimitedConstraints(12_V), GoalEndState( + fullTrajStates[fullTrajStates.size() - 1].linearVelocity, + fullTrajStates[fullTrajStates.size() - 1].pose.Rotation())); + fullPath->m_idealStartingState = IdealStartingState( + units::math::hypot(fullTrajStates[0].fieldSpeeds.vx, + fullTrajStates[0].fieldSpeeds.vy), + fullTrajStates[0].pose.Rotation()); + + std::vector < PathPoint > fullPathPoints; + for (auto state : fullTrajStates) { + fullPathPoints.emplace_back(state.pose.Translation()); + } + + fullPath->m_allPoints = fullPathPoints; + fullPath->m_isChoreoPath = true; + fullPath->m_idealTrajectory = PathPlannerTrajectory(fullTrajStates, + fullEvents); + fullPath->name = trajectoryName; + PathPlannerPath::getChoreoPathCache().emplace(trajectoryName, fullPath); + + auto splitsJson = trajJson.at("splits"); + std::vector < size_t > splits; + + if (splitsJson.is_array()) { + for (auto split : splitsJson) { + splits.emplace_back(split.get()); + } + } - return path; + if (splits.empty() || splits[0] != 0) { + splits.insert(splits.begin(), 0); + } + + for (size_t i = 0; i < splits.size(); i++) { + std::string name = trajectoryName + "." + std::to_string(i); + std::vector < PathPlannerTrajectoryState > states; + + size_t splitStartIdx = splits[i]; + + size_t splitEndIdx = fullTrajStates.size(); + if (i < splits.size()) { + splitEndIdx = splits[i + 1]; + } + + auto startTime = fullTrajStates[splitStartIdx].time; + auto endTime = fullTrajStates[splitEndIdx - 1].time; + for (size_t s = splitStartIdx; s < splitEndIdx; s++) { + states.emplace_back( + fullTrajStates[s].copyWithTime( + fullTrajStates[s].time - startTime)); + } + + std::vector < std::shared_ptr < Event >> events; + for (auto originalEvent : fullEvents) { + if (originalEvent->getTimestamp() >= startTime + && originalEvent->getTimestamp() < endTime) { + events.emplace_back( + originalEvent->copyWithTimestamp( + originalEvent->getTimestamp() - startTime)); + } + } + + auto path = std::make_shared < PathPlannerPath + > (PathConstraints::unlimitedConstraints(12_V), GoalEndState( + states[states.size() - 1].linearVelocity, + states[states.size() - 1].pose.Rotation())); + path->m_idealStartingState = IdealStartingState( + units::math::hypot(states[0].fieldSpeeds.vx, + states[0].fieldSpeeds.vy), states[0].pose.Rotation()); + + std::vector < PathPoint > pathPoints; + for (auto state : states) { + pathPoints.emplace_back(state.pose.Translation()); + } + + path->m_allPoints = pathPoints; + path->m_isChoreoPath = true; + path->m_idealTrajectory = PathPlannerTrajectory(states, events); + path->name = name; + PathPlannerPath::getChoreoPathCache().emplace(name, path); + } } std::shared_ptr PathPlannerPath::fromJson( const wpi::json &json) { - std::vector < frc::Translation2d > bezierPoints = - PathPlannerPath::bezierPointsFromWaypointsJson( - json.at("waypoints")); + std::vector < Waypoint > waypoints = PathPlannerPath::waypointsFromJson( + json.at("waypoints")); PathConstraints globalConstraints = PathConstraints::fromJson( json.at("globalConstraints")); GoalEndState goalEndState = GoalEndState::fromJson(json.at("goalEndState")); + IdealStartingState idealStartingState = IdealStartingState::fromJson( + json.at("idealStartingState")); bool reversed = json.at("reversed").get(); std::vector < RotationTarget > rotationTargets; + std::vector < PointTowardsZone > pointTowardsZones; std::vector < ConstraintsZone > constraintZones; std::vector < EventMarker > eventMarkers; for (wpi::json::const_reference rotJson : json.at("rotationTargets")) { - rotationTargets.push_back(RotationTarget::fromJson(rotJson)); + rotationTargets.emplace_back(RotationTarget::fromJson(rotJson)); } - for (wpi::json::const_reference zoneJson : json.at("constraintZones")) { - constraintZones.push_back(ConstraintsZone::fromJson(zoneJson)); + if (json.contains("pointTowardsZones")) { + for (wpi::json::const_reference zoneJson : json.at("pointTowardsZones")) { + pointTowardsZones.emplace_back( + PointTowardsZone::fromJson(zoneJson)); + } } - for (wpi::json::const_reference markerJson : json.at("eventMarkers")) { - eventMarkers.push_back(EventMarker::fromJson(markerJson)); + for (wpi::json::const_reference zoneJson : json.at("constraintZones")) { + constraintZones.emplace_back(ConstraintsZone::fromJson(zoneJson)); } - frc::Rotation2d previewStartingRotation; - if (json.contains("previewStartingState") - && !json.at("previewStartingState").is_null()) { - auto jsonStartingState = json.at("previewStartingState"); - previewStartingRotation = frc::Rotation2d( - units::degree_t( - jsonStartingState.at("rotation").get())); + for (wpi::json::const_reference markerJson : json.at("eventMarkers")) { + eventMarkers.emplace_back(EventMarker::fromJson(markerJson)); } return std::make_shared < PathPlannerPath - > (bezierPoints, rotationTargets, constraintZones, eventMarkers, globalConstraints, goalEndState, reversed, previewStartingRotation); -} - -std::vector PathPlannerPath::bezierPointsFromWaypointsJson( - const wpi::json &json) { - std::vector < frc::Translation2d > bezierPoints; - - // First point - wpi::json::const_reference firstPoint = json[0]; - bezierPoints.push_back( - PathPlannerPath::pointFromJson(firstPoint.at("anchor"))); - bezierPoints.push_back( - PathPlannerPath::pointFromJson(firstPoint.at("nextControl"))); - - // Mid points - for (size_t i = 1; i < json.size() - 1; i++) { - wpi::json::const_reference point = json[i]; - bezierPoints.push_back( - PathPlannerPath::pointFromJson(point.at("prevControl"))); - bezierPoints.push_back( - PathPlannerPath::pointFromJson(point.at("anchor"))); - bezierPoints.push_back( - PathPlannerPath::pointFromJson(point.at("nextControl"))); - } - - // Last point - wpi::json::const_reference lastPoint = json[json.size() - 1]; - bezierPoints.push_back( - PathPlannerPath::pointFromJson(lastPoint.at("prevControl"))); - bezierPoints.push_back( - PathPlannerPath::pointFromJson(lastPoint.at("anchor"))); - - return bezierPoints; -} - -frc::Translation2d PathPlannerPath::pointFromJson(const wpi::json &json) { - auto x = units::meter_t(json.at("x").get()); - auto y = units::meter_t(json.at("y").get()); - - return frc::Translation2d(x, y); + > (waypoints, rotationTargets, pointTowardsZones, constraintZones, eventMarkers, globalConstraints, idealStartingState, goalEndState, reversed); } std::shared_ptr PathPlannerPath::fromPathPoints( @@ -310,43 +358,229 @@ std::shared_ptr PathPlannerPath::fromPathPoints( return path; } -std::vector PathPlannerPath::createPath( - std::vector bezierPoints, - std::vector holonomicRotations, - std::vector constraintZones) { - if (bezierPoints.size() < 4) { - throw std::runtime_error( - "Failed to create path, not enough bezier points"); +std::vector PathPlannerPath::createPath() { + if (m_waypoints.size() < 2) { + throw std::runtime_error("A path must have at least 2 waypoints"); } + std::vector < RotationTarget > unaddedTargets; + unaddedTargets.insert(unaddedTargets.begin(), m_rotationTargets.begin(), + m_rotationTargets.end()); std::vector < PathPoint > points; + size_t numSegments = m_waypoints.size() - 1; + + // Add the first path point + points.emplace_back(samplePath(0.0), std::nullopt, + constraintsForWaypointPos(0.0)); + points[0].waypointRelativePos = 0.0; - size_t numSegments = (bezierPoints.size() - 1) / 3; - for (size_t s = 0; s < numSegments; s++) { - size_t iOffset = s * 3; - frc::Translation2d p1 = bezierPoints[iOffset]; - frc::Translation2d p2 = bezierPoints[iOffset + 1]; - frc::Translation2d p3 = bezierPoints[iOffset + 2]; - frc::Translation2d p4 = bezierPoints[iOffset + 3]; - - std::vector < RotationTarget > segmentRotations; - for (RotationTarget t : holonomicRotations) { - if (t.getPosition() >= s && t.getPosition() <= s + 1) { - segmentRotations.push_back(t.forSegmentIndex(s)); + double pos = targetIncrement; + + while (pos < numSegments) { + frc::Translation2d position = samplePath(pos); + + units::meter_t distance = points[points.size() - 1].position.Distance( + position); + if (distance <= 0.01_m) { + pos = std::min(pos + targetIncrement, + static_cast(numSegments)); + continue; + } + + double prevWaypointPos = pos - targetIncrement; + + units::meter_t delta = distance - targetSpacing; + if (delta > targetSpacing * 0.25) { + // Points are too far apart, increment t by correct amount + double correctIncrement = (targetSpacing * targetIncrement) + / distance; + pos = pos - targetIncrement + correctIncrement; + + position = samplePath(pos); + + if (points[points.size() - 1].position.Distance(position) + - targetSpacing > targetSpacing * 0.25) { + // Points are still too far apart. Probably because of weird control + // point placement. Just cut the correct increment in half and hope for the best + pos = pos - (correctIncrement * 0.5); + position = samplePath(pos); + } + } else if (delta < -targetSpacing * 0.25) { + // Points are too close, increment waypoint relative pos by correct amount + double correctIncrement = (targetSpacing * targetIncrement) + / distance; + pos = pos - targetIncrement + correctIncrement; + + position = samplePath(pos); + + if (points[points.size() - 1].position.Distance(position) + - targetSpacing < -targetSpacing * 0.25) { + // Points are still too close. Probably because of weird control + // point placement. Just cut the correct increment in half and hope for the best + pos = pos + (correctIncrement * 0.5); + position = samplePath(pos); } } - std::vector < ConstraintsZone > segmentZones; - for (ConstraintsZone z : constraintZones) { - if (z.overlapsRange(s, s + 1)) { - segmentZones.push_back(z.forSegmentIndex(s)); + // Add rotation targets + std::optional < RotationTarget > target = std::nullopt; + PathPoint prevPoint = points[points.size() - 1]; + + while (!unaddedTargets.empty() + && unaddedTargets[0].getPosition() >= prevWaypointPos + && unaddedTargets[0].getPosition() <= pos) { + if (std::abs(unaddedTargets[0].getPosition() - prevWaypointPos) + < 0.001) { + // Close enough to prev pos + prevPoint.rotationTarget = unaddedTargets[0]; + unaddedTargets.erase(unaddedTargets.begin()); + } else if (std::abs(unaddedTargets[0].getPosition() - pos) + < 0.001) { + // Close enough to next pos + target = unaddedTargets[0]; + unaddedTargets.erase(unaddedTargets.begin()); + } else { + // We should insert a point at the exact position + RotationTarget t = unaddedTargets[0]; + unaddedTargets.erase(unaddedTargets.begin()); + points.emplace_back(samplePath(t.getPosition()), t, + constraintsForWaypointPos(t.getPosition())); + points[points.size() - 1].waypointRelativePos = t.getPosition(); } } - PathSegment segment(p1, p2, p3, p4, segmentRotations, segmentZones, - s == numSegments - 1); - auto segmentPoints = segment.getSegmentPoints(); - points.insert(points.end(), segmentPoints.begin(), segmentPoints.end()); + points.emplace_back(position, target, constraintsForWaypointPos(pos)); + points[points.size() - 1].waypointRelativePos = pos; + pos = std::min(pos + targetIncrement, static_cast(numSegments)); + } + + // Keep trying to add the end point until its close enough to the prev point + double trueIncrement = numSegments - (pos - targetIncrement); + pos = numSegments; + bool invalid = true; + while (invalid) { + frc::Translation2d position = samplePath(pos); + + units::meter_t distance = points[points.size() - 1].position.Distance( + position); + if (distance <= 0.01_m) { + invalid = false; + break; + } + + double prevPos = pos - trueIncrement; + + units::meter_t delta = distance - targetSpacing; + if (delta > targetSpacing * 0.25) { + // Points are too far apart, increment waypoint relative pos by correct amount + double correctIncrement = (targetSpacing * trueIncrement) + / distance; + pos = pos - trueIncrement + correctIncrement; + trueIncrement = correctIncrement; + + position = samplePath(pos); + + if (points[points.size() - 1].position.Distance(position) + - targetSpacing > targetSpacing * 0.25) { + // Points are still too far apart. Probably because of weird control + // point placement. Just cut the correct increment in half and hope for the best + pos = pos - (correctIncrement * 0.5); + trueIncrement = correctIncrement * 0.5; + position = samplePath(pos); + } + } else { + invalid = false; + } + + // Add a rotation target to the previous point if it is closer to it than + // the current point + if (!unaddedTargets.empty()) { + if (std::abs(unaddedTargets[0].getPosition() - prevPos) + <= std::abs(unaddedTargets[0].getPosition() - pos)) { + points[points.size() - 1].rotationTarget = unaddedTargets[0]; + unaddedTargets.erase(unaddedTargets.begin()); + } + } + + points.emplace_back(position, std::nullopt, + constraintsForWaypointPos(pos)); + points[points.size() - 1].waypointRelativePos = pos; + pos = numSegments; + } + + for (size_t i = 1; i < points.size() - 1; i++) { + // Set the rotation target for point towards zones + auto pointZone = pointZoneForWaypointPos(points[i].waypointRelativePos); + if (pointZone.has_value()) { + PointTowardsZone zone = pointZone.value(); + frc::Rotation2d angleToTarget = (zone.getTargetPosition() + - points[i].position).Angle(); + frc::Rotation2d rotation = angleToTarget + zone.getRotationOffset(); + points[i].rotationTarget = RotationTarget( + points[i].waypointRelativePos, rotation); + } + + units::meter_t curveRadius = GeometryUtil::calculateRadius( + points[i - 1].position, points[i].position, + points[i + 1].position); + + if (!GeometryUtil::isFinite(curveRadius)) { + continue; + } + + if (units::math::abs(curveRadius) < 0.25_m) { + // Curve radius is too tight for default spacing, insert 4 more points + double before1WaypointPos = GeometryUtil::doubleLerp( + points[i - 1].waypointRelativePos, + points[i].waypointRelativePos, 0.33); + double before2WaypointPos = GeometryUtil::doubleLerp( + points[i - 1].waypointRelativePos, + points[i].waypointRelativePos, 0.67); + double after1WaypointPos = GeometryUtil::doubleLerp( + points[i].waypointRelativePos, + points[i + 1].waypointRelativePos, 0.33); + double after2WaypointPos = GeometryUtil::doubleLerp( + points[i].waypointRelativePos, + points[i + 1].waypointRelativePos, 0.67); + + PathPoint before1(samplePath(before1WaypointPos), std::nullopt, + points[i].constraints); + before1.waypointRelativePos = before1WaypointPos; + PathPoint before2(samplePath(before2WaypointPos), std::nullopt, + points[i].constraints); + before2.waypointRelativePos = before2WaypointPos; + PathPoint after1(samplePath(after1WaypointPos), std::nullopt, + points[i].constraints); + after1.waypointRelativePos = after1WaypointPos; + PathPoint after2(samplePath(after2WaypointPos), std::nullopt, + points[i].constraints); + after2.waypointRelativePos = after2WaypointPos; + + points.insert(points.begin() + i, before2); + points.insert(points.begin() + i, before1); + points.insert(points.begin() + (i + 3), after2); + points.insert(points.begin() + (i + 3), after1); + i += 4; + } else if (units::math::abs(curveRadius) < 0.5_m) { + // Curve radius is too tight for default spacing, insert 2 more points + double beforeWaypointPos = GeometryUtil::doubleLerp( + points[i - 1].waypointRelativePos, + points[i].waypointRelativePos, 0.5); + double afterWaypointPos = GeometryUtil::doubleLerp( + points[i].waypointRelativePos, + points[i + 1].waypointRelativePos, 0.5); + + PathPoint before(samplePath(beforeWaypointPos), std::nullopt, + points[i].constraints); + before.waypointRelativePos = beforeWaypointPos; + PathPoint after(samplePath(afterWaypointPos), std::nullopt, + points[i].constraints); + after.waypointRelativePos = afterWaypointPos; + + points.insert(points.begin() + i, before); + points.insert(points.begin() + (i + 2), after); + i += 2; + } } return points; @@ -354,8 +588,7 @@ std::vector PathPlannerPath::createPath( frc::Pose2d PathPlannerPath::getStartingDifferentialPose() { frc::Translation2d startPos = getPoint(0).position; - frc::Rotation2d heading = - (getPoint(1).position - getPoint(0).position).Angle(); + frc::Rotation2d heading = getInitialHeading(); if (m_reversed) { heading = frc::Rotation2d( @@ -366,8 +599,25 @@ frc::Pose2d PathPlannerPath::getStartingDifferentialPose() { return frc::Pose2d(startPos, heading); } -frc::Pose2d PathPlannerPath::getPreviewStartingHolonomicPose() { - return frc::Pose2d(getPoint(0).position, m_previewStartingRotation); +std::optional PathPlannerPath::getIdealTrajectory( + RobotConfig robotConfig) { + if (!m_idealTrajectory.has_value() && m_idealStartingState.has_value()) { + // The ideal starting state is known, generate the ideal trajectory + frc::Rotation2d heading = getInitialHeading(); + frc::Translation2d fieldSpeeds( + units::meter_t { m_idealStartingState.value().getVelocity()() }, + heading); + frc::ChassisSpeeds startingSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(frc::ChassisSpeeds { + units::meters_per_second_t { fieldSpeeds.X()() }, + units::meters_per_second_t { fieldSpeeds.Y()() }, + 0.0_rad_per_s }, + m_idealStartingState.value().getRotation()); + m_idealTrajectory = generateTrajectory(startingSpeeds, + m_idealStartingState.value().getRotation(), robotConfig); + } + + return m_idealTrajectory; } void PathPlannerPath::precalcValues() { @@ -375,18 +625,14 @@ void PathPlannerPath::precalcValues() { for (size_t i = 0; i < m_allPoints.size(); i++) { PathConstraints constraints = m_allPoints[i].constraints.value_or( m_globalConstraints); - if (!m_allPoints[i].constraints) { - m_allPoints[i].constraints = m_globalConstraints; - } - m_allPoints[i].curveRadius = units::math::abs( + units::meter_t curveRadius = units::math::abs( getCurveRadiusAtPoint(i, m_allPoints)); - if (GeometryUtil::isFinite(m_allPoints[i].curveRadius)) { + if (GeometryUtil::isFinite(curveRadius)) { m_allPoints[i].maxV = units::math::min( units::math::sqrt( constraints.getMaxAcceleration() - * units::math::abs( - m_allPoints[i].curveRadius)), + * units::math::abs(curveRadius)), constraints.getMaxVelocity()); } else { m_allPoints[i].maxV = constraints.getMaxVelocity(); @@ -401,8 +647,7 @@ void PathPlannerPath::precalcValues() { } m_allPoints[m_allPoints.size() - 1].rotationTarget = RotationTarget(-1, - m_goalEndState.getRotation(), - m_goalEndState.shouldRotateFast()); + m_goalEndState.getRotation()); m_allPoints[m_allPoints.size() - 1].maxV = m_goalEndState.getVelocity(); } } @@ -425,380 +670,112 @@ units::meter_t PathPlannerPath::getCurveRadiusAtPoint(size_t index, } } -std::shared_ptr PathPlannerPath::replan( - const frc::Pose2d startingPose, - const frc::ChassisSpeeds currentSpeeds) { - if (m_isChoreoPath) { - // This path is from choreo, cannot be replanned - return shared_from_this(); +std::shared_ptr PathPlannerPath::flipPath() { + std::optional < PathPlannerTrajectory > flippedTraj = std::nullopt; + if (m_idealTrajectory.has_value()) { + // Flip the ideal trajectory + flippedTraj = m_idealTrajectory.value().flip(); } - frc::ChassisSpeeds currentFieldRelativeSpeeds = - frc::ChassisSpeeds::FromFieldRelativeSpeeds(currentSpeeds, - -startingPose.Rotation()); - - std::optional < frc::Translation2d > robotNextControl = std::nullopt; - units::meters_per_second_t linearVel = units::math::hypot( - currentFieldRelativeSpeeds.vx, currentFieldRelativeSpeeds.vy); - if (linearVel > 0.1_mps) { - units::meter_t stoppingDistance = units::math::pow < 2 - > (linearVel) / (2 * m_globalConstraints.getMaxAcceleration()); - - frc::Rotation2d heading(currentFieldRelativeSpeeds.vx(), - currentFieldRelativeSpeeds.vy()); - robotNextControl = startingPose.Translation() - + frc::Translation2d(stoppingDistance / 2.0, heading); + std::vector < Waypoint > newWaypoints; + for (auto w : m_waypoints) { + newWaypoints.emplace_back(w.flip()); } - size_t closestPointIdx = 0; - frc::Translation2d comparePoint = robotNextControl.value_or( - startingPose.Translation()); - units::meter_t closestDist = positionDelta(comparePoint, - getPoint(closestPointIdx).position); - - for (size_t i = 1; i < numPoints(); i++) { - units::meter_t d = positionDelta(comparePoint, getPoint(i).position); - - if (d < closestDist) { - closestPointIdx = i; - closestDist = d; - } + std::vector < RotationTarget > newRotTargets; + for (auto t : m_rotationTargets) { + newRotTargets.emplace_back(t.getPosition(), + FlippingUtil::flipFieldRotation(t.getTarget())); } - if (closestPointIdx == numPoints() - 1) { - frc::Rotation2d heading = (getPoint(numPoints() - 1).position - - comparePoint).Angle(); - - if (!robotNextControl) { - robotNextControl = startingPose.Translation() - + frc::Translation2d(closestDist / 3.0, heading); - } - - frc::Rotation2d endPrevControlHeading = - (getPoint(numPoints() - 1).position - robotNextControl.value()).Angle(); - - frc::Translation2d endPrevControl = getPoint(numPoints() - 1).position - - frc::Translation2d(closestDist / 3.0, endPrevControlHeading); - - // Throw out rotation targets, event markers, and constraint zones since we are skipping all - // of the path - return std::make_shared < PathPlannerPath - > (std::vector < frc::Translation2d > ( { - startingPose.Translation(), - robotNextControl.value(), - endPrevControl, - getPoint(numPoints() - 1).position - }), std::vector(), std::vector< - ConstraintsZone>(), std::vector(), m_globalConstraints, m_goalEndState, m_reversed); - } else if ((closestPointIdx == 0 && !robotNextControl) - || (units::math::abs( - closestDist - - startingPose.Translation().Distance( - getPoint(0).position)) <= 0.25_m - && linearVel < 0.1_mps)) { - units::meter_t distToStart = startingPose.Translation().Distance( - getPoint(0).position); - - frc::Rotation2d heading = (getPoint(0).position - - startingPose.Translation()).Angle(); - robotNextControl = startingPose.Translation() - + frc::Translation2d(distToStart / 3.0, heading); - - frc::Rotation2d joinHeading = (m_allPoints[0].position - - m_allPoints[1].position).Angle(); - frc::Translation2d joinPrevControl = getPoint(0).position - + frc::Translation2d(distToStart / 2.0, joinHeading); - - if (m_bezierPoints.empty()) { - // We don't have any bezier points to reference - PathSegment joinSegment(startingPose.Translation(), - robotNextControl.value(), joinPrevControl, - m_allPoints[0].position, false); - std::vector < PathPoint > replannedPoints; - auto joinPoints = joinSegment.getSegmentPoints(); - replannedPoints.insert(replannedPoints.end(), joinPoints.begin(), - joinPoints.end()); - replannedPoints.insert(replannedPoints.end(), m_allPoints.begin(), - m_allPoints.end()); - - return PathPlannerPath::fromPathPoints(replannedPoints, - m_globalConstraints, m_goalEndState); - } else { - // We can use the bezier points - std::vector < frc::Translation2d - > replannedBezier( { startingPose.Translation(), - robotNextControl.value(), joinPrevControl }); - replannedBezier.insert(replannedBezier.end(), - m_bezierPoints.begin(), m_bezierPoints.end()); - - // keep all rotations, markers, and zones and increment waypoint pos by 1 - std::vector < RotationTarget > targets; - std::transform(m_rotationTargets.begin(), m_rotationTargets.end(), - std::back_inserter(targets), - [](RotationTarget target) { - return RotationTarget(target.getPosition() + 1, - target.getTarget(), target.shouldRotateFast()); - }); - std::vector < ConstraintsZone > zones; - std::transform(m_constraintZones.begin(), m_constraintZones.end(), - std::back_inserter(zones), - [](ConstraintsZone zone) { - return ConstraintsZone( - zone.getMinWaypointRelativePos() + 1, - zone.getMaxWaypointRelativePos() + 1, - zone.getConstraints()); - }); - std::vector < EventMarker > markers; - std::transform(m_eventMarkers.begin(), m_eventMarkers.end(), - std::back_inserter(markers), - [](EventMarker marker) { - return EventMarker(marker.getWaypointRelativePos() + 1, - marker.getCommand()); - }); - - return std::make_shared < PathPlannerPath - > (replannedBezier, targets, zones, markers, m_globalConstraints, m_goalEndState, m_reversed); - } + std::vector < PointTowardsZone > newPointZones; + for (auto z : m_pointTowardsZones) { + newPointZones.emplace_back(z.flip()); } - size_t joinAnchorIdx = numPoints() - 1; - for (size_t i = closestPointIdx; i < numPoints(); i++) { - if (getPoint(i).distanceAlongPath - >= getPoint(closestPointIdx).distanceAlongPath + closestDist) { - joinAnchorIdx = i; - break; - } + std::vector < PathPoint > newPoints; + for (auto p : m_allPoints) { + newPoints.emplace_back(p.flip()); } - frc::Translation2d joinPrevControl = getPoint(closestPointIdx).position; - frc::Translation2d joinAnchor = getPoint(joinAnchorIdx).position; - - if (!robotNextControl) { - units::meter_t robotToJoinDelta = startingPose.Translation().Distance( - joinAnchor); - frc::Rotation2d heading = - (joinPrevControl - startingPose.Translation()).Angle(); - robotNextControl = startingPose.Translation() - + frc::Translation2d(robotToJoinDelta / 3.0, heading); - } - - if (joinAnchorIdx == numPoints() - 1) { - // Throw out rotation targets, event markers, and constraint zones since we are skipping all - // of the path - return std::make_shared < PathPlannerPath - > (std::vector < frc::Translation2d > ( { - startingPose.Translation(), - robotNextControl.value(), - joinPrevControl, - joinAnchor - }), std::vector(), std::vector< - ConstraintsZone>(), std::vector(), m_globalConstraints, m_goalEndState, m_reversed); - } - - if (m_bezierPoints.empty()) { - // We don't have any bezier points to reference - PathSegment joinSegment(startingPose.Translation(), - robotNextControl.value(), joinPrevControl, joinAnchor, false); - std::vector < PathPoint > replannedPoints; - auto joinPoints = joinSegment.getSegmentPoints(); - replannedPoints.insert(replannedPoints.end(), joinPoints.begin(), - joinPoints.end()); - replannedPoints.insert(replannedPoints.end(), - m_allPoints.begin() + joinAnchorIdx, m_allPoints.end()); - - return PathPlannerPath::fromPathPoints(replannedPoints, - m_globalConstraints, m_goalEndState); - } - - // We can reference bezier points - size_t nextWaypointIdx = static_cast(std::ceil( - (joinAnchorIdx + 1) * PathSegment::RESOLUTION)); - size_t bezierPointIdx = nextWaypointIdx * 3; - units::meter_t waypointDelta = joinAnchor.Distance( - m_bezierPoints[bezierPointIdx]); - - frc::Rotation2d joinHeading = (joinAnchor - joinPrevControl).Angle(); - frc::Translation2d joinNextControl = joinAnchor - + frc::Translation2d(waypointDelta / 3.0, joinHeading); - - frc::Rotation2d nextWaypointHeading; - if (bezierPointIdx == m_bezierPoints.size() - 1) { - nextWaypointHeading = (m_bezierPoints[bezierPointIdx - 1] - - m_bezierPoints[bezierPointIdx]).Angle(); - } else { - nextWaypointHeading = (m_bezierPoints[bezierPointIdx] - - m_bezierPoints[bezierPointIdx + 1]).Angle(); - } - - frc::Translation2d nextWaypointPrevControl = m_bezierPoints[bezierPointIdx] - + frc::Translation2d(units::math::max(waypointDelta / 3.0, 0.15_m), - nextWaypointHeading); - - std::vector < frc::Translation2d - > replannedBezier( - { startingPose.Translation(), robotNextControl.value(), - joinPrevControl, joinAnchor, joinNextControl, - nextWaypointPrevControl }); - replannedBezier.insert(replannedBezier.end(), - m_bezierPoints.begin() + bezierPointIdx, m_bezierPoints.end()); - - units::meter_t segment1Length = 0_m; - frc::Translation2d lastSegment1Pos = startingPose.Translation(); - units::meter_t segment2Length = 0_m; - frc::Translation2d lastSegment2Pos = joinAnchor; - - for (double t = PathSegment::RESOLUTION; t < 1.0; t += - PathSegment::RESOLUTION) { - frc::Translation2d p1 = GeometryUtil::cubicLerp( - startingPose.Translation(), robotNextControl.value(), - joinPrevControl, joinAnchor, t); - frc::Translation2d p2 = GeometryUtil::cubicLerp(joinAnchor, - joinNextControl, nextWaypointPrevControl, - m_bezierPoints[bezierPointIdx], t); - - segment1Length += positionDelta(lastSegment1Pos, p1); - segment2Length += positionDelta(lastSegment2Pos, p2); - - lastSegment1Pos = p1; - lastSegment2Pos = p2; - } - - double segment1Pct = segment1Length() - / (segment1Length() + segment2Length()); - - std::vector < RotationTarget > mappedTargets; - std::vector < ConstraintsZone > mappedZones; - std::vector < EventMarker > mappedMarkers; - - for (RotationTarget t : m_rotationTargets) { - if (t.getPosition() >= nextWaypointIdx) { - mappedTargets.emplace_back(t.getPosition() - nextWaypointIdx + 2, - t.getTarget(), t.shouldRotateFast()); - } else if (t.getPosition() >= nextWaypointIdx - 1) { - double pct = t.getPosition() - (nextWaypointIdx - 1); - mappedTargets.emplace_back(mapPct(pct, segment1Pct), t.getTarget(), - t.shouldRotateFast()); - } - } - - for (ConstraintsZone z : m_constraintZones) { - double minPos = 0; - double maxPos = 0; + GoalEndState newEndState = GoalEndState(m_goalEndState.getVelocity(), + FlippingUtil::flipFieldRotation(m_goalEndState.getRotation())); + + std::optional < IdealStartingState > newStartState = std::nullopt; + if (m_idealStartingState.has_value()) { + newStartState = IdealStartingState( + m_idealStartingState.value().getVelocity(), + FlippingUtil::flipFieldRotation( + m_idealStartingState.value().getRotation())); + } + + auto path = PathPlannerPath::fromPathPoints(newPoints, m_globalConstraints, + newEndState); + path->m_waypoints = newWaypoints; + path->m_rotationTargets = newRotTargets; + path->m_pointTowardsZones = newPointZones; + path->m_constraintZones = m_constraintZones; + path->m_eventMarkers = m_eventMarkers; + path->m_idealStartingState = newStartState; + path->m_reversed = m_reversed; + path->m_isChoreoPath = m_isChoreoPath; + path->m_idealTrajectory = flippedTraj; + path->preventFlipping = preventFlipping; + path->name = name; - if (z.getMinWaypointRelativePos() >= nextWaypointIdx) { - minPos = z.getMinWaypointRelativePos() - nextWaypointIdx + 2; - } else if (z.getMinWaypointRelativePos() >= nextWaypointIdx - 1) { - double pct = z.getMinWaypointRelativePos() - (nextWaypointIdx - 1); - minPos = mapPct(pct, segment1Pct); - } + return path; +} - if (z.getMaxWaypointRelativePos() >= nextWaypointIdx) { - maxPos = z.getMaxWaypointRelativePos() - nextWaypointIdx + 2; - } else if (z.getMaxWaypointRelativePos() >= nextWaypointIdx - 1) { - double pct = z.getMaxWaypointRelativePos() - (nextWaypointIdx - 1); - maxPos = mapPct(pct, segment1Pct); - } +frc::Translation2d PathPlannerPath::samplePath( + double waypointRelativePos) const { + double pos = std::clamp(waypointRelativePos, 0.0, m_waypoints.size() - 1.0); - if (maxPos > 0) { - mappedZones.emplace_back(minPos, maxPos, z.getConstraints()); - } + size_t i = static_cast(waypointRelativePos); + if (i == m_waypoints.size() - 1) { + i--; } - for (EventMarker m : m_eventMarkers) { - if (m.getWaypointRelativePos() >= nextWaypointIdx) { - mappedMarkers.emplace_back( - m.getWaypointRelativePos() - nextWaypointIdx + 2, - m.getCommand()); - } else if (m.getWaypointRelativePos() >= nextWaypointIdx - 1) { - double pct = m.getWaypointRelativePos() - (nextWaypointIdx - 1); - mappedMarkers.emplace_back(mapPct(pct, segment1Pct), - m.getCommand()); - } - } + double t = pos - i; - // Throw out everything before nextWaypointIdx - 1, map everything from nextWaypointIdx - - // 1 to nextWaypointIdx on to the 2 joining segments (waypoint rel pos within old segment = % - // along distance of both new segments) - return std::make_shared < PathPlannerPath - > (replannedBezier, mappedTargets, mappedZones, mappedMarkers, m_globalConstraints, m_goalEndState, m_reversed); + auto p1 = m_waypoints[i].anchor; + auto p2 = m_waypoints[i].nextControl.value(); + auto p3 = m_waypoints[i + 1].prevControl.value(); + auto p4 = m_waypoints[i + 1].anchor; + return GeometryUtil::cubicLerp(p1, p2, p3, p4, t); } -std::shared_ptr PathPlannerPath::flipPath() { - if (m_isChoreoPath) { - // Just mirror the choreo traj - std::vector < PathPlannerTrajectory::State > mirroredStates; - for (auto state : m_choreoTrajectory.getStates()) { - PathPlannerTrajectory::State mirrored; - - mirrored.time = state.time; - mirrored.velocity = state.velocity; - mirrored.acceleration = state.acceleration; - mirrored.headingAngularVelocity = -state.headingAngularVelocity; - mirrored.position = GeometryUtil::flipFieldPosition(state.position); - mirrored.heading = GeometryUtil::flipFieldRotation(state.heading); - mirrored.targetHolonomicRotation = GeometryUtil::flipFieldRotation( - state.targetHolonomicRotation); - mirrored.holonomicAngularVelocityRps = - state.holonomicAngularVelocityRps; - if (state.holonomicAngularVelocityRps) { - mirrored.holonomicAngularVelocityRps = - -state.holonomicAngularVelocityRps.value(); - } - mirrored.curvature = -state.curvature; - mirrored.constraints = state.constraints; - mirroredStates.emplace_back(mirrored); - } - - auto path = - std::make_shared < PathPlannerPath - > (PathConstraints(units::meters_per_second_t { - std::numeric_limits::infinity() }, - units::meters_per_second_squared_t { - std::numeric_limits::infinity() }, - units::radians_per_second_t { - std::numeric_limits::infinity() }, - units::radians_per_second_squared_t { - std::numeric_limits::infinity() }), GoalEndState( - mirroredStates[mirroredStates.size() - 1].velocity, - mirroredStates[mirroredStates.size() - 1].targetHolonomicRotation, - true)); - - std::vector < PathPoint > pathPoints; - for (auto state : mirroredStates) { - pathPoints.emplace_back(state.position); - } - - path->m_allPoints = pathPoints; - path->m_isChoreoPath = true; - path->m_choreoTrajectory = PathPlannerTrajectory(mirroredStates, - m_choreoTrajectory.getEventCommands()); - - return path; - } +std::unordered_map>& PathPlannerPath::getPathCache() { + static std::unordered_map> *pathCache = + new std::unordered_map>(); + return *pathCache; +} - std::vector < frc::Translation2d > newBezier; - std::vector < RotationTarget > newRotTargets; - std::vector < EventMarker > newMarkers; - GoalEndState newEndState = GoalEndState(m_goalEndState.getVelocity(), - GeometryUtil::flipFieldRotation(m_goalEndState.getRotation()), - m_goalEndState.shouldRotateFast()); - frc::Rotation2d newPreviewRot = GeometryUtil::flipFieldRotation( - m_previewStartingRotation); +std::unordered_map>& PathPlannerPath::getChoreoPathCache() { + static std::unordered_map> *choreoPathCache = + new std::unordered_map>(); + return *choreoPathCache; +} - for (auto p : m_bezierPoints) { - newBezier.emplace_back(GeometryUtil::flipFieldPosition(p)); +std::shared_ptr PathPlannerPath::fromChoreoTrajectory( + std::string trajectoryName) { + if (PathPlannerPath::getChoreoPathCache().contains(trajectoryName)) { + return PathPlannerPath::getChoreoPathCache()[trajectoryName]; } - for (auto t : m_rotationTargets) { - newRotTargets.emplace_back(t.getPosition(), - GeometryUtil::flipFieldRotation(t.getTarget()), - t.shouldRotateFast()); + size_t dotIdx = trajectoryName.find_last_of('.'); + size_t splitIdx = std::string::npos; + if (dotIdx != std::string::npos) { + std::stringstream sstream(trajectoryName.substr(dotIdx + 1)); + sstream >> splitIdx; } - for (auto e : m_eventMarkers) { - newMarkers.emplace_back(e.getWaypointRelativePos(), e.getCommand()); + if (splitIdx != std::string::npos) { + // The traj name includes a split index + loadChoreoTrajectoryIntoCache(trajectoryName.substr(0, dotIdx)); + } else { + // The traj name does not include a split index + loadChoreoTrajectoryIntoCache(trajectoryName); } - return std::make_shared < PathPlannerPath - > (newBezier, newRotTargets, m_constraintZones, newMarkers, m_globalConstraints, newEndState, m_reversed, newPreviewRot); + return getChoreoPathCache()[trajectoryName]; } diff --git a/src/main/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp b/src/main/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp deleted file mode 100644 index 40ee8c7..0000000 --- a/src/main/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp +++ /dev/null @@ -1,199 +0,0 @@ -#include "pathplanner/lib/path/PathPlannerTrajectory.h" -#include "pathplanner/lib/path/PathPlannerPath.h" - -using namespace pathplanner; - -PathPlannerTrajectory::PathPlannerTrajectory( - std::shared_ptr path, - const frc::ChassisSpeeds &startingSpeeds, - const frc::Rotation2d &startingRotation) { - if (path->isChoreoPath()) { - m_states = - path->getTrajectory(startingSpeeds, startingRotation).m_states; - m_eventCommands = - path->getTrajectory(startingSpeeds, startingRotation).m_eventCommands; - } else { - m_states = generateStates(path, startingSpeeds, startingRotation); - - for (const EventMarker &m : path->getEventMarkers()) { - size_t pointIndex = static_cast(std::round( - m.getWaypointRelativePos() / PathSegment::RESOLUTION)); - m_eventCommands.emplace_back(m_states[pointIndex].time, - m.getCommand()); - } - - std::sort(m_eventCommands.begin(), m_eventCommands.end(), - [](auto &left, auto &right) { - return left.first < right.first; - }); - } -} - -PathPlannerTrajectory::State PathPlannerTrajectory::sample( - const units::second_t time) { - if (time <= getInitialState().time) - return getInitialState(); - if (time >= getTotalTime()) - return getEndState(); - - size_t low = 1; - size_t high = getStates().size() - 1; - - while (low != high) { - size_t mid = (low + high) / 2; - if (getState(mid).time < time) { - low = mid + 1; - } else { - high = mid; - } - } - - State sample = getState(low); - State prevSample = getState(low - 1); - - if (units::math::abs(sample.time - prevSample.time) < 1E-3_s) - return sample; - - return prevSample.interpolate(sample, - (time() - prevSample.time()) / (sample.time() - prevSample.time())); -} - -size_t PathPlannerTrajectory::getNextRotationTargetIdx( - std::shared_ptr path, const size_t startingIndex) { - size_t idx = path->numPoints() - 1; - - for (size_t i = startingIndex; i < path->numPoints() - 1; i++) { - if (path->getPoint(i).rotationTarget) { - idx = i; - break; - } - } - - return idx; -} - -std::vector PathPlannerTrajectory::generateStates( - std::shared_ptr path, - 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++) { - State state; - - PathConstraints constraints = path->getPoint(i).constraints.value(); - 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; - } - - 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 = GeometryUtil::rotationLerp( - prevRotationTargetRot, nextTarget.getTarget(), t); - } - - state.position = path->getPoint(i).position; - units::meter_t curveRadius = path->getPoint(i).curveRadius; - state.curvature = units::curvature_t { - (GeometryUtil::isFinite(curveRadius) && curveRadius != 0_m) ? - 1.0 / curveRadius() : 0 }; - - if (i == path->numPoints() - 1) { - state.heading = states[states.size() - 1].heading; - state.deltaPos = path->getPoint(i).distanceAlongPath - - path->getPoint(i - 1).distanceAlongPath; - state.velocity = path->getGoalEndState().getVelocity(); - } else if (i == 0) { - state.heading = - (path->getPoint(i + 1).position - state.position).Angle(); - state.deltaPos = 0_m; - state.velocity = startVel; - } else { - state.heading = - (path->getPoint(i + 1).position - state.position).Angle(); - state.deltaPos = path->getPoint(i + 1).distanceAlongPath - - path->getPoint(i).distanceAlongPath; - - units::meters_per_second_t v0 = states[states.size() - 1].velocity; - units::meters_per_second_t vMax = - units::math::sqrt( - units::math::abs( - units::math::pow < 2 - > (v0) - + (2 - * constraints.getMaxAcceleration() - * state.deltaPos))); - state.velocity = units::math::min(vMax, path->getPoint(i).maxV); - } - - states.push_back(state); - } - - // Second pass. Handles linear deceleration - for (size_t i = states.size() - 2; i > 1; i--) { - PathConstraints constraints = states[i].constraints; - - units::meters_per_second_t v0 = states[i + 1].velocity; - units::meters_per_second_t vMax = units::math::sqrt( - units::math::abs( - units::math::pow < 2 - > (v0) - + (2 * constraints.getMaxAcceleration() - * states[i + 1].deltaPos))); - states[i].velocity = units::math::min(vMax, states[i].velocity); - } - - // Final pass. Calculates time, linear acceleration, and angular velocity - units::second_t time = 0_s; - states[0].time = 0_s; - states[0].acceleration = 0_mps_sq; - states[0].headingAngularVelocity = startingSpeeds.omega; - - for (size_t i = 1; i < states.size(); i++) { - units::meters_per_second_t v0 = states[i - 1].velocity; - units::meters_per_second_t v = states[i].velocity; - units::second_t dt = (2 * states[i].deltaPos) / (v + v0); - - time += dt; - states[i].time = time; - - units::meters_per_second_t dv = v - v0; - states[i].acceleration = dv / dt; - - frc::Rotation2d headingDelta = states[i].heading - - states[i - 1].heading; - states[i].headingAngularVelocity = headingDelta.Radians() / dt; - } - - return states; -} diff --git a/src/main/cpp/pathplanner/lib/path/PathSegment.cpp b/src/main/cpp/pathplanner/lib/path/PathSegment.cpp deleted file mode 100644 index c29a66e..0000000 --- a/src/main/cpp/pathplanner/lib/path/PathSegment.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "pathplanner/lib/path/PathSegment.h" -#include "pathplanner/lib/util/GeometryUtil.h" - -using namespace pathplanner; - -PathSegment::PathSegment(frc::Translation2d p1, frc::Translation2d p2, - frc::Translation2d p3, frc::Translation2d p4, - std::vector targetHolonomicRotations, - std::vector constraintZones, bool endSegment) : m_segmentPoints() { - - for (double t = 0.0; t < 1.0; t += PathSegment::RESOLUTION) { - 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]; - targetHolonomicRotations.erase( - targetHolonomicRotations.begin()); - } - } - - std::optional < ConstraintsZone > currentZone = findConstraintsZone( - constraintZones, t); - - if (currentZone) { - m_segmentPoints.push_back( - PathPoint(GeometryUtil::cubicLerp(p1, p2, p3, p4, t), - holonomicRotation, currentZone->getConstraints())); - } else { - m_segmentPoints.push_back( - PathPoint(GeometryUtil::cubicLerp(p1, p2, p3, p4, t), - 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 PathSegment::findConstraintsZone( - std::vector &zones, double t) const { - for (ConstraintsZone &zone : zones) { - if (zone.isWithinZone(t)) { - return zone; - } - } - return std::nullopt; -} diff --git a/src/main/cpp/pathplanner/lib/path/RotationTarget.cpp b/src/main/cpp/pathplanner/lib/path/RotationTarget.cpp index f31dcf8..5729930 100644 --- a/src/main/cpp/pathplanner/lib/path/RotationTarget.cpp +++ b/src/main/cpp/pathplanner/lib/path/RotationTarget.cpp @@ -6,11 +6,6 @@ using namespace pathplanner; RotationTarget RotationTarget::fromJson(const wpi::json &json) { double pos = json.at("waypointRelativePos").get(); auto targetDeg = units::degree_t(json.at("rotationDegrees").get()); - bool rotateFast = false; - if (json.contains("rotateFast")) { - rotateFast = json.at("rotateFast").get(); - } - - return RotationTarget(pos, frc::Rotation2d(targetDeg), rotateFast); + return RotationTarget(pos, frc::Rotation2d(targetDeg)); } diff --git a/src/main/cpp/pathplanner/lib/path/Waypoint.cpp b/src/main/cpp/pathplanner/lib/path/Waypoint.cpp new file mode 100644 index 0000000..24cb790 --- /dev/null +++ b/src/main/cpp/pathplanner/lib/path/Waypoint.cpp @@ -0,0 +1,58 @@ +#include "pathplanner/lib/path/Waypoint.h" +#include "pathplanner/lib/util/JSONUtil.h" + +using namespace pathplanner; + +Waypoint Waypoint::autoControlPoints(frc::Translation2d anchor, + frc::Rotation2d heading, std::optional prevAnchor, + std::optional nextAnchor) { + std::optional < frc::Translation2d > prevControl = std::nullopt; + std::optional < frc::Translation2d > nextControl = std::nullopt; + + if (prevAnchor.has_value()) { + auto d = anchor.Distance(prevAnchor.value()) + * AUTO_CONTROL_DISTANCE_FACTOR; + prevControl = anchor - frc::Translation2d(d, heading); + } + if (nextAnchor.has_value()) { + auto d = anchor.Distance(nextAnchor.value()) + * AUTO_CONTROL_DISTANCE_FACTOR; + nextControl = anchor + frc::Translation2d(d, heading); + } + + return Waypoint(prevControl, anchor, nextControl); +} + +Waypoint Waypoint::fromJson(const wpi::json &waypointJson) { + auto anchor = JSONUtil::translation2dFromJson(waypointJson.at("anchor")); + std::optional < frc::Translation2d > prevControl = std::nullopt; + std::optional < frc::Translation2d > nextControl = std::nullopt; + + if (!waypointJson.at("prevControl").is_null()) { + prevControl = JSONUtil::translation2dFromJson( + waypointJson.at("prevControl")); + } + if (!waypointJson.at("nextControl").is_null()) { + nextControl = JSONUtil::translation2dFromJson( + waypointJson.at("nextControl")); + } + + return Waypoint(prevControl, anchor, nextControl); +} + +Waypoint Waypoint::flip() const { + std::optional < frc::Translation2d > flippedPrevControl = std::nullopt; + frc::Translation2d flippedAnchor = FlippingUtil::flipFieldPosition(anchor); + std::optional < frc::Translation2d > flippedNextControl = std::nullopt; + + if (prevControl.has_value()) { + flippedPrevControl = FlippingUtil::flipFieldPosition( + prevControl.value()); + } + if (nextControl.has_value()) { + flippedNextControl = FlippingUtil::flipFieldPosition( + nextControl.value()); + } + + return Waypoint(flippedPrevControl, flippedAnchor, flippedNextControl); +} diff --git a/src/main/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp b/src/main/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp index 5e4f8df..0350c74 100644 --- a/src/main/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp +++ b/src/main/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp @@ -1,5 +1,4 @@ #include "pathplanner/lib/pathfinding/LocalADStar.h" -#include "pathplanner/lib/path/PathSegment.h" #include "pathplanner/lib/util/GeometryUtil.h" #include #include @@ -15,7 +14,7 @@ LocalADStar::LocalADStar() : fieldLength(16.54), fieldWidth(8.02), nodeSize( 0.2), nodesX(static_cast(std::ceil(fieldLength / nodeSize))), nodesY( static_cast(std::ceil(fieldWidth / nodeSize))), g(), rhs(), open(), incons(), closed(), staticObstacles(), dynamicObstacles(), requestObstacles(), requestStart(), requestRealStartPos(), requestGoal(), requestRealGoalPos(), eps( EPS), planningThread(), pathMutex(), requestMutex(), requestMinor(true), requestMajor( - true), requestReset(true), newPathAvailable(false), currentPathPoints() { + true), requestReset(true), newPathAvailable(false), currentWaypoints() { requestStart = GridPosition(0, 0); requestRealStartPos = frc::Translation2d(0_m, 0_m); requestGoal = GridPosition(0, 0); @@ -27,12 +26,12 @@ LocalADStar::LocalADStar() : fieldLength(16.54), fieldWidth(8.02), nodeSize( const std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/navgrid.json"; - wpi::expected, std::error_code> fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); + auto fileBuffer = wpi::MemoryBuffer::GetFile(filePath); - if (fileBuffer.has_value()) { + if (fileBuffer) { try { - wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); + wpi::json json = wpi::json::parse( + fileBuffer.value()->GetCharBuffer()); nodeSize = json.at("nodeSizeMeters").get(); wpi::json::const_reference grid = json.at("grid"); @@ -137,13 +136,13 @@ void LocalADStar::doWork(const bool needsReset, const bool doMinor, computeOrImprovePath(sStart, sGoal, obstacles); std::vector < GridPosition > pathPositions = extractPath(sStart, sGoal, obstacles); - std::vector < PathPoint > pathPoints = createPathPoints(pathPositions, + std::vector < Waypoint > waypoints = createWaypoints(pathPositions, realStartPos, realGoalPos, obstacles); { std::scoped_lock lock { pathMutex }; currentPathFull = pathPositions; - currentPathPoints = pathPoints; + currentWaypoints = waypoints; } newPathAvailable = true; @@ -159,13 +158,13 @@ void LocalADStar::doWork(const bool needsReset, const bool doMinor, computeOrImprovePath(sStart, sGoal, obstacles); std::vector < GridPosition > pathPositions = extractPath(sStart, sGoal, obstacles); - std::vector < PathPoint > pathPoints = createPathPoints( - pathPositions, realStartPos, realGoalPos, obstacles); + std::vector < Waypoint > waypoints = createWaypoints(pathPositions, + realStartPos, realGoalPos, obstacles); { std::scoped_lock lock { pathMutex }; currentPathFull = pathPositions; - currentPathPoints = pathPoints; + currentWaypoints = waypoints; } newPathAvailable = true; @@ -175,22 +174,23 @@ void LocalADStar::doWork(const bool needsReset, const bool doMinor, std::shared_ptr LocalADStar::getCurrentPath( PathConstraints constraints, GoalEndState goalEndState) { - std::vector < PathPoint > pathPoints; + std::vector < Waypoint > waypoints; { std::scoped_lock lock { pathMutex }; - pathPoints = currentPathPoints; + waypoints.insert(waypoints.end(), currentWaypoints.begin(), + currentWaypoints.end()); } newPathAvailable = false; - if (pathPoints.empty()) { + if (waypoints.size() < 2) { // Not enough points to make a path return nullptr; } - return PathPlannerPath::fromPathPoints(pathPoints, constraints, - goalEndState); + return std::make_shared < PathPlannerPath + > (waypoints, constraints, std::nullopt, goalEndState); } void LocalADStar::setStartPosition(const frc::Translation2d &start) { @@ -344,13 +344,13 @@ std::vector LocalADStar::extractPath(const GridPosition &sStart, return path; } -std::vector LocalADStar::createPathPoints( +std::vector LocalADStar::createWaypoints( const std::vector &path, const frc::Translation2d &realStartPos, const frc::Translation2d &realGoalPos, const std::unordered_set &obstacles) { if (path.empty()) { - return std::vector(); + return std::vector(); } std::vector < GridPosition > simplifiedPath; @@ -368,15 +368,17 @@ std::vector LocalADStar::createPathPoints( fieldPosPath.push_back(gridPosToTranslation2d(pos)); } + if (fieldPosPath.size() < 2) { + return std::vector(); + } + // Replace start and end positions with their real positions fieldPosPath[0] = realStartPos; fieldPosPath[fieldPosPath.size() - 1] = realGoalPos; - std::vector < frc::Translation2d > bezierPoints; - bezierPoints.push_back(fieldPosPath[0]); - bezierPoints.push_back( - ((fieldPosPath[1] - fieldPosPath[0]) * SMOOTHING_CONTROL_PCT) - + fieldPosPath[0]); + std::vector < frc::Pose2d > pathPoses; + pathPoses.emplace_back(fieldPosPath[0], + (fieldPosPath[1] - fieldPosPath[0]).Angle()); for (size_t i = 1; i < fieldPosPath.size() - 1; i++) { frc::Translation2d last = fieldPosPath[i - 1]; frc::Translation2d current = fieldPosPath[i]; @@ -384,62 +386,19 @@ std::vector LocalADStar::createPathPoints( frc::Translation2d anchor1 = ((current - last) * SMOOTHING_ANCHOR_PCT) + last; + frc::Rotation2d heading1 = (current - last).Angle(); frc::Translation2d anchor2 = ((current - next) * SMOOTHING_ANCHOR_PCT) + next; + frc::Rotation2d heading2 = (next - anchor2).Angle(); - units::meter_t controlDist = anchor1.Distance(anchor2) - * SMOOTHING_CONTROL_PCT; - - frc::Translation2d prevControl1 = ((last - anchor1) - * SMOOTHING_CONTROL_PCT) + anchor1; - frc::Translation2d nextControl1 = frc::Translation2d(controlDist, - (anchor1 - prevControl1).Angle()) + anchor1; - - frc::Translation2d prevControl2 = frc::Translation2d(controlDist, - (anchor2 - next).Angle()) + anchor2; - frc::Translation2d nextControl2 = ((next - anchor2) - * SMOOTHING_CONTROL_PCT) + anchor2; - - bezierPoints.push_back(prevControl1); - bezierPoints.push_back(anchor1); - bezierPoints.push_back(nextControl1); - - bezierPoints.push_back(prevControl2); - bezierPoints.push_back(anchor2); - bezierPoints.push_back(nextControl2); - } - bezierPoints.push_back( - ((fieldPosPath[fieldPosPath.size() - 2] - - fieldPosPath[fieldPosPath.size() - 1]) - * SMOOTHING_CONTROL_PCT) - + fieldPosPath[fieldPosPath.size() - 1]); - bezierPoints.push_back(fieldPosPath[fieldPosPath.size() - 1]); - - size_t numSegments = (bezierPoints.size() - 1) / 3; - std::vector < PathPoint > pathPoints; - - for (size_t i = 0; i < numSegments; i++) { - size_t iOffset = i * 3; - - frc::Translation2d p1 = bezierPoints[iOffset]; - frc::Translation2d p2 = bezierPoints[iOffset + 1]; - frc::Translation2d p3 = bezierPoints[iOffset + 2]; - frc::Translation2d p4 = bezierPoints[iOffset + 3]; - - double resolution = PathSegment::RESOLUTION; - if (p1.Distance(p4) <= 1_m) { - resolution = 0.2; - } - - for (double t = 0.0; t < 1.0; t += resolution) { - pathPoints.emplace_back(GeometryUtil::cubicLerp(p1, p2, p3, p4, t), - std::nullopt, std::nullopt); - } + pathPoses.emplace_back(anchor1, heading1); + pathPoses.emplace_back(anchor2, heading2); } - pathPoints.emplace_back(bezierPoints[bezierPoints.size() - 1], std::nullopt, - std::nullopt); + pathPoses.emplace_back(fieldPosPath[fieldPosPath.size() - 1], + (fieldPosPath[fieldPosPath.size() - 1] + - fieldPosPath[fieldPosPath.size() - 2]).Angle()); - return pathPoints; + return PathPlannerPath::waypointsFromPoses(pathPoses); } bool LocalADStar::walkable(const GridPosition &s1, const GridPosition &s2, diff --git a/src/main/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp b/src/main/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp deleted file mode 100644 index c3b428a..0000000 --- a/src/main/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "pathplanner/lib/pathfinding/RemoteADStar.h" -#include -#include -#include -#include - -using namespace pathplanner; - -RemoteADStar::RemoteADStar() { - auto nt = nt::NetworkTableInstance::GetDefault(); - - m_navGridJsonPub = nt.GetStringTopic( - "/PPLibCoprocessor/RemoteADStar/navGrid").Publish(); - m_startPosPub = nt.GetDoubleArrayTopic( - "/PPLibCoprocessor/RemoteADStar/startPos").Publish(); - m_goalPosPub = nt.GetDoubleArrayTopic( - "/PPLibCoprocessor/RemoteADStar/goalPos").Publish(); - m_dynamicObsPub = nt.GetDoubleArrayTopic( - "/PPLibCoprocessor/RemoteADStar/dynamicObstacles").Publish(); - - auto options = nt::PubSubOptions(); - options.keepDuplicates = true; - options.sendAll = true; - m_pathPointsSub = nt.GetDoubleArrayTopic( - "/PPLibCoprocessor/RemoteADStar/pathPoints").Subscribe( - std::vector(), options); - - m_pathListenerHandle = nt.AddListener(m_pathPointsSub, - nt::EventFlags::kValueAll, [this](const nt::Event &event) { - std::scoped_lock lock { m_mutex }; - - auto pathPointsArr = event.GetValueEventData()->value.GetDoubleArray(); - - m_currentPath.clear(); - for (size_t i = 0; i <= pathPointsArr.size() - 2; i += 2) { - units::meter_t x { pathPointsArr[i] }; - units::meter_t y { pathPointsArr[i + 1] }; - - m_currentPath.emplace_back(frc::Translation2d(x, y), - std::nullopt, std::nullopt); - } - - m_newPathAvailable = true; - } - ); - - const std::string filePath = frc::filesystem::GetDeployDirectory() - + "/pathplanner/navgrid.json"; - - wpi::expected, std::error_code> fileBuffer = - wpi::MemoryBuffer::GetFile(filePath); - - if (!fileBuffer.has_value()) { - FRC_ReportError(frc::err::Error, - "RemoteADStar failed to load navgrid. Pathfinding will not be functional."); - } else { - auto charBuffer = fileBuffer.value()->GetCharBuffer(); - m_navGridJsonPub.Set(std::string(charBuffer.begin(), charBuffer.end())); - } - - m_newPathAvailable = false; -} - -std::shared_ptr RemoteADStar::getCurrentPath( - PathConstraints constraints, GoalEndState goalEndState) { - std::vector < PathPoint > pathPointsCopy; - - { - std::scoped_lock lock { m_mutex }; - pathPointsCopy.insert(pathPointsCopy.begin(), m_currentPath.begin(), - m_currentPath.end()); - m_newPathAvailable = false; - } - - if (pathPointsCopy.size() < 2) { - return nullptr; - } - - return PathPlannerPath::fromPathPoints(pathPointsCopy, constraints, - goalEndState); -} - -void RemoteADStar::setStartPosition(const frc::Translation2d &start) { - m_startPosPub.Set(std::vector { start.X()(), start.Y()() }); -} - -void RemoteADStar::setGoalPosition(const frc::Translation2d &goal) { - m_goalPosPub.Set(std::vector { goal.X()(), goal.Y()() }); -} - -void RemoteADStar::setDynamicObstacles( - const std::vector> &obs, - const frc::Translation2d ¤tRobotPos) { - std::vector obsArr; - - // First two doubles represent current robot pos - obsArr.emplace_back(currentRobotPos.X()()); - obsArr.emplace_back(currentRobotPos.Y()()); - - for (auto box : obs) { - obsArr.emplace_back(box.first.X()()); - obsArr.emplace_back(box.first.Y()()); - obsArr.emplace_back(box.second.X()()); - obsArr.emplace_back(box.second.Y()()); - } - - m_dynamicObsPub.Set(obsArr); -} diff --git a/src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp new file mode 100644 index 0000000..155a708 --- /dev/null +++ b/src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp @@ -0,0 +1,665 @@ +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" +#include "pathplanner/lib/path/PathPlannerPath.h" +#include "pathplanner/lib/events/ScheduleCommandEvent.h" +#include "pathplanner/lib/events/CancelCommandEvent.h" +#include "pathplanner/lib/events/TriggerEvent.h" +#include "pathplanner/lib/events/PointTowardsZoneEvent.h" +#include "pathplanner/lib/events/OneShotTriggerEvent.h" +#include +#include +#include + +using namespace pathplanner; + +PathPlannerTrajectory::PathPlannerTrajectory( + std::shared_ptr path, + const frc::ChassisSpeeds &startingSpeeds, + const frc::Rotation2d &startingRotation, const RobotConfig &config) { + if (path->isChoreoPath()) { + PathPlannerTrajectory traj = path->getIdealTrajectory(config).value(); + m_states = traj.m_states; + m_events = traj.m_events; + } else { + // Create all states + generateStates(m_states, path, startingRotation, config); + + // Set the initial module velocities + frc::ChassisSpeeds fieldStartingSpeeds = + frc::ChassisSpeeds::FromRobotRelativeSpeeds(startingSpeeds, + m_states[0].pose.Rotation()); + auto initialStates = config.toSwerveModuleStates(fieldStartingSpeeds); + for (size_t m = 0; m < config.numModules; m++) { + m_states[0].moduleStates[m].speed = initialStates[m].speed; + } + m_states[0].time = 0.0_s; + m_states[0].fieldSpeeds = fieldStartingSpeeds; + m_states[0].linearVelocity = units::math::hypot(fieldStartingSpeeds.vx, + fieldStartingSpeeds.vy); + + // Forward pass + forwardAccelPass(m_states, config); + + // Set the final module velocities + frc::Translation2d endSpeedTrans(units::meter_t { + path->getGoalEndState().getVelocity()() }, + m_states[m_states.size() - 1].heading); + frc::ChassisSpeeds endFieldSpeeds { units::meters_per_second_t { + endSpeedTrans.X()() }, units::meters_per_second_t { + endSpeedTrans.Y()() }, 0_rad_per_s }; + auto endStates = config.toSwerveModuleStates( + frc::ChassisSpeeds::FromFieldRelativeSpeeds(endFieldSpeeds, + m_states[m_states.size() - 1].pose.Rotation())); + for (size_t m = 0; m < config.numModules; m++) { + m_states[m_states.size() - 1].moduleStates[m].speed = + endStates[m].speed; + } + m_states[m_states.size() - 1].fieldSpeeds = endFieldSpeeds; + m_states[m_states.size() - 1].linearVelocity = + path->getGoalEndState().getVelocity(); + + // Reverse pass + reverseAccelPass(m_states, config); + + std::vector < std::shared_ptr < Event >> unaddedEvents; + for (EventMarker marker : path->getEventMarkers()) { + unaddedEvents.emplace_back( + std::make_shared < ScheduleCommandEvent + > (units::second_t { marker.getWaypointRelativePos() }, marker.getCommand())); + + if (marker.getEndWaypointRelativePos() >= 0.0) { + // This marker is zoned + unaddedEvents.emplace_back( + std::make_shared < CancelCommandEvent + > (units::second_t { + marker.getEndWaypointRelativePos() }, marker.getCommand())); + unaddedEvents.emplace_back( + std::make_shared < TriggerEvent + > (units::second_t { + marker.getWaypointRelativePos() }, marker.getTriggerName(), true)); + unaddedEvents.emplace_back( + std::make_shared < TriggerEvent + > (units::second_t { + marker.getEndWaypointRelativePos() }, marker.getTriggerName(), false)); + } else { + unaddedEvents.emplace_back( + std::make_shared < OneShotTriggerEvent + > (units::second_t { + marker.getWaypointRelativePos() }, marker.getTriggerName())); + } + } + for (PointTowardsZone zone : path->getPointTowardsZones()) { + unaddedEvents.emplace_back( + std::make_shared < PointTowardsZoneEvent + > (units::second_t { + zone.getMinWaypointRelativePos() }, zone.getName(), true)); + unaddedEvents.emplace_back( + std::make_shared < PointTowardsZoneEvent + > (units::second_t { + zone.getMaxWaypointRelativePos() }, zone.getName(), false)); + } + std::sort(unaddedEvents.begin(), unaddedEvents.end(), + [](auto left, auto right) { + return left->getTimestamp() < right->getTimestamp(); + }); + + // Loop back over and calculate time and module torque + for (size_t i = 1; i < m_states.size(); i++) { + PathPlannerTrajectoryState &prevState = m_states[i - 1]; + PathPlannerTrajectoryState &state = m_states[i]; + + units::meters_per_second_t v0 = prevState.linearVelocity; + units::meters_per_second_t v = state.linearVelocity; + units::second_t dt = (2 * state.deltaPos) / (v + v0); + state.time = prevState.time + dt; + + frc::ChassisSpeeds prevRobotSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds( + prevState.fieldSpeeds, prevState.pose.Rotation()); + frc::ChassisSpeeds robotSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds( + state.fieldSpeeds, state.pose.Rotation()); + + auto chassisAccelX = (robotSpeeds.vx - prevRobotSpeeds.vx) / dt; + auto chassisAccelY = (robotSpeeds.vy - prevRobotSpeeds.vy) / dt; + auto chassisForceX = chassisAccelX * config.mass; + auto chassisForceY = chassisAccelY * config.mass; + + auto angularAccel = (robotSpeeds.omega - prevRobotSpeeds.omega) + / dt; + auto angTorque = angularAccel * config.MOI; + frc::ChassisSpeeds chassisForces { units::meters_per_second_t { + chassisForceX() }, units::meters_per_second_t { + chassisForceY() }, + units::radians_per_second_t { angTorque() }, }; + + auto wheelForces = config.chassisForcesToWheelForceVectors( + chassisForces); + + for (size_t m = 0; m < config.numModules; m++) { + units::meters_per_second_squared_t accel = + (state.moduleStates[m].speed + - prevState.moduleStates[m].speed) / dt; + units::newton_t appliedForce { + wheelForces[m].Norm()() + * (wheelForces[m].Angle() + - state.moduleStates[m].angle).Cos() }; + units::newton_meter_t wheelTorque = appliedForce + * config.moduleConfig.wheelRadius; + units::ampere_t torqueCurrent = + config.moduleConfig.driveMotor.Current(wheelTorque); + + prevState.feedforwards.emplace_back(DriveFeedforward { accel, + appliedForce, torqueCurrent }); + } + + // Un-added events have their timestamp set to a waypoint relative position + // When adding the event to this trajectory, set its timestamp properly + while (!unaddedEvents.empty() + && std::abs( + unaddedEvents[0]->getTimestamp()() + - prevState.waypointRelativePos) + <= std::abs( + unaddedEvents[0]->getTimestamp()() + - state.waypointRelativePos)) { + unaddedEvents[0]->setTimestamp(prevState.time); + m_events.emplace_back(unaddedEvents[0]); + unaddedEvents.erase(unaddedEvents.begin()); + } + + } + + while (!unaddedEvents.empty()) { + // There are events that need to be added to the last state + unaddedEvents[0]->setTimestamp(m_states[m_states.size() - 1].time); + m_events.emplace_back(unaddedEvents[0]); + unaddedEvents.erase(unaddedEvents.begin()); + } + + // Create feedforwards for the end state + for (size_t m = 0; m < config.numModules; m++) { + m_states[m_states.size() - 1].feedforwards.emplace_back( + DriveFeedforward { 0_mps_sq, 0_N, 0_A }); + } + } +} + +PathPlannerTrajectoryState PathPlannerTrajectory::sample( + const units::second_t time) { + if (time <= getInitialState().time) + return getInitialState(); + if (time >= getTotalTime()) + return getEndState(); + + size_t low = 1; + size_t high = getStates().size() - 1; + + while (low != high) { + size_t mid = (low + high) / 2; + if (getState(mid).time < time) { + low = mid + 1; + } else { + high = mid; + } + } + + PathPlannerTrajectoryState sample = getState(low); + PathPlannerTrajectoryState prevSample = getState(low - 1); + + if (units::math::abs(sample.time - prevSample.time) < 1E-3_s) + return sample; + + return prevSample.interpolate(sample, + (time() - prevSample.time()) / (sample.time() - prevSample.time())); +} + +void PathPlannerTrajectory::generateStates( + std::vector &states, + std::shared_ptr path, + const frc::Rotation2d &startingRotation, const RobotConfig &config) { + size_t prevRotationTargetIdx = 0; + frc::Rotation2d prevRotationTargetRot = startingRotation; + size_t nextRotationTargetIdx = getNextRotationTargetIdx(path, 0); + frc::Rotation2d nextRotationTargetRot = path->getPoint( + nextRotationTargetIdx).rotationTarget.value().getTarget(); + + for (size_t i = 0; i < path->numPoints(); i++) { + PathPoint p = path->getPoint(i); + + if (i > nextRotationTargetIdx) { + prevRotationTargetIdx = nextRotationTargetIdx; + prevRotationTargetRot = nextRotationTargetRot; + nextRotationTargetIdx = getNextRotationTargetIdx(path, i); + nextRotationTargetRot = + path->getPoint(nextRotationTargetIdx).rotationTarget.value().getTarget(); + } + + // Holonomic rotation is interpolated. We use the distance along the path + // to calculate how much to interpolate since the distribution of path points + // is not the same along the whole segment + double t = + (path->getPoint(i).distanceAlongPath + - path->getPoint(prevRotationTargetIdx).distanceAlongPath)() + / (path->getPoint(nextRotationTargetIdx).distanceAlongPath + - path->getPoint(prevRotationTargetIdx).distanceAlongPath)(); + frc::Rotation2d holonomicRot = cosineInterpolate(prevRotationTargetRot, + nextRotationTargetRot, t); + + frc::Pose2d robotPose(p.position, holonomicRot); + PathPlannerTrajectoryState state; + state.pose = robotPose; + state.constraints = p.constraints.value_or( + path->getGlobalConstraints()); + state.waypointRelativePos = p.waypointRelativePos; + + // Calculate robot heading + if (i != path->numPoints() - 1) { + state.heading = (path->getPoint(i + 1).position + - state.pose.Translation()).Angle(); + } else { + state.heading = states[i - 1].heading; + } + + if (!config.isHolonomic) { + state.pose = frc::Pose2d(state.pose.Translation(), + path->isReversed() ? + (state.heading + frc::Rotation2d(180_deg)) : + state.heading); + } + + if (i != 0) { + state.deltaPos = state.pose.Translation().Distance( + states[i - 1].pose.Translation()); + state.deltaRot = state.pose.Rotation() + - states[i - 1].pose.Rotation(); + } + + for (size_t m = 0; m < config.numModules; m++) { + SwerveModuleTrajectoryState s; + s.fieldPos = state.pose.Translation() + + config.moduleLocations[m].RotateBy(state.pose.Rotation()); + + if (i != 0) { + s.deltaPos = s.fieldPos.Distance( + states[i - 1].moduleStates[m].fieldPos); + } + + state.moduleStates.emplace_back(s); + } + + states.emplace_back(state); + } + + // Calculate module headings + for (size_t i = 0; i < states.size(); i++) { + for (size_t m = 0; m < config.numModules; m++) { + if (i != states.size() - 1) { + states[i].moduleStates[m].fieldAngle = + (states[i + 1].moduleStates[m].fieldPos + - states[i].moduleStates[m].fieldPos).Angle(); + states[i].moduleStates[m].angle = + states[i].moduleStates[m].fieldAngle + - states[i].pose.Rotation(); + } else { + states[i].moduleStates[m].fieldAngle = + states[i - 1].moduleStates[m].fieldAngle; + states[i].moduleStates[m].angle = + states[i].moduleStates[m].fieldAngle + - states[i].pose.Rotation(); + } + } + } +} + +void PathPlannerTrajectory::forwardAccelPass( + std::vector &states, + const RobotConfig &config) { + for (size_t i = 1; i < states.size() - 1; i++) { + PathPlannerTrajectoryState &prevState = states[i - 1]; + PathPlannerTrajectoryState &state = states[i]; + PathPlannerTrajectoryState &nextState = states[i + 1]; + + // Calculate the linear force vector and torque acting on the whole robot + frc::Translation2d linearForceVec; + units::newton_meter_t totalTorque = 0_Nm; + for (size_t m = 0; m < config.numModules; m++) { + units::meters_per_second_t lastVel = prevState.moduleStates[m].speed; + // This pass will only be handling acceleration of the robot, meaning that the "torque" + // acting on the module due to friction and other losses will be fighting the motor + units::radians_per_second_t lastVelRadPerSec { lastVel() + / config.moduleConfig.wheelRadius() }; + units::ampere_t currentDraw = units::math::min( + config.moduleConfig.driveMotor.Current(lastVelRadPerSec, + state.constraints.getNominalVoltage()), + config.moduleConfig.driveCurrentLimit); + units::newton_meter_t availableTorque = + config.moduleConfig.driveMotor.Torque(currentDraw) + - config.moduleConfig.torqueLoss; + availableTorque = units::math::min(availableTorque, + config.maxTorqueFriction); + units::newton_t forceAtCarpet = availableTorque + / config.moduleConfig.wheelRadius; + + frc::Translation2d forceVec(units::meter_t { forceAtCarpet() }, + state.moduleStates[m].fieldAngle); + + // Add the module force vector to the robot force vector + linearForceVec = linearForceVec + forceVec; + + // Calculate the torque this module will apply to the robot + frc::Rotation2d angleToModule = (state.moduleStates[m].fieldPos + - state.pose.Translation()).Angle(); + frc::Rotation2d theta = forceVec.Angle() - angleToModule; + totalTorque += forceAtCarpet * config.modulePivotDistance[m] + * theta.Sin(); + } + + // Use the robot accelerations to calculate how each module should accelerate + // Even though kinematics is usually used for velocities, it can still + // convert chassis accelerations to module accelerations + units::radians_per_second_squared_t maxAngAccel = + state.constraints.getMaxAngularAcceleration(); + units::radians_per_second_squared_t angularAccel = units::math::min( + units::math::max( + units::radians_per_second_squared_t { (totalTorque + / config.MOI)() }, -maxAngAccel), maxAngAccel); + + frc::Translation2d accelVec = linearForceVec / config.mass(); + units::meters_per_second_squared_t maxAccel = + state.constraints.getMaxAcceleration(); + units::meters_per_second_squared_t accel { accelVec.Norm()() }; + if (accel > maxAccel) { + accelVec = accelVec * (maxAccel() / accel()); + } + + frc::ChassisSpeeds chassisAccel = + frc::ChassisSpeeds::FromFieldRelativeSpeeds( + units::meters_per_second_t { accelVec.X()() }, + units::meters_per_second_t { accelVec.Y()() }, + units::radians_per_second_t { angularAccel() }, + state.pose.Rotation()); + auto accelStates = config.toSwerveModuleStates(chassisAccel); + for (size_t m = 0; m < config.numModules; m++) { + units::meters_per_second_squared_t moduleAcceleration { + accelStates[m].speed() }; + + // Calculate the module velocity at the current state + // vf^2 = v0^2 + 2ad + state.moduleStates[m].speed = + units::math::sqrt( + units::math::abs( + units::math::pow < 2 + > (prevState.moduleStates[m].speed) + + (2 * moduleAcceleration + * state.moduleStates[m].deltaPos))); + + units::meter_t curveRadius = GeometryUtil::calculateRadius( + prevState.moduleStates[m].fieldPos, + state.moduleStates[m].fieldPos, + nextState.moduleStates[m].fieldPos); + // Find the max velocity that would keep the centripetal force under the friction force + // Fc = M * v^2 / R + if (GeometryUtil::isFinite(curveRadius)) { + units::meters_per_second_t maxSafeVel = units::math::sqrt( + (config.wheelFrictionForce + * units::math::abs(curveRadius)) + / (config.mass / config.numModules)); + state.moduleStates[m].speed = units::math::min( + state.moduleStates[m].speed, maxSafeVel); + } + } + + // Go over the modules again to make sure they take the same amount of time to reach the next + // state + units::second_t maxDT = 0_s; + units::second_t realMaxDT = 0_s; + for (size_t m = 0; m < config.numModules; m++) { + frc::Rotation2d prevRotDelta = state.moduleStates[m].angle + - prevState.moduleStates[m].angle; + units::meters_per_second_t modVel = state.moduleStates[m].speed; + units::second_t dt = nextState.moduleStates[m].deltaPos / modVel; + + if (GeometryUtil::isFinite(dt)) { + realMaxDT = units::math::max(dt, realMaxDT); + + if (units::math::abs(prevRotDelta.Degrees()) < 60_deg) { + maxDT = units::math::max(dt, maxDT); + } + } + } + + if (maxDT == 0_s) { + maxDT = realMaxDT; + } + + // Recalculate all module velocities with the allowed DT + for (size_t m = 0; m < config.numModules; m++) { + frc::Rotation2d prevRotDelta = state.moduleStates[m].angle + - prevState.moduleStates[m].angle; + if (units::math::abs(prevRotDelta.Degrees()) >= 60_deg) { + continue; + } + + state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos + / maxDT; + } + + // Use the calculated module velocities to calculate the robot speeds + frc::ChassisSpeeds desiredSpeeds = config.toChassisSpeeds( + state.moduleStates); + + units::meters_per_second_t maxChassisVel = + state.constraints.getMaxVelocity(); + units::radians_per_second_t maxChassisAngVel = + state.constraints.getMaxAngularVelocity(); + + desaturateWheelSpeeds(state.moduleStates, desiredSpeeds, + config.moduleConfig.maxDriveVelocityMPS, maxChassisVel, + maxChassisAngVel); + + state.fieldSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds( + config.toChassisSpeeds(state.moduleStates), + state.pose.Rotation()); + state.linearVelocity = units::math::hypot(state.fieldSpeeds.vx, + state.fieldSpeeds.vy); + } +} + +void PathPlannerTrajectory::reverseAccelPass( + std::vector &states, + const RobotConfig &config) { + for (size_t i = states.size() - 2; i > 0; i--) { + PathPlannerTrajectoryState &state = states[i]; + PathPlannerTrajectoryState &nextState = states[i + 1]; + + // Calculate the linear force vector and torque acting on the whole robot + frc::Translation2d linearForceVec; + units::newton_meter_t totalTorque = 0_Nm; + for (size_t m = 0; m < config.numModules; m++) { + units::meters_per_second_t lastVel = nextState.moduleStates[m].speed; + // This pass will only be handling deceleration of the robot, meaning that the "torque" + // acting on the module due to friction and other losses will not be fighting the motor + units::radians_per_second_t lastVelRadPerSec { lastVel() + / config.moduleConfig.wheelRadius() }; + units::ampere_t currentDraw = units::math::min( + config.moduleConfig.driveMotor.Current(lastVelRadPerSec, + state.constraints.getNominalVoltage()), + config.moduleConfig.driveCurrentLimit); + units::newton_meter_t availableTorque = + config.moduleConfig.driveMotor.Torque(currentDraw); + availableTorque = units::math::min(availableTorque, + config.maxTorqueFriction); + units::newton_t forceAtCarpet = availableTorque + / config.moduleConfig.wheelRadius; + + frc::Translation2d forceVec(units::meter_t { forceAtCarpet() }, + state.moduleStates[m].fieldAngle + + frc::Rotation2d(180_deg)); + + // Add the module force vector to the robot force vector + linearForceVec = linearForceVec + forceVec; + + // Calculate the torque this module will apply to the robot + frc::Rotation2d angleToModule = (state.moduleStates[m].fieldPos + - state.pose.Translation()).Angle(); + frc::Rotation2d theta = forceVec.Angle() - angleToModule; + totalTorque += forceAtCarpet * config.modulePivotDistance[m] + * theta.Sin(); + } + + // Use the robot accelerations to calculate how each module should accelerate + // Even though kinematics is usually used for velocities, it can still + // convert chassis accelerations to module accelerations + units::radians_per_second_squared_t maxAngAccel = + state.constraints.getMaxAngularAcceleration(); + units::radians_per_second_squared_t angularAccel = units::math::min( + units::math::max( + units::radians_per_second_squared_t { (totalTorque + / config.MOI)() }, -maxAngAccel), maxAngAccel); + + frc::Translation2d accelVec = linearForceVec / config.mass(); + units::meters_per_second_squared_t maxAccel = + state.constraints.getMaxAcceleration(); + units::meters_per_second_squared_t accel { accelVec.Norm()() }; + if (accel > maxAccel) { + accelVec = accelVec * (maxAccel() / accel()); + } + + frc::ChassisSpeeds chassisAccel = + frc::ChassisSpeeds::FromFieldRelativeSpeeds( + units::meters_per_second_t { accelVec.X()() }, + units::meters_per_second_t { accelVec.Y()() }, + units::radians_per_second_t { angularAccel() }, + state.pose.Rotation()); + auto accelStates = config.toSwerveModuleStates(chassisAccel); + for (size_t m = 0; m < config.numModules; m++) { + units::meters_per_second_squared_t moduleAcceleration { + accelStates[m].speed() }; + + // Calculate the module velocity at the current state + // vf^2 = v0^2 + 2ad + units::meters_per_second_t maxVel = + units::math::sqrt( + units::math::abs( + units::math::pow < 2 + > (nextState.moduleStates[m].speed) + + (2 * moduleAcceleration + * nextState.moduleStates[m].deltaPos))); + state.moduleStates[m].speed = units::math::min(maxVel, + state.moduleStates[m].speed); + } + + // Go over the modules again to make sure they take the same amount of time to reach the next + // state + units::second_t maxDT = 0_s; + units::second_t realMaxDT = 0_s; + for (size_t m = 0; m < config.numModules; m++) { + frc::Rotation2d prevRotDelta = state.moduleStates[m].angle + - states[i - 1].moduleStates[m].angle; + units::meters_per_second_t modVel = state.moduleStates[m].speed; + units::second_t dt = nextState.moduleStates[m].deltaPos / modVel; + + if (GeometryUtil::isFinite(dt)) { + realMaxDT = units::math::max(dt, realMaxDT); + + if (units::math::abs(prevRotDelta.Degrees()) < 60_deg) { + maxDT = units::math::max(dt, maxDT); + } + } + } + + if (maxDT == 0_s) { + maxDT = realMaxDT; + } + + // Recalculate all module velocities with the allowed DT + for (size_t m = 0; m < config.numModules; m++) { + frc::Rotation2d prevRotDelta = state.moduleStates[m].angle + - states[i - 1].moduleStates[m].angle; + if (units::math::abs(prevRotDelta.Degrees()) >= 60_deg) { + continue; + } + + state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos + / maxDT; + } + + // Use the calculated module velocities to calculate the robot speeds + frc::ChassisSpeeds desiredSpeeds = config.toChassisSpeeds( + state.moduleStates); + + units::meters_per_second_t maxChassisVel = + state.constraints.getMaxVelocity(); + units::radians_per_second_t maxChassisAngVel = + state.constraints.getMaxAngularVelocity(); + + maxChassisVel = units::math::min(maxChassisVel, state.linearVelocity); + maxChassisAngVel = units::math::min(maxChassisAngVel, + units::math::abs(state.fieldSpeeds.omega)); + + desaturateWheelSpeeds(state.moduleStates, desiredSpeeds, + config.moduleConfig.maxDriveVelocityMPS, maxChassisVel, + maxChassisAngVel); + + state.fieldSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds( + config.toChassisSpeeds(state.moduleStates), + state.pose.Rotation()); + state.linearVelocity = units::math::hypot(state.fieldSpeeds.vx, + state.fieldSpeeds.vy); + } +} + +void PathPlannerTrajectory::desaturateWheelSpeeds( + std::vector &moduleStates, + const frc::ChassisSpeeds &desiredSpeeds, + units::meters_per_second_t maxModuleSpeed, + units::meters_per_second_t maxTranslationSpeed, + units::radians_per_second_t maxRotationSpeed) { + units::meters_per_second_t realMaxSpeed = 0_mps; + for (const SwerveModuleTrajectoryState &s : moduleStates) { + realMaxSpeed = units::math::max(realMaxSpeed, + units::math::abs(s.speed)); + } + + if (realMaxSpeed == 0_mps) { + return; + } + + double translationPct = 0.0; + if (units::math::abs(maxTranslationSpeed) > 1e-8_mps) { + translationPct = std::sqrt( + std::pow(desiredSpeeds.vx(), 2) + + std::pow(desiredSpeeds.vy(), 2)) + / maxTranslationSpeed(); + } + + double rotationPct = 0.0; + if (units::math::abs(maxRotationSpeed) > 1e-8_rad_per_s) { + rotationPct = std::abs(desiredSpeeds.omega()) + / std::abs(maxRotationSpeed()); + } + + double maxPct = std::max(translationPct, rotationPct); + + double scale = std::min(1.0, maxModuleSpeed() / realMaxSpeed()); + if (maxPct > 0) { + scale = std::min(scale, 1.0 / maxPct); + } + + for (SwerveModuleTrajectoryState &s : moduleStates) { + s.speed *= scale; + } +} + +size_t PathPlannerTrajectory::getNextRotationTargetIdx( + std::shared_ptr path, const size_t startingIndex) { + size_t idx = path->numPoints() - 1; + + for (size_t i = startingIndex; i < path->numPoints() - 1; i++) { + if (path->getPoint(i).rotationTarget) { + idx = i; + break; + } + } + + return idx; +} diff --git a/src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp b/src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp new file mode 100644 index 0000000..bbbc97f --- /dev/null +++ b/src/main/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp @@ -0,0 +1,84 @@ +#include "pathplanner/lib/trajectory/PathPlannerTrajectoryState.h" +#include "pathplanner/lib/util/FlippingUtil.h" + +using namespace pathplanner; + +PathPlannerTrajectoryState PathPlannerTrajectoryState::interpolate( + const PathPlannerTrajectoryState &endVal, const double t) const { + PathPlannerTrajectoryState lerpedState; + + lerpedState.time = GeometryUtil::unitLerp(time, endVal.time, t); + + auto deltaT = lerpedState.time - time; + if (deltaT < 0_s) { + return endVal.interpolate(*this, 1.0 - t); + } + + lerpedState.fieldSpeeds = frc::ChassisSpeeds { GeometryUtil::unitLerp( + fieldSpeeds.vx, endVal.fieldSpeeds.vx, t), GeometryUtil::unitLerp( + fieldSpeeds.vy, endVal.fieldSpeeds.vy, t), GeometryUtil::unitLerp( + fieldSpeeds.omega, endVal.fieldSpeeds.omega, t) }; + lerpedState.pose = frc::Pose2d( + GeometryUtil::translationLerp(pose.Translation(), + endVal.pose.Translation(), t), + GeometryUtil::rotationLerp(pose.Rotation(), endVal.pose.Rotation(), + t)); + lerpedState.linearVelocity = GeometryUtil::unitLerp(linearVelocity, + endVal.linearVelocity, t); + for (size_t m = 0; m < feedforwards.size(); m++) { + lerpedState.feedforwards.emplace_back( + feedforwards[m].interpolate(endVal.feedforwards[m], t)); + } + + return lerpedState; +} + +PathPlannerTrajectoryState PathPlannerTrajectoryState::reverse() const { + PathPlannerTrajectoryState reversed; + + reversed.time = time; + auto reversedSpeeds = frc::Translation2d( + units::meter_t { fieldSpeeds.vx() }, units::meter_t { + fieldSpeeds.vy() }).RotateBy(frc::Rotation2d(180_deg)); + reversed.fieldSpeeds = frc::ChassisSpeeds { units::meters_per_second_t { + reversedSpeeds.X()() }, units::meters_per_second_t { + reversedSpeeds.Y()() }, fieldSpeeds.omega }; + reversed.pose = frc::Pose2d(pose.Translation(), + pose.Rotation() + frc::Rotation2d(180_deg)); + reversed.linearVelocity = -linearVelocity; + for (auto ff : feedforwards) { + reversed.feedforwards.emplace_back(ff.reverse()); + } + + return reversed; +} + +PathPlannerTrajectoryState PathPlannerTrajectoryState::flip() const { + PathPlannerTrajectoryState flipped; + + flipped.time = time; + flipped.linearVelocity = linearVelocity; + flipped.pose = FlippingUtil::flipFieldPose(pose); + flipped.fieldSpeeds = FlippingUtil::flipFieldSpeeds(fieldSpeeds); + flipped.feedforwards = FlippingUtil::flipFeedforwards(feedforwards); + + return flipped; +} + +PathPlannerTrajectoryState PathPlannerTrajectoryState::copyWithTime( + units::second_t time) const { + PathPlannerTrajectoryState copy; + copy.time = time; + copy.fieldSpeeds = fieldSpeeds; + copy.pose = pose; + copy.linearVelocity = linearVelocity; + copy.feedforwards = feedforwards; + copy.heading = heading; + copy.deltaPos = deltaPos; + copy.deltaRot = deltaRot; + copy.moduleStates = moduleStates; + copy.constraints = constraints; + copy.waypointRelativePos = waypointRelativePos; + + return copy; +} diff --git a/src/main/cpp/pathplanner/lib/util/FlippingUtil.cpp b/src/main/cpp/pathplanner/lib/util/FlippingUtil.cpp new file mode 100644 index 0000000..fb5f005 --- /dev/null +++ b/src/main/cpp/pathplanner/lib/util/FlippingUtil.cpp @@ -0,0 +1,8 @@ +#include "pathplanner/lib/util/FlippingUtil.h" + +using namespace pathplanner; + +FlippingUtil::FieldSymmetry FlippingUtil::symmetryType = + FlippingUtil::FieldSymmetry::kMirrored; +units::meter_t FlippingUtil::fieldSizeX = 16.54175_m; +units::meter_t FlippingUtil::fieldSizeY = 8.211_m; diff --git a/src/main/cpp/pathplanner/lib/util/PPLibTelemetry.cpp b/src/main/cpp/pathplanner/lib/util/PPLibTelemetry.cpp index 514f30e..5b342ff 100644 --- a/src/main/cpp/pathplanner/lib/util/PPLibTelemetry.cpp +++ b/src/main/cpp/pathplanner/lib/util/PPLibTelemetry.cpp @@ -12,18 +12,15 @@ bool PPLibTelemetry::m_compMode = false; nt::DoubleArrayPublisher PPLibTelemetry::m_velPub = nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic( "/PathPlanner/vel").Publish(); -nt::DoublePublisher PPLibTelemetry::m_inaccuracyPub = - nt::NetworkTableInstance::GetDefault().GetDoubleTopic( - "/PathPlanner/inaccuracy").Publish(); -nt::DoubleArrayPublisher PPLibTelemetry::m_posePub = - nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic( - "/PathPlanner/currentPose").Publish(); -nt::DoubleArrayPublisher PPLibTelemetry::m_pathPub = - nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic( - "/PathPlanner/activePath").Publish(); -nt::DoubleArrayPublisher PPLibTelemetry::m_targetPosePub = - nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic( - "/PathPlanner/targetPose").Publish(); +nt::StructPublisher PPLibTelemetry::m_posePub = + nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d + > ("/PathPlanner/currentPose").Publish(); +nt::StructArrayPublisher PPLibTelemetry::m_pathPub = + nt::NetworkTableInstance::GetDefault().GetStructArrayTopic < frc::Pose2d + > ("/PathPlanner/activePath").Publish(); +nt::StructPublisher PPLibTelemetry::m_targetPosePub = + nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d + > ("/PathPlanner/targetPose").Publish(); std::unordered_map>> PPLibTelemetry::m_hotReloadPaths = std::unordered_map>> P std::optional PPLibTelemetry::m_hotReloadPathListener = std::nullopt; -void PPLibTelemetry::setCurrentPath(std::shared_ptr path) { - std::vector arr; - - for (const PathPoint &p : path->getAllPathPoints()) { - frc::Translation2d pos = p.position; - arr.push_back(pos.X()()); - arr.push_back(pos.Y()()); - // Just add 0 as a heading since it's not needed for displaying a path - arr.push_back(0.0); - } - - m_pathPub.Set(std::span { arr.data(), arr.size() }); -} - void PPLibTelemetry::ensureHotReloadListenersInitialized() { if (!m_hotReloadPathListener) { nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index eeca0f0..e1c9a2b 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include @@ -31,7 +32,7 @@ SwerveSubsystem::SwerveSubsystem() consts::swerve::pathplanning::ROTATION_D})) { SetName("SwerveSubsystem"); frc::SmartDashboard::PutData(this); - // SetupPathplanner(); + SetupPathplanner(); // LoadChoreoTrajectories(); } @@ -144,17 +145,31 @@ frc2::CommandPtr SwerveSubsystem::XPattern() { } void SwerveSubsystem::SetupPathplanner() { - pathplanner::AutoBuilder::configureHolonomic( - [this] { return swerveDrive.GetPose(); }, - [this](frc::Pose2d resetPose) { - return swerveDrive.ResetPose(resetPose); - }, - [this] { return swerveDrive.GetRobotRelativeSpeeds(); }, - [this](frc::ChassisSpeeds robotRelativeOutput) { - swerveDrive.DriveRobotRelative(robotRelativeOutput); - }, - consts::swerve::pathplanning::PATH_CONFIG, [] { return str::IsOnRed(); }, - this); + + ppControllers = std::make_shared( + pathplanner::PIDConstants{consts::swerve::pathplanning::POSE_P, consts::swerve::pathplanning::POSE_I, consts::swerve::pathplanning::POSE_D}, + pathplanner::PIDConstants{consts::swerve::pathplanning::ROTATION_P, consts::swerve::pathplanning::ROTATION_I, consts::swerve::pathplanning::ROTATION_D} + ); + + pathplanner::AutoBuilder::configure( + [this]() { return GetRobotPose(); }, + [this](frc::Pose2d pose) { swerveDrive.ResetPose(pose); }, + [this]() { return GetRobotRelativeSpeed(); }, + [this](frc::ChassisSpeeds speeds, std::vector ff) { + frc::ChassisSpeeds speedsToSend = + frc::ChassisSpeeds::Discretize(speeds, consts::LOOP_PERIOD); + + swerveDrive.SetModuleStates( + consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(speedsToSend), + true, false, {ff[0].torqueCurrent, ff[1].torqueCurrent, ff[2].torqueCurrent, ff[3].torqueCurrent}); + }, + ppControllers, + consts::swerve::pathplanning::config, + []() { + return str::IsOnRed(); + }, + this + ); } void SwerveSubsystem::LoadChoreoTrajectories() { @@ -172,7 +187,7 @@ void SwerveSubsystem::LoadChoreoTrajectories() { frc::Translation2d SwerveSubsystem::GetAmpLocation() { frc::Translation2d ampToGoTo = consts::yearSpecific::ampLocation; if (str::IsOnRed()) { - ampToGoTo = pathplanner::GeometryUtil::flipFieldPosition(ampToGoTo); + ampToGoTo = pathplanner::FlippingUtil::flipFieldPosition(ampToGoTo); } return ampToGoTo; } @@ -180,7 +195,7 @@ frc::Translation2d SwerveSubsystem::GetAmpLocation() { frc::Translation2d SwerveSubsystem::GetFrontAmpLocation() { frc::Translation2d ampToGoTo = consts::yearSpecific::inFrontOfAmpLocation; if (str::IsOnRed()) { - ampToGoTo = pathplanner::GeometryUtil::flipFieldPosition(ampToGoTo); + ampToGoTo = pathplanner::FlippingUtil::flipFieldPosition(ampToGoTo); } return ampToGoTo; } diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 93e316d..701d198 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -5644691 \ No newline at end of file +5940fab \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpSideFive.auto b/src/main/deploy/pathplanner/autos/AmpSideFive.auto deleted file mode 100644 index e025415..0000000 --- a/src/main/deploy/pathplanner/autos/AmpSideFive.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.6563693284988403, - "y": 6.678221225738525 - }, - "rotation": -120.06858690870702 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "AmpSideFive.1" - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DriveForward.auto b/src/main/deploy/pathplanner/autos/DriveForward.auto deleted file mode 100644 index 30c43ee..0000000 --- a/src/main/deploy/pathplanner/autos/DriveForward.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": -5.333031905724765e-27, - "y": 1.305903366078543e-33 - }, - "rotation": 0.05729577951308232 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "GoForward.1" - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PPTest.auto b/src/main/deploy/pathplanner/autos/PPTest.auto new file mode 100644 index 0000000..ad25e8a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PPTest.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "PPTest" + } + }, + { + "type": "named", + "data": { + "name": "Print" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PPTest.path b/src/main/deploy/pathplanner/paths/PPTest.path new file mode 100644 index 0000000..2bd7653 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/PPTest.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.338083655093211, + "y": 5.5720424697137645 + }, + "prevControl": null, + "nextControl": { + "x": 2.170169289352855, + "y": 7.359902143325701 + }, + "isLocked": false, + "linkedName": "CenterSub" + }, + { + "anchor": { + "x": 4.610204189816946, + "y": 7.157502934992274 + }, + "prevControl": { + "x": 3.069721326390307, + "y": 7.146258534529306 + }, + "nextControl": { + "x": 6.150687053243599, + "y": 7.168747335455242 + }, + "isLocked": false, + "linkedName": "CenterNote" + }, + { + "anchor": { + "x": 6.802862280095737, + "y": 6.730215717399484 + }, + "prevControl": { + "x": 6.038243048613902, + "y": 6.898881724344006 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8190854870775388, + "rotationDegrees": 73.18153656063558 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -75.25643716352927 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..77ab3c8 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,21 @@ +{ + "robotWidth": 0.868, + "robotLength": 0.85725, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "robotMass": 56.14840824451008, + "robotMOI": 6.883, + "robotWheelbase": 0.40005, + "robotTrackwidth": 0.55245, + "driveWheelRadius": 0.051, + "driveGearing": 5.357142, + "maxDriveSpeed": 6.038, + "driveMotorType": "falcon500FOC", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2 +} \ No newline at end of file diff --git a/src/main/include/Autos.h b/src/main/include/Autos.h index 20a8ea3..bc66414 100644 --- a/src/main/include/Autos.h +++ b/src/main/include/Autos.h @@ -23,17 +23,20 @@ class Autos { intakeSub(intakeSub), feederSub(feederSub) { pathplanner::NamedCommands::registerCommand( - "Shoot", frc2::cmd::Print("Shooting Note")); + "Print", frc2::cmd::Print("Test Named Command")); selectCommand = frc2::cmd::Select( [this] { return autoChooser.GetSelected(); }, std::pair{CHOREO_TEST, swerveSub.FollowChoreoTrajectory([] { return "TestPath"; })}, std::pair{CHOREO_LIME, - swerveSub.FollowChoreoTrajectory([] { return "Lime"; })}); + swerveSub.FollowChoreoTrajectory([] { return "Lime"; })}, + std::pair{PP_TEST, + pathplanner::PathPlannerAuto("PPTest").ToPtr()}); autoChooser.AddOption("Choreo Test", AutoSelector::CHOREO_TEST); autoChooser.AddOption("Choreo Lime", AutoSelector::CHOREO_LIME); + autoChooser.AddOption("Path Planner Test", AutoSelector::PP_TEST); frc::SmartDashboard::PutData("Auto Chooser", &autoChooser); } @@ -41,7 +44,7 @@ class Autos { frc2::Command* GetSelectedCommand() { return selectCommand.get(); } private: - enum AutoSelector { CHOREO_TEST, CHOREO_LIME }; + enum AutoSelector { CHOREO_TEST, CHOREO_LIME, PP_TEST }; frc::SendableChooser autoChooser; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 17d1548..6876429 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -46,6 +46,6 @@ class RobotContainer { // str::Vision vision; str::NoteVisualizer noteVisualizer; - // Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem, - // feederSubsystem}; + Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem, + feederSubsystem}; }; diff --git a/src/main/include/constants/SwerveConstants.h b/src/main/include/constants/SwerveConstants.h index 27fd9b5..7c07342 100644 --- a/src/main/include/constants/SwerveConstants.h +++ b/src/main/include/constants/SwerveConstants.h @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include @@ -190,17 +190,7 @@ inline constexpr bool DYNAMIC_REPLAN = false; inline constexpr units::meter_t DYNAMIC_REPLAN_THRESHOLD_TOTAL = 3_ft; inline constexpr units::meter_t DYNAMIC_REPLAN_THRESHOLD_SPIKE = 1_ft; -inline const pathplanner::HolonomicPathFollowerConfig PATH_CONFIG{ - pathplanner::PIDConstants{POSE_P, POSE_I, POSE_D}, - pathplanner::PIDConstants{ROTATION_P, ROTATION_I, ROTATION_D}, - physical::DRIVE_MAX_SPEED, physical::DRIVEBASE_RADIUS, - pathplanner::ReplanningConfig{INITIAL_REPLAN, DYNAMIC_REPLAN, - DYNAMIC_REPLAN_THRESHOLD_TOTAL, - DYNAMIC_REPLAN_THRESHOLD_SPIKE}}; - -inline constexpr pathplanner::PathConstraints constraints{ - physical::DRIVE_MAX_SPEED, physical::DRIVE_MAX_ACCEL, - physical::DRIVE_MAX_ROT_SPEED, physical::DRIVE_MAX_ROT_ACCEL}; +inline static pathplanner::RobotConfig config = pathplanner::RobotConfig::fromGUISettings(); inline constexpr units::meter_t translationalPIDTolerance = .5_in; inline constexpr units::meters_per_second_t translationalVelPIDTolerance = diff --git a/src/main/include/pathplanner/lib/auto/AutoBuilder.h b/src/main/include/pathplanner/lib/auto/AutoBuilder.h index 46a0ce8..c2fca1c 100644 --- a/src/main/include/pathplanner/lib/auto/AutoBuilder.h +++ b/src/main/include/pathplanner/lib/auto/AutoBuilder.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -14,96 +15,86 @@ #include #include #include "pathplanner/lib/path/PathPlannerPath.h" -#include "pathplanner/lib/util/HolonomicPathFollowerConfig.h" -#include "pathplanner/lib/config/ReplanningConfig.h" +#include "pathplanner/lib/config/RobotConfig.h" +#include "pathplanner/lib/controllers/PathFollowingController.h" +#include "pathplanner/lib/util/DriveFeedforward.h" +#include "pathplanner/lib/util/FlippingUtil.h" namespace pathplanner { + +class PathPlannerAuto; + class AutoBuilder { public: /** - * Configures the AutoBuilder for a holonomic drivetrain. + * Configures the AutoBuilder for using PathPlanner's built-in commands. * * @param poseSupplier a function that returns the robot's current pose * @param resetPose a function used for resetting the robot's pose * @param robotRelativeSpeedsSupplier a function that returns the robot's current robot relative chassis speeds - * @param robotRelativeOutput a function for setting the robot's robot-relative chassis speeds - * @param config HolonomicPathFollowerConfig for configuring the - * path following commands + * @param output Output function that accepts robot-relative ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *

NOTE: These feedforwards are assuming unoptimized module states. When you optimize your + * module states, you will need to reverse the feedforwards for modules that have been flipped + * @param controller Path following controller that will be used to follow the path + * @param robotConfig The robot configuration * @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of * the field. This will maintain a global blue alliance origin. * @param driveSubsystem a pointer to the subsystem for the robot's drive */ - static void configureHolonomic(std::function poseSupplier, + static void configure(std::function poseSupplier, std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function robotRelativeOutput, - HolonomicPathFollowerConfig config, - std::function shouldFlipPath, + std::function< + void(frc::ChassisSpeeds, std::vector)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, frc2::Subsystem *driveSubsystem); /** - * Configures the AutoBuilder for a differential drivetrain using a LTVUnicycleController path - * follower. + * Configures the AutoBuilder for using PathPlanner's built-in commands. * - * @param poseSupplier a supplier for the robot's current pose - * @param resetPose a consumer for resetting the robot's pose - * @param speedsSupplier a supplier for the robot's current chassis speeds - * @param output a consumer for setting the robot's chassis speeds - * @param qelems The maximum desired error tolerance for each state. - * @param relems The maximum desired control effort for each input. - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration + * @param poseSupplier a function that returns the robot's current pose + * @param resetPose a function used for resetting the robot's pose + * @param robotRelativeSpeedsSupplier a function that returns the robot's current robot relative chassis speeds + * @param output Output function that accepts robot-relative ChassisSpeeds. + * @param controller Path following controller that will be used to follow the path + * @param robotConfig The robot configuration * @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of * the field. This will maintain a global blue alliance origin. - * @param driveSubsystem the subsystem for the robot's drive + * @param driveSubsystem a pointer to the subsystem for the robot's drive */ - static void configureLTV(std::function poseSupplier, + static inline void configure(std::function poseSupplier, std::function resetPose, - std::function speedsSupplier, + std::function robotRelativeSpeedsSupplier, std::function output, - const wpi::array &Qelms, - const wpi::array &Relms, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Subsystem *driveSubsystem); - - /** - * Configures the AutoBuilder for a differential drivetrain using a LTVUnicycleController path - * follower. - * - * @param poseSupplier a supplier for the robot's current pose - * @param resetPose a consumer for resetting the robot's pose - * @param speedsSupplier a supplier for the robot's current chassis speeds - * @param output a consumer for setting the robot's chassis speeds - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Supplier that determines if paths should be flipped to the other side of - * the field. This will maintain a global blue alliance origin. - * @param driveSubsystem the subsystem for the robot's drive - */ - static void configureLTV(std::function poseSupplier, - std::function resetPose, - std::function speedsSupplier, - std::function output, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Subsystem *driveSubsystem); + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, + frc2::Subsystem *driveSubsystem) { + configure(poseSupplier, resetPose, robotRelativeSpeedsSupplier, + [output](auto speeds, auto feedforwards) { + output(speeds); + }, controller, robotConfig, shouldFlipPath, driveSubsystem); + } /** - * Configures the AutoBuilder with custom path following command builder. Building pathfinding commands is not supported when using a custom path following command builder. + * Configures the AutoBuilder with custom path following command builder. Building pathfinding + * commands is not supported if using a custom command builder. Custom path following commands + * will not have the path flipped for them, and event markers will not be triggered automatically. * - * @param pathFollowingCommandBuilder a function that builds a command to follow a given path * @param poseSupplier a function that returns the robot's current pose + * @param pathFollowingCommandBuilder a function that builds a command to follow a given path * @param resetPose a function for resetting the robot's pose + * @param isHolonomic Does the robot have a holonomic drivetrain * @param shouldFlipPose Supplier that determines if the starting pose should be flipped to the * other side of the field. This will maintain a global blue alliance origin. NOTE: paths will * not be flipped when configured with a custom path following command. Flipping the paths * must be handled in your command. */ - static void configureCustom( + static void configureCustom(std::function poseSupplier, std::function)> pathFollowingCommandBuilder, - std::function poseSupplier, - std::function resetPose, + std::function resetPose, bool isHolonomic, std::function shouldFlipPose = []() { return false; }); @@ -118,25 +109,39 @@ class AutoBuilder { } /** - * Builds a command to follow a path with event markers. + * Returns whether the AutoBuilder has been configured for a holonomic drivetrain. * - * @param path the path to follow - * @return a path following command with events for the given path + * @return true if the AutoBuilder has been configured for a holonomic drivetrain, false otherwise */ - static frc2::CommandPtr followPath(std::shared_ptr path); + static inline bool isHolonomic() { + return m_isHolonomic; + } + + /** + * Get the current robot pose + * + * @return Current robot pose + */ + static inline frc::Pose2d getCurrentPose() { + return m_poseSupplier(); + } + + /** + * Get if a path or field position should currently be flipped + * + * @return True if path/positions should be flipped + */ + static inline bool shouldFlip() { + return m_shouldFlipPath(); + } /** * Builds a command to follow a path with event markers. * * @param path the path to follow * @return a path following command with events for the given path - * @deprecated Renamed to followPath */ - [[deprecated("Renamed to followPath")]] - static frc2::CommandPtr followPathWithEvents( - std::shared_ptr path) { - return followPath(path); - } + static frc2::CommandPtr followPath(std::shared_ptr path); /** * Builds an auto command for the given auto name. @@ -144,17 +149,15 @@ class AutoBuilder { * @param autoName the name of the auto to build * @return an auto command for the given auto name */ - static frc2::CommandPtr buildAuto(std::string autoName); + static inline frc2::CommandPtr buildAuto(std::string autoName); /** - * Builds an auto command from the given JSON. - * - * @param json the JSON to build the command from - * @return an auto command built from the JSON + * Create a command to reset the robot's odometry to a given blue alliance pose + * + * @param bluePose The pose to reset to, relative to blue alliance origin + * @return Command to reset the robot's odometry */ - static frc2::CommandPtr getAutoCommandFromJson(const wpi::json &json); - - static frc::Pose2d getStartingPoseFromJson(const wpi::json &json); + static frc2::CommandPtr resetOdom(frc::Pose2d bluePose); /** * Build a command to pathfind to a given pose. If not using a holonomic drivetrain, the pose @@ -163,13 +166,11 @@ class AutoBuilder { * @param pose The pose to pathfind to * @param constraints The constraints to use while pathfinding * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose - * @param rotationDelayDistance The distance the robot should move from the start position before - * attempting to rotate to the final rotation * @return A command to pathfind to a given pose */ static frc2::CommandPtr pathfindToPose(frc::Pose2d pose, PathConstraints constraints, units::meters_per_second_t goalEndVel = - 0_mps, units::meter_t rotationDelayDistance = 0_m); + 0_mps); /** * Build a command to pathfind to a given pose that will be flipped based on the value of the path @@ -180,18 +181,15 @@ class AutoBuilder { * true * @param constraints The constraints to use while pathfinding * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose - * @param rotationDelayDistance The distance the robot should move from the start position before - * attempting to rotate to the final rotation * @return A command to pathfind to a given pose */ static frc2::CommandPtr pathfindToPoseFlipped(frc::Pose2d pose, PathConstraints constraints, units::meters_per_second_t goalEndVel = - 0_mps, units::meter_t rotationDelayDistance = 0_m) { + 0_mps) { return frc2::cmd::Either( - pathfindToPose(GeometryUtil::flipFieldPose(pose), constraints, - goalEndVel, rotationDelayDistance), - pathfindToPose(pose, constraints, goalEndVel, - rotationDelayDistance), m_shouldFlipPath); + pathfindToPose(FlippingUtil::flipFieldPose(pose), constraints, + goalEndVel), + pathfindToPose(pose, constraints, goalEndVel), m_shouldFlipPath); } /** @@ -200,17 +198,24 @@ class AutoBuilder { * * @param goalPath The path to pathfind to, then follow * @param pathfindingConstraints The constraints to use while pathfinding - * @param rotationDelayDistance The distance the robot should move from the start position before - * attempting to rotate to the final rotation * @return A command to pathfind to a given path, then follow the path */ static frc2::CommandPtr pathfindThenFollowPath( std::shared_ptr goalPath, - PathConstraints pathfindingConstraints, - units::meter_t rotationDelayDistance = 0_m); + PathConstraints pathfindingConstraints); /** - * Create and populate a sendable chooser with all PathPlannerAutos in the project + * Modifies the existing references that buildAutoChooser returns in SendableChooser to the most recent in the pathplanner/auto deploy directory + * + * Loads PathPlannerAutos from deploy/pathplanner/auto directory (recursively) on every call + * Adds new auto paths from the pathplanner/auto deploy directory however doesn't remove autos already previously loaded + */ + + static void regenerateSendableReferences(); + + /** + * Populate a sendable chooser with all loaded PathPlannerAutos in the project in pathplanner/auto deploy directory (recursively) + * Loads PathPlannerAutos from deploy/pathplanner/auto directory (recursively) on first call * * @param defaultAutoName The name of the auto that should be the default option. If this is an * empty string, or if an auto with the given name does not exist, the default option will be @@ -221,27 +226,71 @@ class AutoBuilder { std::string defaultAutoName = ""); /** - * Get a vector of all auto names in the project + * Populate a sendable chooser with all loaded PathPlannerAutos in the project in pathplanner/auto deploy directory (recursively) + * Loads PathPlannerAutos from deploy/pathplanner/auto directory (recursively) on first call + * Filters certain PathPlannerAuto bases on their properties * - * @return vector of all auto names + * @param filter Function which filters the auto commands out, returning true allows the command to be uploaded to sendable chooser + * while returning false prevents it from being added. + * autoCommand, const reference to PathPlannerAuto command which was generated + * @param defaultAutoName The name of the auto that should be the default option. If this is an + * empty string, or if an auto with the given name does not exist, the default option will be + * frc2::cmd::None(), defaultAutoName doesn't get filter out and always is in final sendable chooser (if found) + * @return SendableChooser populated with all autos + */ + static frc::SendableChooser buildAutoChooserFilter( + std::function filter, + std::string defaultAutoName = ""); + + /** + * Populate a sendable chooser with all loaded PathPlannerAutos in the project in pathplanner/auto deploy directory (recursively) + * Loads PathPlannerAutos from deploy/pathplanner/auto directory (recursively) on first call + * Filters certain PathPlannerAuto bases on their properties and their filepath + * + * @param filter Function which filters the auto commands out, returning true allows the command to be uploaded to sendable chooser + * while returning false prevents it from being added. + * autoCommand, const reference to PathPlannerAuto command which was generated + * autoPath, path to the autoCommand relative to pathplanner/auto deploy directory with extension ".auto" + * @param defaultAutoName The name of the auto that should be the default option. If this is an + * empty string, or if an auto with the given name does not exist, the default option will be + * frc2::cmd::None(), defaultAutoName doesn't get filter out and always is in final sendable chooser (if found) + * @return SendableChooser populated with all autos + */ + static frc::SendableChooser buildAutoChooserFilterPath( + std::function filter, + std::string defaultAutoName = ""); + + /** + * Get a vector of all auto names in the pathplanner/auto deploy directory (recursively) + * + * @return Vector of strings containing all auto names */ static std::vector getAllAutoNames(); + /** + * Get a vector of all auto paths in the pathplanner/auto deploy directory (recursively) + * + * @return Vector of paths relative to autos deploy directory + */ + static std::vector getAllAutoPaths(); + private: static bool m_configured; static std::function)> m_pathFollowingCommandBuilder; - static std::function m_getPose; + static std::function m_poseSupplier; static std::function m_resetPose; static std::function m_shouldFlipPath; + static bool m_isHolonomic; - static std::vector m_autoCommands; + static bool m_commandRefsGeneratedForSendable; + static frc2::CommandPtr m_noneCommand; + static std::map m_autoCommands; static bool m_pathfindingConfigured; static std::function< frc2::CommandPtr(frc::Pose2d, PathConstraints, - units::meters_per_second_t, units::meter_t)> m_pathfindToPoseCommandBuilder; + units::meters_per_second_t)> m_pathfindToPoseCommandBuilder; static std::function< - frc2::CommandPtr(std::shared_ptr, PathConstraints, - units::meter_t)> m_pathfindThenFollowPathCommandBuilder; + frc2::CommandPtr(std::shared_ptr, PathConstraints)> m_pathfindThenFollowPathCommandBuilder; }; } diff --git a/src/main/include/pathplanner/lib/auto/NamedCommands.h b/src/main/include/pathplanner/lib/auto/NamedCommands.h index a171909..7c14260 100644 --- a/src/main/include/pathplanner/lib/auto/NamedCommands.h +++ b/src/main/include/pathplanner/lib/auto/NamedCommands.h @@ -16,7 +16,7 @@ class NamedCommands { */ static inline void registerCommand(std::string name, std::shared_ptr command) { - NamedCommands::namedCommands.emplace(name, command); + NamedCommands::GetNamedCommands().emplace(name, command); } static inline void registerCommand(std::string name, @@ -33,7 +33,7 @@ class NamedCommands { * @return true if a command with the given name has been registered, false otherwise */ static inline bool hasCommand(std::string name) { - return NamedCommands::namedCommands.contains(name); + return NamedCommands::GetNamedCommands().contains(name); } /** @@ -44,7 +44,6 @@ class NamedCommands { */ static frc2::CommandPtr getCommand(std::string name); -private: - static std::unordered_map> namedCommands; + static std::unordered_map>& GetNamedCommands(); }; } diff --git a/src/main/include/pathplanner/lib/commands/FollowPathCommand.h b/src/main/include/pathplanner/lib/commands/FollowPathCommand.h index 638fcab..50852cf 100644 --- a/src/main/include/pathplanner/lib/commands/FollowPathCommand.h +++ b/src/main/include/pathplanner/lib/commands/FollowPathCommand.h @@ -13,11 +13,13 @@ #include #include #include "pathplanner/lib/path/PathPlannerPath.h" -#include "pathplanner/lib/path/PathPlannerTrajectory.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" #include "pathplanner/lib/controllers/PathFollowingController.h" -#include "pathplanner/lib/config/ReplanningConfig.h" +#include "pathplanner/lib/config/RobotConfig.h" #include "pathplanner/lib/util/PathPlannerLogging.h" #include "pathplanner/lib/util/PPLibTelemetry.h" +#include "pathplanner/lib/events/EventScheduler.h" +#include "pathplanner/lib/util/DriveFeedforward.h" namespace pathplanner { class FollowPathCommand: public frc2::CommandHelperNOTE: These feedforwards are assuming unoptimized module states. When you optimize your + * module states, you will need to reverse the feedforwards for modules that have been flipped * @param controller Path following controller that will be used to follow the path - * @param replanningConfig Path replanning configuration + * @param robotConfig The robot configuration * @param shouldFlipPath Should the path be flipped to the other side of the field? This will * maintain a global blue alliance origin. * @param requirements Subsystems required by this command, usually just the drive subsystem @@ -40,10 +45,10 @@ class FollowPathCommand: public frc2::CommandHelper path, std::function poseSupplier, std::function speedsSupplier, - std::function output, - std::unique_ptr controller, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, + std::function< + void(frc::ChassisSpeeds, std::vector)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements); void Initialize() override; @@ -59,25 +64,14 @@ class FollowPathCommand: public frc2::CommandHelper m_originalPath; std::function m_poseSupplier; std::function m_speedsSupplier; - std::function m_output; - std::unique_ptr m_controller; - ReplanningConfig m_replanningConfig; + std::function)> m_output; + std::shared_ptr m_controller; + RobotConfig m_robotConfig; std::function m_shouldFlipPath; - // For event markers - std::vector, bool>> m_currentEventCommands; - std::deque>> m_untriggeredEvents; + EventScheduler m_eventScheduler; std::shared_ptr m_path; - PathPlannerTrajectory m_generatedTrajectory; - - inline void replanPath(const frc::Pose2d ¤tPose, - const frc::ChassisSpeeds ¤tSpeeds) { - auto replanned = m_path->replan(currentPose, currentSpeeds); - m_generatedTrajectory = replanned->getTrajectory(currentSpeeds, - currentPose.Rotation()); - PathPlannerLogging::logActivePath(replanned); - PPLibTelemetry::setCurrentPath(replanned); - } + PathPlannerTrajectory m_trajectory; }; } diff --git a/src/main/include/pathplanner/lib/commands/FollowPathHolonomic.h b/src/main/include/pathplanner/lib/commands/FollowPathHolonomic.h deleted file mode 100644 index d973206..0000000 --- a/src/main/include/pathplanner/lib/commands/FollowPathHolonomic.h +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include "pathplanner/lib/commands/FollowPathCommand.h" -#include "pathplanner/lib/controllers/PPHolonomicDriveController.h" -#include "pathplanner/lib/config/PIDConstants.h" -#include "pathplanner/lib/util/HolonomicPathFollowerConfig.h" - -namespace pathplanner { -class FollowPathHolonomic: public FollowPathCommand { -public: - /** - * Construct a path following command that will use a holonomic drive controller for holonomic - * drive trains - * - * @param path The path to follow - * @param poseSupplier Function that supplies the current field-relative pose of the robot - * @param speedsSupplier Function that supplies the current robot-relative chassis speeds - * @param output Function that will apply the robot-relative output speeds of this - * command - * @param translationConstants PID constants for the translation PID controllers - * @param rotationConstants PID constants for the rotation controller - * @param maxModuleSpeed The max speed of a drive module in meters/sec - * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the - * distance from the center of the robot to the furthest module. For mecanum, this is the - * drive base width / 2 - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the path be flipped to the other side of the field? This will - * maintain a global blue alliance origin. - * @param requirements Subsystems required by this command, usually just the drive subsystem - * @param period Period of the control loop in seconds, default is 0.02s - */ - FollowPathHolonomic(std::shared_ptr path, - std::function poseSupplier, - std::function speedsSupplier, - std::function output, - PIDConstants translationConstants, PIDConstants rotationConstants, - units::meters_per_second_t maxModuleSpeed, - units::meter_t driveBaseRadius, ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements, units::second_t period = 0.02_s) : FollowPathCommand( - path, poseSupplier, speedsSupplier, output, - std::make_unique < PPHolonomicDriveController - > (translationConstants, rotationConstants, maxModuleSpeed, driveBaseRadius, period), - replanningConfig, shouldFlipPath, requirements) { - } - - /** - * Construct a path following command that will use a holonomic drive controller for holonomic - * drive trains - * - * @param path The path to follow - * @param poseSupplier Function that supplies the current field-relative pose of the robot - * @param speedsSupplier Function that supplies the current robot-relative chassis speeds - * @param output Function that will apply the robot-relative output speeds of this - * command - * @param config Holonomic path follower configuration - * @param shouldFlipPath Should the path be flipped to the other side of the field? This will - * maintain a global blue alliance origin. - * @param requirements Subsystems required by this command, usually just the drive subsystem - */ - FollowPathHolonomic(std::shared_ptr path, - std::function poseSupplier, - std::function speedsSupplier, - std::function output, - HolonomicPathFollowerConfig config, - std::function shouldFlipPath, - frc2::Requirements requirements) : FollowPathHolonomic(path, - poseSupplier, speedsSupplier, output, config.translationConstants, - config.rotationConstants, config.maxModuleSpeed, - config.driveBaseRadius, config.replanningConfig, shouldFlipPath, - requirements, config.period) { - } -}; -} diff --git a/src/main/include/pathplanner/lib/commands/FollowPathLTV.h b/src/main/include/pathplanner/lib/commands/FollowPathLTV.h deleted file mode 100644 index c55db54..0000000 --- a/src/main/include/pathplanner/lib/commands/FollowPathLTV.h +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -#include "pathplanner/lib/commands/FollowPathCommand.h" -#include "pathplanner/lib/controllers/PPLTVController.h" - -namespace pathplanner { -class FollowPathLTV: public FollowPathCommand { -public: - /** - * Create a path following command that will use an LTV unicycle controller for differential drive - * trains - * - * @param path The path to follow - * @param poseSupplier Function that supplies the current field-relative pose of the robot - * @param speedsSupplier Function that supplies the current robot-relative chassis speeds - * @param output Function that will apply the robot-relative output speeds of this command - * @param Qelems The maximum desired error tolerance for each state. - * @param Relems The maximum desired control effort for each input. - * @param dt The amount of time between each robot control loop, default is 0.02s - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the path be flipped to the other side of the field? This will - * maintain a global blue alliance origin. - * @param requirements Subsystems required by this command, usually just the drive subsystem - */ - FollowPathLTV(std::shared_ptr path, - std::function poseSupplier, - std::function speedsSupplier, - std::function output, - const wpi::array &Qelms, - const wpi::array &Relms, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements) : FollowPathCommand(path, - poseSupplier, speedsSupplier, output, - std::make_unique < PPLTVController > (Qelms, Relms, dt), - replanningConfig, shouldFlipPath, requirements) { - if (path->isChoreoPath()) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Paths loaded from Choreo cannot be used with differential drivetrains"); - } - } - - /** - * Create a path following command that will use an LTV unicycle controller for differential drive - * trains - * - * @param path The path to follow - * @param poseSupplier Function that supplies the current field-relative pose of the robot - * @param speedsSupplier Function that supplies the current robot-relative chassis speeds - * @param output Function that will apply the robot-relative output speeds of this command - * @param dt The amount of time between each robot control loop, default is 0.02s - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the path be flipped to the other side of the field? This will - * maintain a global blue alliance origin. - * @param requirements Subsystems required by this command, usually just the drive subsystem - */ - FollowPathLTV(std::shared_ptr path, - std::function poseSupplier, - std::function speedsSupplier, - std::function output, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements) : FollowPathCommand(path, - poseSupplier, speedsSupplier, output, - std::make_unique < PPLTVController > (dt), replanningConfig, - shouldFlipPath, requirements) { - if (path->isChoreoPath()) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Paths loaded from Choreo cannot be used with differential drivetrains"); - } - } -}; -} diff --git a/src/main/include/pathplanner/lib/commands/PathPlannerAuto.h b/src/main/include/pathplanner/lib/commands/PathPlannerAuto.h index ce8488a..746793d 100644 --- a/src/main/include/pathplanner/lib/commands/PathPlannerAuto.h +++ b/src/main/include/pathplanner/lib/commands/PathPlannerAuto.h @@ -1,12 +1,19 @@ #pragma once #include +#include #include #include #include #include #include +#include +#include +#include #include "pathplanner/lib/path/PathPlannerPath.h" +#include "pathplanner/lib/events/EventTrigger.h" +#include "pathplanner/lib/events/PointTowardsZoneTrigger.h" +#include "pathplanner/lib/auto/AutoBuilder.h" namespace pathplanner { /** @@ -14,6 +21,8 @@ namespace pathplanner { */ class PathPlannerAuto: public frc2::CommandHelper { public: + static std::string currentPathName; + /** * Constructs a new PathPlannerAuto command. * @@ -21,6 +30,15 @@ class PathPlannerAuto: public frc2::CommandHelper condition) { + return frc2::Trigger(m_autoLoop.get(), condition); + } + + /** + * Create a trigger that is high when this auto is running, and low when it is not running * - * @param autoName Name of the auto to get the pose from - * @return Starting pose from the given auto + * @return isRunning trigger */ - static frc::Pose2d getStartingPoseFromAutoFile(std::string autoName); + inline frc2::Trigger isRunning() { + return condition([this]() { + return m_isRunning; + }); + } + + /** + * Trigger that is high when the given time has elapsed + * + * @param time The amount of time this auto should run before the trigger is activated + * @return timeElapsed trigger + */ + inline frc2::Trigger timeElapsed(units::second_t time) { + return condition([this, time]() { + return m_timer.HasElapsed(time); + }); + } + + /** + * Trigger that is high when within a range of time since the start of this auto + * + * @param startTime The starting time of the range + * @param endTime The ending time of the range + * @return timeRange trigger + */ + inline frc2::Trigger timeRange(units::second_t startTime, + units::second_t endTime) { + return condition([this, startTime, endTime]() { + return m_timer.Get() >= startTime && m_timer.Get() <= endTime; + }); + } + + /** + * Create an EventTrigger that will be polled by this auto instead of globally across all path + * following commands + * + * @param eventName The event name that controls this trigger + * @return EventTrigger for this auto + */ + inline EventTrigger event(std::string eventName) { + return EventTrigger(m_autoLoop.get(), eventName); + } + + /** + * Create a PointTowardsZoneTrigger that will be polled by this auto instead of globally across + * all path following commands + * + * @param zoneName The point towards zone name that controls this trigger + * @return PointTowardsZoneTrigger for this auto + */ + inline PointTowardsZoneTrigger pointTowardsZone(std::string zoneName) { + return PointTowardsZoneTrigger(m_autoLoop.get(), zoneName); + } + + /** + * Create a trigger that is high when a certain path is being followed + * + * @param pathName The name of the path to check for + * @return activePath trigger + */ + inline frc2::Trigger activePath(std::string pathName) { + return condition([pathName]() { + return pathName == PathPlannerAuto::currentPathName; + }); + } + + /** + * Create a trigger that is high when near a given field position. This field position is not + * automatically flipped + * + * @param fieldPosition The target field position + * @param tolerance The position tolerance, in meters. The trigger will be high when within + * this distance from the target position + * @return nearFieldPosition trigger + */ + inline frc2::Trigger nearFieldPosition(frc::Translation2d fieldPosition, + units::meter_t tolerance) { + return condition( + [fieldPosition, tolerance]() { + return AutoBuilder::getCurrentPose().Translation().Distance( + fieldPosition) <= tolerance; + }); + } + + /** + * Create a trigger that is high when near a given field position. This field position will be + * automatically flipped + * + * @param blueFieldPosition The target field position if on the blue alliance + * @param tolerance The position tolerance, in meters. The trigger will be high when within + * this distance from the target position + * @return nearFieldPositionAutoFlipped trigger + */ + frc2::Trigger nearFieldPositionAutoFlipped( + frc::Translation2d blueFieldPosition, units::meter_t tolerance); + + /** + * Create a trigger that will be high when the robot is within a given area on the field. These + * positions will not be automatically flipped + * + * @param boundingBoxMin The minimum position of the bounding box for the target field area. The X + * & Y coordinates of this position should be less than the max position. + * @param boundingBoxMax The maximum position of the bounding box for the target field area. The X + * & Y coordinates of this position should be greater than the min position. + * @return inFieldArea trigger + */ + frc2::Trigger inFieldArea(frc::Translation2d boundingBoxMin, + frc::Translation2d boundingBoxMax); + + /** + * Create a trigger that will be high when the robot is within a given area on the field. These + * positions will be automatically flipped + * + * @param blueBoundingBoxMin The minimum position of the bounding box for the target field area if + * on the blue alliance. The X & Y coordinates of this position should be less than the max + * position. + * @param blueBoundingBoxMax The maximum position of the bounding box for the target field area if + * on the blue alliance. The X & Y coordinates of this position should be greater than the min + * position. + * @return inFieldAreaAutoFlipped trigger + */ + frc2::Trigger inFieldAreaAutoFlipped(frc::Translation2d blueBoundingBoxMin, + frc::Translation2d blueBoundingBoxMax); void Initialize() override; @@ -48,10 +208,18 @@ class PathPlannerAuto: public frc2::CommandHelper m_autoCommand; + frc::Pose2d m_startingPose; + + // Use a unique_ptr to avoid delted copy constructor shenanigans + std::unique_ptr m_autoLoop; + frc::Timer m_timer; + bool m_isRunning; static std::vector> pathsFromCommandJson( const wpi::json &json, bool choreoPaths); + void initFromJson(const wpi::json &json); + static int m_instances; }; } diff --git a/src/main/include/pathplanner/lib/commands/PathfindHolonomic.h b/src/main/include/pathplanner/lib/commands/PathfindHolonomic.h deleted file mode 100644 index 68c4420..0000000 --- a/src/main/include/pathplanner/lib/commands/PathfindHolonomic.h +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once - -#include "pathplanner/lib/commands/PathfindingCommand.h" -#include "pathplanner/lib/controllers/PPHolonomicDriveController.h" - -namespace pathplanner { -class PathfindHolonomic: public PathfindingCommand { -public: - /** - * Constructs a new PathfindHolonomic command that will generate a path towards the given path. - * - * @param targetPath the path to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (robot relative) - * @param config HolonomicPathFollowerConfig object with the configuration parameters for path - * following - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command - * @param rotationDelayDistance Distance to delay the target rotation of the robot. This will - * cause the robot to hold its current rotation until it reaches the given distance along the - * path. Default = 0 m - */ - PathfindHolonomic(std::shared_ptr targetPath, - PathConstraints constraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, - HolonomicPathFollowerConfig config, - std::function shouldFlipPath, - frc2::Requirements requirements, - units::meter_t rotationDelayDistance = 0_m) : PathfindingCommand( - targetPath, constraints, poseSupplier, currentRobotRelativeSpeeds, - output, - std::make_unique < PPHolonomicDriveController - > (config.translationConstants, config.rotationConstants, config.maxModuleSpeed, config.driveBaseRadius, config.period), - rotationDelayDistance, config.replanningConfig, shouldFlipPath, - requirements) { - } - - /** - * Constructs a new PathfindHolonomic command that will generate a path towards the given pose. - * - * @param targetPose the pose to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param goalEndVel The goal end velocity when reaching the given pose - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (field relative if holonomic, robot relative if - * differential) - * @param config HolonomicPathFollowerConfig object with the configuration parameters for path - * following - * @param requirements the subsystems required by this command - * @param rotationDelayDistance Distance to delay the target rotation of the robot. This will - * cause the robot to hold its current rotation until it reaches the given distance along the - * path. Default = 0 m - */ - PathfindHolonomic(frc::Pose2d targetPose, PathConstraints constraints, - units::meters_per_second_t goalEndVel, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, - HolonomicPathFollowerConfig config, frc2::Requirements requirements, - units::meter_t rotationDelayDistance = 0_m) : PathfindingCommand( - targetPose, constraints, goalEndVel, poseSupplier, - currentRobotRelativeSpeeds, output, - std::make_unique < PPHolonomicDriveController - > (config.translationConstants, config.rotationConstants, config.maxModuleSpeed, config.driveBaseRadius, config.period), - rotationDelayDistance, config.replanningConfig, requirements) { - } - - /** - * Constructs a new PathfindHolonomic command that will generate a path towards the given pose. - * - * @param targetPose the pose to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (field relative if holonomic, robot relative if - * differential) - * @param config HolonomicPathFollowerConfig object with the configuration parameters for path - * following - * @param requirements the subsystems required by this command - * @param rotationDelayDistance Distance to delay the target rotation of the robot. This will - * cause the robot to hold its current rotation until it reaches the given distance along the - * path. Default = 0 m - */ - PathfindHolonomic(frc::Pose2d targetPose, PathConstraints constraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, - HolonomicPathFollowerConfig config, frc2::Requirements requirements, - units::meter_t rotationDelayDistance = 0_m) : PathfindHolonomic( - targetPose, constraints, 0_mps, poseSupplier, - currentRobotRelativeSpeeds, output, config, requirements, - rotationDelayDistance) { - } -}; -} diff --git a/src/main/include/pathplanner/lib/commands/PathfindLTV.h b/src/main/include/pathplanner/lib/commands/PathfindLTV.h deleted file mode 100644 index 44f8007..0000000 --- a/src/main/include/pathplanner/lib/commands/PathfindLTV.h +++ /dev/null @@ -1,129 +0,0 @@ -#pragma once - -#include "pathplanner/lib/commands/PathfindingCommand.h" -#include "pathplanner/lib/controllers/PPLTVController.h" - -namespace pathplanner { -class PathfindLTV: public PathfindingCommand { -public: - /** - * Constructs a new PathfindLTV command that will generate a path towards the given path. - * - * @param targetPath the path to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (robot relative) - * @param Qelems The maximum desired error tolerance for each state. - * @param Relems The maximum desired control effort for each input. - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command - */ - PathfindLTV(std::shared_ptr targetPath, - PathConstraints constraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, - const wpi::array &Qelems, - const wpi::array &Relems, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements) : PathfindingCommand(targetPath, - constraints, poseSupplier, currentRobotRelativeSpeeds, output, - std::make_unique < PPLTVController > (Qelems, Relems, dt), 0_m, - replanningConfig, shouldFlipPath, requirements) { - if (targetPath->isChoreoPath()) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Paths loaded from Choreo cannot be used with differential drivetrains"); - } - } - - /** - * Constructs a new PathfindLTV command that will generate a path towards the given path. - * - * @param targetPath the path to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (robot relative) - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command - */ - PathfindLTV(std::shared_ptr targetPath, - PathConstraints constraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements) : PathfindingCommand(targetPath, - constraints, poseSupplier, currentRobotRelativeSpeeds, output, - std::make_unique < PPLTVController > (dt), 0_m, replanningConfig, - shouldFlipPath, requirements) { - if (targetPath->isChoreoPath()) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Paths loaded from Choreo cannot be used with differential drivetrains"); - } - } - - /** - * Constructs a new PathfindLTV command that will generate a path towards the given position. - * - * @param targetPosition the position to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param goalEndVel The goal end velocity when reaching the given position - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (robot relative) - * @param Qelems The maximum desired error tolerance for each state. - * @param Relems The maximum desired control effort for each input. - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param requirements the subsystems required by this command - */ - PathfindLTV(frc::Translation2d targetPosition, PathConstraints constraints, - units::meters_per_second_t goalEndVel, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, - const wpi::array &Qelems, - const wpi::array &Relems, units::second_t dt, - ReplanningConfig replanningConfig, frc2::Requirements requirements) : PathfindingCommand( - frc::Pose2d(targetPosition, frc::Rotation2d()), constraints, - goalEndVel, poseSupplier, currentRobotRelativeSpeeds, output, - std::make_unique < PPLTVController > (Qelems, Relems, dt), 0_m, - replanningConfig, requirements) { - } - - /** - * Constructs a new PathfindLTV command that will generate a path towards the given position. - * - * @param targetPosition the position to pathfind to - * @param constraints the path constraints to use while pathfinding - * @param goalEndVel The goal end velocity when reaching the given position - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param output a consumer for the output speeds (robot relative) - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param requirements the subsystems required by this command - */ - PathfindLTV(frc::Translation2d targetPosition, PathConstraints constraints, - units::meters_per_second_t goalEndVel, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function output, units::second_t dt, - ReplanningConfig replanningConfig, frc2::Requirements requirements) : PathfindingCommand( - frc::Pose2d(targetPosition, frc::Rotation2d()), constraints, - goalEndVel, poseSupplier, currentRobotRelativeSpeeds, output, - std::make_unique < PPLTVController > (dt), 0_m, replanningConfig, - requirements) { - } -}; -} diff --git a/src/main/include/pathplanner/lib/commands/PathfindThenFollowPath.h b/src/main/include/pathplanner/lib/commands/PathfindThenFollowPath.h new file mode 100644 index 0000000..ad3b8bf --- /dev/null +++ b/src/main/include/pathplanner/lib/commands/PathfindThenFollowPath.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include "pathplanner/lib/commands/FollowPathCommand.h" +#include "pathplanner/lib/commands/PathfindingCommand.h" + +namespace pathplanner { +class PathfindThenFollowPath: public frc2::SequentialCommandGroup { +public: + /** + * Constructs a new PathfindThenFollowPath command group. + * + * @param goalPath the goal path to follow + * @param pathfindingConstraints the path constraints for pathfinding + * @param poseSupplier a supplier for the robot's current pose + * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds + * @param output Output function that accepts robot-relative ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *

NOTE: These feedforwards are assuming unoptimized module states. When you optimize your + * module states, you will need to reverse the feedforwards for modules that have been flipped + * @param controller Path following controller that will be used to follow the path + * @param robotConfig The robot configuration + * @param shouldFlipPath Should the target path be flipped to the other side of the field? This + * will maintain a global blue alliance origin. + * @param requirements the subsystems required by this command (drive subsystem) + */ + PathfindThenFollowPath(std::shared_ptr goalPath, + PathConstraints pathfindingConstraints, + std::function poseSupplier, + std::function currentRobotRelativeSpeeds, + std::function< + void(frc::ChassisSpeeds, std::vector)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, + frc2::Requirements requirements) { + AddCommands( + PathfindingCommand(goalPath, pathfindingConstraints, + poseSupplier, currentRobotRelativeSpeeds, output, + controller, robotConfig, shouldFlipPath, requirements), + FollowPathCommand(goalPath, poseSupplier, + currentRobotRelativeSpeeds, output, controller, + robotConfig, shouldFlipPath, requirements)); + } +}; +} diff --git a/src/main/include/pathplanner/lib/commands/PathfindThenFollowPathHolonomic.h b/src/main/include/pathplanner/lib/commands/PathfindThenFollowPathHolonomic.h deleted file mode 100644 index 59ef989..0000000 --- a/src/main/include/pathplanner/lib/commands/PathfindThenFollowPathHolonomic.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include -#include "pathplanner/lib/commands/FollowPathHolonomic.h" -#include "pathplanner/lib/commands/PathfindHolonomic.h" - -namespace pathplanner { -class PathfindThenFollowPathHolonomic: public frc2::SequentialCommandGroup { -public: - /** - * Constructs a new PathfindThenFollowPathHolonomic command group. - * - * @param goalPath the goal path to follow - * @param pathfindingConstraints the path constraints for pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param robotRelativeOutput a consumer for the output speeds (robot relative) - * @param config HolonomicPathFollowerConfig for configuring the path following commands - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command (drive subsystem) - * @param rotationDelayDistance Distance to delay the target rotation of the robot. This will - * cause the robot to hold its current rotation until it reaches the given distance along the - * path. Default = 0 m - */ - PathfindThenFollowPathHolonomic(std::shared_ptr goalPath, - PathConstraints pathfindingConstraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function robotRelativeOutput, - HolonomicPathFollowerConfig config, - std::function shouldFlipPath, - frc2::Requirements requirements, - units::meter_t rotationDelayDistance = 0_m) { - AddCommands( - PathfindHolonomic(goalPath, pathfindingConstraints, - poseSupplier, currentRobotRelativeSpeeds, - robotRelativeOutput, config, shouldFlipPath, - requirements, rotationDelayDistance), - FollowPathHolonomic(goalPath, poseSupplier, - currentRobotRelativeSpeeds, robotRelativeOutput, config, - shouldFlipPath, requirements)); - } -}; -} diff --git a/src/main/include/pathplanner/lib/commands/PathfindThenFollowPathLTV.h b/src/main/include/pathplanner/lib/commands/PathfindThenFollowPathLTV.h deleted file mode 100644 index dcc819f..0000000 --- a/src/main/include/pathplanner/lib/commands/PathfindThenFollowPathLTV.h +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -#include -#include "pathplanner/lib/commands/FollowPathLTV.h" -#include "pathplanner/lib/commands/PathfindLTV.h" - -namespace pathplanner { -class PathfindThenFollowPathLTV: public frc2::SequentialCommandGroup { -public: - /** - * Constructs a new PathfindThenFollowPathLTV command group. - * - * @param goalPath the goal path to follow - * @param pathfindingConstraints the path constraints for pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param robotRelativeOutput a consumer for the output speeds (robot relative) - * @param qelems The maximum desired error tolerance for each state. - * @param relems The maximum desired control effort for each input. - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command (drive subsystem) - */ - PathfindThenFollowPathLTV(std::shared_ptr goalPath, - PathConstraints pathfindingConstraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function robotRelativeOutput, - const wpi::array &Qelems, - const wpi::array &Relems, units::second_t dt, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements) { - AddCommands( - PathfindLTV(goalPath, pathfindingConstraints, poseSupplier, - currentRobotRelativeSpeeds, robotRelativeOutput, Qelems, - Relems, dt, replanningConfig, shouldFlipPath, - requirements), - FollowPathLTV(goalPath, poseSupplier, - currentRobotRelativeSpeeds, robotRelativeOutput, Qelems, - Relems, dt, replanningConfig, shouldFlipPath, - requirements)); - } - - /** - * Constructs a new PathfindThenFollowPathLTV command group. - * - * @param goalPath the goal path to follow - * @param pathfindingConstraints the path constraints for pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds - * @param robotRelativeOutput a consumer for the output speeds (robot relative) - * @param dt Period of the robot control loop in seconds (default 0.02) - * @param replanningConfig Path replanning configuration - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command (drive subsystem) - */ - PathfindThenFollowPathLTV(std::shared_ptr goalPath, - PathConstraints pathfindingConstraints, - std::function poseSupplier, - std::function currentRobotRelativeSpeeds, - std::function robotRelativeOutput, - units::second_t dt, ReplanningConfig replanningConfig, - std::function shouldFlipPath, - frc2::Requirements requirements) { - AddCommands( - PathfindLTV(goalPath, pathfindingConstraints, poseSupplier, - currentRobotRelativeSpeeds, robotRelativeOutput, dt, - replanningConfig, shouldFlipPath, requirements), - FollowPathLTV(goalPath, poseSupplier, - currentRobotRelativeSpeeds, robotRelativeOutput, dt, - replanningConfig, shouldFlipPath, requirements)); - } -}; -} diff --git a/src/main/include/pathplanner/lib/commands/PathfindingCommand.h b/src/main/include/pathplanner/lib/commands/PathfindingCommand.h index b852c03..caf2df2 100644 --- a/src/main/include/pathplanner/lib/commands/PathfindingCommand.h +++ b/src/main/include/pathplanner/lib/commands/PathfindingCommand.h @@ -12,11 +12,12 @@ #include #include #include "pathplanner/lib/path/PathPlannerPath.h" -#include "pathplanner/lib/path/PathPlannerTrajectory.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" #include "pathplanner/lib/controllers/PathFollowingController.h" -#include "pathplanner/lib/config/ReplanningConfig.h" +#include "pathplanner/lib/config/RobotConfig.h" #include "pathplanner/lib/util/PathPlannerLogging.h" #include "pathplanner/lib/util/PPLibTelemetry.h" +#include "pathplanner/lib/util/DriveFeedforward.h" namespace pathplanner { class PathfindingCommand: public frc2::CommandHelperNOTE: These feedforwards are assuming unoptimized module states. When you optimize your + * module states, you will need to reverse the feedforwards for modules that have been flipped * @param controller Path following controller that will be used to follow the path - * @param rotationDelayDistance How far the robot should travel before attempting to rotate to the - * final rotation - * @param replanningConfig Path replanning configuration + * @param robotConfig The robot configuration * @param shouldFlipPath Should the target path be flipped to the other side of the field? This * will maintain a global blue alliance origin. * @param requirements the subsystems required by this command @@ -42,11 +45,10 @@ class PathfindingCommand: public frc2::CommandHelper poseSupplier, std::function speedsSupplier, - std::function output, - std::unique_ptr controller, - units::meter_t rotationDelayDistance, - ReplanningConfig replanningConfig, - std::function shouldFlipPath, + std::function< + void(frc::ChassisSpeeds, std::vector)> output, + std::shared_ptr controller, + RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements); /** @@ -58,21 +60,23 @@ class PathfindingCommand: public frc2::CommandHelperNOTE: These feedforwards are assuming unoptimized module states. When you optimize your + * module states, you will need to reverse the feedforwards for modules that have been flipped * @param controller Path following controller that will be used to follow the path - * @param rotationDelayDistance How far the robot should travel before attempting to rotate to the - * final rotation - * @param replanningConfig Path replanning configuration + * @param robotConfig The robot configuration * @param requirements the subsystems required by this command */ PathfindingCommand(frc::Pose2d targetPose, PathConstraints constraints, units::meters_per_second_t goalEndVel, std::function poseSupplier, std::function speedsSupplier, - std::function output, - std::unique_ptr controller, - units::meter_t rotationDelayDistance, - ReplanningConfig replanningConfig, frc2::Requirements requirements); + std::function< + void(frc::ChassisSpeeds, std::vector)> output, + std::shared_ptr controller, + RobotConfig robotConfig, frc2::Requirements requirements); void Initialize() override; @@ -91,27 +95,16 @@ class PathfindingCommand: public frc2::CommandHelper m_poseSupplier; std::function m_speedsSupplier; - std::function m_output; - std::unique_ptr m_controller; - units::meter_t m_rotationDelayDistance; - ReplanningConfig m_replanningConfig; + std::function)> m_output; + std::shared_ptr m_controller; + RobotConfig m_robotConfig; std::function m_shouldFlipPath; std::shared_ptr m_currentPath; PathPlannerTrajectory m_currentTrajectory; - frc::Pose2d m_startingPose; units::second_t m_timeOffset; - inline void replanPath(const frc::Pose2d ¤tPose, - const frc::ChassisSpeeds ¤tSpeeds) { - auto replanned = m_currentPath->replan(currentPose, currentSpeeds); - m_currentTrajectory = replanned->getTrajectory(currentSpeeds, - currentPose.Rotation()); - PathPlannerLogging::logActivePath(replanned); - PPLibTelemetry::setCurrentPath(replanned); - } - static int m_instances; }; } diff --git a/src/main/include/pathplanner/lib/config/ModuleConfig.h b/src/main/include/pathplanner/lib/config/ModuleConfig.h index 1a7f10d..3a363df 100644 --- a/src/main/include/pathplanner/lib/config/ModuleConfig.h +++ b/src/main/include/pathplanner/lib/config/ModuleConfig.h @@ -4,51 +4,52 @@ #include #include #include -#include "pathplanner/lib/config/MotorTorqueCurve.h" +#include +#include #ifndef M_PI #define M_PI 3.14159265358979323846 #endif -namespace units { -UNIT_ADD(velocity, mps_per_rpm, mps_per_rpm, mps_per_rpm, compound_unit>) -} - namespace pathplanner { class ModuleConfig { public: -units::meter_t wheelRadius; -double driveGearing; -units::revolutions_per_minute_t maxDriveVelocityRPM; -double wheelCOF; -MotorTorqueCurve driveMotorTorqueCurve; + units::meter_t wheelRadius; + units::meters_per_second_t maxDriveVelocityMPS; + double wheelCOF; + frc::DCMotor driveMotor; + units::ampere_t driveCurrentLimit; -units::mps_per_rpm_t rpmToMps; -units::meters_per_second_t maxDriveVelocityMPS; + units::radians_per_second_t maxDriveVelocityRadPerSec; -units::newton_meter_t torqueLoss; + units::newton_meter_t torqueLoss; -/** - * Configuration of a robot drive module. This can either be a swerve module, or one side of a - * differential drive train. - * - * @param wheelRadius Radius of the drive wheels, in meters. - * @param driveGearing The gear ratio between the drive motor and the wheel. Values > 1 indicate a - * reduction. - * @param maxDriveVelocityRPM The max RPM that the drive motor can reach while actually driving - * the robot at full output. - * @param wheelCOF The coefficient of friction between the drive wheel and the carpet. If you are - * unsure, just use a placeholder value of 1.0. - * @param driveMotorTorqueCurve The torque curve for the drive motor - */ -ModuleConfig(units::meter_t wheelRadius, double driveGearing, - units::revolutions_per_minute_t maxDriveVelocityRPM, double wheelCOF, - MotorTorqueCurve driveMotorTorqueCurve) : wheelRadius(wheelRadius), driveGearing( - driveGearing), maxDriveVelocityRPM(maxDriveVelocityRPM), wheelCOF( - wheelCOF), driveMotorTorqueCurve(driveMotorTorqueCurve), rpmToMps { - ((1.0 / 60.0) / driveGearing) * (2.0 * M_PI * wheelRadius()) }, maxDriveVelocityMPS( - maxDriveVelocityRPM * rpmToMps), torqueLoss( - driveMotorTorqueCurve[maxDriveVelocityRPM]) { -} + /** + * Configuration of a robot drive module. This can either be a swerve module, or one side of a + * differential drive train. + * + * @param wheelRadius Radius of the drive wheels, in meters. + * @param maxDriveVelocityMPS The max speed that the drive motor can reach while actually driving + * the robot at full output, in M/S. + * @param wheelCOF The coefficient of friction between the drive wheel and the carpet. If you are + * unsure, just use a placeholder value of 1.0. + * @param driveMotor The DCMotor representing the drive motor gearbox, including gear reduction + * @param driveCurrentLimit The current limit of the drive motor, in Amps + * @param numMotors The number of motors per module. For swerve, this is 1. For differential, this is usually 2. + */ + ModuleConfig(units::meter_t wheelRadius, + units::meters_per_second_t maxDriveVelocityMPS, double wheelCOF, + frc::DCMotor driveMotor, units::ampere_t driveCurrentLimit, + int numMotors) : wheelRadius(wheelRadius), maxDriveVelocityMPS( + maxDriveVelocityMPS), wheelCOF(wheelCOF), driveMotor(driveMotor), driveCurrentLimit( + driveCurrentLimit * numMotors), maxDriveVelocityRadPerSec { + maxDriveVelocityMPS() / wheelRadius() }, torqueLoss( + units::math::max( + driveMotor.Torque( + units::math::min( + driveMotor.Current( + maxDriveVelocityRadPerSec, 12_V), + driveCurrentLimit)), 0_Nm)) { + } }; } diff --git a/src/main/include/pathplanner/lib/config/MotorTorqueCurve.h b/src/main/include/pathplanner/lib/config/MotorTorqueCurve.h deleted file mode 100644 index 5a3575b..0000000 --- a/src/main/include/pathplanner/lib/config/MotorTorqueCurve.h +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace units { -UNIT_ADD(torque, newton_meter_per_amp, newton_meters_per_amp, nm_per_amp, compound_unit>) -} - -namespace pathplanner { -enum MotorType { -/** Kraken X60 */ -krakenX60, -/** Kraken X60 with FOC */ -krakenX60_FOC, -/** Falcon 500 */ -falcon500, -/** Falcon 500 with FOC */ -falcon500_FOC, -/** NEO Vortex */ -neoVortex, -/** NEO */ -neo, -/** CIM */ -cim, -/** Mini CIM */ -miniCim -}; - -enum CurrentLimit { -/** 40 Amp limit */ -k40A, -/** 60 Amp Limit */ -k60A, -/** 80 Amp limit */ -k80A -}; - -class MotorTorqueCurve: public wpi::interpolating_map< - units::revolutions_per_minute_t, units::newton_meter_t> { -public: -/** - * Create an empty motor torque curve. This can be used to make a custom curve. Only use this if - * you know what you're doing - * - * @param nmPerAmp Yhe motor's "kT" value, or the conversion from current draw to torque, in - * Newton-meters per Amp - */ -MotorTorqueCurve(const units::newton_meter_per_amp_t nmPerAmp) : m_nmPerAmp( - nmPerAmp) { -} - -/** - * Create a new motor torque curve - * - * @param motorType The type of motor - * @param currentLimit The current limit of the motor - */ -MotorTorqueCurve(MotorType motorType, CurrentLimit currentLimit); - -/** - * Get the motor's "kT" value, or the conversion from current draw to torque - * - * @return Newton-meters per Amp - */ -constexpr units::newton_meter_per_amp_t getNmPerAmp() const { - return m_nmPerAmp; -} - -/** - * Create a motor torque curve for the string representing a motor and current limit saved in the - * GUI settings - * - * @param torqueCurveName The name of the torque curve - * @return The torque curve corresponding to the given name - */ -static MotorTorqueCurve fromSettingsString(std::string torqueCurveName); - -private: -units::newton_meter_per_amp_t m_nmPerAmp; - -static MotorType motorTypeFromSettingsString(std::string name); - -static CurrentLimit currentLimitFromSettingsString(std::string name); - -void initKrakenX60(const CurrentLimit currentLimit); - -void initKrakenX60FOC(const CurrentLimit currentLimit); - -void initFalcon500(const CurrentLimit currentLimit); - -void initFalcon500FOC(const CurrentLimit currentLimit); - -void initNEOVortex(const CurrentLimit currentLimit); - -void initNEO(const CurrentLimit currentLimit); - -void initCIM(const CurrentLimit currentLimit); - -void initMiniCIM(const CurrentLimit currentLimit); -}; -} diff --git a/src/main/include/pathplanner/lib/config/ReplanningConfig.h b/src/main/include/pathplanner/lib/config/ReplanningConfig.h deleted file mode 100644 index 0f42834..0000000 --- a/src/main/include/pathplanner/lib/config/ReplanningConfig.h +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include - -namespace pathplanner { -class ReplanningConfig { -public: - const bool enableInitialReplanning; - const bool enableDynamicReplanning; - const units::meter_t dynamicReplanningTotalErrorThreshold; - const units::meter_t dynamicReplanningErrorSpikeThreshold; - - /** - * Create a path replanning configuration - * - * @param enableInitialReplanning Should the path be replanned at the start of path following if - * the robot is not already at the starting point? - * @param enableDynamicReplanning Should the path be replanned if the error grows too large or if - * a large error spike happens while following the path? - * @param dynamicReplanningTotalErrorThreshold The total error threshold, in meters, that will - * cause the path to be replanned. Default = 1.0m - * @param dynamicReplanningErrorSpikeThreshold The error spike threshold, in meters, that will - * cause the path to be replanned. Default = 0.25m - */ - constexpr ReplanningConfig(const bool enableInitialReplanning = true, - const bool enableDynamicReplanning = false, - const units::meter_t dynamicReplanningTotalErrorThreshold = 1_m, - const units::meter_t dynamicReplanningErrorSpikeThreshold = 0.25_m) : enableInitialReplanning( - enableInitialReplanning), enableDynamicReplanning( - enableDynamicReplanning), dynamicReplanningTotalErrorThreshold( - dynamicReplanningTotalErrorThreshold), dynamicReplanningErrorSpikeThreshold( - dynamicReplanningErrorSpikeThreshold) { - } -}; -} diff --git a/src/main/include/pathplanner/lib/config/RobotConfig.h b/src/main/include/pathplanner/lib/config/RobotConfig.h index 1fe86ed..901b1d8 100644 --- a/src/main/include/pathplanner/lib/config/RobotConfig.h +++ b/src/main/include/pathplanner/lib/config/RobotConfig.h @@ -3,11 +3,15 @@ #include #include #include +#include #include #include #include +#include +#include #include #include "pathplanner/lib/config/ModuleConfig.h" +#include "pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h" namespace pathplanner { class RobotConfig { @@ -17,16 +21,12 @@ class RobotConfig { ModuleConfig moduleConfig; std::vector moduleLocations; - - // Need two different kinematics objects since the class is templated but having - // RobotConfig also templated would be pretty bad to work with - frc::SwerveDriveKinematics<4> swerveKinematics; - frc::SwerveDriveKinematics<2> diffKinematics; bool isHolonomic; size_t numModules; std::vector modulePivotDistance; units::newton_t wheelFrictionForce; + units::newton_meter_t maxTorqueFriction; RobotConfig(units::kilogram_t mass, units::kilogram_square_meter_t MOI, ModuleConfig moduleConfig, units::meter_t trackwidth, @@ -36,5 +36,37 @@ class RobotConfig { ModuleConfig moduleConfig, units::meter_t trackwidth); static RobotConfig fromGUISettings(); + + /** + * Convert robot-relative chassis speeds to a vector of swerve module states. This will use + * differential kinematics for diff drive robots, then convert the wheel speeds to module states. + * + * @param speeds Robot-relative chassis speeds + * @return Vector of swerve module states + */ + std::vector toSwerveModuleStates( + frc::ChassisSpeeds speeds) const; + + /** + * Convert a vector of swerve module states to robot-relative chassis speeds. This will use + * differential kinematics for diff drive robots. + * + * @param states Vector of swerve module states + * @return Robot-relative chassis speeds + */ + frc::ChassisSpeeds toChassisSpeeds( + std::vector states) const; + + std::vector chassisForcesToWheelForceVectors( + frc::ChassisSpeeds chassisForces) const; + +private: + frc::SwerveDriveKinematics<4> swerveKinematics; + frc::DifferentialDriveKinematics diffKinematics; + frc::Matrixd<4 * 2, 3> swerveForceKinematics; + frc::Matrixd<2 * 2, 3> diffForceKinematics; + + static frc::DCMotor getMotorFromSettingsString(std::string motorStr, + int numMotors); }; } diff --git a/src/main/include/pathplanner/lib/controllers/PPHolonomicDriveController.h b/src/main/include/pathplanner/lib/controllers/PPHolonomicDriveController.h index ffc525f..935babd 100644 --- a/src/main/include/pathplanner/lib/controllers/PPHolonomicDriveController.h +++ b/src/main/include/pathplanner/lib/controllers/PPHolonomicDriveController.h @@ -14,8 +14,7 @@ #include #include "pathplanner/lib/util/GeometryUtil.h" #include "pathplanner/lib/config/PIDConstants.h" -#include "pathplanner/lib/util/HolonomicPathFollowerConfig.h" -#include "pathplanner/lib/path/PathPlannerTrajectory.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" #include "pathplanner/lib/controllers/PathFollowingController.h" namespace pathplanner { @@ -26,16 +25,10 @@ class PPHolonomicDriveController: public PathFollowingController { * * @param translationConstants PID constants for the translation PID controllers * @param rotationConstants PID constants for the rotation controller - * @param maxModuleSpeed The max speed of a drive module in meters/sec - * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the - * distance from the center of the robot to the furthest module. For mecanum, this is the - * drive base width / 2 * @param period Period of the control loop in seconds */ PPHolonomicDriveController(PIDConstants translationConstants, - PIDConstants rotationConstants, - units::meters_per_second_t maxModuleSpeed, - units::meter_t driveBaseRadius, units::second_t period = 0.02_s); + PIDConstants rotationConstants, units::second_t period = 0.02_s); /** * Enables and disables the controller for troubleshooting. When calculate() is called on a @@ -49,8 +42,9 @@ class PPHolonomicDriveController: public PathFollowingController { inline void reset(const frc::Pose2d ¤tPose, const frc::ChassisSpeeds ¤tSpeeds) override { - m_rotationController.Reset(currentPose.Rotation().Radians(), - currentSpeeds.omega); + m_xController.Reset(); + m_yController.Reset(); + m_rotationController.Reset(); } /** @@ -71,7 +65,7 @@ class PPHolonomicDriveController: public PathFollowingController { */ frc::ChassisSpeeds calculateRobotRelativeSpeeds( const frc::Pose2d ¤tPose, - const PathPlannerTrajectory::State &referenceState) override; + const PathPlannerTrajectoryState &referenceState) override; /** * Is this controller for holonomic drivetrains? Used to handle some differences in functionality @@ -89,24 +83,108 @@ class PPHolonomicDriveController: public PathFollowingController { * This function should return an empty optional to use the rotation targets in the path * @param rotationTargetOverride Supplier to override rotation targets */ + [[deprecated("Use overrideRotationFeedback instead, with the output of your own PID controller")]] static inline void setRotationTargetOverride( std::function()> rotationTargetOverride) { PPHolonomicDriveController::rotationTargetOverride = rotationTargetOverride; } -private: - using rpsPerMps_t = units::unit_t>>; + /** + * Begin overriding the X axis feedback. + * + * @param xFeedbackOverride Function that returns the desired FIELD-RELATIVE X feedback in meters/sec + */ + static inline void overrideXFeedback( + std::function xFeedbackOverride) { + PPHolonomicDriveController::xFeedbackOverride = xFeedbackOverride; + } + + /** + * Stop overriding the X axis feedback, and return to calculating it based on path following + * error. + */ + static inline void clearXFeedbackOverride() { + PPHolonomicDriveController::xFeedbackOverride = { }; + } + + /** + * Begin overriding the Y axis feedback. + * + * @param yFeedbackOverride Function that returns the desired FIELD-RELATIVE Y feedback in meters/sec + */ + static inline void overrideYFeedback( + std::function yFeedbackOverride) { + PPHolonomicDriveController::yFeedbackOverride = yFeedbackOverride; + } + + /** + * Stop overriding the Y axis feedback, and return to calculating it based on path following + * error. + */ + static inline void clearYFeedbackOverride() { + PPHolonomicDriveController::yFeedbackOverride = { }; + } + + /** + * Begin overriding the X and Y axis feedback. + * + * @param xFeedbackOverride Function that returns the desired FIELD-RELATIVE X feedback in meters/sec + * @param yFeedbackOverride Function that returns the desired FIELD-RELATIVE Y feedback in meters/sec + */ + static inline void overrideXYFeedback( + std::function xFeedbackOverride, + std::function yFeedbackOverride) { + overrideXFeedback(xFeedbackOverride); + overrideYFeedback(yFeedbackOverride); + } + + /** + * Stop overriding the X and Y axis feedback, and return to calculating them based on path + * following error. + */ + static inline void clearXYFeedbackOverride() { + clearXFeedbackOverride(); + clearYFeedbackOverride(); + } + + /** + * Begin overriding the rotation feedback. + * + * @param rotationFeedbackOverride Function that returns the desired rotation feedback in radians/sec + */ + static inline void overrideRotationFeedback( + std::function rotationFeedbackOverride) { + PPHolonomicDriveController::rotationFeedbackOverride = + rotationFeedbackOverride; + } + + /** + * Stop overriding the rotation feedback, and return to calculating it based on path following + * error. + */ + static inline void clearRotationFeedbackOverride() { + PPHolonomicDriveController::rotationFeedbackOverride = { }; + } + + /** Clear all feedback overrides and return to purely using path following error for feedback */ + static inline void clearFeedbackOverrides() { + clearXYFeedbackOverride(); + clearRotationFeedbackOverride(); + } +private: frc::PIDController m_xController; frc::PIDController m_yController; - frc::ProfiledPIDController m_rotationController; - units::meters_per_second_t m_maxModuleSpeed; - rpsPerMps_t m_mpsToRps; + frc::PIDController m_rotationController; frc::Translation2d m_translationError; bool m_enabled = true; static std::function()> rotationTargetOverride; + + static std::function xFeedbackOverride; + static std::function yFeedbackOverride; + static std::function rotationFeedbackOverride; }; } diff --git a/src/main/include/pathplanner/lib/controllers/PPLTVController.h b/src/main/include/pathplanner/lib/controllers/PPLTVController.h index 5748baa..6c1d979 100644 --- a/src/main/include/pathplanner/lib/controllers/PPLTVController.h +++ b/src/main/include/pathplanner/lib/controllers/PPLTVController.h @@ -43,11 +43,12 @@ class PPLTVController: public frc::LTVUnicycleController, frc::ChassisSpeeds calculateRobotRelativeSpeeds( const frc::Pose2d ¤tPose, - const PathPlannerTrajectory::State &targetState) override { - m_lastError = currentPose.Translation().Distance(targetState.position); + const PathPlannerTrajectoryState &targetState) override { + m_lastError = currentPose.Translation().Distance( + targetState.pose.Translation()); - return Calculate(currentPose, targetState.getDifferentialPose(), - targetState.velocity, targetState.headingAngularVelocity); + return Calculate(currentPose, targetState.pose, + targetState.linearVelocity, targetState.fieldSpeeds.omega); } inline void reset(const frc::Pose2d ¤tPose, diff --git a/src/main/include/pathplanner/lib/controllers/PathFollowingController.h b/src/main/include/pathplanner/lib/controllers/PathFollowingController.h index dba487b..6085996 100644 --- a/src/main/include/pathplanner/lib/controllers/PathFollowingController.h +++ b/src/main/include/pathplanner/lib/controllers/PathFollowingController.h @@ -3,7 +3,7 @@ #include #include #include -#include "pathplanner/lib/path/PathPlannerTrajectory.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" namespace pathplanner { class PathFollowingController { @@ -20,7 +20,7 @@ class PathFollowingController { */ virtual frc::ChassisSpeeds calculateRobotRelativeSpeeds( const frc::Pose2d ¤tPose, - const PathPlannerTrajectory::State &targetState) = 0; + const PathPlannerTrajectoryState &targetState) = 0; /** * Resets the controller based on the current state of the robot diff --git a/src/main/include/pathplanner/lib/events/CancelCommandEvent.h b/src/main/include/pathplanner/lib/events/CancelCommandEvent.h new file mode 100644 index 0000000..6b8d4d0 --- /dev/null +++ b/src/main/include/pathplanner/lib/events/CancelCommandEvent.h @@ -0,0 +1,38 @@ +#pragma once + +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/EventScheduler.h" +#include +#include + +namespace pathplanner { +class CancelCommandEvent: public Event { +public: + /** + * Create an event to cancel a command + * + * @param timestamp The trajectory timestamp for this event + * @param command The command to cancel + */ + CancelCommandEvent(units::second_t timestamp, + std::shared_ptr command) : Event(timestamp), m_command( + command) { + } + + inline void handleEvent(EventScheduler *eventScheduler) override { + eventScheduler->cancelCommand(m_command); + } + + inline void cancelEvent(EventScheduler *eventScheduler) override { + // Do nothing + } + + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < CancelCommandEvent > (timestamp, m_command); + } + +private: + std::shared_ptr m_command; +}; +} diff --git a/src/main/include/pathplanner/lib/events/Event.h b/src/main/include/pathplanner/lib/events/Event.h new file mode 100644 index 0000000..e1bafff --- /dev/null +++ b/src/main/include/pathplanner/lib/events/Event.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +namespace pathplanner { + +class EventScheduler; + +class Event { +public: + /** + * Create a new event + * + * @param timestamp The trajectory timestamp this event should be handled at + */ + constexpr Event(units::second_t timestamp) : m_timestamp(timestamp) { + } + + virtual ~Event() { + } + + /** + * Get the trajectory timestamp for this event + * + * @return Trajectory timestamp, in seconds + */ + constexpr units::second_t getTimestamp() const { + return m_timestamp; + } + + /** + * Set the trajectory timestamp of this event + * + * @param timestamp Timestamp of this event along the trajectory + */ + constexpr void setTimestamp(units::second_t timestamp) { + m_timestamp = timestamp; + } + + /** + * Handle this event + * + * @param eventScheduler Pointer to the EventScheduler handling this event + */ + virtual void handleEvent(EventScheduler *eventScheduler) = 0; + + /** + * Cancel this event. This will be called if a path following command ends before this event gets + * handled. + * + * @param eventScheduler Reference to the EventScheduler handling this event + */ + virtual void cancelEvent(EventScheduler *eventScheduler) = 0; + + /** + * Copy this event with a different timestamp + * + * @param timestamp The new timestamp + * @return Copied event with new time + */ + virtual std::shared_ptr copyWithTimestamp( + units::second_t timestamp) = 0; + +private: + units::second_t m_timestamp; +}; +} diff --git a/src/main/include/pathplanner/lib/events/EventScheduler.h b/src/main/include/pathplanner/lib/events/EventScheduler.h new file mode 100644 index 0000000..4c9dc79 --- /dev/null +++ b/src/main/include/pathplanner/lib/events/EventScheduler.h @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" +#include "pathplanner/lib/path/PathPlannerPath.h" + +namespace pathplanner { +class EventScheduler { +public: + /** Create a new EventScheduler */ + EventScheduler() { + } + + /** + * Initialize the EventScheduler for the given trajectory. This should be called from the + * initialize method of the command running this scheduler. + * + * @param trajectory The trajectory this scheduler should handle events for + */ + inline void initialize(PathPlannerTrajectory trajectory) { + m_eventCommands.clear(); + m_upcomingEvents.clear(); + auto events = trajectory.getEvents(); + m_upcomingEvents.insert(m_upcomingEvents.end(), events.begin(), + events.end()); + } + + /** + * Run the scheduler. This should be called from the execute method of the command running this + * scheduler. + * + * @param time The current time along the trajectory + */ + void execute(units::second_t time); + + /** + * End commands currently/events currently being handled by this scheduler. This should be called + * from the end method of the command running this scheduler. + */ + void end(); + + /** + * Get the event requirements for the given path + * + * @param path The path to get all requirements for + * @return Set of event requirements for the given path + */ + static inline wpi::SmallSet getSchedulerRequirements( + std::shared_ptr path) { + wpi::SmallSet allReqs; + for (auto m : path->getEventMarkers()) { + auto markerReqs = m.getCommand()->GetRequirements(); + allReqs.insert(markerReqs.begin(), markerReqs.end()); + } + return allReqs; + } + + /** + * Schedule a command on this scheduler. This will cancel other commands that share requirements + * with the given command. Do not call this. + * + * @param command The command to schedule + */ + void scheduleCommand(std::shared_ptr command); + + /** + * Cancel a command on this scheduler. Do not call this. + * + * @param command The command to cancel + */ + void cancelCommand(std::shared_ptr command); + + static inline frc::EventLoop* getEventLoop() { + static frc::EventLoop *eventLoop = new frc::EventLoop(); + return eventLoop; + } + +private: + std::vector, bool>> m_eventCommands; + std::deque> m_upcomingEvents; +}; +} diff --git a/src/main/include/pathplanner/lib/events/EventTrigger.h b/src/main/include/pathplanner/lib/events/EventTrigger.h new file mode 100644 index 0000000..60b588e --- /dev/null +++ b/src/main/include/pathplanner/lib/events/EventTrigger.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include "pathplanner/lib/events/EventScheduler.h" + +namespace pathplanner { +class EventTrigger: public frc2::Trigger { +public: + /** + * Create a new EventTrigger + * + * @param name The name of the event. This will be the name of the event marker in the GUI + */ + EventTrigger(std::string name) : frc2::Trigger( + EventScheduler::getEventLoop(), pollCondition(name)) { + } + + /** + * Create a new EventTrigger that gets polled by the given event loop instead of the + * EventScheduler + * + * @param eventLoop The event loop to poll this trigger + * @param name The name of the event. This will be the name of the event marker in the GUI + */ + EventTrigger(frc::EventLoop *eventLoop, std::string name) : frc2::Trigger( + eventLoop, pollCondition(name)) { + } + + static inline void setCondition(std::string name, bool value) { + getEventConditions()[name] = value; + } + +private: + + static inline std::unordered_map& getEventConditions() { + static std::unordered_map *eventConditions = + new std::unordered_map(); + return *eventConditions; + } + + /** + * Create a boolean supplier that will poll a condition. + * + * @param name The name of the event + * @return A boolean supplier to poll the event's condition + */ + static inline std::function pollCondition(std::string name) { + // Ensure there is a condition in the map for this name + if (!getEventConditions().contains(name)) { + getEventConditions().emplace(name, false); + } + + return [name]() { + return getEventConditions()[name]; + }; + } +}; +} diff --git a/src/main/include/pathplanner/lib/events/OneShotTriggerEvent.h b/src/main/include/pathplanner/lib/events/OneShotTriggerEvent.h new file mode 100644 index 0000000..d96bb72 --- /dev/null +++ b/src/main/include/pathplanner/lib/events/OneShotTriggerEvent.h @@ -0,0 +1,47 @@ +#pragma once + +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/EventTrigger.h" +#include +#include +#include + +namespace pathplanner { +class OneShotTriggerEvent: public Event { +public: + /** + * Create an event for activating a trigger, then deactivating it the next loop + * + * @param timestamp The trajectory timestamp of this event + * @param name The name of the trigger to control + */ + OneShotTriggerEvent(units::second_t timestamp, std::string name) : Event( + timestamp), m_name(name), m_resetCommand( + frc2::cmd::Wait(0_s).AndThen(frc2::cmd::RunOnce([this]() { + EventTrigger::setCondition(m_name, false); + } + ) + ).IgnoringDisable(true)) { + } + + inline void handleEvent(EventScheduler *eventScheduler) override { + EventTrigger::setCondition(m_name, true); + // We schedule this command with the main command scheduler so that it is guaranteed to be run + // in its entirety, since the EventScheduler could cancel this command before it finishes + frc2::CommandScheduler::GetInstance().Schedule(m_resetCommand); + } + + inline void cancelEvent(EventScheduler *eventScheduler) override { + // Do nothing + } + + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < OneShotTriggerEvent > (timestamp, m_name); + } + +private: + std::string m_name; + frc2::CommandPtr m_resetCommand; +}; +} diff --git a/src/main/include/pathplanner/lib/events/PointTowardsZoneEvent.h b/src/main/include/pathplanner/lib/events/PointTowardsZoneEvent.h new file mode 100644 index 0000000..32ef78f --- /dev/null +++ b/src/main/include/pathplanner/lib/events/PointTowardsZoneEvent.h @@ -0,0 +1,42 @@ +#pragma once + +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/PointTowardsZoneTrigger.h" +#include + +namespace pathplanner { +class PointTowardsZoneEvent: public Event { +public: + /** + * Create an event for changing the value of a point towards zone trigger + * + * @param timestamp The trajectory timestamp of this event + * @param name The name of the point towards zone trigger to control + * @param active Should the trigger be activated by this event + */ + PointTowardsZoneEvent(units::second_t timestamp, std::string name, + bool active) : Event(timestamp), m_name(name), m_active(active) { + } + + inline void handleEvent(EventScheduler *eventScheduler) override { + PointTowardsZoneTrigger::setWithinZone(m_name, m_active); + } + + inline void cancelEvent(EventScheduler *eventScheduler) override { + if (!m_active) { + // Ensure this zone's condition gets set to false + PointTowardsZoneTrigger::setWithinZone(m_name, false); + } + } + + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < PointTowardsZoneEvent + > (timestamp, m_name, m_active); + } + +private: + std::string m_name; + bool m_active; +}; +} diff --git a/src/main/include/pathplanner/lib/events/PointTowardsZoneTrigger.h b/src/main/include/pathplanner/lib/events/PointTowardsZoneTrigger.h new file mode 100644 index 0000000..82810d2 --- /dev/null +++ b/src/main/include/pathplanner/lib/events/PointTowardsZoneTrigger.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include "pathplanner/lib/events/EventScheduler.h" + +namespace pathplanner { +class PointTowardsZoneTrigger: public frc2::Trigger { +public: + /** + * Create a new PointTowardsZoneTrigger + * + * @param name The name of the point towards zone + */ + PointTowardsZoneTrigger(std::string name) : frc2::Trigger( + EventScheduler::getEventLoop(), pollCondition(name)) { + } + + /** + * Create a new PointTowardsZoneTrigger that gets polled by the given event loop instead of the + * EventScheduler + * + * @param eventLoop The event loop to poll this trigger + * @param name The name of the point towards zone + */ + PointTowardsZoneTrigger(frc::EventLoop *eventLoop, std::string name) : frc2::Trigger( + eventLoop, pollCondition(name)) { + } + + static inline void setWithinZone(std::string name, bool withinZone) { + getZoneConditions()[name] = withinZone; + } + +private: + + static inline std::unordered_map& getZoneConditions() { + static std::unordered_map *zoneConditions = + new std::unordered_map(); + return *zoneConditions; + } + + /** + * Create a boolean supplier that will poll a condition. + * + * @param name The name of the event + * @return A boolean supplier to poll the event's condition + */ + static inline std::function pollCondition(std::string name) { + // Ensure there is a condition in the map for this name + if (!getZoneConditions().contains(name)) { + getZoneConditions().emplace(name, false); + } + + return [name]() { + return getZoneConditions()[name]; + }; + } +}; +} diff --git a/src/main/include/pathplanner/lib/events/ScheduleCommandEvent.h b/src/main/include/pathplanner/lib/events/ScheduleCommandEvent.h new file mode 100644 index 0000000..c3d21b3 --- /dev/null +++ b/src/main/include/pathplanner/lib/events/ScheduleCommandEvent.h @@ -0,0 +1,38 @@ +#pragma once + +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/EventScheduler.h" +#include +#include + +namespace pathplanner { +class ScheduleCommandEvent: public Event { +public: + /** + * Create an event to schedule a command + * + * @param timestamp The trajectory timestamp for this event + * @param command The command to schedule + */ + ScheduleCommandEvent(units::second_t timestamp, + std::shared_ptr command) : Event(timestamp), m_command( + command) { + } + + inline void handleEvent(EventScheduler *eventScheduler) override { + eventScheduler->scheduleCommand(m_command); + } + + inline void cancelEvent(EventScheduler *eventScheduler) override { + // Do nothing + } + + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < ScheduleCommandEvent > (timestamp, m_command); + } + +private: + std::shared_ptr m_command; +}; +} diff --git a/src/main/include/pathplanner/lib/events/TriggerEvent.h b/src/main/include/pathplanner/lib/events/TriggerEvent.h new file mode 100644 index 0000000..07e0a9a --- /dev/null +++ b/src/main/include/pathplanner/lib/events/TriggerEvent.h @@ -0,0 +1,41 @@ +#pragma once + +#include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/EventTrigger.h" +#include + +namespace pathplanner { +class TriggerEvent: public Event { +public: + /** + * Create an event for changing the value of a named trigger + * + * @param timestamp The trajectory timestamp of this event + * @param name The name of the trigger to control + * @param active Should the trigger be activated by this event + */ + TriggerEvent(units::second_t timestamp, std::string name, bool active) : Event( + timestamp), m_name(name), m_active(active) { + } + + inline void handleEvent(EventScheduler *eventScheduler) override { + EventTrigger::setCondition(m_name, m_active); + } + + inline void cancelEvent(EventScheduler *eventScheduler) override { + if (!m_active) { + // Ensure this event's condition gets set to false + EventTrigger::setCondition(m_name, false); + } + } + + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < TriggerEvent > (timestamp, m_name, m_active); + } + +private: + std::string m_name; + bool m_active; +}; +} diff --git a/src/main/include/pathplanner/lib/path/ConstraintsZone.h b/src/main/include/pathplanner/lib/path/ConstraintsZone.h index 6073c79..3677254 100644 --- a/src/main/include/pathplanner/lib/path/ConstraintsZone.h +++ b/src/main/include/pathplanner/lib/path/ConstraintsZone.h @@ -54,40 +54,6 @@ class ConstraintsZone { return m_constraints; } - /** - * Get if a given waypoint relative position is within this zone - * - * @param t Waypoint relative position - * @return True if given position is within this zone - */ - constexpr bool isWithinZone(double t) const { - return t >= m_minPos && t <= m_maxPos; - } - - /** - * Get if this zone overlaps a given range - * - * @param minPos The minimum waypoint relative position of the range - * @param maxPos The maximum waypoint relative position of the range - * @return True if any part of this zone is within the given range - */ - constexpr bool overlapsRange(double minPos, double maxPos) const { - return std::max(minPos, m_minPos) <= std::min(maxPos, m_maxPos); - } - - /** - * Transform the positions of this zone for a given segment number. - * - *

For example, a zone from [1.5, 2.0] for the segment 1 will have the positions [0.5, 1.0] - * - * @param segmentIndex The segment index to transform positions for - * @return The transformed zone - */ - constexpr ConstraintsZone forSegmentIndex(int segmentIndex) const { - return ConstraintsZone(m_minPos - segmentIndex, m_maxPos - segmentIndex, - m_constraints); - } - inline bool operator==(const ConstraintsZone &other) const { return std::abs(m_minPos - other.m_minPos) < 1E-9 && std::abs(m_maxPos - other.m_maxPos) < 1E-9 diff --git a/src/main/include/pathplanner/lib/path/EventMarker.h b/src/main/include/pathplanner/lib/path/EventMarker.h index 7e8ac73..8307871 100644 --- a/src/main/include/pathplanner/lib/path/EventMarker.h +++ b/src/main/include/pathplanner/lib/path/EventMarker.h @@ -13,22 +13,79 @@ class EventMarker { /** * Create a new event marker * + * @param triggerName The name of the trigger this event marker will control * @param waypointRelativePos The waypoint relative position of the marker + * @param endWaypointRelativePos The end waypoint relative position of the event's zone. A value + * of -1.0 indicates that this event is not zoned. * @param command The command that should be triggered at this marker */ - EventMarker(double waypointRelativePos, frc2::CommandPtr &&command) : m_pos( - waypointRelativePos), m_command(std::move(command).Unwrap()) { + EventMarker(std::string triggerName, double waypointRelativePos, + double endWaypointRelativePos, frc2::CommandPtr &&command) : m_triggerName( + triggerName), m_pos(waypointRelativePos), m_endWaypointRelativePos( + endWaypointRelativePos), m_command(std::move(command).Unwrap()) { } /** * Create a new event marker * + * @param triggerName The name of the trigger this event marker will control + * @param waypointRelativePos The waypoint relative position of the marker + * @param endWaypointRelativePos The end waypoint relative position of the event's zone. A value + * of -1.0 indicates that this event is not zoned. + */ + EventMarker(std::string triggerName, double waypointRelativePos, + double endWaypointRelativePos) : EventMarker(triggerName, + waypointRelativePos, endWaypointRelativePos, frc2::cmd::None()) { + } + + /** + * Create a new event marker + * + * @param triggerName The name of the trigger this event marker will control + * @param waypointRelativePos The waypoint relative position of the marker + * @param command The command that should be triggered at this marker + */ + EventMarker(std::string triggerName, double waypointRelativePos, + frc2::CommandPtr &&command) : EventMarker(triggerName, + waypointRelativePos, -1.0, std::move(command)) { + } + + /** + * Create a new event marker + * + * @param triggerName The name of the trigger this event marker will control + * @param waypointRelativePos The waypoint relative position of the marker + */ + EventMarker(std::string triggerName, double waypointRelativePos) : EventMarker( + triggerName, waypointRelativePos, -1.0, frc2::cmd::None()) { + } + + /** + * Create a new event marker + * + * @param triggerName The name of the trigger this event marker will control + * @param waypointRelativePos The waypoint relative position of the marker + * @param endWaypointRelativePos The end waypoint relative position of the event's zone. A value + * of -1.0 indicates that this event is not zoned. + * @param command The command that should be triggered at this marker + */ + EventMarker(std::string triggerName, double waypointRelativePos, + double endWaypointRelativePos, + std::shared_ptr command) : m_triggerName( + triggerName), m_pos(waypointRelativePos), m_endWaypointRelativePos( + endWaypointRelativePos), m_command(command) { + } + + /** + * Create a new event marker + * + * @param triggerName The name of the trigger this event marker will control * @param waypointRelativePos The waypoint relative position of the marker * @param command The command that should be triggered at this marker */ - EventMarker(double waypointRelativePos, - std::shared_ptr command) : m_pos( - waypointRelativePos), m_command(command) { + EventMarker(std::string triggerName, double waypointRelativePos, + std::shared_ptr command) : EventMarker(triggerName, + waypointRelativePos, -1.0, command) { } /** @@ -57,8 +114,29 @@ class EventMarker { return m_pos; } + /** + * Get the waypoint relative position of the end of this event's zone. A value of -1.0 indicates + * this marker is not zoned. + * + * @return The end position of the zone, -1.0 if not zoned + */ + constexpr double getEndWaypointRelativePos() const { + return m_endWaypointRelativePos; + } + + /** + * Get the name of the trigger this marker will control + * + * @return The name of the trigger + */ + constexpr const std::string& getTriggerName() { + return m_triggerName; + } + private: + std::string m_triggerName; double m_pos; + double m_endWaypointRelativePos; std::shared_ptr m_command; }; } diff --git a/src/main/include/pathplanner/lib/path/GoalEndState.h b/src/main/include/pathplanner/lib/path/GoalEndState.h index 8a0b746..3376462 100644 --- a/src/main/include/pathplanner/lib/path/GoalEndState.h +++ b/src/main/include/pathplanner/lib/path/GoalEndState.h @@ -13,11 +13,10 @@ 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, bool rotateFast = false) : m_velocity( - velocity), m_rotation(rotation), m_rotateFast(rotateFast) { + frc::Rotation2d rotation) : m_velocity(velocity), m_rotation( + rotation) { } /** @@ -46,24 +45,13 @@ 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_rotateFast == other.m_rotateFast; + && m_rotation == other.m_rotation; } private: units::meters_per_second_t m_velocity; frc::Rotation2d m_rotation; - bool m_rotateFast; }; } diff --git a/src/main/include/pathplanner/lib/path/IdealStartingState.h b/src/main/include/pathplanner/lib/path/IdealStartingState.h new file mode 100644 index 0000000..0876edc --- /dev/null +++ b/src/main/include/pathplanner/lib/path/IdealStartingState.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include +#include + +namespace pathplanner { +class IdealStartingState { +public: + /** + * Create a new ideal starting state + * + * @param velocity The ideal starting velocity (M/S) + * @param rotation The ideal starting rotation + */ + constexpr IdealStartingState(units::meters_per_second_t velocity, + frc::Rotation2d rotation) : m_velocity(velocity), m_rotation( + rotation) { + } + + /** + * Create an ideal starting state from json + * + * @param json json reference representing an ideal starting state + * @return The ideal starting state defined by the given json + */ + static IdealStartingState fromJson(const wpi::json &json); + + /** + * Get the ideal starting velocity + * + * @return Ideal starting velocity (M/S) + */ + constexpr units::meters_per_second_t getVelocity() const { + return m_velocity; + } + + /** + * Get the ideal starting rotation + * + * @return Ideal starting rotation + */ + constexpr const frc::Rotation2d& getRotation() const { + return m_rotation; + } + + inline bool operator==(const IdealStartingState &other) const { + return std::abs(m_velocity() - other.m_velocity()) < 1E-9 + && m_rotation == other.m_rotation; + } + +private: + units::meters_per_second_t m_velocity; + frc::Rotation2d m_rotation; +}; +} diff --git a/src/main/include/pathplanner/lib/path/PathConstraints.h b/src/main/include/pathplanner/lib/path/PathConstraints.h index 00647dd..4f19f18 100644 --- a/src/main/include/pathplanner/lib/path/PathConstraints.h +++ b/src/main/include/pathplanner/lib/path/PathConstraints.h @@ -4,7 +4,9 @@ #include #include #include +#include #include +#include namespace pathplanner { class PathConstraints { @@ -16,13 +18,17 @@ class PathConstraints { * @param maxAccel Max linear acceleration (M/S^2) * @param maxAngularVel Max angular velocity (Deg/S) * @param maxAngularAccel Max angular acceleration (Deg/S^2) + * @param nominalVoltage The nominal battery voltage (Volts) + * @param unlimited Should the constraints be unlimited */ constexpr PathConstraints(units::meters_per_second_t maxVel, units::meters_per_second_squared_t maxAccel, units::radians_per_second_t maxAngularVel, - units::radians_per_second_squared_t maxAngularAccel) : m_maxVelocity( + units::radians_per_second_squared_t maxAngularAccel, + units::volt_t nominalVoltage = 12_V, bool unlimited = false) : m_maxVelocity( maxVel), m_maxAcceleration(maxAccel), m_maxAngularVelocity( - maxAngularVel), m_maxAngularAcceleration(maxAngularAccel) { + maxAngularVel), m_maxAngularAcceleration(maxAngularAccel), m_nominalVoltage( + nominalVoltage), m_unlimited(unlimited) { } /** @@ -33,6 +39,22 @@ class PathConstraints { */ static PathConstraints fromJson(const wpi::json &json); + /** + * Get unlimited PathConstraints + * + * @param nominalVoltage The nominal battery voltage (Volts) + * @return Unlimited constraints + */ + static constexpr PathConstraints unlimitedConstraints( + units::volt_t nominalVoltage) { + double inf = std::numeric_limits::infinity(); + return PathConstraints(units::meters_per_second_t { inf }, + units::meters_per_second_squared_t { inf }, + units::radians_per_second_t { inf }, + units::radians_per_second_squared_t { inf }, nominalVoltage, + true); + } + /** * Get the max linear velocity * @@ -69,6 +91,19 @@ class PathConstraints { return m_maxAngularAcceleration; } + /** + * Get the nominal voltage + * + * @return Nominal Voltage (Volts) + */ + constexpr units::volt_t getNominalVoltage() const { + return m_nominalVoltage; + } + + constexpr bool isUnlimited() const { + return m_unlimited; + } + bool operator==(const PathConstraints &other) const { return std::abs(m_maxVelocity() - other.m_maxVelocity()) < 1E-9 && std::abs(m_maxAcceleration() - other.m_maxAcceleration()) @@ -78,7 +113,9 @@ class PathConstraints { < 1E-9 && std::abs( m_maxAngularAcceleration() - - other.m_maxAngularAcceleration()) < 1E-9; + - other.m_maxAngularAcceleration()) < 1E-9 + && std::abs(m_nominalVoltage() - other.m_nominalVoltage()) + < 1E-9 && m_unlimited == other.m_unlimited; } private: @@ -86,5 +123,7 @@ class PathConstraints { units::meters_per_second_squared_t m_maxAcceleration; units::radians_per_second_t m_maxAngularVelocity; units::radians_per_second_squared_t m_maxAngularAcceleration; + units::volt_t m_nominalVoltage; + bool m_unlimited; }; } diff --git a/src/main/include/pathplanner/lib/path/PathPlannerPath.h b/src/main/include/pathplanner/lib/path/PathPlannerPath.h index b795bc4..8632239 100644 --- a/src/main/include/pathplanner/lib/path/PathPlannerPath.h +++ b/src/main/include/pathplanner/lib/path/PathPlannerPath.h @@ -1,18 +1,23 @@ #pragma once #include "pathplanner/lib/path/RotationTarget.h" +#include "pathplanner/lib/path/PointTowardsZone.h" #include "pathplanner/lib/path/ConstraintsZone.h" #include "pathplanner/lib/path/EventMarker.h" #include "pathplanner/lib/path/PathConstraints.h" +#include "pathplanner/lib/path/IdealStartingState.h" #include "pathplanner/lib/path/GoalEndState.h" #include "pathplanner/lib/path/PathPoint.h" -#include "pathplanner/lib/path/PathSegment.h" -#include "pathplanner/lib/path/PathPlannerTrajectory.h" +#include "pathplanner/lib/path/Waypoint.h" +#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h" +#include "pathplanner/lib/config/RobotConfig.h" #include +#include #include #include #include #include +#include #include #include #include @@ -20,43 +25,51 @@ namespace pathplanner { class PathPlannerPath: public std::enable_shared_from_this { public: + std::string name; bool preventFlipping = false; /** * Create a new path planner path * - * @param bezierPoints List of points representing the cubic Bezier curve of the path + * @param waypoints List of waypoints representing the path. For on-the-fly paths, you likely want + * to use waypointsFromPoses to create these. * @param holonomicRotations List of rotation targets along the path + * @param pointTowardsZones List of point towards zones along the path * @param constraintZones List of constraint zones along the path * @param eventMarkers List of event markers along the path * @param globalConstraints The global constraints of the path + * @param idealStartingState The ideal starting state of the path. Can be nullopt if unknown * @param goalEndState The goal end state of the path * @param reversed Should the robot follow the path reversed (differential drive only) */ - PathPlannerPath(std::vector bezierPoints, + PathPlannerPath(std::vector waypoints, std::vector rotationTargets, + std::vector pointTowardsZones, std::vector constraintZones, std::vector eventMarkers, - PathConstraints globalConstraints, GoalEndState goalEndState, - bool reversed, frc::Rotation2d previewStartingRotation = - frc::Rotation2d()); + PathConstraints globalConstraints, + std::optional idealStartingState, + GoalEndState goalEndState, bool reversed); /** * Simplified constructor to create a path with no rotation targets, constraint zones, or event * markers. * - *

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 waypoints List of waypoints representing the path. For on-the-fly paths, you likely want + * to use waypointsFromPoses to create these. * @param constraints The global constraints of the path + * @param idealStartingState The ideal starting state of the path. Can be nullopt if unknown * @param goalEndState The goal end state of the path * @param reversed Should the robot follow the path reversed (differential drive only) */ - PathPlannerPath(std::vector bezierPoints, - PathConstraints constraints, GoalEndState goalEndState, - bool reversed = false) : PathPlannerPath(bezierPoints, - std::vector(), std::vector(), - std::vector(), constraints, goalEndState, reversed) { + PathPlannerPath(std::vector waypoints, + PathConstraints constraints, + std::optional idealStartingState, + GoalEndState goalEndState, bool reversed = false) : PathPlannerPath( + waypoints, std::vector(), + std::vector(), std::vector(), + std::vector(), constraints, idealStartingState, + goalEndState, reversed) { } /** @@ -67,14 +80,26 @@ class PathPlannerPath: public std::enable_shared_from_this { void hotReload(const wpi::json &json); /** - * Create the bezier points necessary to create a path using a list of poses + * Create the bezier waypoints necessary to create a path using a list of poses * * @param poses List of poses. Each pose represents one waypoint. - * @return Bezier points + * @return Bezier curve waypoints */ - static std::vector bezierFromPoses( + static std::vector waypointsFromPoses( std::vector poses); + /** + * Create the bezier waypoints necessary to create a path using a list of poses + * + * @param poses List of poses. Each pose represents one waypoint. + * @return Bezier curve waypoints + */ + [[deprecated("Renamed to waypointsFromPoses")]] + static inline std::vector bezierFromPoses( + std::vector poses) { + return waypointsFromPoses(poses); + } + /** * Load a path from a path file in storage * @@ -94,30 +119,32 @@ class PathPlannerPath: public std::enable_shared_from_this { std::string trajectoryName); /** - * Get the differential pose for the start point of this path + * Load a Choreo trajectory as a PathPlannerPath * - * @return Pose at the path's starting point + * @param trajectoryName The name of the Choreo trajectory to load. This should be just the name + * of the trajectory. + * @param splitIndex The index of the split to use + * @return PathPlannerPath created from the given Choreo trajectory and split index */ - frc::Pose2d getStartingDifferentialPose(); + static inline std::shared_ptr fromChoreoTrajectory( + std::string trajectoryName, size_t splitIndex) { + std::string cacheName = trajectoryName + "." + + std::to_string(splitIndex); + if (getChoreoPathCache().contains(cacheName)) { + return getChoreoPathCache()[cacheName]; + } - /** - * Get the starting pose for the holomonic path based on the preview settings. - * - * NOTE: This should only be used for the first path you are running, and only if you are not using an auto mode file. Using this pose to reset the robots pose between sequential paths will cause a loss of accuracy. - * - * @return Pose at the path's starting point - */ - frc::Pose2d getPreviewStartingHolonomicPose(); + // Path is not in the cache, load the main trajectory to load all splits + loadChoreoTrajectoryIntoCache(trajectoryName); + return getChoreoPathCache()[cacheName]; + } /** - * Get the constraints for a point along the path + * Get the differential pose for the start point of this path * - * @param idx Index of the point to get constraints for - * @return The constraints that should apply to the point + * @return Pose at the path's starting point */ - inline PathConstraints getConstraintsForPoint(size_t idx) { - return getPoint(idx).constraints.value_or(m_globalConstraints); - } + frc::Pose2d getStartingDifferentialPose(); /** * Create a path planner path from pre-generated path points. This is used internally, and you @@ -127,12 +154,6 @@ class PathPlannerPath: public std::enable_shared_from_this { std::vector pathPoints, PathConstraints globalConstraints, GoalEndState goalEndState); - /** Generate path points for a path. This is used internally and should not be used directly. */ - static std::vector createPath( - std::vector bezierPoints, - std::vector holonomicRotations, - std::vector constraintZones); - /** * Get all the path points in this path * @@ -161,6 +182,60 @@ class PathPlannerPath: public std::enable_shared_from_this { return m_allPoints[index]; } + /** + * If possible, get the ideal trajectory for this path. This trajectory can be used if the robot + * is currently near the start of the path and at the ideal starting state. If there is no ideal + * starting state, there can be no ideal trajectory. + * + * @param robotConfig The config to generate the ideal trajectory with if it has not already been + * generated + * @return An optional containing the ideal trajectory if it exists, an empty optional otherwise + */ + std::optional getIdealTrajectory( + RobotConfig robotConfig); + + /** + * Get the initial heading, or direction of travel, at the start of the path. + * + * @return Initial heading + */ + inline frc::Rotation2d getInitialHeading() const { + return (getPoint(1).position - getPoint(0).position).Angle(); + } + + /** + * Get the waypoints for this path + * @return vector of this path's waypoints + */ + constexpr std::vector& getWaypoints() { + return m_waypoints; + } + + /** + * Get the rotation targets for this path + * @return vector of this path's rotation targets + */ + constexpr std::vector& getRotationTargets() { + return m_rotationTargets; + } + + /** + * Get the point towards zones for this path + * + * @return vector of this path's point towards zones + */ + constexpr std::vector& getPointTowardsZones() { + return m_pointTowardsZones; + } + + /** + * Get the constraint zones for this path + * @return vector of this path's constraint zones + */ + constexpr std::vector& getConstraintZones() { + return m_constraintZones; + } + /** * Get the global constraints for this path * @@ -179,6 +254,15 @@ class PathPlannerPath: public std::enable_shared_from_this { return m_goalEndState; } + /** + * Get the ideal starting state of this path + * + * @return The ideal starting state + */ + constexpr const std::optional& getIdealStartingState() const { + return m_idealStartingState; + } + /** * Get all the event markers for this path * @@ -206,27 +290,17 @@ class PathPlannerPath: public std::enable_shared_from_this { return m_isChoreoPath; } - inline PathPlannerTrajectory getTrajectory( - frc::ChassisSpeeds startingSpeeds, - frc::Rotation2d startingRotation) { + inline PathPlannerTrajectory generateTrajectory( + frc::ChassisSpeeds startingSpeeds, frc::Rotation2d startingRotation, + const RobotConfig &config) { if (m_isChoreoPath) { - return m_choreoTrajectory; + return m_idealTrajectory.value(); } else { return PathPlannerTrajectory(shared_from_this(), startingSpeeds, - startingRotation); + startingRotation, config); } } - /** - * Replan this path based on the current robot position and speeds - * - * @param startingPose New starting pose for the replanned path - * @param currentSpeeds Current chassis speeds of the robot - * @return The replanned path - */ - std::shared_ptr replan(const frc::Pose2d startingPose, - const frc::ChassisSpeeds currentSpeeds); - /** * Flip a path to the other side of the field, maintaining a global blue alliance origin * @@ -248,59 +322,84 @@ class PathPlannerPath: public std::enable_shared_from_this { return poses; } + /** Clear the cache of previously loaded paths. */ + static inline void clearPathCache() { + PathPlannerPath::getPathCache().clear(); + PathPlannerPath::getChoreoPathCache().clear(); + } + private: + std::vector createPath(); + static std::shared_ptr fromJson(const wpi::json &json); - static std::vector bezierPointsFromWaypointsJson( - const wpi::json &json); + static inline std::vector waypointsFromJson( + const wpi::json &waypointsJson) { + std::vector < Waypoint > waypoints; + for (wpi::json::const_reference waypoint : waypointsJson) { + waypoints.emplace_back(Waypoint::fromJson(waypoint)); + } + return waypoints; + } - static frc::Translation2d pointFromJson(const wpi::json &json); + static void loadChoreoTrajectoryIntoCache(std::string trajectoryName); void precalcValues(); static units::meter_t getCurveRadiusAtPoint(size_t index, std::vector &points); - /** - * Map a given percentage/waypoint relative position over 2 segments - * - * @param pct The percent to map - * @param seg1Pct The percentage of the 2 segments made up by the first segment - * @return The waypoint relative position over the 2 segments - */ - static double mapPct(double pct, double seg1Pct) { - double mappedPct; - if (pct <= seg1Pct) { - // Map to segment 1 - mappedPct = pct / seg1Pct; - } else { - // Map to segment 2 - mappedPct = 1.0 + ((pct - seg1Pct) / (1.0 - seg1Pct)); + inline PathConstraints constraintsForWaypointPos(double pos) const { + for (auto z : m_constraintZones) { + if (pos >= z.getMinWaypointRelativePos() + && pos <= z.getMaxWaypointRelativePos()) { + return z.getConstraints(); + } } - return std::round(mappedPct * (1.0 / PathSegment::RESOLUTION)) - / (1.0 / PathSegment::RESOLUTION); - } + // Check if constraints should be unlimited + if (m_globalConstraints.isUnlimited()) { + return PathConstraints::unlimitedConstraints( + m_globalConstraints.getNominalVoltage()); + } - static inline units::meter_t positionDelta(const frc::Translation2d &a, - const frc::Translation2d &b) { - frc::Translation2d delta = a - b; + return m_globalConstraints; + } - return units::math::abs(delta.X()) + units::math::abs(delta.Y()); + inline std::optional pointZoneForWaypointPos( + double pos) const { + for (auto z : m_pointTowardsZones) { + if (pos >= z.getMinWaypointRelativePos() + && pos <= z.getMaxWaypointRelativePos()) { + return z; + } + } + return std::nullopt; } - std::vector m_bezierPoints; + frc::Translation2d samplePath(double waypointRelativePos) const; + + static std::unordered_map>& getPathCache(); + + static std::unordered_map>& getChoreoPathCache(); + + std::vector m_waypoints; std::vector m_rotationTargets; + std::vector m_pointTowardsZones; std::vector m_constraintZones; std::vector m_eventMarkers; PathConstraints m_globalConstraints; + std::optional m_idealStartingState; GoalEndState m_goalEndState; std::vector m_allPoints; bool m_reversed; - frc::Rotation2d m_previewStartingRotation; + bool m_isChoreoPath; - PathPlannerTrajectory m_choreoTrajectory; + std::optional m_idealTrajectory = std::nullopt; static int m_instances; + + static constexpr double targetIncrement = 0.05; + static constexpr units::meter_t targetSpacing = 0.2_m; }; } diff --git a/src/main/include/pathplanner/lib/path/PathPlannerTrajectory.h b/src/main/include/pathplanner/lib/path/PathPlannerTrajectory.h deleted file mode 100644 index 05488e0..0000000 --- a/src/main/include/pathplanner/lib/path/PathPlannerTrajectory.h +++ /dev/null @@ -1,259 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "pathplanner/lib/path/PathConstraints.h" -#include "pathplanner/lib/util/GeometryUtil.h" - -namespace pathplanner { - -class PathPlannerPath; - -class PathPlannerTrajectory { -public: - class State { - public: - units::second_t time; - units::meters_per_second_t velocity; - units::meters_per_second_squared_t acceleration; - units::radians_per_second_t headingAngularVelocity; - frc::Translation2d position; - frc::Rotation2d heading; - frc::Rotation2d targetHolonomicRotation; - std::optional holonomicAngularVelocityRps; - units::curvature_t curvature; - PathConstraints constraints; - units::meter_t deltaPos; - - constexpr State() : holonomicAngularVelocityRps(std::nullopt), constraints( - 0_mps, 0_mps_sq, 0_rad_per_s, 0_rad_per_s_sq) { - } - - constexpr State interpolate(const State &endValue, double t) const { - State lerpedState; - - lerpedState.time = GeometryUtil::unitLerp(time, endValue.time, t); - units::second_t deltaT = lerpedState.time - time; - - if (deltaT < 0_s) { - return endValue.interpolate(*this, 1.0 - t); - } - - lerpedState.velocity = GeometryUtil::unitLerp(velocity, - endValue.velocity, t); - lerpedState.acceleration = GeometryUtil::unitLerp(acceleration, - endValue.acceleration, t); - lerpedState.position = GeometryUtil::translationLerp(position, - endValue.position, t); - lerpedState.heading = GeometryUtil::rotationLerp(heading, - endValue.heading, t); - lerpedState.headingAngularVelocity = GeometryUtil::unitLerp( - headingAngularVelocity, endValue.headingAngularVelocity, t); - lerpedState.curvature = GeometryUtil::unitLerp(curvature, - endValue.curvature, t); - lerpedState.deltaPos = GeometryUtil::unitLerp(deltaPos, - endValue.deltaPos, t); - - if (holonomicAngularVelocityRps - && endValue.holonomicAngularVelocityRps) { - lerpedState.holonomicAngularVelocityRps = - GeometryUtil::unitLerp( - holonomicAngularVelocityRps.value(), - endValue.holonomicAngularVelocityRps.value(), - t); - } - - lerpedState.targetHolonomicRotation = GeometryUtil::rotationLerp( - targetHolonomicRotation, endValue.targetHolonomicRotation, - t); - - if (t < 0.5) { - lerpedState.constraints = constraints; - } else { - lerpedState.constraints = endValue.constraints; - } - - return lerpedState; - } - - /** - * Get the target pose for a holonomic drivetrain NOTE: This is a "target" pose, meaning the - * rotation will be the value of the next rotation target along the path, not what the rotation - * should be at the start of the path - * - * @return The target pose - */ - constexpr frc::Pose2d getTargetHolonomicPose() const { - return frc::Pose2d(position, targetHolonomicRotation); - } - - /** - * Get this pose for a differential drivetrain - * - * @return The pose - */ - constexpr frc::Pose2d getDifferentialPose() const { - return frc::Pose2d(position, heading); - } - - /** - * Get the state reversed, used for following a trajectory reversed with a differential - * drivetrain - * - * @return The reversed state - */ - constexpr State reverse() const { - State reversed; - - reversed.time = time; - reversed.velocity = -velocity; - reversed.acceleration = -acceleration; - reversed.headingAngularVelocity = -headingAngularVelocity; - reversed.position = position; - reversed.heading = frc::Rotation2d( - frc::InputModulus(heading.Degrees() + 180_deg, -180_deg, - 180_deg)); - reversed.targetHolonomicRotation = targetHolonomicRotation; - reversed.holonomicAngularVelocityRps = holonomicAngularVelocityRps; - reversed.curvature = -curvature; - reversed.deltaPos = deltaPos; - reversed.constraints = constraints; - - return reversed; - } - }; - - PathPlannerTrajectory() { - } - - PathPlannerTrajectory(std::vector states, - std::vector< - std::pair>> eventCommands) : m_states( - states), m_eventCommands(eventCommands) { - } - - PathPlannerTrajectory(std::vector states) : m_states(states) { - } - - /** - * Generate a 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 path, - const frc::ChassisSpeeds &startingSpeeds, - const frc::Rotation2d &startingRotation); - - /** - * Get the target state at the given point in time along the trajectory - * - * @param time The time to sample the trajectory at in seconds - * @return The target state - */ - State sample(const units::second_t time); - - /** - * Get all of the pre-generated states in the trajectory - * - * @return vector of all states - */ - constexpr std::vector& getStates() { - return m_states; - } - - /** - * Get the total run time of the trajectory - * - * @return Total run time in seconds - */ - inline units::second_t getTotalTime() { - return getEndState().time; - } - - /** - * Get the goal state at the given index - * - * @param index Index of the state to get - * @return The state at the given index - */ - inline State getState(size_t index) { - return m_states[index]; - } - - /** - * Get the initial state of the trajectory - * - * @return The initial state - */ - inline State getInitialState() { - return m_states[0]; - } - - /** - * Get the initial target pose for a holonomic drivetrain NOTE: This is a "target" pose, meaning - * the rotation will be the value of the next rotation target along the path, not what the - * rotation should be at the start of the path - * - * @return The initial target pose - */ - inline frc::Pose2d getInitialTargetHolonomicPose() { - return m_states[0].getTargetHolonomicPose(); - } - - /** - * Get this initial pose for a differential drivetrain - * - * @return The initial pose - */ - inline frc::Pose2d getInitialDifferentialPose() { - return m_states[0].getDifferentialPose(); - } - - /** - * Get the end state of the trajectory - * - * @return The end state - */ - inline State getEndState() { - return m_states[m_states.size() - 1]; - } - - /** - * Get all of the pairs of timestamps + commands to run at those timestamps - * - * @return Pairs of timestamps and event commands - */ - inline std::vector< - std::pair>> getEventCommands() { - return m_eventCommands; - } - -private: - std::vector m_states; - std::vector>> m_eventCommands; - - static size_t getNextRotationTargetIdx( - std::shared_ptr path, const size_t startingIndex); - - static std::vector generateStates( - std::shared_ptr path, - const frc::ChassisSpeeds &startingSpeeds, - const frc::Rotation2d &startingRotation); -}; -} diff --git a/src/main/include/pathplanner/lib/path/PathPoint.h b/src/main/include/pathplanner/lib/path/PathPoint.h index 78c498a..aeb7e8c 100644 --- a/src/main/include/pathplanner/lib/path/PathPoint.h +++ b/src/main/include/pathplanner/lib/path/PathPoint.h @@ -7,17 +7,18 @@ #include #include "pathplanner/lib/path/PathConstraints.h" #include "pathplanner/lib/path/RotationTarget.h" +#include "pathplanner/lib/util/FlippingUtil.h" namespace pathplanner { class PathPoint { public: frc::Translation2d position; units::meter_t distanceAlongPath = 0_m; - units::meter_t curveRadius = 0_m; units::meters_per_second_t maxV = units::meters_per_second_t { std::numeric_limits::infinity() }; std::optional rotationTarget = std::nullopt; std::optional constraints = std::nullopt; + double waypointRelativePos = 0.0; constexpr PathPoint(frc::Translation2d pos, std::optional rot, @@ -27,5 +28,20 @@ class PathPoint { constexpr PathPoint(frc::Translation2d pos) : position(pos) { } + + inline PathPoint flip() const { + PathPoint flipped(FlippingUtil::flipFieldPosition(position)); + flipped.distanceAlongPath = distanceAlongPath; + flipped.maxV = maxV; + if (rotationTarget.has_value()) { + flipped.rotationTarget = RotationTarget( + rotationTarget.value().getPosition(), + FlippingUtil::flipFieldRotation( + rotationTarget.value().getTarget())); + } + flipped.constraints = constraints; + flipped.waypointRelativePos = waypointRelativePos; + return flipped; + } }; } diff --git a/src/main/include/pathplanner/lib/path/PathSegment.h b/src/main/include/pathplanner/lib/path/PathSegment.h deleted file mode 100644 index f8ffedc..0000000 --- a/src/main/include/pathplanner/lib/path/PathSegment.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include "pathplanner/lib/path/PathPoint.h" -#include "pathplanner/lib/path/RotationTarget.h" -#include "pathplanner/lib/path/ConstraintsZone.h" -#include -#include -#include -#include - -namespace pathplanner { -class PathSegment { -public: - static constexpr double RESOLUTION = 0.05; - - /** - * Generate a new path segment - * - * @param p1 Start anchor point - * @param p2 Start next control - * @param p3 End prev control - * @param p4 End anchor point - * @param targetHolonomicRotations Rotation targets for within this segment - * @param constraintZones Constraint zones for within this segment - * @param endSegment Is this the last segment in the path - */ - PathSegment(frc::Translation2d p1, frc::Translation2d p2, - frc::Translation2d p3, frc::Translation2d p4, - std::vector targetHolonomicRotations, - std::vector constraintZones, bool endSegment); - - /** - * Generate a new path segment without constraint zones or rotation targets - * - * @param p1 Start anchor point - * @param p2 Start next control - * @param p3 End prev control - * @param p4 End anchor point - * @param endSegment Is this the last segment in the path - */ - PathSegment(frc::Translation2d p1, frc::Translation2d p2, - frc::Translation2d p3, frc::Translation2d p4, bool endSegment) : PathSegment( - p1, p2, p3, p4, std::vector(), - std::vector(), endSegment) { - } - - /** - * Get the path points for this segment - * - * @return Path points for this segment - */ - constexpr std::vector& getSegmentPoints() { - return m_segmentPoints; - } - -private: - std::vector m_segmentPoints; - - std::optional findConstraintsZone( - std::vector &zones, double t) const; -}; -} diff --git a/src/main/include/pathplanner/lib/path/PointTowardsZone.h b/src/main/include/pathplanner/lib/path/PointTowardsZone.h new file mode 100644 index 0000000..22dc478 --- /dev/null +++ b/src/main/include/pathplanner/lib/path/PointTowardsZone.h @@ -0,0 +1,123 @@ +#pragma once + +#include +#include +#include +#include +#include "pathplanner/lib/util/FlippingUtil.h" +#include "pathplanner/lib/util/JSONUtil.h" + +namespace pathplanner { +class PointTowardsZone { +public: + /** + * Create a new point towards zone + * + * @param name The name of this zone. Used for point towards zone triggers + * @param targetPosition The target field position in meters + * @param rotationOffset A rotation offset to add on top of the angle to the target position. For + * example, if you want the robot to point away from the target position, use a rotation + * offset of 180 degrees + * @param minWaypointRelativePos Starting position of the zone + * @param maxWaypointRelativePos End position of the zone + */ + PointTowardsZone(std::string name, frc::Translation2d targetPosition, + frc::Rotation2d rotationOffset, double minWaypointRelativePos, + double maxWaypointRelativePos) : m_name(name), m_targetPos( + targetPosition), m_rotationOffset(rotationOffset), m_minPos( + minWaypointRelativePos), m_maxPos(maxWaypointRelativePos) { + } + + /** + * Create a new point towards zone + * + * @param name The name of this zone. Used for point towards zone triggers + * @param targetPosition The target field position in meters + * @param minWaypointRelativePos Starting position of the zone + * @param maxWaypointRelativePos End position of the zone + */ + PointTowardsZone(std::string name, frc::Translation2d targetPosition, + double minWaypointRelativePos, double maxWaypointRelativePos) : PointTowardsZone( + name, targetPosition, frc::Rotation2d(), minWaypointRelativePos, + maxWaypointRelativePos) { + } + + /** + * Create a point towards zone from json + * + * @param json A json reference representing a point towards zone + * @return The point towards zone defined by the given json object + */ + static inline PointTowardsZone fromJson(const wpi::json &json) { + std::string name = json.at("name").get(); + frc::Translation2d targetPos = JSONUtil::translation2dFromJson( + json.at("fieldPosition")); + frc::Rotation2d rotationOffset = frc::Rotation2d( + units::degree_t { json.at("rotationOffset").get() }); + double minPos = json.at("minWaypointRelativePos").get(); + double maxPos = json.at("maxWaypointRelativePos").get(); + return PointTowardsZone(name, targetPos, rotationOffset, minPos, maxPos); + } + + constexpr const std::string& getName() { + return m_name; + } + + /** + * Get the target field position to point at + * + * @return Target field position in meters + */ + constexpr frc::Translation2d& getTargetPosition() { + return m_targetPos; + } + + /** + * Get the rotation offset + * + * @return Rotation offset + */ + constexpr frc::Rotation2d& getRotationOffset() { + return m_rotationOffset; + } + + /** + * Get the starting position of the zone + * + * @return Waypoint relative starting position + */ + constexpr double getMinWaypointRelativePos() const { + return m_minPos; + } + + /** + * Get the end position of the zone + * + * @return Waypoint relative end position + */ + constexpr double getMaxWaypointRelativePos() const { + return m_maxPos; + } + + inline PointTowardsZone flip() const { + return PointTowardsZone(m_name, + FlippingUtil::flipFieldPosition(m_targetPos), m_rotationOffset, + m_minPos, m_maxPos); + } + + inline bool operator==(const PointTowardsZone &other) const { + return m_name == other.m_name + && std::abs(m_minPos - other.m_minPos) < 1E-9 + && std::abs(m_maxPos - other.m_maxPos) < 1E-9 + && m_targetPos == other.m_targetPos + && m_rotationOffset == other.m_rotationOffset; + } + +private: + std::string m_name; + frc::Translation2d m_targetPos; + frc::Rotation2d m_rotationOffset; + double m_minPos; + double m_maxPos; +}; +} diff --git a/src/main/include/pathplanner/lib/path/RotationTarget.h b/src/main/include/pathplanner/lib/path/RotationTarget.h index 6b72300..e9c8eec 100644 --- a/src/main/include/pathplanner/lib/path/RotationTarget.h +++ b/src/main/include/pathplanner/lib/path/RotationTarget.h @@ -11,12 +11,10 @@ class RotationTarget { * * @param waypointRelativePosition Waypoint relative position of this target * @param target Target rotation - * @param rotateFast Should the robot reach the rotation as fast as possible */ constexpr RotationTarget(double waypointRelativePosition, - frc::Rotation2d target, bool rotateFast = false) : m_position( - waypointRelativePosition), m_target(target), m_rotateFast( - rotateFast) { + frc::Rotation2d target) : m_position(waypointRelativePosition), m_target( + target) { } /** @@ -45,36 +43,13 @@ class RotationTarget { return m_target; } - /** - * 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; - } - - /** - * Transform the position of this target for a given segment number. - * - *

For example, a target with position 1.5 for the segment 1 will have the position 0.5 - * - * @param segmentIndex The segment index to transform position for - * @return The transformed target - */ - constexpr RotationTarget forSegmentIndex(int segmentIndex) const { - return RotationTarget(m_position - segmentIndex, m_target, m_rotateFast); - } - inline bool operator==(const RotationTarget &other) const { return std::abs(m_position - other.m_position) < 1E-9 - && m_target == other.m_target - && m_rotateFast == other.m_rotateFast; + && m_target == other.m_target; } private: double m_position; frc::Rotation2d m_target; - bool m_rotateFast; }; } diff --git a/src/main/include/pathplanner/lib/path/Waypoint.h b/src/main/include/pathplanner/lib/path/Waypoint.h new file mode 100644 index 0000000..2459e8e --- /dev/null +++ b/src/main/include/pathplanner/lib/path/Waypoint.h @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include +#include +#include "pathplanner/lib/util/FlippingUtil.h" + +namespace pathplanner { + +#define AUTO_CONTROL_DISTANCE_FACTOR 1.0 / 3.0 + +class Waypoint { +public: + std::optional prevControl; + frc::Translation2d anchor; + std::optional nextControl; + + /** + * Create a waypoint from its anchor point and control points + * + * @param prevControl The previous control point position + * @param anchor The anchor position + * @param nextControl The next control point position + */ + constexpr Waypoint(std::optional prevControl, + frc::Translation2d anchor, + std::optional nextControl) : prevControl( + prevControl), anchor(anchor), nextControl(nextControl) { + } + + /** + * Flip this waypoint to the other side of the field, maintaining a blue alliance origin + * + * @return The flipped waypoint + */ + Waypoint flip() const; + + /** + * Create a waypoint with auto calculated control points based on the positions of adjacent + * waypoints. This is used internally, and you probably shouldn't use this. + * + * @param anchor The anchor point of the waypoint to create + * @param heading The heading of this waypoint + * @param prevAnchor The position of the previous anchor point. This can be nullopt for the start point + * @param nextAnchor The position of the next anchor point. This can be nullopt for the end point + * @return Waypoint with auto calculated control points + */ + static Waypoint autoControlPoints(frc::Translation2d anchor, + frc::Rotation2d heading, + std::optional prevAnchor, + std::optional nextAnchor); + + /** + * Create a waypoint from JSON + * + * @param waypointJson JSON object representing a waypoint + * @return The waypoint created from JSON + */ + static Waypoint fromJson(const wpi::json &waypointJson); +}; +} diff --git a/src/main/include/pathplanner/lib/pathfinding/LocalADStar.h b/src/main/include/pathplanner/lib/pathfinding/LocalADStar.h index 59ce586..1ff0f85 100644 --- a/src/main/include/pathplanner/lib/pathfinding/LocalADStar.h +++ b/src/main/include/pathplanner/lib/pathfinding/LocalADStar.h @@ -2,6 +2,7 @@ #include "pathplanner/lib/pathfinding/Pathfinder.h" #include "pathplanner/lib/path/PathPoint.h" +#include "pathplanner/lib/path/Waypoint.h" #include #include #include @@ -68,7 +69,6 @@ class LocalADStar: public Pathfinder { private: const double SMOOTHING_ANCHOR_PCT = 0.8; - const double SMOOTHING_CONTROL_PCT = 0.33; const double EPS = 2.5; double fieldLength; @@ -104,7 +104,7 @@ class LocalADStar: public Pathfinder { bool requestReset; bool newPathAvailable; - std::vector currentPathPoints; + std::vector currentWaypoints; std::vector currentPathFull; void runThread(); @@ -122,8 +122,7 @@ class LocalADStar: public Pathfinder { const GridPosition &sGoal, const std::unordered_set &obstacles); - std::vector createPathPoints( - const std::vector &path, + std::vector createWaypoints(const std::vector &path, const frc::Translation2d &realStartPos, const frc::Translation2d &realGoalPos, const std::unordered_set &obstacles); diff --git a/src/main/include/pathplanner/lib/pathfinding/RemoteADStar.h b/src/main/include/pathplanner/lib/pathfinding/RemoteADStar.h deleted file mode 100644 index b1cda64..0000000 --- a/src/main/include/pathplanner/lib/pathfinding/RemoteADStar.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include "pathplanner/lib/pathfinding/Pathfinder.h" -#include "pathplanner/lib/path/PathPoint.h" -#include -#include -#include -#include -#include -#include - -namespace pathplanner { -class RemoteADStar: public Pathfinder { -public: - RemoteADStar(); - - ~RemoteADStar() { - nt::NetworkTableInstance::GetDefault().RemoveListener( - m_pathListenerHandle); - } - - inline bool isNewPathAvailable() override { - std::scoped_lock lock { m_mutex }; - - return m_newPathAvailable; - } - - std::shared_ptr getCurrentPath(PathConstraints constraints, - GoalEndState goalEndState) override; - - void setStartPosition(const frc::Translation2d &start) override; - - void setGoalPosition(const frc::Translation2d &goal) override; - - void setDynamicObstacles( - const std::vector> &obs, - const frc::Translation2d ¤tRobotPos) override; - -private: - nt::StringPublisher m_navGridJsonPub; - nt::DoubleArrayPublisher m_startPosPub; - nt::DoubleArrayPublisher m_goalPosPub; - nt::DoubleArrayPublisher m_dynamicObsPub; - - nt::DoubleArraySubscriber m_pathPointsSub; - NT_Listener m_pathListenerHandle; - - std::vector m_currentPath; - bool m_newPathAvailable; - - wpi::mutex m_mutex; -}; -} diff --git a/src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectory.h b/src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectory.h new file mode 100644 index 0000000..28c9263 --- /dev/null +++ b/src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectory.h @@ -0,0 +1,182 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pathplanner/lib/trajectory/PathPlannerTrajectoryState.h" +#include "pathplanner/lib/path/PathConstraints.h" +#include "pathplanner/lib/util/GeometryUtil.h" +#include "pathplanner/lib/config/RobotConfig.h" +#include "pathplanner/lib/events/Event.h" + +namespace pathplanner { + +class PathPlannerPath; + +class PathPlannerTrajectory { +public: + PathPlannerTrajectory() { + } + + /** + * Create a trajectory with pre-generated states and list of events + * + * @param states Pre-generated states + * @param events Events for this trajectory + */ + PathPlannerTrajectory(std::vector states, + std::vector> events) : m_states(states), m_events( + events) { + } + + /** + * Create a trajectory with pre-generated states + * + * @param states Pre-generated states + */ + PathPlannerTrajectory(std::vector states) : m_states( + states) { + } + + /** + * Generate a new trajectory for a given path + * + * @param path The path to generate a trajectory for + * @param startingSpeeds The starting robot-relative chassis speeds of the robot + * @param startingRotation The starting field-relative rotation of the robot + * @param config The RobotConfig describing the robot + */ + PathPlannerTrajectory(std::shared_ptr path, + const frc::ChassisSpeeds &startingSpeeds, + const frc::Rotation2d &startingRotation, const RobotConfig &config); + + /** + * Get all the events to run while following this trajectory + * + * @return Events in this trajectory + */ + inline std::vector> getEvents() { + return m_events; + } + + /** + * Get all of the pre-generated states in the trajectory + * + * @return vector of all states + */ + constexpr std::vector& getStates() { + return m_states; + } + + /** + * Get the goal state at the given index + * + * @param index Index of the state to get + * @return The state at the given index + */ + inline PathPlannerTrajectoryState getState(size_t index) { + return m_states[index]; + } + + /** + * Get the initial state of the trajectory + * + * @return The initial state + */ + inline PathPlannerTrajectoryState getInitialState() { + return m_states[0]; + } + + /** + * Get the end state of the trajectory + * + * @return The end state + */ + inline PathPlannerTrajectoryState getEndState() { + return m_states[m_states.size() - 1]; + } + + /** + * Get the total run time of the trajectory + * + * @return Total run time in seconds + */ + inline units::second_t getTotalTime() { + return getEndState().time; + } + + /** + * Get the initial robot pose at the start of the trajectory + * + * @return Pose of the robot at the initial state + */ + inline frc::Pose2d getInitialPose() { + return getInitialState().pose; + } + + /** + * Get the target state at the given point in time along the trajectory + * + * @param time The time to sample the trajectory at in seconds + * @return The target state + */ + PathPlannerTrajectoryState sample(const units::second_t time); + + /** + * Flip this trajectory for the other side of the field, maintaining a blue alliance origin + * + * @return This trajectory with all states flipped to the other side of the field + */ + inline PathPlannerTrajectory flip() { + std::vector < PathPlannerTrajectoryState > mirroredStates; + for (auto state : m_states) { + mirroredStates.emplace_back(state.flip()); + } + return PathPlannerTrajectory(mirroredStates, getEvents()); + } +private: + std::vector m_states; + std::vector> m_events; + + static void generateStates(std::vector &states, + std::shared_ptr path, + const frc::Rotation2d &startingRotation, const RobotConfig &config); + + static void forwardAccelPass( + std::vector &states, + const RobotConfig &config); + + static void reverseAccelPass( + std::vector &states, + const RobotConfig &config); + + static void desaturateWheelSpeeds( + std::vector &moduleStates, + const frc::ChassisSpeeds &desiredSpeeds, + units::meters_per_second_t maxModuleSpeed, + units::meters_per_second_t maxTranslationSpeed, + units::radians_per_second_t maxRotationSpeed); + + static size_t getNextRotationTargetIdx( + std::shared_ptr path, const size_t startingIndex); + + static inline frc::Rotation2d cosineInterpolate(const frc::Rotation2d start, + const frc::Rotation2d end, const double t) { + double t2 = (1.0 - std::cos(t * M_PI)) / 2.0; + return GeometryUtil::rotationLerp(start, end, t2); + } +}; +} diff --git a/src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h b/src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h new file mode 100644 index 0000000..1b9e49e --- /dev/null +++ b/src/main/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include "pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h" +#include "pathplanner/lib/path/PathConstraints.h" +#include "pathplanner/lib/util/GeometryUtil.h" +#include "pathplanner/lib/util/DriveFeedforward.h" + +namespace pathplanner { +class PathPlannerTrajectoryState { +public: + units::second_t time = 0_s; + frc::ChassisSpeeds fieldSpeeds; + frc::Pose2d pose; + units::meters_per_second_t linearVelocity = 0_mps; + std::vector feedforwards; + + frc::Rotation2d heading; + units::meter_t deltaPos = 0_m; + frc::Rotation2d deltaRot; + std::vector moduleStates; + PathConstraints constraints; + double waypointRelativePos = 0.0; + + PathPlannerTrajectoryState() : constraints(0_mps, 0_mps_sq, 0_rad_per_s, + 0_rad_per_s_sq) { + } + + /** + * Interpolate between this state and the given state + * + * @param endVal State to interpolate with + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated state + */ + PathPlannerTrajectoryState interpolate( + const PathPlannerTrajectoryState &endVal, const double t) const; + + /** + * Get the state reversed, used for following a trajectory reversed with a differential drivetrain + * + * @return The reversed state + */ + PathPlannerTrajectoryState reverse() const; + + /** + * Flip this trajectory state for the other side of the field, maintaining a blue alliance origin + * + * @return This trajectory state flipped to the other side of the field + */ + PathPlannerTrajectoryState flip() const; + + /** + * Copy this state and change the timestamp + * + * @param time The new time to use + * @return Copied state with the given time + */ + PathPlannerTrajectoryState copyWithTime(units::second_t time) const; +}; +} diff --git a/src/main/include/pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h b/src/main/include/pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h new file mode 100644 index 0000000..3de07e5 --- /dev/null +++ b/src/main/include/pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include +#include +#include + +namespace pathplanner { +class SwerveModuleTrajectoryState { +public: + units::meters_per_second_t speed = 0_mps; + frc::Rotation2d angle; + frc::Rotation2d fieldAngle; + frc::Translation2d fieldPos; + + units::meter_t deltaPos = 0_m; +}; +} diff --git a/src/main/include/pathplanner/lib/util/DriveFeedforward.h b/src/main/include/pathplanner/lib/util/DriveFeedforward.h new file mode 100644 index 0000000..ac1ecd2 --- /dev/null +++ b/src/main/include/pathplanner/lib/util/DriveFeedforward.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include +#include "pathplanner/lib/util/GeometryUtil.h" + +namespace pathplanner { +struct DriveFeedforward { + /** + * Linear acceleration at the wheel in meters per second + */ + units::meters_per_second_squared_t acceleration = 0_mps_sq; + + /** + * Linear force applied by the motor at the wheel in newtons + */ + units::newton_t force = 0_N; + + /** + * Torque-current of the motor in amps + */ + units::ampere_t torqueCurrent = 0_A; + + constexpr DriveFeedforward interpolate(const DriveFeedforward &endValue, + const double t) const { + return DriveFeedforward { GeometryUtil::unitLerp(acceleration, + endValue.acceleration, t), GeometryUtil::unitLerp(force, + endValue.force, t), GeometryUtil::unitLerp(torqueCurrent, + endValue.torqueCurrent, t) }; + } + + constexpr DriveFeedforward reverse() const { + return DriveFeedforward { -acceleration, -force, -torqueCurrent }; + } +}; +} diff --git a/src/main/include/pathplanner/lib/util/FlippingUtil.h b/src/main/include/pathplanner/lib/util/FlippingUtil.h new file mode 100644 index 0000000..db81883 --- /dev/null +++ b/src/main/include/pathplanner/lib/util/FlippingUtil.h @@ -0,0 +1,108 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "pathplanner/lib/util/DriveFeedforward.h" + +namespace pathplanner { +class FlippingUtil { +public: + enum FieldSymmetry { + kRotational, kMirrored + }; + + static FieldSymmetry symmetryType; + static units::meter_t fieldSizeX; + static units::meter_t fieldSizeY; + + /** + * Flip a field position to the other side of the field, maintaining a blue alliance origin + * + * @param pos The position to flip + * @return The flipped position + */ + static inline frc::Translation2d flipFieldPosition( + const frc::Translation2d &pos) { + switch (symmetryType) { + case kRotational: + return frc::Translation2d(fieldSizeX - pos.X(), + fieldSizeY - pos.Y()); + case kMirrored: + default: + return frc::Translation2d(fieldSizeX - pos.X(), pos.Y()); + } + } + + /** + * Flip a field rotation to the other side of the field, maintaining a blue alliance origin + * + * @param rotation The rotation to flip + * @return The flipped rotation + */ + static inline frc::Rotation2d flipFieldRotation( + const frc::Rotation2d &rotation) { + switch (symmetryType) { + case kMirrored: + case kRotational: + default: + return frc::Rotation2d(180_deg) - rotation; + } + } + + /** + * Flip a field pose to the other side of the field, maintaining a blue alliance origin + * + * @param pose The pose to flip + * @return The flipped pose + */ + static inline frc::Pose2d flipFieldPose(const frc::Pose2d &pose) { + return frc::Pose2d(flipFieldPosition(pose.Translation()), + flipFieldRotation(pose.Rotation())); + } + + /** + * Flip field relative chassis speeds for the other side of the field, maintaining a blue alliance + * origin + * + * @param fieldSpeeds Field relative chassis speeds + * @return Flipped speeds + */ + static inline frc::ChassisSpeeds flipFieldSpeeds( + const frc::ChassisSpeeds &fieldSpeeds) { + switch (symmetryType) { + case kRotational: + return frc::ChassisSpeeds { -fieldSpeeds.vx, -fieldSpeeds.vy, + fieldSpeeds.omega }; + case kMirrored: + default: + return frc::ChassisSpeeds { -fieldSpeeds.vx, fieldSpeeds.vy, + -fieldSpeeds.omega }; + } + } + + static inline std::vector flipFeedforwards( + const std::vector &feedforwards) { + switch (symmetryType) { + case kRotational: + return feedforwards; + case kMirrored: + default: + if (feedforwards.size() == 4) { + std::vector < DriveFeedforward > flipped; + flipped.emplace_back(feedforwards[1]); + flipped.emplace_back(feedforwards[0]); + flipped.emplace_back(feedforwards[3]); + flipped.emplace_back(feedforwards[2]); + } else if (feedforwards.size() == 2) { + std::vector < DriveFeedforward > flipped; + flipped.emplace_back(feedforwards[1]); + flipped.emplace_back(feedforwards[0]); + } + return feedforwards; // idk + } + } +}; +} diff --git a/src/main/include/pathplanner/lib/util/GeometryUtil.h b/src/main/include/pathplanner/lib/util/GeometryUtil.h index 74cb85f..cdcf22a 100644 --- a/src/main/include/pathplanner/lib/util/GeometryUtil.h +++ b/src/main/include/pathplanner/lib/util/GeometryUtil.h @@ -13,61 +13,21 @@ #include #include #include +#include #define PI 3.14159265358979323846 -#define FIELD_LENGTH 16.54_m namespace pathplanner { namespace GeometryUtil { -constexpr frc::Translation2d flipFieldPosition(const frc::Translation2d &pos) { - return frc::Translation2d(FIELD_LENGTH - pos.X(), pos.Y()); -} - -constexpr frc::Rotation2d flipFieldRotation(const frc::Rotation2d &rotation) { - return frc::Rotation2d(180_deg) - rotation; -} - -constexpr frc::Pose2d flipFieldPose(const frc::Pose2d &pose) { - return frc::Pose2d(flipFieldPosition(pose.Translation()), - flipFieldRotation(pose.Rotation())); -} - -constexpr units::second_t unitLerp(units::second_t const startVal, - units::second_t const endVal, double const t) { - return startVal + (endVal - startVal) * t; -} - -constexpr units::meters_per_second_t unitLerp( - units::meters_per_second_t const startVal, - units::meters_per_second_t const endVal, double const t) { - return startVal + (endVal - startVal) * t; -} - -constexpr units::meters_per_second_squared_t unitLerp( - units::meters_per_second_squared_t const startVal, - units::meters_per_second_squared_t const endVal, double const t) { - return startVal + (endVal - startVal) * t; -} - -constexpr units::radians_per_second_t unitLerp( - units::radians_per_second_t const startVal, - units::radians_per_second_t const endVal, double const t) { - return startVal + (endVal - startVal) * t; -} - -constexpr units::radians_per_second_squared_t unitLerp( - units::radians_per_second_squared_t const startVal, - units::radians_per_second_squared_t const endVal, double const t) { - return startVal + (endVal - startVal) * t; -} - -constexpr units::meter_t unitLerp(units::meter_t const startVal, - units::meter_t const endVal, double const t) { +template::value>> +constexpr UnitType unitLerp(UnitType const startVal, UnitType const endVal, + double const t) { return startVal + (endVal - startVal) * t; } -constexpr units::curvature_t unitLerp(units::curvature_t const startVal, - units::curvature_t const endVal, double const t) { +constexpr double doubleLerp(const double startVal, const double endVal, + const double t) { return startVal + (endVal - startVal) * t; } @@ -107,16 +67,21 @@ constexpr frc::Rotation2d cosineInterpolate(frc::Rotation2d const y1, units::meter_t calculateRadius(const frc::Translation2d a, const frc::Translation2d b, const frc::Translation2d c); -inline units::degree_t modulo(units::degree_t const a, - units::degree_t const b) { +template::value>> +inline UnitType modulo(UnitType const a, UnitType const b) { return a - (b * units::math::floor(a / b)); } -inline bool isFinite(units::meter_t const u) { +template::value>> +inline bool isFinite(UnitType const u) { return std::isfinite(u()); } -inline bool isNaN(units::meter_t const u) { +template::value>> +inline bool isNaN(UnitType const u) { return std::isnan(u()); } } diff --git a/src/main/include/pathplanner/lib/util/HolonomicPathFollowerConfig.h b/src/main/include/pathplanner/lib/util/HolonomicPathFollowerConfig.h deleted file mode 100644 index cf685ab..0000000 --- a/src/main/include/pathplanner/lib/util/HolonomicPathFollowerConfig.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once - -#include -#include -#include -#include "pathplanner/lib/config/PIDConstants.h" -#include "pathplanner/lib/config/ReplanningConfig.h" - -namespace pathplanner { -class HolonomicPathFollowerConfig { -public: - const PIDConstants translationConstants; - const PIDConstants rotationConstants; - const units::meters_per_second_t maxModuleSpeed; - const units::meter_t driveBaseRadius; - const ReplanningConfig replanningConfig; - const units::second_t period; - - /** - * Create a new holonomic path follower config - * - * @param translationConstants {@link com.pathplanner.lib.util.PIDConstants} used for creating the - * translation PID controllers - * @param rotationConstants {@link com.pathplanner.lib.util.PIDConstants} used for creating the - * rotation PID controller - * @param maxModuleSpeed Max speed of an individual drive module in meters/sec - * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the - * distance from the center of the robot to the furthest module. For mecanum, this is the - * drive base width / 2 - * @param replanningConfig Path replanning configuration - * @param period Control loop period in seconds (Default = 0.02) - */ - constexpr HolonomicPathFollowerConfig( - const PIDConstants translationConstants, - const PIDConstants rotationConstants, - const units::meters_per_second_t maxModuleSpeed, - const units::meter_t driveBaseRadius, - const ReplanningConfig replanningConfig, - const units::second_t period = 0.02_s) : translationConstants( - translationConstants), rotationConstants(rotationConstants), maxModuleSpeed( - maxModuleSpeed), driveBaseRadius(driveBaseRadius), replanningConfig( - replanningConfig), period(period) { - } - - /** - * Create a new holonomic path follower config - * - * @param maxModuleSpeed Max speed of an individual drive module in meters/sec - * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the - * distance from the center of the robot to the furthest module. For mecanum, this is the - * drive base width / 2 - * @param replanningConfig Path replanning configuration - * @param period Control loop period in seconds (Default = 0.02) - */ - constexpr HolonomicPathFollowerConfig( - const units::meters_per_second_t maxModuleSpeed, - const units::meter_t driveBaseRadius, - const ReplanningConfig replanningConfig, - const units::second_t period = 0.02_s) : HolonomicPathFollowerConfig( - PIDConstants(5.0, 0.0, 0.0), PIDConstants(5.0, 0.0, 0.0), - maxModuleSpeed, driveBaseRadius, replanningConfig, period) { - } -}; -} diff --git a/src/main/include/pathplanner/lib/util/JSONUtil.h b/src/main/include/pathplanner/lib/util/JSONUtil.h new file mode 100644 index 0000000..e227b58 --- /dev/null +++ b/src/main/include/pathplanner/lib/util/JSONUtil.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace pathplanner { + +namespace JSONUtil { + +/** + * Create a Translation2d from a json object containing x and y fields + * + * @param translationJson The json object representing a translation + * @return Translation2d from the given json + */ +inline frc::Translation2d translation2dFromJson( + wpi::json::const_reference translationJson) { + auto x = units::meter_t { translationJson.at("x").get() }; + auto y = units::meter_t { translationJson.at("y").get() }; + return frc::Translation2d(x, y); +} + +} + +} diff --git a/src/main/include/pathplanner/lib/util/PPLibTelemetry.h b/src/main/include/pathplanner/lib/util/PPLibTelemetry.h index e1ff5a5..27986b8 100644 --- a/src/main/include/pathplanner/lib/util/PPLibTelemetry.h +++ b/src/main/include/pathplanner/lib/util/PPLibTelemetry.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include #include #include @@ -28,25 +30,29 @@ class PPLibTelemetry { units::meters_per_second_t commandedVel, units::degrees_per_second_t actualAngVel, units::degrees_per_second_t commandedAngVel) { - m_velPub.Set(std::span( { actualVel(), commandedVel(), - actualAngVel(), commandedAngVel() })); - } - - static inline void setPathInaccuracy(units::meter_t inaccuracy) { - m_inaccuracyPub.Set(inaccuracy()); + if (!m_compMode) { + m_velPub.Set(std::span( { actualVel(), commandedVel(), + actualAngVel(), commandedAngVel() })); + } } static inline void setCurrentPose(frc::Pose2d pose) { - m_posePub.Set( - std::span( { pose.X()(), pose.Y()(), - pose.Rotation().Radians()() })); + if (!m_compMode) { + m_posePub.Set(pose); + } } - static void setCurrentPath(std::shared_ptr path); + static inline void setCurrentPath(std::shared_ptr path) { + if (!m_compMode) { + auto poses = path->getPathPoses(); + m_pathPub.Set(std::span { poses.data(), poses.size() }); + } + } static inline void setTargetPose(frc::Pose2d targetPose) { - m_targetPosePub.Set(std::span( { targetPose.X()(), - targetPose.Y()(), targetPose.Rotation().Radians()() })); + if (!m_compMode) { + m_targetPosePub.Set(targetPose); + } } static void registerHotReloadPath(std::string pathName, @@ -60,10 +66,9 @@ class PPLibTelemetry { static bool m_compMode; static nt::DoubleArrayPublisher m_velPub; - static nt::DoublePublisher m_inaccuracyPub; - static nt::DoubleArrayPublisher m_posePub; - static nt::DoubleArrayPublisher m_pathPub; - static nt::DoubleArrayPublisher m_targetPosePub; + static nt::StructPublisher m_posePub; + static nt::StructArrayPublisher m_pathPub; + static nt::StructPublisher m_targetPosePub; static std::unordered_map>> m_hotReloadPaths; diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h index a9432ea..c1a1eef 100644 --- a/src/main/include/subsystems/SwerveSubsystem.h +++ b/src/main/include/subsystems/SwerveSubsystem.h @@ -10,6 +10,9 @@ #include #include #include +#include +#include +#include #include #include @@ -81,6 +84,8 @@ class SwerveSubsystem : public frc2::SubsystemBase { units::meter_t cachedNoteDist; frc::Pose2d latestNotePose; + std::shared_ptr ppControllers; + nt::StructPublisher foundNotePose{ nt::NetworkTableInstance::GetDefault() .GetTable("Vision")