From 0a496415ac62036b4258178d73a5dfd267da1446 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:24:30 -0400 Subject: [PATCH 01/11] fix type checking shenanigans --- pathplannerlib-python/pathplannerlib/events.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/events.py b/pathplannerlib-python/pathplannerlib/events.py index 97939c71..f111f323 100644 --- a/pathplannerlib-python/pathplannerlib/events.py +++ b/pathplannerlib-python/pathplannerlib/events.py @@ -167,7 +167,7 @@ def __init__(self): self._eventCommands = {} self._upcomingEvents = [] - def initialize(self, trajectory: PathPlannerTrajectory) -> None: + def initialize(self, trajectory: 'PathPlannerTrajectory') -> None: """ Initialize the EventScheduler for the given trajectory. This should be called from the initialize method of the command running this scheduler. @@ -220,7 +220,7 @@ def end(self) -> None: self._upcomingEvents.clear() @staticmethod - def getSchedulerRequirements(path: PathPlannerPath) -> set[Subsystem]: + def getSchedulerRequirements(path: 'PathPlannerPath') -> set[Subsystem]: """ Get the event requirements for the given path From fb2c0b6869d80067f73cacae7c3ba4761ebe21c0 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:30:26 -0400 Subject: [PATCH 02/11] fix default dataclass value shenanigans --- pathplannerlib-python/pathplannerlib/path.py | 4 ++-- pathplannerlib-python/pathplannerlib/trajectory.py | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 86ff28d8..33859c48 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -1,7 +1,7 @@ from __future__ import annotations import math -from dataclasses import dataclass +from dataclasses import dataclass, field from typing import Final, List, Union from wpimath.geometry import Rotation2d, Translation2d, Pose2d from wpimath.kinematics import ChassisSpeeds @@ -177,7 +177,7 @@ class PointTowardsZone: targetPosition: Translation2d minWaypointRelativePos: float maxWaypointRelativePos: float - rotationOffset: Rotation2d = Rotation2d() + rotationOffset: Rotation2d = field(default_factory=Rotation2d) @staticmethod def fromJson(json_dict: dict) -> PointTowardsZone: diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index e897bfdc..2335baaf 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -15,8 +15,8 @@ @dataclass class SwerveModuleTrajectoryState(SwerveModuleState): - fieldAngle: Rotation2d = Rotation2d() - fieldPos: Translation2d = Translation2d() + fieldAngle: Rotation2d = field(default_factory=Rotation2d) + fieldPos: Translation2d = field(default_factory=Translation2d) deltaPos: float = 0.0 @@ -25,13 +25,13 @@ class SwerveModuleTrajectoryState(SwerveModuleState): class PathPlannerTrajectoryState: timeSeconds: float = 0.0 fieldSpeeds: ChassisSpeeds = ChassisSpeeds() - pose: Pose2d = Pose2d() + pose: Pose2d = field(default_factory=Pose2d) linearVelocity: float = 0.0 feedforwards: List[DriveFeedforward] = field(default_factory=list) - heading: Rotation2d = Rotation2d() + heading: Rotation2d = field(default_factory=Rotation2d) deltaPos: float = 0.0 - deltaRot: Rotation2d = Rotation2d() + deltaRot: Rotation2d = field(default_factory=Rotation2d) moduleStates: List[SwerveModuleTrajectoryState] = field(default_factory=list) constraints: PathConstraints = None waypointRelativePos: float = 0.0 From be68745b6c0dfc99655a992370c6fbdbe9bdca01 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:31:45 -0400 Subject: [PATCH 03/11] fix telemetry struct topics --- pathplannerlib-python/pathplannerlib/telemetry.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/telemetry.py b/pathplannerlib-python/pathplannerlib/telemetry.py index 37581ca5..8744a929 100644 --- a/pathplannerlib-python/pathplannerlib/telemetry.py +++ b/pathplannerlib-python/pathplannerlib/telemetry.py @@ -6,11 +6,11 @@ class PPLibTelemetry: _velPub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic('/PathPlanner/vel').publish() _posePub: StructPublisher = NetworkTableInstance.getDefault().getStructTopic( - '/PathPlanner/currentPose', Pose2d.WPIStruct).publish() + '/PathPlanner/currentPose', Pose2d).publish() _pathPub: StructArrayPublisher = NetworkTableInstance.getDefault().getStructArrayTopic( - '/PathPlanner/activePath', Pose2d.WPIStruct).publish() + '/PathPlanner/activePath', Pose2d).publish() _targetPosePub: StructPublisher = NetworkTableInstance.getDefault().getStructTopic( - '/PathPlanner/targetPose', Pose2d.WPIStruct).publish() + '/PathPlanner/targetPose', Pose2d).publish() @staticmethod def setVelocities(actual_vel: float, commanded_vel: float, actual_ang_vel: float, commanded_ang_vel: float) -> None: From 763fc6c40b4eeaf4f827836a69418faaf4c5f1de Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:34:01 -0400 Subject: [PATCH 04/11] fix some incorrect field names --- pathplannerlib-python/pathplannerlib/path.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 33859c48..73a811db 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -388,9 +388,9 @@ def __init__(self, waypoints: List[Waypoint], constraints: PathConstraints, self._rotationTargets = holonomic_rotations self._rotationTargets.sort(key=lambda x: x.waypointRelativePosition) if point_towards_zones is None: - self.point_towards_zones = [] + self._pointTowardsZones = [] else: - self.point_towards_zones = point_towards_zones + self._pointTowardsZones = point_towards_zones if constraint_zones is None: self._constraintZones = [] else: @@ -399,7 +399,7 @@ def __init__(self, waypoints: List[Waypoint], constraints: PathConstraints, self._eventMarkers = [] else: self._eventMarkers = event_markers - self._eventMarkers.sort(key=lambda x: x.waypointRelativePosition) + self._eventMarkers.sort(key=lambda x: x.waypointRelativePos) self._globalConstraints = constraints self._idealStartingState = ideal_starting_state self._goalEndState = goal_end_state From 4f45c778beae494632642d3c5ac06e2eec4c970f Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:37:45 -0400 Subject: [PATCH 05/11] init shenanigans --- pathplannerlib-python/pathplannerlib/trajectory.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 2335baaf..8a6d4838 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -20,6 +20,9 @@ class SwerveModuleTrajectoryState(SwerveModuleState): deltaPos: float = 0.0 + def __init__(self): + super().__init__() + @dataclass class PathPlannerTrajectoryState: @@ -226,7 +229,7 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch for m in range(config.numModules): accel = (state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt appliedForce = wheelForces[m].norm() * ( - wheelForces[m].angle() - state.moduleStates[m].angle).cos() + wheelForces[m].angle() - state.moduleStates[m].angle).cos() wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.Kt From 1b9c22fb3638dc523586659b19aa474b3fada713 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:43:00 -0400 Subject: [PATCH 06/11] oops --- pathplannerlib-python/pathplannerlib/trajectory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 8a6d4838..c675bde2 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -610,7 +610,7 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon # vf^2 = v0^2 + 2ad maxVel = math.sqrt(abs(math.pow(nextState.moduleStates[m].speed, 2) + ( 2 * moduleAcceleration * nextState.moduleStates[m].deltaPos))) - state.moduleStates[m].speed = min(maxVel, nextState.moduleStates[m].speed) + state.moduleStates[m].speed = min(maxVel, state.moduleStates[m].speed) # Go over the modules again to make sure they take the same amount of time to reach the next state maxDT = 0.0 From 0bad98954a9d0956e03875b0e37eede4a59bf4d7 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:44:30 -0400 Subject: [PATCH 07/11] oops pt 2 --- pathplannerlib-python/pathplannerlib/trajectory.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index c675bde2..ace0290c 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -240,13 +240,13 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch while len(unaddedEvents) > 0 and abs( unaddedEvents[0].getTimestamp() - prevState.waypointRelativePos) <= abs( unaddedEvents[0].getTimestamp() - state.waypointRelativePos): - events.append(unaddedEvents.pop(0)) - events[-1].setTimestamp(prevState.timeSeconds) + self._events.append(unaddedEvents.pop(0)) + self._events[-1].setTimestamp(prevState.timeSeconds) while len(unaddedEvents) != 0: # There are events that need to be added to the last state - events.append(unaddedEvents.pop(0)) - events[-1].setTimestamp(self._states[-1].timeSeconds) + self._events.append(unaddedEvents.pop(0)) + self._events[-1].setTimestamp(self._states[-1].timeSeconds) # Create feedforwards for the end state self._states[-1].feedforwards = [DriveFeedforward(0, 0, 0)] * config.numModules From 4dd57d2f3d9f201a931d4221ea8f9097a349bcbc Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 12:57:56 -0400 Subject: [PATCH 08/11] oops pt 3 --- pathplannerlib-python/pathplannerlib/events.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pathplannerlib-python/pathplannerlib/events.py b/pathplannerlib-python/pathplannerlib/events.py index f111f323..ab1ea273 100644 --- a/pathplannerlib-python/pathplannerlib/events.py +++ b/pathplannerlib-python/pathplannerlib/events.py @@ -194,7 +194,7 @@ def execute(self, time: float) -> None: if not self._eventCommands[command]: continue - command.execute(self) + command.execute() if command.isFinished(): command.end(False) From 256d6594134f8ebe5a20f018480e781d276b923b Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 13:04:01 -0400 Subject: [PATCH 09/11] fix point towards zone json --- pathplannerlib-python/pathplannerlib/path.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 73a811db..95b0a37c 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -183,7 +183,7 @@ class PointTowardsZone: def fromJson(json_dict: dict) -> PointTowardsZone: targetPos = PointTowardsZone._translationFromJson(json_dict['fieldPosition']) minPos = float(json_dict['minWaypointRelativePos']) - maxPos = float(json_dict['minWaypointRelativePos']) + maxPos = float(json_dict['maxWaypointRelativePos']) deg = float(json_dict['rotationOffset']) return PointTowardsZone(targetPos, minPos, maxPos, Rotation2d.fromDegrees(deg)) From 0600e6e2e9d5ff0b09e184394c61bd7df9b18a4f Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 13:12:22 -0400 Subject: [PATCH 10/11] oops pt 4 --- pathplannerlib-python/pathplannerlib/auto.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/auto.py b/pathplannerlib-python/pathplannerlib/auto.py index 0fc2e8ff..c6f17414 100644 --- a/pathplannerlib-python/pathplannerlib/auto.py +++ b/pathplannerlib-python/pathplannerlib/auto.py @@ -201,7 +201,7 @@ def configure(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d] AutoBuilder._isHolonomic = robot_config.isHolonomic AutoBuilder._pathfindToPoseCommandBuilder = \ - lambda pose, constraints, goal_end_vel, rotation_delay_distance: PathfindingCommand( + lambda pose, constraints, goal_end_vel: PathfindingCommand( constraints, pose_supplier, robot_relative_speeds_supplier, @@ -214,7 +214,7 @@ def configure(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d] goal_end_vel=goal_end_vel ) AutoBuilder._pathfindThenFollowPathCommandBuilder = \ - lambda path, constraints, rotation_delay_distance: PathfindThenFollowPath( + lambda path, constraints: PathfindThenFollowPath( path, constraints, pose_supplier, From ce80ad8c9f2abbfc3eb277357339a2455f5b60fc Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 5 Oct 2024 13:33:20 -0400 Subject: [PATCH 11/11] oops pt 5 --- pathplannerlib-python/pathplannerlib/trajectory.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index ace0290c..bf8cd490 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -493,7 +493,7 @@ def _forwardAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon # Even though kinematics is usually used for velocities, it can still # convert chassis accelerations to module accelerations maxAngAccel = state.constraints.maxAngularAccelerationRpsSq - angularAccel = max(min(totalTorque / config.MOI, -maxAngAccel), maxAngAccel) + angularAccel = min(max(totalTorque / config.MOI, -maxAngAccel), maxAngAccel) accelVec = linearForceVec / config.massKG maxAccel = state.constraints.maxAccelerationMpsSq @@ -592,7 +592,7 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon # Even though kinematics is usually used for velocities, it can still # convert chassis accelerations to module accelerations maxAngAccel = state.constraints.maxAngularAccelerationRpsSq - angularAccel = max(min(totalTorque / config.MOI, -maxAngAccel), maxAngAccel) + angularAccel = min(max(totalTorque / config.MOI, -maxAngAccel), maxAngAccel) accelVec = linearForceVec / config.massKG maxAccel = state.constraints.maxAccelerationMpsSq