Hi everyone, my team is trying to write code to automatically align to the scoring location using on-the-fly WPILib trajectory generation using data from the limelight. We have had a lot of success so far with this, however the output trajectories we are getting are following what we feel is an suboptimal path. I am unable to currently post my full code so I apologize in advance, but in short we are currently using WPILib’s TrajectoryGenerator class with the constructor that takes in a starting Pose2d, a list of intermediate Translate2ds, and a final Pose2d. We have one intermediate waypoint that we use to make sure we come at the goal straight so we don’t crash into any obstacles.
I tried to recreate the path we are currently generating at an example starting location in Path Planner, as well as the type of trajectory we would like to generate. Please see the images below:
Current Trajectory being generated with routes going away from the goal first, then approaching the goal sideways:
Desired path, where the wheels cut straight to the side and turn at the intermediate waypoint and go straight to the scoring location:
I understand that to achieve this affect I am changing the “Heading” setting in Path Planner, but I do not know what I can do to achieve this in WPILib. I have experienced this before with WPIlib trajectories in a simple case seen below:
I am sure I am just not understanding something simple, but I have not been able to understand how to fix this since we started working on swerve autonomous during the offseason. If anyone is able to give some advice or needs more information please let me know! Thanks!!