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, diff --git a/pathplannerlib-python/pathplannerlib/events.py b/pathplannerlib-python/pathplannerlib/events.py index 97939c71..ab1ea273 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. @@ -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) @@ -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 diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 86ff28d8..95b0a37c 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,13 +177,13 @@ class PointTowardsZone: targetPosition: Translation2d minWaypointRelativePos: float maxWaypointRelativePos: float - rotationOffset: Rotation2d = Rotation2d() + rotationOffset: Rotation2d = field(default_factory=Rotation2d) @staticmethod 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)) @@ -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 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: diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index e897bfdc..bf8cd490 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -15,23 +15,26 @@ @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 + def __init__(self): + super().__init__() + @dataclass 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 @@ -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 @@ -237,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 @@ -490,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 @@ -589,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 @@ -607,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