Swerve Bézier Trajectory Command for Vision #539
Conversation
…8/SeriouslyCommonLib into SSTCBezierCurveCommand
| for (int i = 1; i <= steps; i++) { | ||
| double lerpFraction = i / (double) steps; | ||
| XbotSwervePoint point = new XbotSwervePoint( | ||
| deCasteljauIterative(allPoints, lerpFraction), |
There was a problem hiding this comment.
Highly recommend watching
There was a problem hiding this comment.
Both videos are worth watching in their entirety, but the first one has a section about deCasteljau early on
| * @param endPoint of our command | ||
| * @param steps to split our Bézier curve into | ||
| */ | ||
| public void setBezierConfiguration(List<Translation2d> controlPoints, Pose2d endPoint, int steps) { |
There was a problem hiding this comment.
Do we know the n of the highest order bezier curve the vision coprocessor will be sending us?
There was a problem hiding this comment.
I don't expect too-too much, probably 2-3 control points, no more than 5. I don't think we need a bajillion control points to traverse through an obstacle.
| List<Translation2d> temp = new ArrayList<>(points); | ||
|
|
||
| // Compute the position using de Casteljau's algorithm (all we know is that it works) | ||
| for (int level = 1; level < n; level++) { |
There was a problem hiding this comment.
You could do some profiling on the Rio, for example, using higher and higher order bezier curves and see how long this function takes to calculate. This would help us set an upper bound for how many points the Rio is willing to accept
There was a problem hiding this comment.
agreed, level^2 gets scary fast!
yes I believe having multiple curves is a lot better, as there will be less data needing to be transferred & computed and also allowing for much easier sharp turns. |
| List<Translation2d> temp = new ArrayList<>(points); | ||
|
|
||
| // Compute the position using de Casteljau's algorithm (all we know is that it works) | ||
| for (int level = 1; level < n; level++) { |
There was a problem hiding this comment.
agreed, level^2 gets scary fast!
| * @param lerpFraction is our completion percentage | ||
| * @return our position during lerpFraction (progress of operation completion) | ||
| */ | ||
| private Translation2d deCasteljauIterative(List<Translation2d> points, double lerpFraction) { |
There was a problem hiding this comment.
⭐ I wonder if these should be static to make testing easier?
Why are we doing this?
Vision wants some cool curves (minimize driving time and potentially award-worthy?)
What's changing?
A new command that builds off of the SwerveSimpleTrajectoryCommand logic and does currrrrvesss
Questions/notes for reviewers
Vision wants this thing to be HEAVILY REVIEWED for ANY potential optimizations. Logic written by AI has been pointed out in the JavaDocs and we can only pray that it IS the best way written.
How this was tested