RobotPy Pathfinder Trajectory Generation Invoking MemoryError

Hello CD,

I have been working on a pathfinder project with all this spare time, and for the first time today, I have repeatedly received a memory error. It’s incredibly annoying and I’ve done almost everything I could think of from re-imaging the Rio to reducing the code I’m deploying.

The error I am receiving looks like this:

Traceback (most recent call last):
[3.38]   File "/usr/local/lib/python3.8/site-packages/wpilib/_impl/start.py", line 106, in start
[3.38]     self.robot.startCompetition()
[3.38]   File "/usr/local/lib/python3.8/site-packages/commandbased/commandbasedrobot.py", line 15, in startCompetition
[3.38]     super().startCompetition()
[3.38]   File "/usr/local/lib/python3.8/site-packages/commandbased/commandbasedrobot.py", line 24, in commandPeriodic
[3.38]     self.scheduler.run()
[3.38]   File "/home/lvuser/py/commands/drivetrain/pathfindermovecommand.py", line 36, in initialize
[3.38]     info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
[3.39] MemoryError: std::bad_alloc
[3.39] 
[3.39] 
[3.39] Locals at innermost frame:
[3.39] 
[3.39] { 'points': [ <Waypoint x=0.000000 y=5.000000 angle=0.000000>,
[3.39]               <Waypoint x=0.000000 y=1.000000 angle=0.000000>],
[3.39]   'self': <commands.drivetrain.pathfindermovecommand.PathfinderMoveCommand object at 0xb633b578>}
[3.39] 
[3.39] 1 Unhandled exception /home/lvuser/py/commands/drivetrain/pathfindermovecommand.py.36:initialize 
Traceback (most recent call last):
  File "/usr/local/lib/python3.8/site-packages/wpilib/_impl/start.py", line 106, in start
    self.robot.startCompetition()
  File "/usr/local/lib/python3.8/site-packages/commandbased/commandbasedrobot.py", line 15, in startCompetition
    super().startCompetition()
  File "/usr/local/lib/python3.8/site-packages/commandbased/commandbasedrobot.py", line 24, in commandPeriodic
    self.scheduler.run()
  File "/home/lvuser/py/commands/drivetrain/pathfindermovecommand.py", line 36, in initialize
    info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
  File "/usr/local/lib/python3.8/site-packages/wpilib/_impl/start.py", line 106, in start
    self.robot.startCompetition()
  File "/usr/local/lib/python3.8/site-packages/commandbased/commandbasedrobot.py", line 15, in startCompetition
    super().startCompetition()
  File "/usr/local/lib/python3.8/site-packages/commandbased/commandbasedrobot.py", line 24, in commandPeriodic
    self.scheduler.run()
  File "/home/lvuser/py/commands/drivetrain/pathfindermovecommand.py", line 36, in initialize
    info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
MemoryError: std::bad_alloc

My code looks like this:

points = [
            pf.Waypoint(0, 5, 0),
            pf.Waypoint(0, 1, 0)                       
        ]
info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
                                    dt=0.02, # 20ms
                                    max_velocity=5.0,
                                    max_acceleration=6.0,
                                    max_jerk=120.0
                                    )

modifier = pf.modifiers.TankModifier(trajectory).modify(2.0)

self.left = EncoderFollower(modifier.getLeftTrajectory())
self.right = EncoderFollower(modifier.getRightTrajectory())

The entire code base can be found here, at our GitHub.

Any feedback and advice would be appreciated!

Is it reproducable off the RoboRIO?

My initial assumption would be you do not have enough memory to allocate that many samples. Have you tried pf.SAMPLES_FAST ?

Oh, I just noticed that you’re using our port of Jaci’s pathfinder library. I would recommend using the new WPILib trajectory stuff – it’s more capable and much faster. Probably less likely to run into this sort of problem too.

1 Like

You’re trying to ask the robot to do an impossible movement. From Pathfinder - Fast Motion Profiling for Java and C++, Tank and Swerve Drive

By having motion only along the Y axis, you’re asking the robot to move sideways, which a tank drive can’t do.

Does it work if you change it to:

points = [
            pf.Waypoint(5, 0, 0),
            pf.Waypoint(1, 0, 0)                       
        ]
1 Like

This is true if you try to move only sideways. However, trajectories exist that can move between those two poses. Here’s an example using WPILib trajectory generation:

sideways-trajectory

The fact Pathfinder crashes is an implementation issue in Pathfinder. There’s nothing fundamentally wrong with the selection of those waypoints.

1 Like

A tank drive can rotate in its own footprint (more or less). Is this path faster than rotate 90° straight line path, rotate back 90°?

In my experience with stop-and-turn autonomous modes, yes, but it depends on the drivetrain. The simulation above is basically already using a turning radius that’s inside the robot trackwidth. The robot being simulated is a 2-NEO-per-side drivetrain geared 1:1 with 3" radius wheels and a trackwidth of ~0.99 m.

I would have compared the two for you in simulation, but stitching some linear and angular trapezoid profiles together would take a lot of work since my FRC team’s code is built around the WPILib trajectory generation API.

2 Likes

Thank you for all the responses! I’ll switch up the points tomorrow and make sure that the points are legal. I will also look into the trajectory library a little more; however, I do not use the drivetrain object in my code; instead, we create and control motors ourselves. If I am not mistaken, we need a drivetrain object to use the library, right? I’m sure I don’t need to actually drive with it, but is it worth setting up?

What do you mean by “drivetrain object”? Is there a specific class you had in mind?

The trajectory generation and tracking examples in C++ and Java use Ramsete to generate velocity and angular velocity commands, then those are turned into setpoints for left and right side PID controllers. The outputs of each are passed to their respective motors directly. Idk what robotpy’s examples do.

I never got around to really using the trajectory library myself, but I posted some initial code here: RobotPy 2020 Trajectory Support

RobotPy does not currently support the new command framework, so you’d need to write your own Ramsete commands as they don’t exist for the old framework. However, it does have the Ramsete controller objects so it shouldn’t be that bad?

Sorry for the late response!

I thought I needed to create an object of a wpilib.drive class. I suppose I misread parts of the documentation. I’ll look into creating my own Ramsete commands.

Thanks for the help!