search

mjansen4857 / pathplanner

A simple yet powerful path planning tool for FRC robots
https://pathplanner.dev
MIT License
393 stars 116 forks source link

Divide by zero fatal exception from pathFindThenFollowPath command - photonlibpy-2024.3.1 #695

Closed roberttuttle closed 4 months ago

roberttuttle commented 4 months ago

Describe the bug AutoBuilder.pathFindThenFollowPath using Python lib is generating a path that produces a ZeroDivisionError: float division by zero unhandled fatal exception / crash

To Reproduce Steps to reproduce the behavior: Execute command that uses AutoBuilder.pathFindThenFollowPath where starting pose is different than the starting pose of a fixed path to run after dynamic generation / pathfinding.

Expected behavior Generated paths that produce a distanceBetweenTargets of 0.0 handles divide by zero cases correctly / gracefully

Versions: (please complete the following information):

Additional context

Exception tracing from DSLog events: (notice the 'distanceBetweenTargets': 0.0 entry which is the source of the div by zero error)

<TagVersion>1 <time> 46.405 <message>   File "/usr/local/lib/python3.12/site-packages/pathplannerlib/path.py", line 799, in getTrajectory <TagVersion>1 <time> 46.404 <message>                                 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ <TagVersion>1 <time> 46.402 <message>     self._generatedTrajectory = replanned.getTrajectory(current_speeds, current_pose.rotation()) <TagVersion>1 <time> 46.401 <message>   File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 193, in _replanPath <TagVersion>1 <time> 46.400 <message>     self._replanPath(currentPose, currentSpeeds) <TagVersion>1 <time> 46.399 <message>   File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 93, in initialize <TagVersion>1 <time> 46.398 <message>     self._commands[self._currentCommandIndex].initialize() <TagVersion>1 <time> 46.398 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 76, in execute <TagVersion>1 <time> 46.397 <message>     self._command.execute() <TagVersion>1 <time> 46.397 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute <TagVersion>1 <time> 46.396 <message>     command.execute() <TagVersion>1 <time> 46.396 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/paralleldeadlinegroup.py", line 112, in execute <TagVersion>1 <time> 46.395 <message>     command.execute() <TagVersion>1 <time> 46.395 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/parallelracegroup.py", line 75, in execute <TagVersion>1 <time> 46.395 <message>     self._command.execute() <TagVersion>1 <time> 46.394 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute <TagVersion>1 <time> 46.394 <message>     currentCommand.execute() <TagVersion>1 <time> 46.393 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 71, in execute <TagVersion>1 <time> 46.393 <message>     self._command.execute() <TagVersion>1 <time> 46.393 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute <TagVersion>1 <time> 46.392 <message>     command.execute() <TagVersion>1 <time> 46.391 <message>   File "/usr/local/lib/python3.12/site-packages/commands2/commandscheduler.py", line 271, in run <TagVersion>1 <time> 46.391 <count> 1 <flags> 0 <Code> 1 <details> Unhandled exception <location> /usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py.283:_generateStates <stack> 
Traceback (most recent call last):
  File "/usr/local/lib/python3.12/site-packages/wpilib/_impl/start.py", line 247, in _start
    self.robot.startCompetition()
  File "/home/lvuser/py/robot.py", line 16, in robotPeriodic
    CommandScheduler.getInstance().run()
  File "/usr/local/lib/python3.12/site-packages/commands2/commandscheduler.py", line 271, in run
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 71, in execute
    currentCommand.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/parallelracegroup.py", line 75, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/paralleldeadlinegroup.py", line 112, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 76, in execute
    self._commands[self._currentCommandIndex].initialize()
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 93, in initialize
    self._replanPath(currentPose, currentSpeeds)
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 193, in _replanPath
    self._generatedTrajectory = replanned.getTrajectory(current_speeds, current_pose.rotation())
                                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/path.py", line 799, in getTrajectory
    return PathPlannerTrajectory(self, starting_speeds, starting_rotation)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 136, in __init__
    self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
                   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 283, in _generateStates
    t = (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets
        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~
  File "/usr/local/lib/python3.12/site-packages/wpilib/_impl/start.py", line 247, in _start
    self.robot.startCompetition()
  File "/home/lvuser/py/robot.py", line 16, in robotPeriodic
    CommandScheduler.getInstance().run()
  File "/usr/local/lib/python3.12/site-packages/commands2/commandscheduler.py", line 271, in run
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 71, in execute
    currentCommand.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/parallelracegroup.py", line 75, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/paralleldeadlinegroup.py", line 112, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 76, in execute
    self._commands[self._currentCommandIndex].initialize()
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 93, in initialize
    self._replanPath(currentPose, currentSpeeds)
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 193, in _replanPath
    self._generatedTrajectory = replanned.getTrajectory(current_speeds, current_pose.rotation())
                                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/path.py", line 799, in getTrajectory
    return PathPlannerTrajectory(self, starting_speeds, starting_rotation)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 136, in __init__
    self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
                   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 283, in _generateStates
    t = (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets
        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~
ZeroDivisionError: float division by zero

↑↓
<TagVersion>1 <time> 46.427 <message>   'prevRotationTargetRot': Rotation2d(0.00
3719), <TagVersion>1 <time> 46.427 <message>   'prevRotationTargetDist': 0.0, <T
agVersion>1 <time> 46.426 <message>   'path': <pathplannerlib.path.PathPlannerPa
th object at 0xae4213d8>, <TagVersion>1 <time> 46.425 <message>                 
               rotateFast=False), <TagVersion>1 <time> 46.424 <message>         
                       target=Rotation2d(0.000000), <TagVersion>1 <time> 46.424 
<message>   'nextTarget': RotationTarget(waypointRelativePosition=0.0, <TagVersi
on>1 <time> 46.423 <message>   'nextRotationTargetIdx': 0, <TagVersion>1 <time> 
46.421 <message>   'i': 0, <TagVersion>1 <time> 46.421 <message>   'distanceBetw
eenTargets': 0.0, <TagVersion>1 <time> 46.420 <message>                         
         maxAngularAccelerationRpsSq=12.566370614359172), <TagVersion>1 <time> 4
6.419 <message>                                  maxAngularVelocityRps=9.4247779
6076938, <TagVersion>1 <time> 46.418 <message>                                  
maxAccelerationMpsSq=3.6, <TagVersion>1 <time> 46.418 <message> { 'constraints':
 PathConstraints(maxVelocityMps=5.8, <TagVersion>1 <time> 46.417 <message>  <Tag
Version>1 <time> 46.416 <message> Locals at innermost frame: <TagVersion>1 <time
> 46.415 <message>  <TagVersion>1 <time> 46.414 <message>  <TagVersion>1 <time> 
46.414 <message> ZeroDivisionError: float division by zero <TagVersion>1 <time> 
46.413 <message>         ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ <TagVersion>1 <time> 46.412 <message>     t = (p
ath.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTar
gets <TagVersion>1 <time> 46.411 <message>   File "/usr/local/lib/python3.12/sit
e-packages/pathplannerlib/trajectory.py", line 283, in _generateStates <TagVersi
on>1 <time> 46.411 <message>                    ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ <TagVersion>1 <time> 46.410 <mes
sage>     self._states = PathPlannerTrajectory._generateStates(path, starting_sp
eeds, starting_rotation) <TagVersion>1 <time> 46.409 <message>   File "/usr/loca
l/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 136, in __ini
t__ <TagVersion>1 <time> 46.408 <message>            ^^^^^^^^^^^^^^^^^^^^^^^^^^^
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ <TagVersion>1 <time> 46.407 <message>     r
eturn PathPlannerTrajectory(self, starting_speeds, starting_rotation) 
#N : 31

<TagVersion>1 <time> 46.454 <count> 1 <flags> 0 <Code> 1 <details> The robot program quit unexpectedly. This is usually due to a code error.
The above stacktrace can help determine where the error occurred.
 <location> /usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py.283:_generateStates <stack> 
Traceback (most recent call last):
  File "/usr/local/lib/python3.12/site-packages/wpilib/_impl/start.py", line 160, in start
    return self._start(robot_cls)
           ^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/wpilib/_impl/start.py", line 247, in _start
    self.robot.startCompetition()
  File "/home/lvuser/py/robot.py", line 16, in robotPeriodic
    CommandScheduler.getInstance().run()
  File "/usr/local/lib/python3.12/site-packages/commands2/commandscheduler.py", line 271, in run
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 71, in execute
    currentCommand.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/parallelracegroup.py", line 75, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/paralleldeadlinegroup.py", line 112, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 76, in execute
    self._commands[self._currentCommandIndex].initialize()
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 93, in initialize
    self._replanPath(currentPose, currentSpeeds)
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 193, in _replanPath
    self._generatedTrajectory = replanned.getTrajectory(current_speeds, current_pose.rotation())
                                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/path.py", line 799, in getTrajectory
    return PathPlannerTrajectory(self, starting_speeds, starting_rotation)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 136, in __init__
    self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
                   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 283, in _generateStates
    t = (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets
        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~
  File "/usr/local/lib/python3.12/site-packages/wpilib/_impl/start.py", line 160, in start
    return self._start(robot_cls)
           ^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/wpilib/_impl/start.py", line 247, in _start
    self.robot.startCompetition()
  File "/home/lvuser/py/robot.py", line 16, in robotPeriodic
    CommandScheduler.getInstance().run()
  File "/usr/local/lib/python3.12/site-packages/commands2/commandscheduler.py", line 271, in run
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 71, in execute
    currentCommand.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/parallelracegroup.py", line 75, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/paralleldeadlinegroup.py", line 112, in execute
    command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/wrappercommand.py", line 43, in execute
    self._command.execute()
  File "/usr/local/lib/python3.12/site-packages/commands2/sequentialcommandgroup.py", line 76, in execute
    self._commands[self._currentCommandIndex].initialize()
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 93, in initialize
    self._replanPath(currentPose, currentSpeeds)
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/commands.py", line 193, in _replanPath
    self._generatedTrajectory = replanned.getTrajectory(current_speeds, current_pose.rotation())
                                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/path.py", line 799, in getTrajectory
    return PathPlannerTrajectory(self, starting_speeds, starting_rotation)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 136, in __init__
    self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
                   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/usr/local/lib/python3.12/site-packages/pathplannerlib/trajectory.py", line 283, in _generateStates
    t = (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets
        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~
ZeroDivisionError: float division by zero
roberttuttle commented 4 months ago

Closing this issue as reported, but it should be noted that the PathPlanner GUI does seem to allow the data state (rotation target with a zero waypoint init) that does end up producing the exception / crash. It is unclear how the rotation targets in the affected paths ended up in this data state.