diff --git a/.github/labeler.yml b/.github/labeler.yml index 55ef2984..bf3c2e4a 100644 --- a/.github/labeler.yml +++ b/.github/labeler.yml @@ -8,6 +8,7 @@ GUI: PathPlannerLib: - 'pathplannerlib/**' +- 'pathplannerlib-python/**' workflows: - '.github/workflows/**' \ No newline at end of file diff --git a/.github/workflows/pplib-ci.yml b/.github/workflows/pplib-ci.yml index e1f5336c..c537512a 100644 --- a/.github/workflows/pplib-ci.yml +++ b/.github/workflows/pplib-ci.yml @@ -53,6 +53,67 @@ jobs: # name: pplib-coverage # files: pathplannerlib/build/reports/jacoco/test/jacocoTestReport.xml + test-python: + name: "[PPLib Python] Unit Tests" + runs-on: ubuntu-22.04 + container: wpilib/ubuntu-base:22.04 + + steps: + - name: Checkout repo + uses: actions/checkout@v4 + + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + + - name: Install dependencies + working-directory: ./pathplannerlib-python + run: | + python -m pip install --upgrade pip + pip install pytest + if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + + - name: Test with pytest + working-directory: ./pathplannerlib-python + run: pytest + + build-python: + name: "[PPLib Python] Build" + runs-on: ubuntu-22.04 + container: wpilib/ubuntu-base:22.04 + needs: [test-python] + + steps: + - name: Checkout repo + uses: actions/checkout@v4 + + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + + - name: Install dependencies + working-directory: ./pathplannerlib-python + run: | + python -m pip install --upgrade pip + pip install build + if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + + - name: Set version + run: sed -i "s/0.0.0/${{ env.VERSION_NAME }}/g" pyproject.toml + working-directory: ./pathplannerlib-python + + - name: Build + working-directory: ./pathplannerlib-python + run: python -m build + + - name: Upload artifact + uses: actions/upload-artifact@v3 + with: + name: robotpy-pathplannerlib + path: pathplannerlib-python/dist + build-docker: strategy: fail-fast: false diff --git a/.github/workflows/publish-docs.yml b/.github/workflows/publish-docs.yml index 425add3c..e2d96c5c 100644 --- a/.github/workflows/publish-docs.yml +++ b/.github/workflows/publish-docs.yml @@ -29,12 +29,34 @@ jobs: with: working-directory: pathplannerlib - - name: Move to docs dir + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + + - name: Install python dependencies + working-directory: ./pathplannerlib-python + run: | + python -m pip install --upgrade pip + pip install build handsdown mkdocs mkdocs-material + if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + + - name: Generate python docs + working-directory: ./pathplannerlib-python + run: | + handsdown --external https://github.com/mjansen4857/pathplanner.git -o docsmd -n pathplannerlib --theme=material --create-configs include pathplannerlib + python -m mkdocs build + + - name: Move java/cpp to docs dir working-directory: pathplannerlib/build/docs run: | mkdir docs sudo mv javadoc docs/java sudo mv cpp/html docs/cpp + + - name: Move python to docs dir + run: | + sudo mv pathplannerlib-python/docs pathplannerlib/build/docs/docs/python - name: Deploy generated documentation to GitHub Pages uses: s0/git-publish-subdir-action@v2.6.0 diff --git a/.github/workflows/publish-pplib-python.yaml b/.github/workflows/publish-pplib-python.yaml new file mode 100644 index 00000000..e60d2290 --- /dev/null +++ b/.github/workflows/publish-pplib-python.yaml @@ -0,0 +1,48 @@ +name: Build PPLib Release + +on: + workflow_dispatch: + inputs: + version: + type: string + description: The full version string, i.e. 2024.0.0-alpha-1 or 2024.1.1 + required: true + +jobs: + build: + name: "Build & Publish to PyPI" + runs-on: ubuntu-22.04 + environment: + name: pypi + url: https://pypi.org/project/robotpy-pathplannerlib/ + permissions: + id-token: write + + steps: + - name: Checkout repo + uses: actions/checkout@v4 + + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + + - name: Install dependencies + working-directory: ./pathplannerlib-python + run: | + python -m pip install --upgrade pip + pip install build + if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + + - name: Set version + run: sed -i "s/0.0.0/${{ github.event.inputs.version }}/g" pyproject.toml + working-directory: ./pathplannerlib-python + + - name: Build + working-directory: ./pathplannerlib-python + run: python -m build + + - name: Publish to PyPI + uses: pypa/gh-action-pypi-publish@release/v1 + with: + packages-dir: pathplannerlib-python/dist/ diff --git a/pathplannerlib-python/.gitignore b/pathplannerlib-python/.gitignore new file mode 100644 index 00000000..4a5a185b --- /dev/null +++ b/pathplannerlib-python/.gitignore @@ -0,0 +1,4 @@ +/dist +/*.egg-info +/docs +/docsmd \ No newline at end of file diff --git a/pathplannerlib-python/LICENSE b/pathplannerlib-python/LICENSE new file mode 100644 index 00000000..5afa6e55 --- /dev/null +++ b/pathplannerlib-python/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2022 Michael Jansen + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/pathplannerlib-python/pathplannerlib/__init__.py b/pathplannerlib-python/pathplannerlib/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/pathplannerlib-python/pathplannerlib/auto.py b/pathplannerlib-python/pathplannerlib/auto.py new file mode 100644 index 00000000..b2c93981 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/auto.py @@ -0,0 +1,521 @@ +from commands2.functionalcommand import FunctionalCommand +import commands2.cmd as cmd +from .path import PathPlannerPath, PathConstraints +from typing import Callable, Tuple, List +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.kinematics import ChassisSpeeds +from .commands import FollowPathWithEvents, FollowPathRamsete, FollowPathHolonomic, FollowPathLTV, PathfindLTV, \ + PathfindHolonomic, PathfindRamsete, PathfindThenFollowPathHolonomic, PathfindThenFollowPathRamsete, \ + PathfindThenFollowPathLTV +import os +from wpilib import getDeployDirectory +import json +from commands2.command import Command +from commands2.subsystem import Subsystem +from .config import HolonomicPathFollowerConfig, ReplanningConfig + + +class NamedCommands: + _namedCommands: dict = {} + + @staticmethod + def registerCommand(name: str, command: Command) -> None: + """ + Registers a command with the given name + + :param name: the name of the command + :param command: the command to register + """ + NamedCommands._namedCommands[name] = command + + @staticmethod + def hasCommand(name: str) -> bool: + """ + Returns whether a command with the given name has been registered. + + :param name: the name of the command to check + :return: true if a command with the given name has been registered, false otherwise + """ + return name in NamedCommands._namedCommands + + @staticmethod + def getCommand(name: str) -> Command: + """ + Returns the command with the given name. + + :param name: the name of the command to get + :return: the command with the given name, wrapped in a functional command, or a none command if it has not been registered + """ + if NamedCommands.hasCommand(name): + return CommandUtil.wrappedEventCommand(NamedCommands._namedCommands[name]) + else: + return cmd.none() + + +class CommandUtil: + @staticmethod + def wrappedEventCommand(event_command: Command) -> Command: + """ + Wraps a command with a functional command that calls the command's initialize, execute, end, + and isFinished methods. This allows a command in the event map to be reused multiple times in + different command groups + + :param event_command: the command to wrap + :return: a functional command that wraps the given command + """ + return FunctionalCommand( + lambda: event_command.initialize(), + lambda: event_command.execute(), + lambda interupted: event_command.end(interupted), + lambda: event_command.isFinished(), + *event_command.getRequirements() + ) + + @staticmethod + def commandFromJson(command_json: dict) -> Command: + """ + Builds a command from the given json object + + :param command_json: the json dict to build the command from + :return: a command built from the json dict + """ + cmd_type = str(command_json['type']) + data = command_json['data'] + + if cmd_type == 'wait': + return CommandUtil._waitCommandFromData(data) + elif cmd_type == 'named': + return CommandUtil._namedCommandFromData(data) + elif cmd_type == 'path': + return CommandUtil._pathCommandFromData(data) + elif cmd_type == 'sequential': + return CommandUtil._sequentialGroupFromData(data) + elif cmd_type == 'parallel': + return CommandUtil._parallelGroupFromData(data) + elif cmd_type == 'race': + return CommandUtil._raceGroupFromData(data) + elif cmd_type == 'deadline': + return CommandUtil._deadlineGroupFromData(data) + + return cmd.none() + + @staticmethod + def _waitCommandFromData(data_json: dict) -> Command: + waitTime = float(data_json['waitTime']) + return cmd.waitSeconds(waitTime) + + @staticmethod + def _namedCommandFromData(data_json: dict) -> Command: + name = str(data_json['name']) + return NamedCommands.getCommand(name) + + @staticmethod + def _pathCommandFromData(data_json: dict) -> Command: + pathName = str(data_json['pathName']) + path = PathPlannerPath.fromPathFile(pathName) + return AutoBuilder.followPathWithEvents(path) + + @staticmethod + def _sequentialGroupFromData(data_json: dict) -> Command: + commands = [CommandUtil.commandFromJson(cmd_json) for cmd_json in data_json['commands']] + return cmd.sequence(*commands) + + @staticmethod + def _parallelGroupFromData(data_json: dict) -> Command: + commands = [CommandUtil.commandFromJson(cmd_json) for cmd_json in data_json['commands']] + return cmd.parallel(*commands) + + @staticmethod + def _raceGroupFromData(data_json: dict) -> Command: + commands = [CommandUtil.commandFromJson(cmd_json) for cmd_json in data_json['commands']] + return cmd.race(*commands) + + @staticmethod + def _deadlineGroupFromData(data_json: dict) -> Command: + commands = [CommandUtil.commandFromJson(cmd_json) for cmd_json in data_json['commands']] + return cmd.deadline(*commands) + + +class AutoBuilder: + _configured: bool = False + + _pathFollowingCommandBuilder: Callable[[PathPlannerPath], Command] = None + _getPose: Callable[[], Pose2d] = None + _resetPose: Callable[[Pose2d], None] = None + + _pathfindingConfigured: bool = False + _pathfindToPoseCommandBuilder: Callable[[Pose2d, PathConstraints, float, float], Command] = None + _pathfindThenFollowPathCommandBuilder: Callable[[PathPlannerPath, PathConstraints, float], Command] = None + + @staticmethod + def configureHolonomic(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d], None], + robot_relative_speeds_supplier: Callable[[], ChassisSpeeds], + robot_relative_output: Callable[[ChassisSpeeds], None], + config: HolonomicPathFollowerConfig, drive_subsystem: Subsystem) -> None: + """ + Configures the AutoBuilder for a holonomic drivetrain. + + :param pose_supplier: a supplier for the robot's current pose + :param reset_pose: a consumer for resetting the robot's pose + :param robot_relative_speeds_supplier: a supplier for the robot's current robot relative chassis speeds + :param robot_relative_output: a consumer for setting the robot's robot-relative chassis speeds + :param config: HolonomicPathFollowerConfig for configuring the path following commands + :param drive_subsystem: the subsystem for the robot's drive + """ + if AutoBuilder._configured: + raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once') + + AutoBuilder._pathFollowingCommandBuilder = lambda path: FollowPathHolonomic( + path, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + config, + drive_subsystem + ) + AutoBuilder._getPose = pose_supplier + AutoBuilder._resetPose = reset_pose + AutoBuilder._configured = True + + AutoBuilder._pathfindToPoseCommandBuilder = \ + lambda pose, constraints, goal_end_vel, rotation_delay_distance: PathfindHolonomic( + constraints, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + config, + drive_subsystem, + rotation_delay_distance=rotation_delay_distance, + target_pose=pose, + goal_end_vel=goal_end_vel + ) + AutoBuilder._pathfindThenFollowPathCommandBuilder = \ + lambda path, constraints, rotation_delay_distance: PathfindThenFollowPathHolonomic( + path, + constraints, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + config, + drive_subsystem, + rotation_delay_distance=rotation_delay_distance + ) + AutoBuilder._pathfindingConfigured = True + + @staticmethod + def configureRamsete(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d], None], + robot_relative_speeds_supplier: Callable[[], ChassisSpeeds], + robot_relative_output: Callable[[ChassisSpeeds], None], + replanning_config: ReplanningConfig, drive_subsystem: Subsystem) -> None: + """ + Configures the AutoBuilder for a differential drivetrain using a RAMSETE path follower. + + :param pose_supplier: a supplier for the robot's current pose + :param reset_pose: a consumer for resetting the robot's pose + :param robot_relative_speeds_supplier: a supplier for the robot's current robot relative chassis speeds + :param robot_relative_output: a consumer for setting the robot's robot-relative chassis speeds + :param replanning_config: Path replanning configuration + :param drive_subsystem: the subsystem for the robot's drive + """ + if AutoBuilder._configured: + raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once') + + AutoBuilder._pathFollowingCommandBuilder = lambda path: FollowPathRamsete( + path, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + replanning_config, + drive_subsystem + ) + AutoBuilder._getPose = pose_supplier + AutoBuilder._resetPose = reset_pose + AutoBuilder._configured = True + + AutoBuilder._pathfindToPoseCommandBuilder = \ + lambda pose, constraints, goal_end_vel, rotation_delay_distance: PathfindRamsete( + constraints, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + replanning_config, + drive_subsystem, + target_position=pose.translation(), + goal_end_vel=goal_end_vel + ) + AutoBuilder._pathfindThenFollowPathCommandBuilder = \ + lambda path, constraints, rotation_delay_distance: PathfindThenFollowPathRamsete( + path, + constraints, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + replanning_config, + drive_subsystem + ) + AutoBuilder._pathfindingConfigured = True + + @staticmethod + def configureLTV(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d], None], + robot_relative_speeds_supplier: Callable[[], ChassisSpeeds], + robot_relative_output: Callable[[ChassisSpeeds], None], + qelems: Tuple[float, float, float], relems: Tuple[float, float], dt: float, + replanning_config: ReplanningConfig, drive_subsystem: Subsystem) -> None: + """ + Configures the AutoBuilder for a differential drivetrain using a LTVUnicycleController path follower. + + :param pose_supplier: a supplier for the robot's current pose + :param reset_pose: a consumer for resetting the robot's pose + :param robot_relative_speeds_supplier: a supplier for the robot's current robot relative chassis speeds + :param robot_relative_output: a consumer for setting the robot's robot-relative chassis speeds + :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 replanning_config: Path replanning configuration + :param drive_subsystem: the subsystem for the robot's drive + """ + if AutoBuilder._configured: + raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once') + + AutoBuilder._pathFollowingCommandBuilder = lambda path: FollowPathLTV( + path, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + qelems, relems, dt, + replanning_config, + drive_subsystem + ) + AutoBuilder._getPose = pose_supplier + AutoBuilder._resetPose = reset_pose + AutoBuilder._configured = True + + AutoBuilder._pathfindToPoseCommandBuilder = \ + lambda pose, constraints, goal_end_vel, rotation_delay_distance: PathfindLTV( + constraints, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + qelems, relems, dt, + replanning_config, + drive_subsystem, + target_position=pose.translation(), + goal_end_vel=goal_end_vel + ) + AutoBuilder._pathfindThenFollowPathCommandBuilder = \ + lambda path, constraints, rotation_delay_distance: PathfindThenFollowPathLTV( + path, + constraints, + pose_supplier, + robot_relative_speeds_supplier, + robot_relative_output, + qelems, relems, dt, + replanning_config, + drive_subsystem + ) + AutoBuilder._pathfindingConfigured = True + + @staticmethod + def configureCustom(path_following_command_builder: Callable[[PathPlannerPath], Command], + pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d], None]) -> None: + """ + Configures the AutoBuilder with custom path following command builder. Building pathfinding commands is not supported if using a custom command builder. + + :param path_following_command_builder: a function that builds a command to follow a given path + :param pose_supplier: a supplier for the robot's current pose + :param reset_pose: a consumer for resetting the robot's pose + """ + if AutoBuilder._configured: + raise RuntimeError('AutoBuilder has already been configured. Please only configure AutoBuilder once') + + AutoBuilder._pathFollowingCommandBuilder = path_following_command_builder + AutoBuilder._getPose = pose_supplier + AutoBuilder._resetPose = reset_pose + AutoBuilder._configured = True + + AutoBuilder._pathfindingConfigured = False + + @staticmethod + def isConfigured() -> bool: + """ + Returns whether the AutoBuilder has been configured. + + :return: true if the AutoBuilder has been configured, false otherwise + """ + return AutoBuilder._configured + + @staticmethod + def isPathfindingConfigured() -> bool: + """ + Returns whether the AutoBuilder has been configured for pathfinding. + + :return: true if the AutoBuilder has been configured for pathfinding, false otherwise + """ + return AutoBuilder._pathfindingConfigured + + @staticmethod + def followPathWithEvents(path: PathPlannerPath) -> Command: + """ + 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 + """ + if not AutoBuilder.isConfigured(): + raise RuntimeError('Auto builder was used to build a path following command before being configured') + + return FollowPathWithEvents(AutoBuilder._pathFollowingCommandBuilder(path), path, AutoBuilder._getPose) + + @staticmethod + def pathfindToPose(pose: Pose2d, constraints: PathConstraints, goal_end_vel: float = 0.0, + rotation_delay_distance: float = 0.0) -> Command: + """ + Build a command to pathfind to a given pose. If not using a holonomic drivetrain, the pose rotation and rotation delay distance will have no effect. + + :param pose: The pose to pathfind to + :param constraints: The constraints to use while pathfinding + :param goal_end_vel: The goal end velocity of the robot when reaching the target pose + :param rotation_delay_distance: 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 + """ + if not AutoBuilder.isPathfindingConfigured(): + raise RuntimeError('Auto builder was used to build a pathfinding command before being configured') + + return AutoBuilder._pathfindToPoseCommandBuilder(pose, constraints, goal_end_vel, rotation_delay_distance) + + @staticmethod + def pathfindThenFollowPath(goal_path: PathPlannerPath, pathfinding_constraints: PathConstraints, + rotation_delay_distance: float = 0.0) -> Command: + """ + Build a command to pathfind to a given path, then follow that path. If not using a holonomic drivetrain, the pose rotation delay distance will have no effect. + + :param goal_path: The path to pathfind to, then follow + :param pathfinding_constraints: The constraints to use while pathfinding + :param rotation_delay_distance: 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 + """ + if not AutoBuilder.isPathfindingConfigured(): + raise RuntimeError('Auto builder was used to build a pathfinding command before being configured') + + return AutoBuilder._pathfindThenFollowPathCommandBuilder(goal_path, pathfinding_constraints, + rotation_delay_distance) + + @staticmethod + def getStartingPoseFromJson(starting_pose_json: dict) -> Pose2d: + """ + Get the starting pose from its JSON representation. This is only used internally. + + :param starting_pose_json: JSON dict representing a starting pose. + :return: The Pose2d starting pose + """ + pos = starting_pose_json['position'] + x = float(pos['x']) + y = float(pos['y']) + deg = float(starting_pose_json['rotation']) + + return Pose2d(x, y, Rotation2d.fromDegrees(deg)) + + @staticmethod + def getAutoCommandFromJson(auto_json: dict) -> Command: + """ + Builds an auto command from the given JSON dict. + + :param auto_json: the JSON dict to build the command from + :return: an auto command built from the JSON object + """ + commandJson = auto_json['command'] + + autoCommand = CommandUtil.commandFromJson(commandJson) + if auto_json['startingPose'] is not None: + startPose = AutoBuilder.getStartingPoseFromJson(auto_json['startingPose']) + return cmd.sequence(cmd.runOnce(lambda: AutoBuilder._resetPose(startPose)), autoCommand) + else: + return autoCommand + + @staticmethod + def buildAuto(auto_name: str) -> Command: + """ + Builds an auto command for the given auto name. + + :param auto_name: the name of the auto to build + :return: an auto command for the given auto name + """ + filePath = os.path.join(getDeployDirectory(), 'pathplanner', 'autos', auto_name + '.auto') + + with open(filePath, 'r') as f: + auto_json = json.loads(f.read()) + return AutoBuilder.getAutoCommandFromJson(auto_json) + + +class PathPlannerAuto(Command): + _autoCommand: Command + + def __init__(self, auto_name: str): + """ + Constructs a new PathPlannerAuto command. + + :param auto_name: the name of the autonomous routine to load and run + """ + super().__init__() + + if not AutoBuilder.isConfigured(): + raise RuntimeError('AutoBuilder was not configured before attempting to load a PathPlannerAuto from file') + + self._autoCommand = AutoBuilder.buildAuto(auto_name) + self.addRequirements(*self._autoCommand.getRequirements()) + self.setName(auto_name) + + @staticmethod + def getStartingPoseFromAutoFile(auto_name: str) -> Pose2d: + """ + Get the starting pose from the given auto file + + :param auto_name: Name of the auto to get the pose from + :return: Starting pose from the given auto + """ + filePath = os.path.join(getDeployDirectory(), 'pathplanner', 'autos', auto_name + '.auto') + + with open(filePath, 'r') as f: + auto_json = json.loads(f.read()) + return AutoBuilder.getStartingPoseFromJson(auto_json['startingPose']) + + @staticmethod + def _pathsFromCommandJson(command_json: dict) -> List[PathPlannerPath]: + paths = [] + + cmdType = str(command_json['type']) + data = command_json['data'] + + if cmdType == 'path': + pathName = str(data['pathName']) + paths.append(PathPlannerPath.fromPathFile(pathName)) + elif cmdType == 'sequential' or cmdType == 'parallel' or cmdType == 'race' or cmdType == 'deadline': + for cmdJson in data['commands']: + paths.extend(PathPlannerAuto._pathsFromCommandJson(cmdJson)) + return paths + + @staticmethod + def getPathGroupFromAutoFile(auto_name: str) -> List[PathPlannerPath]: + """ + Get a list of every path in the given auto (depth first) + + :param auto_name: Name of the auto to get the path group from + :return: List of paths in the auto + """ + filePath = os.path.join(getDeployDirectory(), 'pathplanner', 'autos', auto_name + '.auto') + + with open(filePath, 'r') as f: + auto_json = json.loads(f.read()) + return PathPlannerAuto._pathsFromCommandJson(auto_json['command']) + + def initialize(self): + self._autoCommand.initialize() + + def execute(self): + self._autoCommand.execute() + + def isFinished(self) -> bool: + return self._autoCommand.isFinished() + + def end(self, interrupted: bool): + self._autoCommand.end(interrupted) diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py new file mode 100644 index 00000000..1c5c25cc --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/commands.py @@ -0,0 +1,703 @@ +import math + +from .controller import * +from .path import PathPlannerPath, EventMarker, GoalEndState, PathConstraints +from .trajectory import PathPlannerTrajectory +from .telemetry import PPLibTelemetry +from .logging import PathPlannerLogging +from .geometry_util import floatLerp +from wpimath.geometry import Pose2d +from wpimath.kinematics import ChassisSpeeds +from wpilib import Timer +from commands2 import Command, Subsystem, SequentialCommandGroup +from typing import Callable, Tuple, List +from .config import ReplanningConfig, HolonomicPathFollowerConfig +from .pathfinding import Pathfinding + + +class FollowPathWithEvents(Command): + _pathFollowingCommand: Command + _path: PathPlannerPath + _poseSupplier: Callable[[], Pose2d] + + _currentCommands: dict = {} + _untriggeredMarkers: List[EventMarker] = [] + _isFinished: bool = False + + def __init__(self, path_following_command: Command, path: PathPlannerPath, pose_supplier: Callable[[], Pose2d]): + """ + Constructs a new FollowPathWithEvents command. + + :param path_following_command: the command to follow the path + :param path: the path to follow + :param pose_supplier: a supplier for the robot's current pose + """ + super().__init__() + self._pathFollowingCommand = path_following_command + self._path = path + self._poseSupplier = pose_supplier + + self.addRequirements(*self._pathFollowingCommand.getRequirements()) + for marker in self._path.getEventMarkers(): + reqs = marker.command.getRequirements() + + for req in self._pathFollowingCommand.getRequirements(): + if req in reqs: + raise RuntimeError( + 'Events that are triggered during path following cannot require the drive subsystem') + + self.addRequirements(*reqs) + + def initialize(self): + self._isFinished = False + + self._currentCommands.clear() + + currentPose = self._poseSupplier() + for marker in self._path.getEventMarkers(): + marker.reset(currentPose) + + self._untriggeredMarkers.clear() + for marker in self._path.getEventMarkers(): + self._untriggeredMarkers.append(marker) + + self._pathFollowingCommand.initialize() + self._currentCommands[self._pathFollowingCommand] = True + + def execute(self): + for command in self._currentCommands: + if not self._currentCommands[command]: + continue + + command.execute() + + if command.isFinished(): + command.end(False) + self._currentCommands[command] = False + if command == self._pathFollowingCommand: + self._isFinished = True + + currentPose = self._poseSupplier() + toTrigger = [marker for marker in self._untriggeredMarkers if marker.shouldTrigger(currentPose)] + + for marker in toTrigger: + self._untriggeredMarkers.remove(marker) + + for marker in toTrigger: + for command in self._currentCommands: + if not self._currentCommands[command]: + continue + + for req in command.getRequirements(): + if req in marker.command.getRequirements(): + command.end(True) + self._currentCommands[command] = False + break + + marker.command.initialize() + self._currentCommands[marker.command] = True + + def isFinished(self) -> bool: + return self._isFinished + + def end(self, interrupted: bool): + for command in self._currentCommands: + if self._currentCommands[command]: + command.end(True) + + +class FollowPathCommand(Command): + _path: PathPlannerPath + _poseSupplier: Callable[[], Pose2d] + _speedsSupplier: Callable[[], ChassisSpeeds] + _output: Callable[[ChassisSpeeds], None] + _controller: PathFollowingController + _replanningConfig: ReplanningConfig + + _timer: Timer = Timer() + _generatedTrajectory: PathPlannerTrajectory = None + + def __init__(self, path: PathPlannerPath, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + controller: PathFollowingController, replanning_config: ReplanningConfig, *requirements: Subsystem): + """ + Construct a base path following command + + :param path: The path to follow + :param pose_supplier: Function that supplies the current field-relative pose of the robot + :param speeds_supplier: Function that supplies the current robot-relative chassis speeds + :param output_robot_relative: Function that will apply the robot-relative output speeds of this command + :param controller: Path following controller that will be used to follow the path + :param replanning_config: Path replanning configuration + :param requirements: Subsystems required by this command, usually just the drive subsystem + """ + super().__init__() + + self._path = path + self._poseSupplier = pose_supplier + self._speedsSupplier = speeds_supplier + self._output = output_robot_relative + self._controller = controller + self._replanningConfig = replanning_config + + self.addRequirements(*requirements) + + def initialize(self): + currentPose = self._poseSupplier() + currentSpeeds = self._speedsSupplier() + + self._controller.reset(currentPose, currentSpeeds) + + if self._replanningConfig.enableInitialReplanning and ( + currentPose.translation().distance(self._path.getPoint(0).position) >= 0.25 or math.hypot( + currentSpeeds.vx, currentSpeeds.vy) >= 0.25): + self._replanPath(currentPose, currentSpeeds) + else: + self._generatedTrajectory = PathPlannerTrajectory(self._path, currentSpeeds, currentPose.rotation()) + PathPlannerLogging.logActivePath(self._path) + PPLibTelemetry.setCurrentPath(self._path) + + self._timer.reset() + self._timer.start() + + def execute(self): + currentTime = self._timer.get() + targetState = self._generatedTrajectory.sample(currentTime) + if not self._controller.isHolonomic() and self._path.isReversed(): + targetState = targetState.reverse() + + currentPose = self._poseSupplier() + currentSpeeds = self._speedsSupplier() + + if self._replanningConfig.enableDynamicReplanning: + previousError = abs(self._controller.getPositionalError()) + currentError = currentPose.translation().distance(targetState.positionMeters) + + if currentError >= self._replanningConfig.dynamicReplanningTotalErrorThreshold or currentError - previousError >= self._replanningConfig.dynamicReplanningErrorSpikeThreshold: + self._replanPath(currentPose, currentSpeeds) + self._timer.reset() + targetState = self._generatedTrajectory.sample(0.0) + + targetSpeeds = self._controller.calculateRobotRelativeSpeeds(currentPose, targetState) + + currentVel = math.hypot(currentSpeeds.vx, currentSpeeds.vy) + + PPLibTelemetry.setCurrentPose(currentPose) + PathPlannerLogging.logCurrentPose(currentPose) + + if self._controller.isHolonomic(): + PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose()) + PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose()) + else: + PPLibTelemetry.setTargetPose(targetState.getDifferentialPose()) + PathPlannerLogging.logTargetPose(targetState.getDifferentialPose()) + + PPLibTelemetry.setVelocities(currentVel, targetState.velocityMps, currentSpeeds.omega, targetSpeeds.omega) + PPLibTelemetry.setPathInaccuracy(self._controller.getPositionalError()) + + self._output(targetSpeeds) + + def isFinished(self) -> bool: + return self._timer.hasElapsed(self._generatedTrajectory.getTotalTimeSeconds()) + + def end(self, interrupted: bool): + self._timer.stop() + + # 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 not interrupted and self._path.getGoalEndState().velocity < 0.1: + self._output(ChassisSpeeds()) + + PathPlannerLogging.logActivePath(None) + + def _replanPath(self, current_pose: Pose2d, current_speeds: ChassisSpeeds) -> None: + replanned = self._path.replan(current_pose, current_speeds) + self._generatedTrajectory = PathPlannerTrajectory(replanned, current_speeds, current_pose.rotation()) + PathPlannerLogging.logActivePath(replanned) + PPLibTelemetry.setCurrentPath(replanned) + + +class FollowPathHolonomic(FollowPathCommand): + def __init__(self, path: PathPlannerPath, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + config: HolonomicPathFollowerConfig, *requirements: Subsystem): + """ + Construct a path following command that will use a holonomic drive controller for holonomic drive trains + + :param path: The path to follow + :param pose_supplier: Function that supplies the current field-relative pose of the robot + :param speeds_supplier: Function that supplies the current robot-relative chassis speeds + :param output_robot_relative: Function that will apply the robot-relative output speeds of this command + :param config: Holonomic path follower configuration + :param requirements: Subsystems required by this command, usually just the drive subsystem + """ + super().__init__(path, pose_supplier, speeds_supplier, output_robot_relative, PPHolonomicDriveController( + config.translationConstants, config.rotationConstants, config.maxModuleSpeed, config.driveBaseRadius, + config.period + ), config.replanningConfig, *requirements) + + +class FollowPathRamsete(FollowPathCommand): + def __init__(self, path: PathPlannerPath, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + replanning_config: ReplanningConfig, *requirements: Subsystem): + """ + Construct a path following command that will use a Ramsete path following controller for differential drive trains + + :param path: The path to follow + :param pose_supplier: Function that supplies the current field-relative pose of the robot + :param speeds_supplier: Function that supplies the current robot-relative chassis speeds + :param output_robot_relative: Function that will apply the robot-relative output speeds of this command + :param replanning_config: Path replanning configuration + :param requirements: Subsystems required by this command, usually just the drive subsystem + """ + super().__init__(path, pose_supplier, speeds_supplier, output_robot_relative, PPRamseteController(), + replanning_config, *requirements) + + +class FollowPathLTV(FollowPathCommand): + def __init__(self, path: PathPlannerPath, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + qelems: Tuple[float, float, float], relems: Tuple[float, float], dt: float, + replanning_config: ReplanningConfig, *requirements: Subsystem): + """ + Construct a path following command that will use a LTV path following controller for differential drive trains + + :param path: The path to follow + :param pose_supplier: Function that supplies the current field-relative pose of the robot + :param speeds_supplier: Function that supplies the current robot-relative chassis speeds + :param output_robot_relative: 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 replanning_config: Path replanning configuration + :param requirements: Subsystems required by this command, usually just the drive subsystem + """ + super().__init__(path, pose_supplier, speeds_supplier, output_robot_relative, + PPLTVController(qelems, relems, dt), + replanning_config, *requirements) + + +class PathfindingCommand(Command): + _timer: Timer = Timer() + _targetPath: Union[PathPlannerPath, None] + _targetPose: Pose2d + _goalEndState: GoalEndState + _constraints: PathConstraints + _poseSupplier: Callable[[], Pose2d] + _speedsSupplier: Callable[[], ChassisSpeeds] + _output: Callable[[ChassisSpeeds], None] + _controller: PathFollowingController + _rotationDelayDistance: float + _replanningConfig: ReplanningConfig + + _currentPath: Union[PathPlannerPath, None] + _currentTrajectory: Union[PathPlannerTrajectory, None] + _startingPose: Pose2d + + _timeOffset: float = 0 + + def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + controller: PathFollowingController, replanning_config: ReplanningConfig, *requirements: Subsystem, + rotation_delay_distance: float = 0.0, target_path: PathPlannerPath = None, target_pose: Pose2d = None, + goal_end_vel: float = 0): + super().__init__() + if target_path is None and target_pose is None: + raise ValueError('Either target_path or target_pose must be specified for PathfindingCommand') + + self.addRequirements(*requirements) + + Pathfinding.ensureInitialized() + + self._constraints = constraints + self._controller = controller + self._poseSupplier = pose_supplier + self._speedsSupplier = speeds_supplier + self._output = output_robot_relative + self._rotationDelayDistance = rotation_delay_distance + self._replanningConfig = replanning_config + + if target_path is not None: + targetRotation = Rotation2d() + for p in target_path.getAllPathPoints(): + if p.rotationTarget is not None: + targetRotation = p.rotationTarget.target + break + self._targetPath = target_path + self._targetPose = Pose2d(target_path.getPoint(0).position, targetRotation) + self._goalEndState = GoalEndState(target_path.getGlobalConstraints().maxVelocityMps, targetRotation, True) + else: + self._targetPath = None + self._targetPose = target_pose + self._goalEndState = GoalEndState(goal_end_vel, target_pose.rotation(), True) + + def initialize(self): + self._currentTrajectory = None + self._timeOffset = 0.0 + + currentPose = self._poseSupplier() + + self._controller.reset(currentPose, self._speedsSupplier()) + + if self._targetPath is not None: + self._targetPose = Pose2d(self._targetPath.getPoint(0).position, self._goalEndState.rotation) + + if currentPose.translation().distance(self._targetPose.translation()) < 0.25: + self.cancel() + else: + Pathfinding.setStartPosition(currentPose.translation()) + Pathfinding.setGoalPosition(self._targetPose.translation()) + + self._startingPose = currentPose + + def execute(self): + currentPose = self._poseSupplier() + currentSpeeds = self._speedsSupplier() + + PathPlannerLogging.logCurrentPose(currentPose) + PPLibTelemetry.setCurrentPose(currentPose) + + # Skip new paths if we are close to the end + skipUpdates = self._currentTrajectory is not None and currentPose.translation().distance( + self._currentTrajectory.getEndState().positionMeters) < 2.0 + + if not skipUpdates and Pathfinding.isNewPathAvailable(): + self._currentPath = Pathfinding.getCurrentPath(self._constraints, self._goalEndState) + + if self._currentPath is not None: + self._currentTrajectory = PathPlannerTrajectory(self._currentPath, currentSpeeds, + currentPose.rotation()) + + # Find the two closest states in front of and behind robot + closestState1Idx = 0 + closestState2Idx = 1 + while True: + closest2Dist = self._currentTrajectory.getState(closestState2Idx).positionMeters.distance( + currentPose.translation()) + nextDist = self._currentTrajectory.getState(closestState2Idx + 1).positionMeters.distance( + currentPose.translation()) + + if nextDist < closest2Dist: + closestState1Idx += 1 + closestState2Idx += 1 + else: + break + + # Use the closest 2 states to interpolate what the time offset should be + # This will account for the delay in pathfinding + closestState1 = self._currentTrajectory.getState(closestState1Idx) + closestState2 = self._currentTrajectory.getState(closestState2Idx) + + fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(currentSpeeds, currentPose.rotation()) + currentHeading = Rotation2d(fieldRelativeSpeeds.vx, fieldRelativeSpeeds.vy) + headingError = currentHeading - closestState1.heading + onHeading = math.hypot(currentSpeeds.vx, currentSpeeds.vy) < 1.0 or abs(headingError.degrees()) < 30 + + # Replan the path if we are more than 0.25m away or our heading is off + if not onHeading or ( + self._replanningConfig.enableInitialReplanning and currentPose.translation().distance( + closestState1.positionMeters) > 0.25): + self._currentPath = self._currentPath.replan(currentPose, currentSpeeds) + self._currentTrajectory = PathPlannerTrajectory(self._currentPath, currentSpeeds, + currentPose.rotation()) + + self._timeOffset = 0.0 + else: + d = closestState1.positionMeters.distance(closestState2.positionMeters) + t = (currentPose.translation().distance(closestState1.positionMeters)) / d + + self._timeOffset = floatLerp(closestState1.timeSeconds, closestState2.timeSeconds, t) + + PathPlannerLogging.logActivePath(self._currentPath) + PPLibTelemetry.setCurrentPath(self._currentPath) + + self._timer.reset() + self._timer.start() + + if self._currentTrajectory is not None: + targetState = self._currentTrajectory.sample(self._timer.get() + self._timeOffset) + + if self._replanningConfig.enableDynamicReplanning: + previousError = abs(self._controller.getPositionalError()) + currentError = currentPose.translation().distance(targetState.positionMeters) + + if currentError >= self._replanningConfig.dynamicReplanningTotalErrorThreshold or currentError - previousError >= self._replanningConfig.dynamicReplanningErrorSpikeThreshold: + replanned = self._currentPath.replan(currentPose, currentSpeeds) + self._currentTrajectory = PathPlannerTrajectory(replanned, currentSpeeds, currentPose.rotation()) + PathPlannerLogging.logActivePath(replanned) + PPLibTelemetry.setCurrentPath(replanned) + + self._timer.reset() + self._timeOffset = 0.0 + targetState = self._currentTrajectory.sample(0.0) + + # Set the target rotation to the starting rotation if we have not yet traveled the rotation + # delay distance + if currentPose.translation().distance(self._startingPose.translation()) < self._rotationDelayDistance: + targetState.targetHolonomicRotation = self._startingPose.rotation() + + targetSpeeds = self._controller.calculateRobotRelativeSpeeds(currentPose, targetState) + + currentVel = math.hypot(currentSpeeds.vx, currentSpeeds.vy) + + PPLibTelemetry.setCurrentPose(currentPose) + PathPlannerLogging.logCurrentPose(currentPose) + + if self._controller.isHolonomic(): + PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose()) + PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose()) + else: + PPLibTelemetry.setTargetPose(targetState.getDifferentialPose()) + PathPlannerLogging.logTargetPose(targetState.getDifferentialPose()) + + PPLibTelemetry.setVelocities(currentVel, targetState.velocityMps, currentSpeeds.omega, targetSpeeds.omega) + PPLibTelemetry.setPathInaccuracy(self._controller.getPositionalError()) + + self._output(targetSpeeds) + + def isFinished(self) -> bool: + if self._targetPath is not None: + currentPose = self._poseSupplier() + currentSpeeds = self._speedsSupplier() + + currentVel = math.hypot(currentSpeeds.vx, currentSpeeds.vy) + stoppingDistance = (currentVel ** 2) / (2 * self._constraints.maxAccelerationMpsSq) + + return currentPose.translation().distance(self._targetPath.getPoint(0).position) <= stoppingDistance + + if self._currentTrajectory is not None: + return self._timer.hasElapsed(self._currentTrajectory.getTotalTimeSeconds() - self._timeOffset) + + return False + + def end(self, interrupted: bool): + self._timer.stop() + + # 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 not interrupted and self._goalEndState.velocity < 0.1: + self._output(ChassisSpeeds()) + + PathPlannerLogging.logActivePath(None) + + +class PathfindHolonomic(PathfindingCommand): + def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + config: HolonomicPathFollowerConfig, *requirements: Subsystem, + rotation_delay_distance: float = 0.0, target_path: PathPlannerPath = None, target_pose: Pose2d = None, + goal_end_vel: float = 0): + """ + Constructs a new PathfindHolonomic command that will generate a path towards the given path or pose. + NOTE: Either target_path or target_pose must be specified + + :param constraints: the path constraints to use while pathfinding + :param pose_supplier: a supplier for the robot's current pose + :param speeds_supplier: a supplier for the robot's current robot relative speeds + :param output_robot_relative: a consumer for the output speeds (robot relative) + :param config: HolonomicPathFollowerConfig object with the configuration parameters for path following + :param requirements: the subsystems required by this command + :param rotation_delay_distance: 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. + :param target_path: the path to pathfind to + :param target_pose: the pose to pathfind to + :param goal_end_vel: The goal end velocity when reaching the given pose + """ + super().__init__(constraints, pose_supplier, speeds_supplier, output_robot_relative, + PPHolonomicDriveController(config.translationConstants, config.rotationConstants, + config.maxModuleSpeed, config.driveBaseRadius, config.period), + config.replanningConfig, *requirements, rotation_delay_distance=rotation_delay_distance, + target_path=target_path, + target_pose=target_pose, goal_end_vel=goal_end_vel) + + +class PathfindRamsete(PathfindingCommand): + def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + replanning_config: ReplanningConfig, *requirements: Subsystem, + target_path: PathPlannerPath = None, target_position: Translation2d = None, + goal_end_vel: float = 0): + """ + Constructs a new PathfindRamsete command that will generate a path towards the given path or pose. + NOTE: Either target_path or target_position must be specified. + + :param constraints: the path constraints to use while pathfinding + :param pose_supplier: a supplier for the robot's current pose + :param speeds_supplier: a supplier for the robot's current robot relative speeds + :param output_robot_relative: a consumer for the output speeds (robot relative) + :param replanning_config: Path replanning configuration + :param requirements: the subsystems required by this command + :param target_path: the path to pathfind to + :param target_position: the position to pathfind to + :param goal_end_vel: The goal end velocity when reaching the given position + """ + super().__init__(constraints, pose_supplier, speeds_supplier, output_robot_relative, + PPRamseteController(), replanning_config, *requirements, target_path=target_path, + target_pose=Pose2d(target_position, Rotation2d()), goal_end_vel=goal_end_vel) + + +class PathfindLTV(PathfindingCommand): + def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + qelems: Tuple[float, float, float], relems: Tuple[float, float], dt: float, + replanning_config: ReplanningConfig, *requirements: Subsystem, + target_path: PathPlannerPath = None, target_position: Translation2d = None, + goal_end_vel: float = 0): + """ + Constructs a new PathfindLTV command that will generate a path towards the given path or pose. + NOTE: Either target_path or target_position must be specified. + + :param constraints: the path constraints to use while pathfinding + :param pose_supplier: a supplier for the robot's current pose + :param speeds_supplier: a supplier for the robot's current robot relative speeds + :param output_robot_relative: 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: The amount of time between each robot control loop, default is 0.02s + :param replanning_config: Path replanning configuration + :param requirements: the subsystems required by this command + :param target_path: the path to pathfind to + :param target_position: the position to pathfind to + :param goal_end_vel: The goal end velocity when reaching the given position + """ + super().__init__(constraints, pose_supplier, speeds_supplier, output_robot_relative, + PPLTVController(qelems, relems, dt), replanning_config, *requirements, + target_path=target_path, target_pose=Pose2d(target_position, Rotation2d()), + goal_end_vel=goal_end_vel) + + +class PathfindThenFollowPathHolonomic(SequentialCommandGroup): + def __init__(self, goal_path: PathPlannerPath, pathfinding_constraints: PathConstraints, + pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + config: HolonomicPathFollowerConfig, *requirements: Subsystem, rotation_delay_distance: float = 0.0): + """ + Constructs a new PathfindThenFollowPathHolonomic command group. + + :param goal_path: the goal path to follow + :param pathfinding_constraints: the path constraints for pathfinding + :param pose_supplier: a supplier for the robot's current pose + :param speeds_supplier: a supplier for the robot's current robot relative speeds + :param output_robot_relative: a consumer for the output speeds (robot relative) + :param config: HolonomicPathFollowerConfig for configuring the path following commands + :param requirements: the subsystems required by this command (drive subsystem) + :param rotation_delay_distance: 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. + """ + super().__init__() + + self.addCommands( + PathfindHolonomic( + pathfinding_constraints, + pose_supplier, + speeds_supplier, + output_robot_relative, + config, + *requirements, + target_path=goal_path, + rotation_delay_distance=rotation_delay_distance + ), + FollowPathWithEvents( + FollowPathHolonomic( + goal_path, + pose_supplier, + speeds_supplier, + output_robot_relative, + config, + *requirements + ), + goal_path, + pose_supplier + ) + ) + + +class PathfindThenFollowPathRamsete(SequentialCommandGroup): + def __init__(self, goal_path: PathPlannerPath, pathfinding_constraints: PathConstraints, + pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + replanning_config: ReplanningConfig, *requirements: Subsystem): + """ + Constructs a new PathfindThenFollowPathRamsete command group. + + :param goal_path: the goal path to follow + :param pathfinding_constraints: the path constraints for pathfinding + :param pose_supplier: a supplier for the robot's current pose + :param speeds_supplier: a supplier for the robot's current robot relative speeds + :param output_robot_relative: a consumer for the output speeds (robot relative) + :param replanning_config: Path replanning configuration + :param requirements: the subsystems required by this command (drive subsystem) + """ + super().__init__() + + self.addCommands( + PathfindRamsete( + pathfinding_constraints, + pose_supplier, + speeds_supplier, + output_robot_relative, + replanning_config, + *requirements, + target_path=goal_path + ), + FollowPathWithEvents( + FollowPathRamsete( + goal_path, + pose_supplier, + speeds_supplier, + output_robot_relative, + replanning_config, + *requirements + ), + goal_path, + pose_supplier + ) + ) + + +class PathfindThenFollowPathLTV(SequentialCommandGroup): + def __init__(self, goal_path: PathPlannerPath, pathfinding_constraints: PathConstraints, + pose_supplier: Callable[[], Pose2d], + speeds_supplier: Callable[[], ChassisSpeeds], output_robot_relative: Callable[[ChassisSpeeds], None], + qelems: Tuple[float, float, float], relems: Tuple[float, float], dt: float, + replanning_config: ReplanningConfig, *requirements: Subsystem): + """ + Constructs a new PathfindThenFollowPathRamsete command group. + + :param goal_path: the goal path to follow + :param pathfinding_constraints: the path constraints for pathfinding + :param pose_supplier: a supplier for the robot's current pose + :param speeds_supplier: a supplier for the robot's current robot relative speeds + :param output_robot_relative: 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: The amount of time between each robot control loop, default is 0.02s + :param replanning_config: Path replanning configuration + :param requirements: the subsystems required by this command (drive subsystem) + """ + super().__init__() + + self.addCommands( + PathfindLTV( + pathfinding_constraints, + pose_supplier, + speeds_supplier, + output_robot_relative, + qelems, relems, dt, + replanning_config, + *requirements, + target_path=goal_path + ), + FollowPathWithEvents( + FollowPathLTV( + goal_path, + pose_supplier, + speeds_supplier, + output_robot_relative, + qelems, relems, dt, + replanning_config, + *requirements + ), + goal_path, + pose_supplier + ) + ) diff --git a/pathplannerlib-python/pathplannerlib/config.py b/pathplannerlib-python/pathplannerlib/config.py new file mode 100644 index 00000000..7ef85fd4 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/config.py @@ -0,0 +1,56 @@ +from dataclasses import dataclass + + +@dataclass +class PIDConstants: + """ + PID constants used to create PID controllers + + Args: + kP (float): P + kI (float): I + kD (float): D + iZone (float): Integral range + """ + kP: float = 0.0 + kI: float = 0.0 + kD: float = 0.0 + iZone: float = 0.0 + + +@dataclass +class ReplanningConfig: + """ + Configuration for path replanning + + Args: + enableInitialReplanning (bool): Should the path be replanned at the start of path following if the robot is not already at the starting point? + enableDynamicReplanning (bool): Should the path be replanned if the error grows too large or if a large error spike happens while following the path? + dynamicReplanningTotalErrorThreshold (float): The total error threshold, in meters, that will cause the path to be replanned + dynamicReplanningErrorSpikeThreshold (float): The error spike threshold, in meters, that will cause the path to be replanned + """ + enableInitialReplanning: bool = True + enableDynamicReplanning: bool = False + dynamicReplanningTotalErrorThreshold: float = 1.0 + dynamicReplanningErrorSpikeThreshold: float = 0.25 + + +@dataclass +class HolonomicPathFollowerConfig: + """ + Configuration for the holonomic path following commands + + Args: + translationConstants (PIDConstants): PIDConstants used for creating the translation PID controllers + rotationConstants (PIDConstants): PIDConstants used for creating the rotation PID controller + maxModuleSpeed (float): Max speed of an individual drive module in meters/sec + driveBaseRadius (float): The radius of the drive base in meters. This is the distance from the center of the robot to the furthest module. + replanningConfig (ReplanningConfig): Path replanning configuration + period (float): Control loop period in seconds (Default = 0.02) + """ + translationConstants: PIDConstants + rotationConstants: PIDConstants + maxModuleSpeed: float + driveBaseRadius: float + replanningConfig: ReplanningConfig + period: float = 0.02 diff --git a/pathplannerlib-python/pathplannerlib/controller.py b/pathplannerlib-python/pathplannerlib/controller.py new file mode 100644 index 00000000..b0801491 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/controller.py @@ -0,0 +1,314 @@ +from wpimath.geometry import Pose2d, Translation2d, Rotation2d +from wpimath.kinematics import ChassisSpeeds +from wpimath.controller import PIDController, ProfiledPIDController, RamseteController, LTVUnicycleController +from wpimath.trajectory import TrapezoidProfile +from .trajectory import State +from typing import Callable, Union +from .config import PIDConstants +import math + + +class PathFollowingController: + def calculateRobotRelativeSpeeds(self, current_pose: Pose2d, target_state: State) -> ChassisSpeeds: + """ + Calculates the next output of the path following controller + + :param current_pose: The current robot pose + :param target_state: The desired trajectory state + :return: The next robot relative output of the path following controller + """ + raise NotImplementedError + + def reset(self, current_pose: Pose2d, current_speeds: ChassisSpeeds) -> None: + """ + Resets the controller based on the current state of the robot + + :param current_pose: Current robot pose + :param current_speeds: Current robot relative chassis speeds + """ + raise NotImplementedError + + def getPositionalError(self) -> float: + """ + Get the current positional error between the robot's actual and target positions + + :return: Positional error, in meters + """ + raise NotImplementedError + + def isHolonomic(self) -> bool: + """ + Is this controller for holonomic drivetrains? Used to handle some differences in functionality in the path following command. + + :return: True if this controller is for a holonomic drive train + """ + raise NotImplementedError + + +class PPHolonomicDriveController(PathFollowingController): + _xController: PIDController + _yController: PIDController + _rotationController: ProfiledPIDController + _maxModuleSpeed: float + _mpsToRps: float + _translationError: Translation2d + + _isEnabled: bool = True + _rotationTargetOverride: Union[Callable[[], Union[Rotation2d, None]], None] = None + + def __init__(self, translation_constants: PIDConstants, rotation_constants: PIDConstants, max_module_speed: float, + drive_base_radius: float, period: float = 0.02): + """ + Constructs a HolonomicDriveController + + :param translation_constants: PID constants for the translation PID controllers + :param rotation_constants: PID constants for the rotation controller + :param max_module_speed: The max speed of a drive module in meters/sec + :param drive_base_radius: 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 + """ + self._xController = PIDController(translation_constants.kP, translation_constants.kI, translation_constants.kD, + period) + self._xController.setIntegratorRange(-translation_constants.iZone, translation_constants.iZone) + + self._yController = PIDController(translation_constants.kP, translation_constants.kI, translation_constants.kD, + period) + self._yController.setIntegratorRange(-translation_constants.iZone, translation_constants.iZone) + + # Temp rate limit of 0, will be changed in calculate + self._rotationController = ProfiledPIDController( + rotation_constants.kP, rotation_constants.kI, rotation_constants.kD, + TrapezoidProfile.Constraints(0, 0), period + ) + self._rotationController.setIntegratorRange(-rotation_constants.iZone, rotation_constants.iZone) + self._rotationController.enableContinuousInput(-math.pi, math.pi) + + self._maxModuleSpeed = max_module_speed + self._mpsToRps = 1.0 / drive_base_radius + + def setEnabled(self, enabled: bool) -> None: + """ + Enables and disables the controller for troubleshooting. When calculate() is called on a disabled controller, only feedforward values are returned. + + :param enabled: If the controller is enabled or not + """ + self._isEnabled = enabled + + def calculateRobotRelativeSpeeds(self, current_pose: Pose2d, target_state: State) -> ChassisSpeeds: + """ + Calculates the next output of the path following controller + + :param current_pose: The current robot pose + :param target_state: The desired trajectory state + :return: The next robot relative output of the path following controller + """ + xFF = target_state.velocityMps * target_state.heading.cos() + yFF = target_state.velocityMps * target_state.heading.sin() + + self._translationError = current_pose.translation() - target_state.positionMeters + + if not self._isEnabled: + return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, 0, current_pose.rotation()) + + xFeedback = self._xController.calculate(current_pose.X(), target_state.positionMeters.X()) + yFeedback = self._yController.calculate(current_pose.Y(), target_state.positionMeters.Y()) + + angVelConstraint = target_state.constraints.maxAngularVelocityRps + # Approximation of available module speed to do rotation with + maxAngVelModule = max(0.0, self._maxModuleSpeed - target_state.velocityMps) * self._mpsToRps + maxAngVel = min(angVelConstraint, maxAngVelModule) + + rotationConstraints = TrapezoidProfile.Constraints(maxAngVel, + target_state.constraints.maxAngularAccelerationRpsSq) + + targetRotation = target_state.targetHolonomicRotation + if PPHolonomicDriveController._rotationTargetOverride is not None: + rot = PPHolonomicDriveController._rotationTargetOverride() + if rot is not None: + targetRotation = rot + + rotationFeedback = self._rotationController.calculate( + current_pose.rotation().radians(), + targetRotation.radians(), + rotationConstraints + ) + rotationFF = self._rotationController.getSetpoint().velocity + + return ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, + current_pose.rotation()) + + def reset(self, current_pose: Pose2d, current_speeds: ChassisSpeeds) -> None: + """ + Resets the controller based on the current state of the robot + + :param current_pose: Current robot pose + :param current_speeds: Current robot relative chassis speeds + """ + self._rotationController.reset(current_pose.rotation().radians(), current_speeds.omega) + + def getPositionalError(self) -> float: + """ + Get the current positional error between the robot's actual and target positions + + :return: Positional error, in meters + """ + return self._translationError.norm() + + def isHolonomic(self) -> bool: + """ + Is this controller for holonomic drivetrains? Used to handle some differences in functionality in the path following command. + + :return: True if this controller is for a holonomic drive train + """ + return True + + @staticmethod + def setRotationTargetOverride(rotation_target_override: Union[Callable[[], Union[Rotation2d, None]], None]) -> None: + """ + Set a supplier that will be used to override the rotation target when path following. + + This function should return an empty optional to use the rotation targets in the path + + :param rotation_target_override: Supplier to override rotation targets + """ + PPHolonomicDriveController._rotationTargetOverride = rotation_target_override + test = PPRamseteController(1.0, 1.0) + + +class PPRamseteController(PathFollowingController, RamseteController): + _lastError: float = 0.0 + + def __init__(self, *args, **kwargs): + """ + __init__(*args, **kwargs) + Overloaded function. + + 1. __init__(self, b: float, zeta: float) -> None + + Construct a Ramsete unicycle controller. + + :param b: Tuning parameter (b > 0 rad²/m²) for which larger values make + convergence more aggressive like a proportional term. + :param zeta: Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger + values provide more damping in response. + + 2. __init__(self) -> None + + Construct a Ramsete unicycle controller. The default arguments for + b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce + desirable results. + """ + super().__init__(*args, **kwargs) + + def calculateRobotRelativeSpeeds(self, current_pose: Pose2d, target_state: State) -> ChassisSpeeds: + """ + Calculates the next output of the path following controller + + :param current_pose: The current robot pose + :param target_state: The desired trajectory state + :return: The next robot relative output of the path following controller + """ + self._lastError = current_pose.translation().distance(target_state.positionMeters) + + return self.calculate(current_pose, target_state.getDifferentialPose(), target_state.velocityMps, + target_state.headingAngularVelocityRps) + + def reset(self, current_pose: Pose2d, current_speeds: ChassisSpeeds) -> None: + """ + Resets the controller based on the current state of the robot + + :param current_pose: Current robot pose + :param current_speeds: Current robot relative chassis speeds + """ + self._lastError = 0.0 + + def getPositionalError(self) -> float: + """ + Get the current positional error between the robot's actual and target positions + + :return: Positional error, in meters + """ + return self._lastError + + def isHolonomic(self) -> bool: + """ + Is this controller for holonomic drivetrains? Used to handle some differences in functionality in the path following command. + + :return: True if this controller is for a holonomic drive train + """ + return False + + +class PPLTVController(PathFollowingController, LTVUnicycleController): + _lastError: float = 0.0 + + def __init__(self, *args, **kwargs): + """ + __init__(*args, **kwargs) + Overloaded function. + + 1. __init__(self, dt: wpimath.units.seconds, maxVelocity: wpimath.units.meters_per_second = 9.0) -> None + + Constructs a linear time-varying unicycle controller with default maximum + desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum + desired control effort of (1 m/s, 2 rad/s). + + :param dt: Discretization timestep. + :param maxVelocity: The maximum velocity for the controller gain lookup + table. + @throws std::domain_error if maxVelocity <= 0. + + 2. __init__(self, Qelems: Tuple[float, float, float], Relems: Tuple[float, float], dt: wpimath.units.seconds, maxVelocity: wpimath.units.meters_per_second = 9.0) -> None + + Constructs a linear time-varying unicycle controller. + + See + https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning + for how to select the tolerances. + + :param Qelems: The maximum desired error tolerance for each state. + :param Relems: The maximum desired control effort for each input. + :param dt: Discretization timestep. + :param maxVelocity: The maximum velocity for the controller gain lookup + table. + @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s. + """ + super().__init__(*args, **kwargs) + + def calculateRobotRelativeSpeeds(self, current_pose: Pose2d, target_state: State) -> ChassisSpeeds: + """ + Calculates the next output of the path following controller + + :param current_pose: The current robot pose + :param target_state: The desired trajectory state + :return: The next robot relative output of the path following controller + """ + self._lastError = current_pose.translation().distance(target_state.positionMeters) + + return self.calculate(current_pose, target_state.getDifferentialPose(), target_state.velocityMps, + target_state.headingAngularVelocityRps) + + def reset(self, current_pose: Pose2d, current_speeds: ChassisSpeeds) -> None: + """ + Resets the controller based on the current state of the robot + + :param current_pose: Current robot pose + :param current_speeds: Current robot relative chassis speeds + """ + self._lastError = 0.0 + + def getPositionalError(self) -> float: + """ + Get the current positional error between the robot's actual and target positions + + :return: Positional error, in meters + """ + return self._lastError + + def isHolonomic(self) -> bool: + """ + Is this controller for holonomic drivetrains? Used to handle some differences in functionality in the path following command. + + :return: True if this controller is for a holonomic drive train + """ + return False diff --git a/pathplannerlib-python/pathplannerlib/geometry_util.py b/pathplannerlib-python/pathplannerlib/geometry_util.py new file mode 100644 index 00000000..76d078f6 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/geometry_util.py @@ -0,0 +1,100 @@ +from wpimath.geometry import Translation2d, Rotation2d +import math + + +def floatLerp(start_val: float, end_val: float, t: float) -> float: + """ + Interpolate between two floats + + :param start_val: Start value + :param end_val: End value + :param t: Interpolation factor (0.0-1.0) + :return: Interpolated value + """ + return start_val + (end_val - start_val) * t + + +def translationLerp(a: Translation2d, b: Translation2d, t: float) -> Translation2d: + """ + Linear interpolation between two Translation2ds + + :param a: Start value + :param b: End value + :param t: Interpolation factor (0.0-1.0) + :return: Interpolated value + """ + return a + ((b - a) * t) + + +def rotationLerp(a: Rotation2d, b: Rotation2d, t: float) -> Rotation2d: + """ + Interpolate between two Rotation2ds + + :param a: Start value + :param b: End value + :param t: Interpolation factor (0.0-1.0) + :return: Interpolated value + """ + return a + ((b - a) * t) + + +def quadraticLerp(a: Translation2d, b: Translation2d, c: Translation2d, t: float) -> Translation2d: + """ + Quadratic interpolation between Translation2ds + + :param a: Position 1 + :param b: Position 2 + :param c: Position 3 + :param t: Interpolation factor (0.0-1.0) + :return: Interpolated value + """ + p0 = translationLerp(a, b, t) + p1 = translationLerp(b, c, t) + return translationLerp(p0, p1, t) + + +def cubicLerp(a: Translation2d, b: Translation2d, c: Translation2d, d: Translation2d, t: float) -> Translation2d: + """ + Cubic interpolation between Translation2ds + + :param a: Position 1 + :param b: Position 2 + :param c: Position 3 + :param d: Position 4 + :param t: Interpolation factor (0.0-1.0) + :return: Interpolated value + """ + p0 = quadraticLerp(a, b, c, t) + p1 = quadraticLerp(b, c, d, t) + return translationLerp(p0, p1, t) + + +def calculateRadius(a: Translation2d, b: Translation2d, c: Translation2d) -> float: + """ + Calculate the curve radius given 3 points on the curve + + :param a: Point A + :param b: Point B + :param c: Point C + :return: Curve radius + """ + vba = a - b + vbc = c - b + cross_z = (vba.X() * vbc.X()) - (vba.X() * vbc.X()) + sign = 1 if cross_z < 0 else -1 + + ab = a.distance(b) + bc = b.distance(c) + ac = a.distance(c) + + p = (ab + bc + ac) / 2 + area = math.sqrt(math.fabs(p * (p - ab) * (p - bc) * (p - ac))) + if area == 0: + return float('inf') + return sign * (ab * bc * ac) / (4 * area) + + +def decimal_range(start: float, stop: float, increment: float): + while start < stop and not math.isclose(start, stop): + yield start + start += increment diff --git a/pathplannerlib-python/pathplannerlib/logging.py b/pathplannerlib-python/pathplannerlib/logging.py new file mode 100644 index 00000000..9e307f57 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/logging.py @@ -0,0 +1,70 @@ +from typing import Callable, List, Union +from wpimath.geometry import Pose2d, Rotation2d +from .path import PathPlannerPath + + +class PathPlannerLogging: + _logCurrentPose: Callable[[Pose2d], None] = None + _logTargetPose: Callable[[Pose2d], None] = None + _logActivePath: Callable[[List[Pose2d]], None] = None + + @staticmethod + def setLogCurrentPoseCallback(log_current_pose: Callable[[Pose2d], None]) -> None: + """ + Set the logging callback for the current robot pose + + :param log_current_pose: Consumer that accepts the current robot pose. Can be null to disable logging this value. + """ + PathPlannerLogging._logCurrentPose = log_current_pose + + @staticmethod + def setLogTargetPoseCallback(log_target_pose: Callable[[Pose2d], None]) -> None: + """ + Set the logging callback for the target robot pose + + :param log_target_pose: Consumer that accepts the target robot pose. Can be null to disable logging this value. + """ + PathPlannerLogging._logTargetPose = log_target_pose + + @staticmethod + def setLogActivePathCallback(log_active_path: Callable[[List[Pose2d]], None]) -> None: + """ + Set the logging callback for the active path + + :param log_active_path: Consumer that accepts the active path as a list of poses. Can be null to disable logging this value. + """ + PathPlannerLogging._logActivePath = log_active_path + + @staticmethod + def logCurrentPose(pose: Pose2d) -> None: + """ + Log the current robot pose. This is used internally. + + :param pose: The current robot pose + """ + if PathPlannerLogging._logCurrentPose is not None: + PathPlannerLogging._logCurrentPose(pose) + + @staticmethod + def logTargetPose(pose: Pose2d) -> None: + """ + Log the target robot pose. This is used internally. + + :param pose: The target robot pose + """ + if PathPlannerLogging._logTargetPose is not None: + PathPlannerLogging._logTargetPose(pose) + + @staticmethod + def logActivePath(path: Union[PathPlannerPath, None]) -> None: + """ + Log the active path. This is used internally. + + :param path: The active path + """ + if PathPlannerLogging._logActivePath is not None: + if path is not None: + poses = [Pose2d(p.position, Rotation2d()) for p in path.getAllPathPoints()] + PathPlannerLogging._logActivePath(poses) + else: + PathPlannerLogging._logActivePath([]) diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py new file mode 100644 index 00000000..14f2ae91 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -0,0 +1,854 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import Final, List +from wpimath.geometry import Rotation2d, Translation2d, Pose2d +from wpimath.kinematics import ChassisSpeeds +import wpimath.units as units +from wpimath import inputModulus +from commands2 import Command +from .geometry_util import decimal_range, cubicLerp, calculateRadius +from wpilib import getDeployDirectory +import os +import json + +RESOLUTION: Final[float] = 0.05 + + +@dataclass(frozen=True) +class PathConstraints: + """ + Kinematic path following constraints + + Args: + maxVelocityMps (float): Max linear velocity (M/S) + maxAccelerationMpsSq (float): Max linear acceleration (M/S^2) + maxAngularVelocityRps (float): Max angular velocity (Rad/S) + maxAngularAccelerationRpsSq (float): Max angular acceleration (Rad/S^2) + """ + maxVelocityMps: float + maxAccelerationMpsSq: float + maxAngularVelocityRps: float + maxAngularAccelerationRpsSq: float + + @staticmethod + def fromJson(json_dict: dict) -> PathConstraints: + maxVel = float(json_dict['maxVelocity']) + maxAccel = float(json_dict['maxAcceleration']) + maxAngularVel = float(json_dict['maxAngularVelocity']) + maxAngularAccel = float(json_dict['maxAngularAcceleration']) + + return PathConstraints( + maxVel, + maxAccel, + units.degreesToRadians(maxAngularVel), + units.degreesToRadians(maxAngularAccel)) + + def __eq__(self, other): + return (isinstance(other, PathConstraints) + and other.maxVelocityMps == self.maxVelocityMps + and other.maxAccelerationMpsSq == self.maxAccelerationMpsSq + and other.maxAngularVelocityRps == self.maxAngularVelocityRps + and other.maxAngularAccelerationRpsSq == self.maxAngularAccelerationRpsSq) + + +@dataclass(frozen=True) +class GoalEndState: + """ + Describes the goal end state of the robot when finishing a path + + Args: + velocity (float): The goal end velocity (M/S) + rotation (Rotation2d): The goal rotation + rotateFast (bool): Should the robot reach the rotation as fast as possible + """ + velocity: float + rotation: Rotation2d + rotateFast: bool = False + + @staticmethod + def fromJson(json_dict: dict) -> GoalEndState: + vel = float(json_dict['velocity']) + deg = float(json_dict['rotation']) + + rotateFast = False + if 'rotateFast' in json_dict: + rotateFast = bool(json_dict['rotateFast']) + + return GoalEndState(vel, Rotation2d.fromDegrees(deg), rotateFast) + + def __eq__(self, other): + return (isinstance(other, GoalEndState) + and other.velocity == self.velocity + and other.rotation == self.rotation + and other.rotateFast == self.rotateFast) + + +@dataclass(frozen=True) +class ConstraintsZone: + """ + A zone on a path with different kinematic constraints + + Args: + minWaypointPos (float): Starting position of the zone + maxWaypointPos (float): End position of the zone + constraints (PathConstraints): PathConstraints to apply within the zone + """ + minWaypointPos: float + maxWaypointPos: float + constraints: PathConstraints + + @staticmethod + def fromJson(json_dict: dict) -> ConstraintsZone: + minPos = float(json_dict['minWaypointRelativePos']) + maxPos = float(json_dict['maxWaypointRelativePos']) + constraints = PathConstraints.fromJson(json_dict['constraints']) + + return ConstraintsZone(minPos, maxPos, constraints) + + def isWithinZone(self, t: float) -> bool: + """ + 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 + """ + return self.minWaypointPos <= t <= self.maxWaypointPos + + def overlapsRange(self, min_pos: float, max_pos: float) -> bool: + """ + Get if this zone overlaps a given range + + :param min_pos: The minimum waypoint relative position of the range + :param max_pos: The maximum waypoint relative position of the range + :return: True if any part of this zone is within the given range + """ + return max(min_pos, self.minWaypointPos) <= min(max_pos, self.maxWaypointPos) + + def forSegmentIndex(self, segment_index: int) -> ConstraintsZone: + """ + 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 segment_index: The segment index to transform positions for + :return: The transformed zone + """ + return ConstraintsZone(self.minWaypointPos - segment_index, self.maxWaypointPos - segment_index, + self.constraints) + + def __eq__(self, other): + return (isinstance(other, ConstraintsZone) + and other.minWaypointPos == self.minWaypointPos + and other.maxWaypointPos == self.maxWaypointPos + and other.constraints == self.constraints) + + +@dataclass(frozen=True) +class RotationTarget: + """ + A target holonomic rotation at a position along a path + + Args: + waypointRelativePosition (float): Waypoint relative position of this target + target (Rotation2d): Target rotation + rotateFast (bool): Should the robot reach the rotation as fast as possible + """ + waypointRelativePosition: float + target: Rotation2d + rotateFast: bool = False + + @staticmethod + def fromJson(json_dict: dict) -> RotationTarget: + pos = float(json_dict['waypointRelativePos']) + deg = float(json_dict['rotationDegrees']) + + rotateFast = False + if 'rotateFast' in json_dict: + rotateFast = bool(json_dict['rotateFast']) + + return RotationTarget(pos, Rotation2d.fromDegrees(deg), rotateFast) + + def forSegmentIndex(self, segment_index: int) -> RotationTarget: + """ + 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 segment_index: The segment index to transform position for + :return: The transformed target + """ + return RotationTarget(self.waypointRelativePosition - segment_index, self.target, self.rotateFast) + + def __eq__(self, other): + return (isinstance(other, RotationTarget) + and other.waypointRelativePosition == self.waypointRelativePosition + and other.target == self.target + and other.rotateFast == self.rotateFast) + + +@dataclass +class EventMarker: + """ + Position along the path that will trigger a command when reached + + Args: + waypointRelativePos (float): The waypoint relative position of the marker + command (Command): The command that should be triggered at this marker + minimumTriggerDistance (float): The minimum distance the robot must be within for this marker to be triggered + """ + waypointRelativePos: float + command: Command + minimumTriggerDistance: float = 0.5 + markerPos: Translation2d = None + lastRobotPos: Translation2d = None + + @staticmethod + def fromJson(json_dict: dict) -> EventMarker: + pos = float(json_dict['waypointRelativePos']) + from .auto import CommandUtil + command = CommandUtil.commandFromJson(json_dict['command']) + return EventMarker(pos, command) + + def reset(self, robot_pose: Pose2d) -> None: + """ + Reset the current robot position + + :param robot_pose: The current pose of the robot + """ + self.lastRobotPos = robot_pose.translation() + + def shouldTrigger(self, robot_pose: Pose2d) -> bool: + """ + Get if this event marker should be triggered + + :param robot_pose: Current pose of the robot + :return: True if this marker should be triggered + """ + if self.lastRobotPos is None or self.markerPos is None: + self.lastRobotPos = robot_pose.translation() + return False + + distanceToMarker = robot_pose.translation().distance(self.markerPos) + trigger = self.minimumTriggerDistance >= distanceToMarker > self.lastRobotPos.distance(self.markerPos) + self.lastRobotPos = robot_pose.translation() + return trigger + + def __eq__(self, other): + return (isinstance(other, EventMarker) + and other.waypointRelativePos == self.waypointRelativePos + and other.minimumTriggerDistance == self.minimumTriggerDistance + and other.command == self.command) + + +@dataclass +class PathPoint: + """ + A point along a pathplanner path + + Args: + position (Translation2d): Position of the point + rotationTarget (RotationTarget): Rotation target at this point + constraints (PathConstraints): The constraints at this point + """ + position: Translation2d + rotationTarget: RotationTarget = None + constraints: PathConstraints = None + distanceAlongPath: float = 0.0 + curveRadius: float = 0.0 + maxV: float = float('inf') + + def __eq__(self, other): + return (isinstance(other, PathPoint) + and other.position == self.position + and other.holonomicRotation == self.rotationTarget + and other.constraints == self.constraints + and other.distanceAlongPath == self.distanceAlongPath + and other.curveRadius == self.curveRadius + and other.maxV == self.maxV) + + +class PathSegment: + segmentPoints: List[PathPoint] + + def __init__(self, p1: Translation2d, p2: Translation2d, p3: Translation2d, p4: Translation2d, + target_holonomic_rotations: List[RotationTarget] = [], constraint_zones: List[ConstraintsZone] = [], + end_segment: bool = False): + """ + 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 target_holonomic_rotations: Rotation targets for within this segment + :param constraint_zones: Constraint zones for within this segment + :param end_segment: Is this the last segment in the path + """ + self.segmentPoints = [] + + for t in decimal_range(0.0, 1.0, RESOLUTION): + holonomicRotation = None + + if len(target_holonomic_rotations) > 0: + if math.fabs(target_holonomic_rotations[0].waypointRelativePosition - t) <= math.fabs( + target_holonomic_rotations[0].waypointRelativePosition - min(t + RESOLUTION, 1.0)): + holonomicRotation = target_holonomic_rotations.pop(0) + + currentZone = self._findConstraintsZone(constraint_zones, t) + + if currentZone is not None: + self.segmentPoints.append( + PathPoint(cubicLerp(p1, p2, p3, p4, t), holonomicRotation, currentZone.constraints)) + else: + self.segmentPoints.append(PathPoint(cubicLerp(p1, p2, p3, p4, t), holonomicRotation)) + + if end_segment: + holonomicRotation = target_holonomic_rotations.pop(0) if len( + target_holonomic_rotations) > 0 else None + self.segmentPoints.append(PathPoint(cubicLerp(p1, p2, p3, p4, 1.0), holonomicRotation)) + + @staticmethod + def _findConstraintsZone(zones: List[ConstraintsZone], t: float) -> ConstraintsZone | None: + for zone in zones: + if zone.isWithinZone(t): + return zone + return None + + +class PathPlannerPath: + _bezierPoints: List[Translation2d] + _rotationTargets: List[RotationTarget] + _constraintZones: List[ConstraintsZone] + _eventMarkers: List[EventMarker] + _globalConstraints: PathConstraints + _goalEndState: GoalEndState + _allPoints: List[PathPoint] + _reversed: bool + _previewStartingRotation: Rotation2d + + def __init__(self, bezier_points: List[Translation2d], constraints: PathConstraints, goal_end_state: GoalEndState, + holonomic_rotations: List[RotationTarget] = [], constraint_zones: List[ConstraintsZone] = [], + event_markers: List[EventMarker] = [], is_reversed: bool = False, + preview_starting_rotation: Rotation2d = Rotation2d()): + """ + Create a new path planner path + + :param bezier_points: List of points representing the cubic Bezier curve of the path + :param constraints: The global constraints of the path + :param goal_end_state: The goal end state of the path + :param holonomic_rotations: List of rotation targets along the path + :param constraint_zones: List of constraint zones along the path + :param event_markers: List of event markers along the path + :param is_reversed: Should the robot follow the path reversed (differential drive only) + :param preview_starting_rotation: The settings used for previews in the UI + """ + self._bezierPoints = bezier_points + self._rotationTargets = holonomic_rotations + self._constraintZones = constraint_zones + self._eventMarkers = event_markers + self._globalConstraints = constraints + self._goalEndState = goal_end_state + self._reversed = is_reversed + if len(bezier_points) >= 4: + self._allPoints = PathPlannerPath._createPath(self._bezierPoints, self._rotationTargets, + self._constraintZones) + self._precalcValues() + self._previewStartingRotation = preview_starting_rotation + + @staticmethod + def fromPathPoints(path_points: List[PathPoint], constraints: PathConstraints, + goal_end_state: GoalEndState) -> PathPlannerPath: + """ + Create a path with pre-generated points. This should already be a smooth path. + + :param path_points: Path points along the smooth curve of the path + :param constraints: The global constraints of the path + :param goal_end_state: The goal end state of the path + :return: A PathPlannerPath following the given pathpoints + """ + path = PathPlannerPath([], constraints, goal_end_state) + path._allPoints = path_points + path._precalcValues() + + return path + + @staticmethod + def fromPathFile(path_name: str) -> PathPlannerPath: + """ + Load a path from a path file in storage + + :param path_name: The name of the path to load + :return: PathPlannerPath created from the given file name + """ + filePath = os.path.join(getDeployDirectory(), 'pathplanner', 'paths', path_name + '.path') + + with open(filePath, 'r') as f: + pathJson = json.loads(f.read()) + return PathPlannerPath._fromJson(pathJson) + + @staticmethod + def bezierFromPoses(poses: List[Pose2d]) -> List[Translation2d]: + """ + Create the bezier points necessary to create a path using a list of poses + + :param poses: List of poses. Each pose represents one waypoint. + :return: Bezier points + """ + if len(poses) < 2: + raise ValueError('Not enough poses') + + # First pose + bezierPoints = [poses[0].translation(), poses[0].translation() + Translation2d( + poses[0].translation().distance(poses[1].translation()) / 3.0, + poses[0].rotation())] + + # Middle poses + for i in range(1, len(poses) - 1): + anchor = poses[i].translation() + + # Prev control + bezierPoints.append(anchor + Translation2d(anchor.distance(poses[i - 1].translation()) / 3.0, + poses[i].rotation() + Rotation2d.fromDegrees(180))) + # Anchor + bezierPoints.append(anchor) + # Next control + bezierPoints.append( + anchor + Translation2d(anchor.distance(poses[i + 1].translation()) / 3.0, poses[i].rotation())) + + # Last pose + bezierPoints.append(poses[len(poses) - 1].translation() + Translation2d( + poses[len(poses) - 1].translation().distance(poses[len(poses) - 2].translation()) / 3.0, + poses[len(poses) - 1].rotation() + Rotation2d.fromDegrees(180))) + bezierPoints.append(poses[len(poses) - 1].translation()) + + return bezierPoints + + def getAllPathPoints(self) -> List[PathPoint]: + """ + Get all the path points in this path + + :return: Path points in the path + """ + return self._allPoints + + def numPoints(self) -> int: + """ + Get the number of points in this path + + :return: Number of points in the path + """ + return len(self._allPoints) + + def getPoint(self, index: int) -> PathPoint: + """ + Get a specific point along this path + + :param index: Index of the point to get + :return: The point at the given index + """ + return self._allPoints[index] + + def getGlobalConstraints(self) -> PathConstraints: + """ + Get the global constraints for this path + + :return: Global constraints that apply to this path + """ + return self._globalConstraints + + def getGoalEndState(self) -> GoalEndState: + """ + Get the goal end state of this path + + :return: The goal end state + """ + return self._goalEndState + + def getEventMarkers(self) -> List[EventMarker]: + """ + Get all the event markers for this path + + :return: The event markers for this path + """ + return self._eventMarkers + + def isReversed(self) -> bool: + """ + Should the path be followed reversed (differential drive only) + + :return: True if reversed + """ + return self._reversed + + def getStartingDifferentialPose(self) -> Pose2d: + """ + Get the differential pose for the start point of this path + + :return: Pose at the path's starting point + """ + startPos = self.getPoint(0).position + heading = (self.getPoint(1).position - self.getPoint(0).position).angle() + + if self._reversed: + heading = Rotation2d.fromDegrees(inputModulus(heading.degrees() + 180, -180, 180)) + + return Pose2d(startPos, heading) + + def getPreviewStartingHolonomicPose(self) -> Pose2d: + """ + 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 + """ + heading = Rotation2d() if self._previewStartingRotation is None else self._previewStartingRotation + return Pose2d(self.getPoint(0).position, heading) + + def replan(self, starting_pose: Pose2d, current_speeds: ChassisSpeeds) -> PathPlannerPath: + """ + Replan this path based on the current robot position and speeds + + :param starting_pose: New starting pose for the replanned path + :param current_speeds: Current chassis speeds of the robot + :return: The replanned path + """ + currentFieldRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(current_speeds.vx, current_speeds.vy, + current_speeds.omega, + -starting_pose.rotation()) + + robotNextControl = None + linearVel = math.hypot(currentFieldRelativeSpeeds.vx, currentFieldRelativeSpeeds.vy) + if linearVel > 0.1: + stoppingDistance = (linearVel ** 2) / (2 * self._globalConstraints.maxAccelerationMpsSq) + + heading = Rotation2d(currentFieldRelativeSpeeds.vx, currentFieldRelativeSpeeds.vy) + robotNextControl = starting_pose.translation() + Translation2d(stoppingDistance, heading) + + closestPointIdx = 0 + comparePoint = robotNextControl if robotNextControl is not None else starting_pose.translation() + closestDist = PathPlannerPath._positionDelta(comparePoint, self.getPoint(closestPointIdx).position) + + for i in range(1, self.numPoints()): + d = PathPlannerPath._positionDelta(comparePoint, self.getPoint(i).position) + + if d < closestDist: + closestPointIdx = i + closestDist = d + + if closestPointIdx == self.numPoints() - 1: + heading = (self.getPoint(self.numPoints() - 1).position - comparePoint).angle() + + if robotNextControl is None: + robotNextControl = starting_pose.translation() + Translation2d(closestDist / 3.0, heading) + + endPrevControlHeading = (self.getPoint(self.numPoints() - 1).position - robotNextControl).angle() + + endPrevControl = self.getPoint(self.numPoints() - 1).position - Translation2d(closestDist / 3.0, + endPrevControlHeading) + + # Throw out rotation targets, event markers, and constraint zones since we are skipping all + # of the path + return PathPlannerPath( + [starting_pose.translation(), robotNextControl, endPrevControl, + self.getPoint(self.numPoints() - 1).position], + self._globalConstraints, + self._goalEndState, [], [], [], self._reversed, self._previewStartingRotation) + elif (closestPointIdx == 0 and robotNextControl is None) or (math.fabs( + closestDist - starting_pose.translation().distance( + self.getPoint(0).position)) <= 0.25 and linearVel < 0.1): + distToStart = starting_pose.translation().distance(self.getPoint(0).position) + + heading = (self.getPoint(0).position - starting_pose.translation()).angle() + robotNextControl = starting_pose.translation() + Translation2d(distToStart / 3.0, heading) + + joinHeading = (self.getPoint(0).position - self.getPoint(1).position).angle() + joinPrevControl = self.getPoint(0).position + Translation2d(distToStart / 2.0, joinHeading) + + if len(self._bezierPoints) == 0: + # We don't have any bezier points to reference + joinSegment = PathSegment(starting_pose.translation(), robotNextControl, joinPrevControl, + self.getPoint(0).position, end_segment=False) + replannedPoints = [] + replannedPoints.extend(joinSegment.segmentPoints) + replannedPoints.extend(self._allPoints) + + return PathPlannerPath.fromPathPoints(replannedPoints, self._globalConstraints, self._goalEndState) + else: + # We can use the bezier points + replannedBezier = [starting_pose.translation(), robotNextControl, joinPrevControl] + replannedBezier.extend(self._bezierPoints) + + # Keep all rotations, markers, and zones and increment waypoint pos by 1 + return PathPlannerPath( + replannedBezier, self._globalConstraints, self._goalEndState, + [RotationTarget(t.waypointRelativePosition + 1, t.target, t.rotateFast) for t in + self._rotationTargets], + [ConstraintsZone(z.minWaypointPos + 1, z.maxWaypointPos + 1, z.constraints) for z in + self._constraintZones], + [EventMarker(m.waypointRelativePos + 1, m.command, m.minimumTriggerDistance) for m in + self._eventMarkers], + self._reversed, + self._previewStartingRotation + ) + + joinAnchorIdx = self.numPoints() - 1 + for i in range(closestPointIdx, self.numPoints()): + if self.getPoint(i).distanceAlongPath >= self.getPoint(closestPointIdx).distanceAlongPath + closestDist: + joinAnchorIdx = i + break + + joinPrevControl = self.getPoint(closestPointIdx).position + joinAnchor = self.getPoint(joinAnchorIdx).position + + if robotNextControl is None: + robotToJoinDelta = starting_pose.translation().distance(joinAnchor) + heading = (joinPrevControl - starting_pose.translation()).angle() + robotNextControl = starting_pose.translation() + Translation2d(robotToJoinDelta / 3.0, heading) + + if joinAnchorIdx == self.numPoints() - 1: + # Throw out rotation targets, event markers, and constraint zones since we are skipping all + # of the path + return PathPlannerPath( + [starting_pose.translation(), robotNextControl, joinPrevControl, joinAnchor], + self._globalConstraints, self._goalEndState, + [], [], [], self._reversed, self._previewStartingRotation + ) + + if len(self._bezierPoints) == 0: + # We don't have any bezier points to reference + joinSegment = PathSegment(starting_pose.translation(), robotNextControl, joinPrevControl, joinAnchor, + end_segment=False) + replannedPoints = [] + replannedPoints.extend(joinSegment.segmentPoints) + replannedPoints.extend(self._allPoints[joinAnchorIdx:]) + + return PathPlannerPath.fromPathPoints(replannedPoints, self._globalConstraints, self._goalEndState) + + # We can reference bezier points + nextWaypointIdx = math.ceil((joinAnchorIdx + 1) * RESOLUTION) + bezierPointIdx = nextWaypointIdx * 3 + waypointDelta = joinAnchor.distance(self._bezierPoints[bezierPointIdx]) + + joinHeading = (joinAnchor - joinPrevControl).angle() + joinNextControl = joinAnchor + Translation2d(waypointDelta / 3.0, joinHeading) + + if bezierPointIdx == len(self._bezierPoints) - 1: + nextWaypointHeading = (self._bezierPoints[bezierPointIdx - 1] - self._bezierPoints[bezierPointIdx]).angle() + else: + nextWaypointHeading = (self._bezierPoints[bezierPointIdx] - self._bezierPoints[bezierPointIdx + 1]).angle() + + nextWaypointPrevControl = self._bezierPoints[bezierPointIdx] + Translation2d(max(waypointDelta / 3.0, 0.15), + nextWaypointHeading) + + replannedBezier = [ + starting_pose.translation(), + robotNextControl, + joinPrevControl, + joinAnchor, + joinNextControl, + nextWaypointPrevControl + ] + replannedBezier.extend(self._bezierPoints[bezierPointIdx:]) + + segment1Length = 0 + lastSegment1Pos = starting_pose.translation() + segment2Length = 0 + lastSegment2Pos = joinAnchor + + for t in decimal_range(RESOLUTION, 1.0, RESOLUTION): + p1 = cubicLerp(starting_pose.translation(), robotNextControl, joinPrevControl, joinAnchor, t) + p2 = cubicLerp(joinAnchor, joinNextControl, nextWaypointPrevControl, self._bezierPoints[bezierPointIdx], t) + + segment1Length += PathPlannerPath._positionDelta(lastSegment1Pos, p1) + segment2Length += PathPlannerPath._positionDelta(lastSegment2Pos, p2) + + lastSegment1Pos = p1 + lastSegment2Pos = p2 + + segment1Pct = segment1Length / (segment1Length + segment2Length) + + mappedTargets = [] + mappedZones = [] + mappedMarkers = [] + + for t in self._rotationTargets: + if t.waypointRelativePosition >= nextWaypointIdx: + mappedTargets.append( + RotationTarget(t.waypointRelativePosition - nextWaypointIdx + 2, t.target, t.rotateFast)) + elif t.waypointRelativePosition >= nextWaypointIdx - 1: + pct = t.waypointRelativePosition - (nextWaypointIdx - 1) + mappedTargets.append(RotationTarget(PathPlannerPath._mapPct(pct, segment1Pct), t.target, t.rotateFast)) + + for z in self._constraintZones: + minPos = 0 + maxPos = 0 + + if z.minWaypointPos >= nextWaypointIdx: + minPos = z.minWaypointPos - nextWaypointIdx + 2 + elif z.minWaypointPos >= nextWaypointIdx - 1: + pct = z.minWaypointPos - (nextWaypointIdx - 1) + minPos = PathPlannerPath._mapPct(pct, segment1Pct) + + if z.maxWaypointPos >= nextWaypointIdx: + maxPos = z.maxWaypointPos - nextWaypointIdx + 2 + elif z.maxWaypointPos >= nextWaypointIdx - 1: + pct = z.maxWaypointPos - (nextWaypointIdx - 1) + maxPos = PathPlannerPath._mapPct(pct, segment1Pct) + + if maxPos > 0: + mappedZones.append(ConstraintsZone(minPos, maxPos, z.constraints)) + + for m in self._eventMarkers: + if m.waypointRelativePos >= nextWaypointIdx: + mappedMarkers.append( + EventMarker(m.waypointRelativePos - nextWaypointIdx + 2, m.command, m.minimumTriggerDistance)) + elif m.waypointRelativePos >= nextWaypointIdx - 1: + pct = m.waypointRelativePos - (nextWaypointIdx - 1) + mappedMarkers.append( + EventMarker(PathPlannerPath._mapPct(pct, segment1Pct), m.command, m.minimumTriggerDistance)) + + # 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 PathPlannerPath( + replannedBezier, self._globalConstraints, self._goalEndState, + mappedTargets, mappedZones, mappedMarkers, self._reversed, self._previewStartingRotation + ) + + @staticmethod + def _mapPct(pct: float, seg1_pct: float) -> float: + if pct <= seg1_pct: + # Map to segment 1 + mappedPct = pct / seg1_pct + else: + # Map to segment 2 + mappedPct = 1 + ((pct - seg1_pct) / (1.0 - seg1_pct)) + + # Round to nearest resolution step + return round(mappedPct * (1.0 / RESOLUTION)) / (1.0 / RESOLUTION) + + @staticmethod + def _positionDelta(a: Translation2d, b: Translation2d) -> float: + delta = a - b + return math.fabs(delta.X()) + math.fabs(delta.Y()) + + @staticmethod + def _fromJson(path_json: dict) -> PathPlannerPath: + bezierPoints = PathPlannerPath._bezierPointsFromWaypointsJson(path_json['waypoints']) + globalConstraints = PathConstraints.fromJson(path_json['globalConstraints']) + goalEndState = GoalEndState.fromJson(path_json['goalEndState']) + isReversed = bool(path_json['reversed']) + rotationTargets = [RotationTarget.fromJson(rotJson) for rotJson in path_json['rotationTargets']] + constraintZones = [ConstraintsZone.fromJson(zoneJson) for zoneJson in path_json['constraintZones']] + eventMarkers = [EventMarker.fromJson(markerJson) for markerJson in path_json['eventMarkers']] + previewStartingRotation = Rotation2d() + if path_json['previewStartingState'] is not None: + previewStartingRotation = Rotation2d.fromDegrees(float(path_json['previewStartingState']['rotation'])) + + return PathPlannerPath(bezierPoints, globalConstraints, goalEndState, rotationTargets, constraintZones, + eventMarkers, isReversed, previewStartingRotation) + + @staticmethod + def _bezierPointsFromWaypointsJson(waypoints_json) -> List[Translation2d]: + bezierPoints = [] + + # First point + firstPointJson = waypoints_json[0] + bezierPoints.append(PathPlannerPath._pointFromJson(firstPointJson['anchor'])) + bezierPoints.append(PathPlannerPath._pointFromJson(firstPointJson['nextControl'])) + + # Mid points + for i in range(1, len(waypoints_json) - 1): + point = waypoints_json[i] + bezierPoints.append(PathPlannerPath._pointFromJson(point['prevControl'])) + bezierPoints.append(PathPlannerPath._pointFromJson(point['anchor'])) + bezierPoints.append(PathPlannerPath._pointFromJson(point['nextControl'])) + + # Last point + lastPointJson = waypoints_json[len(waypoints_json) - 1] + bezierPoints.append(PathPlannerPath._pointFromJson(lastPointJson['prevControl'])) + bezierPoints.append(PathPlannerPath._pointFromJson(lastPointJson['anchor'])) + + return bezierPoints + + @staticmethod + def _pointFromJson(point_json: dict) -> Translation2d: + x = float(point_json['x']) + y = float(point_json['y']) + + return Translation2d(x, y) + + @staticmethod + def _createPath(bezier_points: List[Translation2d], holonomic_rotations: List[RotationTarget], + constraint_zones: List[ConstraintsZone]) -> List[PathPoint]: + if len(bezier_points) < 4: + raise ValueError('Not enough bezier points') + + points = [] + + numSegments = int((len(bezier_points) - 1) / 3) + for s in range(numSegments): + iOffset = s * 3 + p1 = bezier_points[iOffset] + p2 = bezier_points[iOffset + 1] + p3 = bezier_points[iOffset + 2] + p4 = bezier_points[iOffset + 3] + + segmentIdx = s + segmentRotations = [t.forSegmentIndex(segmentIdx) for t in holonomic_rotations if + segmentIdx <= t.waypointRelativePosition <= segmentIdx + 1] + segmentZones = [z.forSegmentIndex(segmentIdx) for z in constraint_zones if + z.overlapsRange(segmentIdx, segmentIdx + 1)] + + segment = PathSegment(p1, p2, p3, p4, segmentRotations, segmentZones, s == numSegments - 1) + points.extend(segment.segmentPoints) + + return points + + def _precalcValues(self) -> None: + if self.numPoints() > 0: + for i in range(self.numPoints()): + point = self.getPoint(i) + if point.constraints is None: + point.constraints = self._globalConstraints + point.curveRadius = self._getCurveRadiusAtPoint(i) + + if math.isfinite(point.curveRadius): + point.maxV = min(math.sqrt(point.constraints.maxAccelerationMpsSq * math.fabs(point.curveRadius)), + point.constraints.maxVelocityMps) + else: + point.maxV = point.constraints.maxVelocityMps + + if i != 0: + point.distanceAlongPath = self.getPoint(i - 1).distanceAlongPath + ( + self.getPoint(i - 1).position.distance(point.position)) + + for m in self._eventMarkers: + pointIndex = int(m.waypointRelativePos / RESOLUTION) + m.markerPos = self.getPoint(pointIndex).position + + self.getPoint(self.numPoints() - 1).rotationTarget = RotationTarget(-1, self._goalEndState.rotation, + self._goalEndState.rotateFast) + self.getPoint(self.numPoints() - 1).maxV = self._goalEndState.velocity + + def _getCurveRadiusAtPoint(self, index: int) -> float: + if self.numPoints() < 3: + return float('inf') + + if index == 0: + return calculateRadius( + self.getPoint(index).position, + self.getPoint(index + 1).position, + self.getPoint(index + 2).position) + elif index == self.numPoints() - 1: + return calculateRadius( + self.getPoint(index - 2).position, + self.getPoint(index - 1).position, + self.getPoint(index).position) + else: + return calculateRadius( + self.getPoint(index - 1).position, + self.getPoint(index).position, + self.getPoint(index + 1).position) diff --git a/pathplannerlib-python/pathplannerlib/pathfinders.py b/pathplannerlib-python/pathplannerlib/pathfinders.py new file mode 100644 index 00000000..b8214edf --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/pathfinders.py @@ -0,0 +1,726 @@ +from __future__ import annotations + +from typing import List, Tuple, Dict, Set, Union +from dataclasses import dataclass +from wpimath.geometry import Translation2d +import time + +from .path import PathConstraints, GoalEndState, PathPlannerPath, PathPoint, RESOLUTION +from .geometry_util import cubicLerp, decimal_range +import math +from threading import Thread, RLock +import os +from wpilib import getDeployDirectory +import json +from ntcore import StringPublisher, DoubleArrayPublisher, DoubleArraySubscriber, NetworkTableInstance, PubSubOptions, \ + EventFlags + + +class Pathfinder: + def isNewPathAvailable(self) -> bool: + """ + Get if a new path has been calculated since the last time a path was retrieved + + :return: True if a new path is available + """ + raise NotImplementedError + + def getCurrentPath(self, constraints: PathConstraints, goal_end_state: GoalEndState) -> Union[ + PathPlannerPath, None]: + """ + Get the most recently calculated path + + :param constraints: The path constraints to use when creating the path + :param goal_end_state: The goal end state to use when creating the path + :return: The PathPlannerPath created from the points calculated by the pathfinder + """ + raise NotImplementedError + + def setStartPosition(self, start_position: Translation2d) -> None: + """ + Set the start position to pathfind from + + :param start_position: Start position on the field. If this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + raise NotImplementedError + + def setGoalPosition(self, goal_position: Translation2d) -> None: + """ + Set the goal position to pathfind to + + :param goal_position: Goal position on the field. f this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + raise NotImplementedError + + def setDynamicObstacles(self, obs: List[Tuple[Translation2d, Translation2d]], + current_robot_pos: Translation2d) -> None: + """ + Set the dynamic obstacles that should be avoided while pathfinding. + + :param obs: A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners of a bounding box. + :param current_robot_pos: The current position of the robot. This is needed to change the start position of the path to properly avoid obstacles + """ + raise NotImplementedError + + +class RemoteADStar(Pathfinder): + _navGridJsonPub: StringPublisher + _startPosPub: DoubleArrayPublisher + _goalPosPub: DoubleArrayPublisher + _dynamicObstaclePub: DoubleArrayPublisher + + _pathPointsSub: DoubleArraySubscriber + + _currentPath: List[PathPoint] = [] + _newPathAvailable: bool = False + + def __init__(self): + nt = NetworkTableInstance.getDefault() + + self._navGridJsonPub = nt.getStringTopic('/PPLibCoprocessor/RemoteADStar/navGrid').publish() + self._startPosPub = nt.getDoubleArrayTopic('/PPLibCoprocessor/RemoteADStar/startPos').publish() + self._goalPosPub = nt.getDoubleArrayTopic('/PPLibCoprocessor/RemoteADStar/goalPos').publish() + self._dynamicObstaclePub = nt.getDoubleArrayTopic('/PPLibCoprocessor/RemoteADStar/dynamicObstacles').publish() + + self._pathPointsSub = nt.getDoubleArrayTopic('/PPLibCoprocessor/RemoteADStar/pathPoints').subscribe([], + PubSubOptions( + sendAll=True, + keepDuplicates=True)) + + nt.addListener(self._pathPointsSub, EventFlags.kValueAll, self._handlePathListenerEvent) + + filePath = os.path.join(getDeployDirectory(), 'pathplanner', 'navgrid.json') + + with open(filePath, 'r') as f: + file_content = f.read() + self._navGridJsonPub.set(file_content) + + def _handlePathListenerEvent(self, event): + pathPointsArr = self._pathPointsSub.get() + + pathPoints = [] + for i in range(0, len(pathPointsArr) - 1, 2): + pathPoints.append(PathPoint(Translation2d(pathPointsArr[i], pathPointsArr[i + 1]))) + + self._currentPath = pathPoints + self._newPathAvailable = True + + def isNewPathAvailable(self) -> bool: + """ + Get if a new path has been calculated since the last time a path was retrieved + + :return: True if a new path is available + """ + return self._newPathAvailable + + def getCurrentPath(self, constraints: PathConstraints, goal_end_state: GoalEndState) -> Union[ + PathPlannerPath, None]: + """ + Get the most recently calculated path + + :param constraints: The path constraints to use when creating the path + :param goal_end_state: The goal end state to use when creating the path + :return: The PathPlannerPath created from the points calculated by the pathfinder + """ + pathPoints = [p for p in self._currentPath] + + self._newPathAvailable = False + + if len(pathPoints) < 2: + return None + + return PathPlannerPath.fromPathPoints(pathPoints, constraints, goal_end_state) + + def setStartPosition(self, start_position: Translation2d) -> None: + """ + Set the start position to pathfind from + + :param start_position: Start position on the field. If this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + self._startPosPub.set([start_position.X(), start_position.Y()]) + + def setGoalPosition(self, goal_position: Translation2d) -> None: + """ + Set the goal position to pathfind to + + :param goal_position: Goal position on the field. f this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + self._goalPosPub.set([goal_position.X(), goal_position.Y()]) + + def setDynamicObstacles(self, obs: List[Tuple[Translation2d, Translation2d]], + current_robot_pos: Translation2d) -> None: + """ + Set the dynamic obstacles that should be avoided while pathfinding. + + :param obs: A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners of a bounding box. + :param current_robot_pos: The current position of the robot. This is needed to change the start position of the path to properly avoid obstacles + """ + # First two doubles represent current robot pos + obsArr = [current_robot_pos.X(), current_robot_pos.Y()] + + for box in obs: + obsArr.append(box[0].X()) + obsArr.append(box[0].Y()) + obsArr.append(box[1].X()) + obsArr.append(box[1].Y()) + + self._dynamicObstaclePub.set(obsArr) + + +@dataclass(frozen=True) +class GridPosition: + x: int + y: int + + def __eq__(self, other): + return isinstance(other, GridPosition) and other.x == self.x and other.y == self.y + + def __hash__(self): + return self.x * 1000 + self.y + + def compareTo(self, o: GridPosition) -> int: + if self.x == o.x: + if self.y == o.y: + return 0 + return -1 if self.y < o.y else 1 + return -1 if self.x < o.x else 1 + + +class LocalADStar(Pathfinder): + _SMOOTHING_ANCHOR_PCT: float = 0.8 + _SMOOTHING_CONTROL_PCT: float = 0.33 + _EPS: float = 2.5 + + _fieldLength: float = 16.54 + _fieldWidth: float = 8.02 + + _nodeSize: float = 0.2 + + _nodesX: int = math.ceil(_fieldLength / _nodeSize) + _nodesY: int = math.ceil(_fieldWidth / _nodeSize) + + _g: Dict[GridPosition, float] = {} + _rhs: Dict[GridPosition, float] = {} + _open: Dict[GridPosition, Tuple[float, float]] = {} + _incons: Dict[GridPosition, Tuple[float, float]] = {} + _closed: Set[GridPosition] = set() + _staticObstacles: Set[GridPosition] = set() + _dynamicObstacles: Set[GridPosition] = set() + _requestObstacles: Set[GridPosition] = set() + + _requestStart: GridPosition + _requestRealStartPos: Translation2d + _requestGoal: GridPosition + _requestRealGoalPos: Translation2d + + _eps: float + + _planningThread: Thread + _requestMinor: bool = True + _requestMajor: bool = True + _requestReset: bool = True + _newPathAvailable: bool = True + + _pathLock: RLock = RLock() + _requestLock: RLock = RLock() + + _currentPathPoints: List[PathPoint] = [] + + def __init__(self): + self._planningThread = Thread(target=self._runThread, daemon=True) + + self._requestStart = GridPosition(0, 0) + self._requestRealStartPos = Translation2d(0, 0) + self._requestGoal = GridPosition(0, 0) + self._requestRealGoalPos = Translation2d(0, 0) + + self._staticObstacles.clear() + self._dynamicObstacles.clear() + + try: + filePath = os.path.join(getDeployDirectory(), 'pathplanner', 'navgrid.json') + + with open(filePath, 'r') as f: + navgrid_json = json.loads(f.read()) + + self._nodeSize = float(navgrid_json['nodeSizeMeters']) + grid = navgrid_json['grid'] + + self._nodesY = len(grid) + for row in range(len(grid)): + rowArr = grid[row] + if row == 0: + self._nodesX = len(rowArr) + for col in range(len(rowArr)): + isObstacle = rowArr[col] + if isObstacle: + self._staticObstacles.add(GridPosition(col, row)) + + fieldSize = navgrid_json['field_size'] + self._fieldLength = fieldSize['x'] + self._fieldWidth = fieldSize['y'] + except: + # Do nothing, use defaults + pass + + self._requestObstacles.clear() + self._requestObstacles.update(self._staticObstacles) + self._requestObstacles.update(self._dynamicObstacles) + + self._requestReset = True + self._requestMinor = True + self._requestMajor = True + + self._newPathAvailable = False + + self._planningThread.start() + + def isNewPathAvailable(self) -> bool: + """ + Get if a new path has been calculated since the last time a path was retrieved + + :return: True if a new path is available + """ + return self._newPathAvailable + + def getCurrentPath(self, constraints: PathConstraints, goal_end_state: GoalEndState) -> Union[ + PathPlannerPath, None]: + """ + Get the most recently calculated path + + :param constraints: The path constraints to use when creating the path + :param goal_end_state: The goal end state to use when creating the path + :return: The PathPlannerPath created from the points calculated by the pathfinder + """ + self._pathLock.acquire() + pathPoints = [p for p in self._currentPathPoints] + self._pathLock.release() + + self._newPathAvailable = False + + if len(pathPoints) == 0: + return None + + return PathPlannerPath.fromPathPoints(pathPoints, constraints, goal_end_state) + + def setStartPosition(self, start_position: Translation2d) -> None: + """ + Set the start position to pathfind from + + :param start_position: Start position on the field. If this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + gridPos = self._findClosestNonObstacle(self._getGridPos(start_position), self._requestObstacles) + + if gridPos is not None and gridPos != self._requestStart: + self._requestLock.acquire() + self._requestStart = gridPos + self._requestRealStartPos = start_position + + self._requestMinor = True + self._requestLock.release() + + def setGoalPosition(self, goal_position: Translation2d) -> None: + """ + Set the goal position to pathfind to + + :param goal_position: Goal position on the field. f this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + gridPos = self._findClosestNonObstacle(self._getGridPos(goal_position), self._requestObstacles) + + if gridPos is not None: + self._requestLock.acquire() + self._requestGoal = gridPos + self._requestRealGoalPos = goal_position + + self._requestMinor = True + self._requestMajor = True + self._requestReset = True + self._requestLock.release() + + def setDynamicObstacles(self, obs: List[Tuple[Translation2d, Translation2d]], + current_robot_pos: Translation2d) -> None: + """ + Set the dynamic obstacles that should be avoided while pathfinding. + + :param obs: A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners of a bounding box. + :param current_robot_pos: The current position of the robot. This is needed to change the start position of the path to properly avoid obstacles + """ + newObs = set() + + for obstacle in obs: + gridPos1 = self._getGridPos(obstacle[0]) + gridPos2 = self._getGridPos(obstacle[1]) + + minX = min(gridPos1.x, gridPos2.x) + maxX = max(gridPos1.x, gridPos2.x) + + minY = min(gridPos1.y, gridPos2.y) + maxY = max(gridPos1.y, gridPos2.y) + + for x in range(minX, maxX + 1): + for y in range(minY, maxY + 1): + newObs.add(GridPosition(x, y)) + + self._dynamicObstacles.clear() + self._dynamicObstacles.update(newObs) + self._requestLock.acquire() + self._requestObstacles.clear() + self._requestObstacles.update(self._staticObstacles) + self._requestObstacles.update(self._dynamicObstacles) + self._requestReset = True + self._requestMinor = True + self._requestMajor = True + self._requestLock.release() + + self.setStartPosition(current_robot_pos) + self.setGoalPosition(self._requestRealGoalPos) + + def _runThread(self) -> None: + while True: + try: + self._requestLock.acquire() + reset = self._requestReset + minor = self._requestMinor + major = self._requestMajor + start = self._requestStart + realStart = self._requestRealStartPos + goal = self._requestGoal + realGoal = self._requestRealGoalPos + obstacles = set() + obstacles.update(self._requestObstacles) + + # Change the request booleans based on what will be done this loop + if reset: + self._requestReset = False + + if minor: + self._requestMinor = False + elif major and (self._eps - 0.5) <= 1.0: + self._requestMajor = False + self._requestLock.release() + + if reset or minor or major: + self._doWork(reset, minor, major, start, goal, realStart, realGoal, obstacles) + else: + time.sleep(0.01) + except: + # Something messed up. Reset and hope for the best + self._requestLock.acquire() + self._requestReset = True + self._requestLock.release() + + def _doWork(self, needs_reset: bool, do_minor: bool, do_major: bool, s_start: GridPosition, s_goal: GridPosition, + real_start_pos: Translation2d, real_goal_pos: Translation2d, obstacles: Set[GridPosition]) -> None: + if needs_reset: + self._reset(s_start, s_goal) + + if do_minor: + self._computeOrImprovePath(s_start, s_goal, obstacles) + path = self._extractPath(s_start, s_goal, real_start_pos, real_goal_pos, obstacles) + + self._pathLock.acquire() + self._currentPathPoints = path + self._pathLock.release() + + self._newPathAvailable = False + elif do_major: + if self._eps > 1.0: + self._eps -= 0.5 + self._open.update(self._incons) + + for key in self._open: + self._open[key] = self._key(key, s_start) + self._closed.clear() + self._computeOrImprovePath(s_start, s_goal, obstacles) + path = self._extractPath(s_start, s_goal, real_start_pos, real_goal_pos, obstacles) + + self._pathLock.acquire() + self._currentPathPoints = path + self._pathLock.release() + + self._newPathAvailable = True + + def _extractPath(self, s_start: GridPosition, s_goal: GridPosition, real_start_pos: Translation2d, + real_goal_pos: Translation2d, obstacles: Set[GridPosition]) -> List[PathPoint]: + if s_goal == s_start: + return [] + + path = [s_start] + + s = s_start + + for k in range(200): + gList = {} + + for x in self._getOpenNeighbors(s, obstacles): + gList[x] = self._g[x] + + min_entry = (s_goal, float('inf')) + for key, val in gList.items(): + if val < min_entry[1]: + min_entry = (key, val) + s = min_entry[0] + + path.append(s) + if s == s_goal: + break + + simplifiedPath = [path[0]] + for i in range(1, len(path) - 1): + if not self._walkable(simplifiedPath[-1], path[i + 1], obstacles): + simplifiedPath.append(path[i]) + simplifiedPath.append(path[-1]) + + fieldPosPath = [self._gridPosToTranslation2d(pos) for pos in simplifiedPath] + + # Replace start and end positions with their real positions + fieldPosPath[0] = real_start_pos + fieldPosPath[-1] = real_goal_pos + + bezierPoints = [fieldPosPath[0], + ((fieldPosPath[1] - fieldPosPath[0]) * LocalADStar._SMOOTHING_CONTROL_PCT) + fieldPosPath[0]] + for i in range(1, len(fieldPosPath) - 1): + last = fieldPosPath[i - 1] + current = fieldPosPath[i] + next = fieldPosPath[i + 1] + + anchor1 = ((current - last) * LocalADStar._SMOOTHING_ANCHOR_PCT) + last + anchor2 = ((current - next) * LocalADStar._SMOOTHING_ANCHOR_PCT) + next + + controlDist = anchor1.distance(anchor2) * LocalADStar._SMOOTHING_CONTROL_PCT + + prevControl1 = ((last - anchor1) * LocalADStar._SMOOTHING_CONTROL_PCT) + anchor1 + nextControl1 = Translation2d(controlDist, (anchor1 - prevControl1).angle()) + anchor1 + + prevControl2 = Translation2d(controlDist, (anchor2 - next).angle()) + anchor2 + nextControl2 = ((next - anchor2) * LocalADStar._SMOOTHING_CONTROL_PCT) + anchor2 + + bezierPoints.append(prevControl1) + bezierPoints.append(anchor1) + bezierPoints.append(nextControl1) + + bezierPoints.append(prevControl2) + bezierPoints.append(anchor2) + bezierPoints.append(nextControl2) + + bezierPoints.append( + ((fieldPosPath[-2] - fieldPosPath[-1]) * LocalADStar._SMOOTHING_CONTROL_PCT) + fieldPosPath[-1]) + bezierPoints.append(fieldPosPath[-1]) + + numSegments = int((len(bezierPoints) - 1) / 3) + pathPoints = [] + + for i in range(numSegments): + iOffset = i * 3 + + p1 = bezierPoints[iOffset] + p2 = bezierPoints[iOffset + 1] + p3 = bezierPoints[iOffset + 2] + p4 = bezierPoints[iOffset + 3] + + resolution = RESOLUTION + if p1.distance(p4) <= 1.0: + resolution = 0.2 + + for t in decimal_range(0.0, 1.0, resolution): + pathPoints.append(PathPoint(cubicLerp(p1, p2, p3, p4, t))) + pathPoints.append(PathPoint(bezierPoints[-1])) + + return pathPoints + + def _findClosestNonObstacle(self, pos: GridPosition, obstacles: Set[GridPosition]) -> Union[GridPosition, None]: + if pos not in obstacles: + return pos + + visited = set() + queue = [p for p in self._getAllNeighbors(pos)] + + while len(queue) > 0: + check = queue.pop(0) + if check not in obstacles: + return check + visited.add(check) + + for neighbor in self._getAllNeighbors(check): + if neighbor not in visited and neighbor not in queue: + queue.append(neighbor) + + return None + + def _walkable(self, s1: GridPosition, s2: GridPosition, obstacles: Set[GridPosition]) -> bool: + x0 = s1.x + y0 = s1.y + x1 = s2.x + y1 = s2.y + + dx = abs(x1 - x0) + dy = abs(y1 - y0) + x = x0 + y = y0 + n = 1 + dx + dy + xInc = 1 if x1 > x0 else -1 + yInc = 1 if y1 > y0 else -1 + error = dx - dy + dx *= 2 + dy *= 2 + + while n > 0: + if GridPosition(x, y) in obstacles: + return False + + if error > 0: + x += xInc + error -= dy + elif error < 0: + y += yInc + error += dx + else: + x += xInc + y += yInc + error -= dy + error += dx + n -= 1 + + n -= 1 + + return True + + def _reset(self, s_start: GridPosition, s_goal: GridPosition) -> None: + self._g.clear() + self._rhs.clear() + self._open.clear() + self._incons.clear() + self._closed.clear() + + for x in range(self._nodesX): + for y in range(self._nodesY): + self._g[GridPosition(x, y)] = float('inf') + self._rhs[GridPosition(x, y)] = float('inf') + + self._rhs[s_goal] = 0.0 + + self._eps = LocalADStar._EPS + + self._open[s_goal] = self._key(s_goal, s_start) + + def _computeOrImprovePath(self, s_start: GridPosition, s_goal: GridPosition, obstacles: Set[GridPosition]) -> None: + while True: + sv = self._topKey() + + if sv is None: + break + + s = sv[0] + v = sv[1] + + if self._comparePair(v, self._key(s_start, s_start)) >= 0 and self._rhs[s_start] == self._g[s_start]: + break + + self._open.pop(s) + + if self._g[s] > self._rhs[s]: + self._g[s] = self._rhs[s] + self._closed.add(s) + + for sn in self._getOpenNeighbors(s, obstacles): + self._updateState(sn, s_start, s_goal, obstacles) + else: + self._g[s] = float('inf') + for sn in self._getOpenNeighbors(s, obstacles): + self._updateState(sn, s_start, s_goal, obstacles) + self._updateState(s, s_start, s_goal, obstacles) + + def _updateState(self, s: GridPosition, s_start: GridPosition, s_goal: GridPosition, + obstacles: Set[GridPosition]) -> None: + if s != s_goal: + self._rhs[s] = float('inf') + + for x in self._getOpenNeighbors(s, obstacles): + self._rhs[s] = min(self._rhs[s], self._g[x] + self._cost(s, x, obstacles)) + + if s in self._open: + self._open.pop(s) + + if self._g[s] != self._rhs[s]: + if s not in self._closed: + self._open[s] = self._key(s, s_start) + else: + self._incons[s] = (0.0, 0.0) + + def _cost(self, s_start: GridPosition, s_goal: GridPosition, obstacles: Set[GridPosition]) -> float: + if self._isCollision(s_start, s_goal, obstacles): + return float('inf') + return self._heuristic(s_start, s_goal) + + def _isCollision(self, s_start: GridPosition, s_end: GridPosition, obstacles: Set[GridPosition]) -> bool: + if s_start in obstacles or s_end in obstacles: + return True + + if s_start.x != s_end.x and s_start.y != s_end.y: + if s_end.x - s_start.x == s_start.y - s_end.y: + s1 = GridPosition(min(s_start.x, s_end.x), min(s_start.y, s_end.y)) + s2 = GridPosition(max(s_start.x, s_end.x), max(s_start.y, s_end.y)) + else: + s1 = GridPosition(min(s_start.x, s_end.x), max(s_start.y, s_end.y)) + s2 = GridPosition(max(s_start.x, s_end.x), min(s_start.y, s_end.y)) + return s1 in obstacles or s2 in obstacles + return False + + def _getOpenNeighbors(self, s: GridPosition, obstacles: Set[GridPosition]) -> List[GridPosition]: + ret = [] + + for xMove in range(-1, 2): + for yMove in range(-1, 2): + sNext = GridPosition(s.x + xMove, s.y + yMove) + if sNext not in obstacles and 0 <= sNext.x < self._nodesX and 0 <= sNext.y < self._nodesY: + ret.append(sNext) + return ret + + def _getAllNeighbors(self, s: GridPosition) -> List[GridPosition]: + ret = [] + + for xMove in range(-1, 2): + for yMove in range(-1, 2): + sNext = GridPosition(s.x + xMove, s.y + yMove) + if 0 <= sNext.x < self._nodesX and 0 <= sNext.y < self._nodesY: + ret.append(sNext) + return ret + + def _key(self, s: GridPosition, s_start: GridPosition) -> Tuple[float, float]: + if self._g[s] > self._rhs[s]: + return self._rhs[s] + self._eps * self._heuristic(s_start, s), self._rhs[s] + else: + return self._g[s] + self._heuristic(s_start, s), self._g[s] + + def _topKey(self) -> Union[Tuple[GridPosition, Tuple[float, float]], None]: + min_key = None + for k, v in self._open.items(): + if min_key is None or self._comparePair(v, self._open[min_key]) < 0: + min_key = k + + if min_key is None: + return None + + return min_key, self._open[min_key] + + def _heuristic(self, s_start: GridPosition, s_goal: GridPosition) -> float: + return math.hypot(s_goal.x - s_start.x, s_goal.y - s_start.y) + + def _comparePair(self, a: Tuple[float, float], b: Tuple[float, float]) -> int: + if a[0] == b[0]: + if a[1] == b[1]: + return 0 + else: + return -1 if a[1] < b[1] else 1 + else: + return -1 if a[0] < b[0] else 1 + + def _getGridPos(self, pos: Translation2d) -> GridPosition: + x = math.floor(pos.X() / self._nodeSize) + y = math.floor(pos.Y() / self._nodeSize) + + return GridPosition(x, y) + + def _gridPosToTranslation2d(self, pos: GridPosition) -> Translation2d: + return Translation2d((pos.x * self._nodeSize) + (self._nodeSize / 2.0), + (pos.y * self._nodeSize) + (self._nodeSize / 2.0)) diff --git a/pathplannerlib-python/pathplannerlib/pathfinding.py b/pathplannerlib-python/pathplannerlib/pathfinding.py new file mode 100644 index 00000000..7e5f148c --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/pathfinding.py @@ -0,0 +1,75 @@ +from __future__ import annotations +from .path import PathPlannerPath, PathConstraints, GoalEndState +from wpimath.geometry import Translation2d +from typing import List, Tuple +from .pathfinders import LocalADStar, Pathfinder + + +class Pathfinding: + _pathfinder: Pathfinder = None + + @staticmethod + def setPathfinder(pathfinder: Pathfinder) -> None: + """ + Set the pathfinder that should be used by the path following commands + + :param pathfinder: The pathfinder to use + """ + Pathfinding._pathfinder = pathfinder + + @staticmethod + def ensureInitialized() -> None: + """ + Ensure that a pathfinding implementation has been chosen. If not, set it to the default. + """ + if Pathfinding._pathfinder is None: + Pathfinding._pathfinder = LocalADStar() + + @staticmethod + def isNewPathAvailable() -> bool: + """ + Get if a new path has been calculated since the last time a path was retrieved + + :return: True if a new path is available + """ + return Pathfinding._pathfinder.isNewPathAvailable() + + @staticmethod + def getCurrentPath(constraints: PathConstraints, goal_end_state: GoalEndState) -> PathPlannerPath: + """ + Get the most recently calculated path + + :param constraints: The path constraints to use when creating the path + :param goal_end_state: The goal end state to use when creating the path + :return: The PathPlannerPath created from the points calculated by the pathfinder + """ + return Pathfinding._pathfinder.getCurrentPath(constraints, goal_end_state) + + @staticmethod + def setStartPosition(start_position: Translation2d) -> None: + """ + Set the start position to pathfind from + + :param start_position: Start position on the field. If this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + Pathfinding._pathfinder.setStartPosition(start_position) + + @staticmethod + def setGoalPosition(goal_position: Translation2d) -> None: + """ + Set the goal position to pathfind to + + :param goal_position: Goal position on the field. f this is within an obstacle it will be moved to the nearest non-obstacle node. + """ + Pathfinding._pathfinder.setGoalPosition(goal_position) + + @staticmethod + def setDynamicObstacles(obs: List[Tuple[Translation2d, Translation2d]], + current_robot_pos: Translation2d) -> None: + """ + Set the dynamic obstacles that should be avoided while pathfinding. + + :param obs: A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners of a bounding box. + :param current_robot_pos: The current position of the robot. This is needed to change the start position of the path to properly avoid obstacles + """ + Pathfinding._pathfinder.setDynamicObstacles(obs, current_robot_pos) diff --git a/pathplannerlib-python/pathplannerlib/telemetry.py b/pathplannerlib-python/pathplannerlib/telemetry.py new file mode 100644 index 00000000..5b57ddd4 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/telemetry.py @@ -0,0 +1,40 @@ +from ntcore import DoubleArrayPublisher, DoublePublisher, NetworkTableInstance +from wpimath.geometry import Pose2d +from .path import PathPlannerPath + + +class PPLibTelemetry: + _velPub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic('/PathPlanner/vel').publish() + _inaccuracyPub: DoublePublisher = NetworkTableInstance.getDefault().getDoubleTopic( + '/PathPlanner/inaccuracy').publish() + _posePub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic( + '/PathPlanner/currentPose').publish() + _pathPub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic( + '/PathPlanner/activePath').publish() + _targetPosePub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic( + '/PathPlanner/targetPose').publish() + + @staticmethod + def setVelocities(actual_vel: float, commanded_vel: float, actual_ang_vel: float, commanded_ang_vel: float) -> None: + PPLibTelemetry._velPub.set([actual_vel, commanded_vel, actual_ang_vel, commanded_ang_vel]) + + @staticmethod + def setPathInaccuracy(inaccuracy: float) -> None: + PPLibTelemetry._inaccuracyPub.set(inaccuracy) + + @staticmethod + def setCurrentPose(pose: Pose2d) -> None: + PPLibTelemetry._posePub.set([pose.X(), pose.Y(), pose.rotation().radians()]) + + @staticmethod + def setTargetPose(pose: Pose2d) -> None: + PPLibTelemetry._targetPosePub.set([pose.X(), pose.Y(), pose.rotation().radians()]) + + @staticmethod + def setCurrentPath(path: PathPlannerPath) -> None: + arr = [] + + for p in path.getAllPathPoints(): + arr.extend([p.position.X(), p.position.Y(), 0.0]) + + PPLibTelemetry._pathPub.set(arr) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py new file mode 100644 index 00000000..5fe2cbb6 --- /dev/null +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -0,0 +1,299 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass +from wpimath.geometry import Translation2d, Rotation2d, Pose2d +from wpimath.kinematics import ChassisSpeeds +from wpimath import inputModulus +from .path import PathConstraints, PathPlannerPath +from .geometry_util import floatLerp, translationLerp, rotationLerp +from typing import List + + +@dataclass +class State: + timeSeconds: float = 0 + velocityMps: float = 0 + accelerationMpsSq: float = 0 + headingAngularVelocityRps: float = 0 + positionMeters: Translation2d = Translation2d() + heading: Rotation2d = Rotation2d() + targetHolonomicRotation: Rotation2d = Rotation2d() + curvatureRadPerMeter: float = 0 + constraints: PathConstraints = None + deltaPos: float = 0 + + def interpolate(self, end_val: State, t: float) -> State: + """ + Interpolate between this state and the given state + + :param end_val: State to interpolate with + :param t: Interpolation factor (0.0-1.0) + :return: Interpolated state + """ + lerpedState = State() + + lerpedState.timeSeconds = floatLerp(self.timeSeconds, end_val.timeSeconds, t) + deltaT = lerpedState.timeSeconds - self.timeSeconds + + if deltaT < 0: + return end_val.interpolate(self, 1 - t) + + lerpedState.velocityMps = floatLerp(self.velocityMps, end_val.velocityMps, t) + lerpedState.accelerationMpsSq = floatLerp(self.accelerationMpsSq, end_val.accelerationMpsSq, t) + lerpedState.positionMeters = translationLerp(self.positionMeters, end_val.positionMeters, t) + lerpedState.heading = rotationLerp(self.heading, end_val.heading, t) + lerpedState.headingAngularVelocityRps = floatLerp(self.headingAngularVelocityRps, + end_val.headingAngularVelocityRps, t) + lerpedState.curvatureRadPerMeter = floatLerp(self.curvatureRadPerMeter, end_val.curvatureRadPerMeter, t) + lerpedState.deltaPos = floatLerp(self.deltaPos, end_val.deltaPos, t) + + if t < 0.5: + lerpedState.constraints = self.constraints + lerpedState.targetHolonomicRotation = self.targetHolonomicRotation + else: + lerpedState.constraints = end_val.constraints + lerpedState.targetHolonomicRotation = end_val.targetHolonomicRotation + + return lerpedState + + def getTargetHolonomicPose(self) -> Pose2d: + """ + 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 + """ + return Pose2d(self.positionMeters, self.targetHolonomicRotation) + + def getDifferentialPose(self) -> Pose2d: + """ + Get this pose for a differential drivetrain + + :return: The pose + """ + return Pose2d(self.positionMeters, self.heading) + + def reverse(self) -> State: + """ + Get the state reversed, used for following a trajectory reversed with a differential drivetrain + + :return: The reversed state + """ + reversedState = State() + + reversedState.timeSeconds = self.timeSeconds + reversedState.velocityMps = -self.velocityMps + reversedState.accelerationMpsSq = -self.accelerationMpsSq + reversedState.headingAngularVelocityRps = -self.headingAngularVelocityRps + reversedState.positionMeters = self.positionMeters + reversedState.heading = Rotation2d.fromDegrees(inputModulus(self.heading.degrees() + 180, -180, 180)) + reversedState.targetHolonomicRotation = self.targetHolonomicRotation + reversedState.curvatureRadPerMeter = -self.curvatureRadPerMeter + reversedState.deltaPos = self.deltaPos + reversedState.constraints = self.constraints + + return reversedState + + +class PathPlannerTrajectory: + _states: List[State] + + def __init__(self, path: PathPlannerPath, starting_speeds: ChassisSpeeds, starting_rotation: Rotation2d): + """ + Generate a PathPlannerTrajectory + + :param path: PathPlannerPath to generate the trajectory for + :param starting_speeds: Starting speeds of the robot when starting the trajectory + :param starting_rotation: Starting rotation of the robot when starting the trajectory + """ + self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation) + + def getStates(self) -> List[State]: + """ + Get all of the pre-generated states in the trajectory + + :return: List of all states + """ + return self._states + + def getState(self, index: int) -> State: + """ + Get the goal state at the given index + + :param index: Index of the state to get + :return: The state at the given index + """ + return self._states[index] + + def getInitialState(self) -> State: + """ + Get the initial state of the trajectory + + :return: The initial state + """ + return self.getState(0) + + def getInitialTargetHolonomicPose(self) -> Pose2d: + """ + 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 + """ + return self.getInitialState().getTargetHolonomicPose() + + def getInitialDifferentialPose(self) -> Pose2d: + """ + Get this initial pose for a differential drivetrain + + :return: The initial pose + """ + return self.getInitialState().getDifferentialPose() + + def getEndState(self) -> State: + """ + Get the end state of the trajectory + + :return: The end state + """ + return self.getState(len(self.getStates()) - 1) + + def getTotalTimeSeconds(self) -> float: + """ + Get the total run time of the trajectory + + :return: Total run time in seconds + """ + return self.getEndState().timeSeconds + + def sample(self, time: float) -> State: + """ + 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 + """ + if time <= self.getInitialState().timeSeconds: + return self.getInitialState() + if time >= self.getTotalTimeSeconds(): + return self.getEndState() + + low = 1 + high = len(self.getStates()) - 1 + + while low != high: + mid = int((low + high) / 2) + if self.getState(mid).timeSeconds < time: + low = mid + 1 + else: + high = mid + + sample = self.getState(low) + prevSample = self.getState(low - 1) + + if math.fabs(sample.timeSeconds - prevSample.timeSeconds) < 1E-3: + return sample + + return prevSample.interpolate(sample, + (time - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds)) + + @staticmethod + def _getNextRotationTarget(path: PathPlannerPath, starting_index: int) -> int: + idx = path.numPoints() - 1 + + for i in range(starting_index, path.numPoints() - 1): + if path.getPoint(i).rotationTarget is not None: + idx = i + break + + return idx + + @staticmethod + def _generateStates(path: PathPlannerPath, starting_speeds: ChassisSpeeds, starting_rotation: Rotation2d) -> List[ + State]: + states = [] + + startVel = math.hypot(starting_speeds.vx, starting_speeds.vy) + + prevRotationTargetDist = 0.0 + prevRotationTargetRot = starting_rotation + nextRotationTargetIdx = PathPlannerTrajectory._getNextRotationTarget(path, 0) + distanceBetweenTargets = path.getPoint(nextRotationTargetIdx).distanceAlongPath + + # Initial pass. Creates all states and handles linear acceleration + for i in range(path.numPoints()): + state = State() + + constraints = path.getPoint(i).constraints + state.constraints = constraints + + if i > nextRotationTargetIdx: + prevRotationTargetDist = path.getPoint(nextRotationTargetIdx).distanceAlongPath + prevRotationTargetRot = path.getPoint(nextRotationTargetIdx).rotationTarget.target + nextRotationTargetIdx = PathPlannerTrajectory._getNextRotationTarget(path, i) + distanceBetweenTargets = path.getPoint(nextRotationTargetIdx).distanceAlongPath - prevRotationTargetDist + + nextTarget = path.getPoint(nextRotationTargetIdx).rotationTarget + + if nextTarget.rotateFast: + state.targetHolonomicRotation = nextTarget.target + else: + t = (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets + t = min(max(0.0, t), 1.0) + if not math.isfinite(t): + t = 0.0 + + state.targetHolonomicRotation = (prevRotationTargetRot + ( + nextTarget.target - prevRotationTargetRot) * t) + + state.positionMeters = path.getPoint(i).position + curveRadius = path.getPoint(i).curveRadius + state.curvatureRadPerMeter = 1.0 / curveRadius if (math.isfinite(curveRadius) and curveRadius != 0) else 0.0 + + if i == path.numPoints() - 1: + state.heading = states[len(states) - 1].heading + state.deltaPos = path.getPoint(i).distanceAlongPath - path.getPoint(i - 1).distanceAlongPath + state.velocityMps = path.getGoalEndState().velocity + elif i == 0: + state.heading = (path.getPoint(i + 1).position - state.positionMeters).angle() + state.deltaPos = 0 + state.velocityMps = startVel + else: + state.heading = (path.getPoint(i + 1).position - state.positionMeters).angle() + state.deltaPos = path.getPoint(i + 1).distanceAlongPath - path.getPoint(i).distanceAlongPath + + v0 = states[len(states) - 1].velocityMps + vMax = math.sqrt(math.fabs((v0 ** 2) + (2 * constraints.maxAccelerationMpsSq * state.deltaPos))) + state.velocityMps = min(vMax, path.getPoint(i).maxV) + + states.append(state) + + # Second pass. Handles linear deceleration + for i in range(len(states) - 2, 1, -1): + constraints = states[i].constraints + + v0 = states[i + 1].velocityMps + + vMax = math.sqrt(math.fabs((v0 ** 2) + (2 * constraints.maxAccelerationMpsSq * states[i + 1].deltaPos))) + states[i].velocityMps = min(vMax, states[i].velocityMps) + + # Final pass. Calculates time, linear acceleration, and angular velocity + time = 0 + states[0].timeSeconds = 0 + states[0].accelerationMpsSq = 0 + states[0].headingAngularVelocityRps = starting_speeds.omega + + for i in range(1, len(states)): + v0 = states[i - 1].velocityMps + v = states[i].velocityMps + dt = (2 * states[i].deltaPos) / (v + v0) + + time += dt + states[i].timeSeconds = time + + dv = v - v0 + states[i].accelerationMpsSq = dv / dt + + headingDelta = states[i].heading - states[i - 1].heading + states[i].headingAngularVelocityRps = headingDelta.radians() / dt + + return states diff --git a/pathplannerlib-python/pyproject.toml b/pathplannerlib-python/pyproject.toml new file mode 100644 index 00000000..1c45eeb1 --- /dev/null +++ b/pathplannerlib-python/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = ["setuptools", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "robotpy-pathplannerlib" +version = "0.0.0" +description = "Python implementation of PathPlannerLib" +authors = [{ name = "mjansen4857", email = "mjansen4857@gmail.com" }] +license = { file = "LICENSE" } +classifiers = [ + "License :: OSI Approved :: MIT License", + "Programming Language :: Python", + "Programming Language :: Python :: 3", +] +keywords = ["pathplannerlib"] +dependencies = [ + "pyntcore ~=2024.0", + "robotpy-wpimath ~=2024.0", + "robotpy-commands-v2 ~=2024.0", + "robotpy-wpiutil ~=2024.0", + "robotpy-hal ~=2024.0", +] +requires-python = ">=3.8" + +[project.urls] +Homepage = "https://github.com/mjansen4857/pathplanner" \ No newline at end of file diff --git a/pathplannerlib-python/requirements.txt b/pathplannerlib-python/requirements.txt new file mode 100644 index 00000000..e14c9f7e --- /dev/null +++ b/pathplannerlib-python/requirements.txt @@ -0,0 +1,6 @@ +setuptools~=68.2.0 +pyntcore~=2024.0.0b3 +robotpy-wpimath~=2024.0.0b3 +robotpy-commands-v2~=2024.0.0b3 +robotpy-wpiutil~=2024.0.0b3 +robotpy-hal~=2024.0.0b3 \ No newline at end of file diff --git a/pathplannerlib-python/setup.py b/pathplannerlib-python/setup.py new file mode 100644 index 00000000..60684932 --- /dev/null +++ b/pathplannerlib-python/setup.py @@ -0,0 +1,3 @@ +from setuptools import setup + +setup() diff --git a/pathplannerlib-python/tests/__init__.py b/pathplannerlib-python/tests/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/pathplannerlib-python/tests/geometry_util_test.py b/pathplannerlib-python/tests/geometry_util_test.py new file mode 100644 index 00000000..ad96db41 --- /dev/null +++ b/pathplannerlib-python/tests/geometry_util_test.py @@ -0,0 +1,6 @@ +from pathplannerlib.geometry_util import * + +def test_floatLerp(): + assert floatLerp(1.0, 2.0, 0.5) == 1.5 + assert floatLerp(-1.0, 2.0, 0.5) == 0.5 + assert floatLerp(-3.0, -1.0, 0.5) == -2.0 diff --git a/pathplannerlib/LICENSE b/pathplannerlib/LICENSE new file mode 100644 index 00000000..5afa6e55 --- /dev/null +++ b/pathplannerlib/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2022 Michael Jansen + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index 722fda37..57fa6851 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -249,6 +249,7 @@ public void execute() { >= replanningConfig.dynamicReplanningErrorSpikeThreshold) { replanPath(currentPose, currentSpeeds); timer.reset(); + timeOffset = 0.0; targetState = currentTrajectory.sample(0); } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 1889ac2c..2729e3f4 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -148,7 +148,7 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS } /** - * Creat a path with pre-generated points. This should already be a smooth path. + * Create a path with pre-generated points. This should already be a smooth path. * * @param pathPoints Path points along the smooth curve of the path * @param constraints The global constraints of the path @@ -811,8 +811,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) bezierPoints.get(bezierPointIdx), t); - segment1Length += lastSegment1Pos.getDistance(p1); - segment2Length += lastSegment2Pos.getDistance(p2); + segment1Length += positionDelta(lastSegment1Pos, p1); + segment2Length += positionDelta(lastSegment2Pos, p2); lastSegment1Pos = p1; lastSegment2Pos = p2; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java index f87037a0..371be72b 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java @@ -28,7 +28,7 @@ public PathPlannerTrajectory( private static int getNextRotationTargetIdx(PathPlannerPath path, int startingIndex) { int idx = path.numPoints() - 1; - for (int i = startingIndex; i < path.numPoints() - 2; i++) { + for (int i = startingIndex; i < path.numPoints() - 1; i++) { if (path.getPoint(i).rotationTarget != null) { idx = i; break; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java index 1a174b94..28c3577c 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java @@ -42,7 +42,7 @@ public static Translation2d translationLerp(Translation2d a, Translation2d b, do } /** - * Cubic interpolation between Translation2ds + * Quadratic interpolation between Translation2ds * * @param a Position 1 * @param b Position 2 @@ -58,7 +58,7 @@ public static Translation2d quadraticLerp( } /** - * Quadratic interpolation between Translation2ds + * Cubic interpolation between Translation2ds * * @param a Position 1 * @param b Position 2 diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java index f7327658..d6bd9e7f 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java @@ -36,7 +36,7 @@ public class PPLibTelemetry { private static NetworkTableListener hotReloadPathListener = null; private static NetworkTableListener hotReloadAutoListener = null; - /** Enable competition mode. This will disable all telemetry and hot reload. */ + /** Enable competition mode. This will disable hot reload. */ public static void enableCompetitionMode() { compMode = true; } @@ -51,9 +51,7 @@ public static void enableCompetitionMode() { */ public static void setVelocities( double actualVel, double commandedVel, double actualAngVel, double commandedAngVel) { - if (!compMode) { - velPub.set(new double[] {actualVel, commandedVel, actualAngVel, commandedAngVel}); - } + velPub.set(new double[] {actualVel, commandedVel, actualAngVel, commandedAngVel}); } /** @@ -62,9 +60,7 @@ public static void setVelocities( * @param inaccuracy Inaccuracy in meters */ public static void setPathInaccuracy(double inaccuracy) { - if (!compMode) { - inaccuracyPub.set(inaccuracy); - } + inaccuracyPub.set(inaccuracy); } /** @@ -73,9 +69,7 @@ public static void setPathInaccuracy(double inaccuracy) { * @param pose Current robot pose */ public static void setCurrentPose(Pose2d pose) { - if (!compMode) { - posePub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getRadians()}); - } + posePub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getRadians()}); } /** @@ -84,21 +78,19 @@ public static void setCurrentPose(Pose2d pose) { * @param path The current path */ public static void setCurrentPath(PathPlannerPath path) { - if (!compMode) { - double[] arr = new double[path.numPoints() * 3]; - - int ndx = 0; - for (PathPoint p : path.getAllPathPoints()) { - Translation2d pos = p.position; - arr[ndx] = pos.getX(); - arr[ndx + 1] = pos.getY(); - // Just add 0 as a heading since it's not needed for displaying a path - arr[ndx + 2] = 0.0; - ndx += 3; - } - - pathPub.set(arr); + double[] arr = new double[path.numPoints() * 3]; + + int ndx = 0; + for (PathPoint p : path.getAllPathPoints()) { + Translation2d pos = p.position; + arr[ndx] = pos.getX(); + arr[ndx + 1] = pos.getY(); + // Just add 0 as a heading since it's not needed for displaying a path + arr[ndx + 2] = 0.0; + ndx += 3; } + + pathPub.set(arr); } /** @@ -107,12 +99,8 @@ public static void setCurrentPath(PathPlannerPath path) { * @param targetPose Target robot pose */ public static void setTargetPose(Pose2d targetPose) { - if (!compMode) { - targetPosePub.set( - new double[] { - targetPose.getX(), targetPose.getY(), targetPose.getRotation().getRadians() - }); - } + targetPosePub.set( + new double[] {targetPose.getX(), targetPose.getY(), targetPose.getRotation().getRadians()}); } /** diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index d6e06bb6..d0eabd5a 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -176,6 +176,7 @@ void PathfindingCommand::Execute() { >= m_replanningConfig.dynamicReplanningErrorSpikeThreshold) { replanPath(currentPose, currentSpeeds); m_timer.Reset(); + m_timeOffset = 0_s; targetState = m_currentTrajectory.sample(0_s); } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 39450f45..3683ca3d 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -557,8 +557,8 @@ std::shared_ptr PathPlannerPath::replan( joinNextControl, nextWaypointPrevControl, m_bezierPoints[bezierPointIdx], t); - segment1Length += lastSegment1Pos.Distance(p1); - segment2Length += lastSegment2Pos.Distance(p2); + segment1Length += positionDelta(lastSegment1Pos, p1); + segment2Length += positionDelta(lastSegment2Pos, p2); lastSegment1Pos = p1; lastSegment2Pos = p2; diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp index 07890d4a..6a607a96 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp @@ -35,7 +35,7 @@ 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() - 2; i++) { + for (size_t i = startingIndex; i < path->numPoints() - 1; i++) { if (path->getPoint(i).rotationTarget) { idx = i; break; diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp index 53260f6c..514f30e6 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp @@ -33,19 +33,17 @@ std::optional PPLibTelemetry::m_hotReloadPathListener = std::nullopt; void PPLibTelemetry::setCurrentPath(std::shared_ptr path) { - if (!m_compMode) { - 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() }); + 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() { diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h index bb69942d..e1ff5a52 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h @@ -28,32 +28,25 @@ class PPLibTelemetry { units::meters_per_second_t commandedVel, units::degrees_per_second_t actualAngVel, units::degrees_per_second_t commandedAngVel) { - if (!m_compMode) { - m_velPub.Set(std::span( { actualVel(), commandedVel(), - actualAngVel(), commandedAngVel() })); - } + m_velPub.Set(std::span( { actualVel(), commandedVel(), + actualAngVel(), commandedAngVel() })); } static inline void setPathInaccuracy(units::meter_t inaccuracy) { - if (!m_compMode) { - m_inaccuracyPub.Set(inaccuracy()); - } + m_inaccuracyPub.Set(inaccuracy()); } static inline void setCurrentPose(frc::Pose2d pose) { - if (!m_compMode) { - m_posePub.Set(std::span( { pose.X()(), pose.Y()(), - pose.Rotation().Radians()() })); - } + m_posePub.Set( + std::span( { pose.X()(), pose.Y()(), + pose.Rotation().Radians()() })); } static void setCurrentPath(std::shared_ptr path); static inline void setTargetPose(frc::Pose2d targetPose) { - if (!m_compMode) { - m_targetPosePub.Set(std::span( { targetPose.X()(), - targetPose.Y()(), targetPose.Rotation().Radians()() })); - } + m_targetPosePub.Set(std::span( { targetPose.X()(), + targetPose.Y()(), targetPose.Rotation().Radians()() })); } static void registerHotReloadPath(std::string pathName,