From f52122bd470b8daaf5bb43cf19e7435dceec479e Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Mon, 10 Feb 2025 15:31:16 -0800 Subject: [PATCH 1/8] Create SwerveBezierTrajectoryCommand.java --- .../drive/SwerveBezierTrajectoryCommand.java | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java new file mode 100644 index 00000000..33d77caa --- /dev/null +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -0,0 +1,75 @@ +package xbot.common.subsystems.drive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import xbot.common.logging.RobotAssertionManager; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.drive.control_logic.HeadingModule; +import xbot.common.subsystems.pose.BasePoseSubsystem; +import xbot.common.trajectory.XbotSwervePoint; + +import javax.inject.Inject; +import java.util.ArrayList; +import java.util.List; + +public class SwerveBezierTrajectoryCommand extends SwerveSimpleTrajectoryCommand { + + List controlPoints; + Pose2d endPoint; + int steps; + @Inject + public SwerveBezierTrajectoryCommand(BaseSwerveDriveSubsystem drive, BasePoseSubsystem pose, PropertyFactory pf, + HeadingModule.HeadingModuleFactory headingModuleFactory, RobotAssertionManager assertionManager) { + super(drive, pose, pf, headingModuleFactory, assertionManager); + } + + @Override + public void initialize() { + setBezierCurve(controlPoints, endPoint, steps); + super.initialize(); + } + + public void setBezierConfiguration(List controlPoints, Pose2d endPoint, int steps) { + this.controlPoints = controlPoints; + this.steps = steps; + this.endPoint = endPoint; + } + + public void setBezierCurve(List controlPoints, Pose2d endPoint, int steps) { + List bezierPoints = new ArrayList<>(); + List allPoints = new ArrayList<>(); + allPoints.add(pose.getCurrentPose2d().getTranslation()); + allPoints.addAll(controlPoints); + allPoints.add(endPoint.getTranslation()); + + for (int i = 1; i <= steps; i++) { + double lerpFraction = i / (double) steps; + XbotSwervePoint point = new XbotSwervePoint(deCasteljau(allPoints, lerpFraction), new Rotation2d(), 10); + bezierPoints.add(point); + } + + logic.setKeyPoints(bezierPoints); + } + + // Taken from ChatGPT lol + private Translation2d deCasteljau(List points, double t) { + if (points.size() == 1) { + return points.get(0); + } + + List newPoints = new ArrayList<>(); + for (int i = 0; i < points.size() - 1; i++) { + Translation2d p1 = points.get(i); + Translation2d p2 = points.get(i + 1); + + double x = (1 - t) * p1.getX() + t * p2.getX(); + double y = (1 - t) * p1.getY() + t * p2.getY(); + + newPoints.add(new Translation2d(x, y)); + } + + return deCasteljau(newPoints, t); + } + +} From 4d397620ad553589fa91806d22d70c7a4febcbfa Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Mon, 10 Feb 2025 15:42:30 -0800 Subject: [PATCH 2/8] Added documentation --- .../drive/SwerveBezierTrajectoryCommand.java | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index 33d77caa..440a780e 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -13,11 +13,16 @@ import java.util.ArrayList; import java.util.List; +/** + * A "not-so-simple" SwerveSimpleTrajectoryCommand that does a Bézier curve!~~ + * NOTE: Currently, rotation is neglected... + */ public class SwerveBezierTrajectoryCommand extends SwerveSimpleTrajectoryCommand { List controlPoints; Pose2d endPoint; int steps; + @Inject public SwerveBezierTrajectoryCommand(BaseSwerveDriveSubsystem drive, BasePoseSubsystem pose, PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, RobotAssertionManager assertionManager) { @@ -30,6 +35,12 @@ public void initialize() { super.initialize(); } + /** + * Set the configuration of the Bézier curve, we can't generate our curve immediately as our pose will change + * @param controlPoints of the Bézier curve + * @param endPoint of our command + * @param steps to split our Bézier curve into + */ public void setBezierConfiguration(List controlPoints, Pose2d endPoint, int steps) { this.controlPoints = controlPoints; this.steps = steps; @@ -52,8 +63,13 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i logic.setKeyPoints(bezierPoints); } - // Taken from ChatGPT lol - private Translation2d deCasteljau(List points, double t) { + /** + * Generated by ChatGPT, this tells us where we SHOULD we at + * @param points include: start, control points, end + * @param lerpFraction is our completion percentage + * @return our position during lerpFraction (% of operation) + */ + private Translation2d deCasteljau(List points, double lerpFraction) { if (points.size() == 1) { return points.get(0); } @@ -63,13 +79,13 @@ private Translation2d deCasteljau(List points, double t) { Translation2d p1 = points.get(i); Translation2d p2 = points.get(i + 1); - double x = (1 - t) * p1.getX() + t * p2.getX(); - double y = (1 - t) * p1.getY() + t * p2.getY(); + double x = (1 - lerpFraction) * p1.getX() + lerpFraction * p2.getX(); + double y = (1 - lerpFraction) * p1.getY() + lerpFraction * p2.getY(); newPoints.add(new Translation2d(x, y)); } - return deCasteljau(newPoints, t); + return deCasteljau(newPoints, lerpFraction); } } From 8c81c2a72669c4800dbc838ec53f02212f334421 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Mon, 10 Feb 2025 15:44:54 -0800 Subject: [PATCH 3/8] Fixed documentation grammar mistakes --- .../subsystems/drive/SwerveBezierTrajectoryCommand.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index 440a780e..a870b40c 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -16,6 +16,7 @@ /** * A "not-so-simple" SwerveSimpleTrajectoryCommand that does a Bézier curve!~~ * NOTE: Currently, rotation is neglected... + * IMPORTANT: This thing may get *expensive* the more control points you got */ public class SwerveBezierTrajectoryCommand extends SwerveSimpleTrajectoryCommand { @@ -64,10 +65,10 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i } /** - * Generated by ChatGPT, this tells us where we SHOULD we at + * Generated by ChatGPT, this tells us where we SHOULD be at * @param points include: start, control points, end * @param lerpFraction is our completion percentage - * @return our position during lerpFraction (% of operation) + * @return our position during lerpFraction (progress of operation completion) */ private Translation2d deCasteljau(List points, double lerpFraction) { if (points.size() == 1) { From 3455aed11e8e6f4b286ce865722e901080ff6250 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Mon, 10 Feb 2025 15:52:53 -0800 Subject: [PATCH 4/8] Iterative approach --- .../drive/SwerveBezierTrajectoryCommand.java | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index a870b40c..3a9b0f3c 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -57,7 +57,7 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i for (int i = 1; i <= steps; i++) { double lerpFraction = i / (double) steps; - XbotSwervePoint point = new XbotSwervePoint(deCasteljau(allPoints, lerpFraction), new Rotation2d(), 10); + XbotSwervePoint point = new XbotSwervePoint(deCasteljauIterative(allPoints, lerpFraction), new Rotation2d(), 10); bezierPoints.add(point); } @@ -89,4 +89,19 @@ private Translation2d deCasteljau(List points, double lerpFractio return deCasteljau(newPoints, lerpFraction); } + // ChatGPT said that this is better + private Translation2d deCasteljauIterative(List points, double lerpFraction) { + int n = points.size(); + List temp = new ArrayList<>(points); + + for (int level = 1; level < n; level++) { + for (int i = 0; i < n - level; i++) { + double x = (1 - lerpFraction) * temp.get(i).getX() + lerpFraction * temp.get(i + 1).getX(); + double y = (1 - lerpFraction) * temp.get(i).getY() + lerpFraction * temp.get(i + 1).getY(); + temp.set(i, new Translation2d(x, y)); // Update in place + } + } + + return temp.get(0); + } } From c984e2d065a05903a08a3e4bc3dfe57c17f878b3 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Mon, 10 Feb 2025 16:21:26 -0800 Subject: [PATCH 5/8] Rotation! --- .../drive/SwerveBezierTrajectoryCommand.java | 49 ++++++++++++------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index 3a9b0f3c..75a2c6ff 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -57,13 +57,44 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i for (int i = 1; i <= steps; i++) { double lerpFraction = i / (double) steps; - XbotSwervePoint point = new XbotSwervePoint(deCasteljauIterative(allPoints, lerpFraction), new Rotation2d(), 10); + XbotSwervePoint point = new XbotSwervePoint(deCasteljauIterative(allPoints, lerpFraction), 10); bezierPoints.add(point); } logic.setKeyPoints(bezierPoints); } + // ChatGPT said that this is better + private Pose2d deCasteljauIterative(List points, double lerpFraction) { + int n = points.size(); + List temp = new ArrayList<>(points); + + // Compute the position using de Casteljau's algorithm + for (int level = 1; level < n; level++) { + for (int i = 0; i < n - level; i++) { + double x = (1 - lerpFraction) * temp.get(i).getX() + lerpFraction * temp.get(i + 1).getX(); + double y = (1 - lerpFraction) * temp.get(i).getY() + lerpFraction * temp.get(i + 1).getY(); + temp.set(i, new Translation2d(x, y)); // Update in place + } + } + + // After calculating the position, compute the desired rotation (angle to the next point) + Translation2d currentPosition = temp.get(0); + Translation2d nextPosition = (n > 1) ? temp.get(1) : currentPosition; // Use next point for rotation or current position + Rotation2d desiredRotation; + if (lerpFraction < 1) { + // Interpolate rotation between current and next points + desiredRotation = new Rotation2d(nextPosition.getX() - currentPosition.getX(), + nextPosition.getY() - currentPosition.getY()); + } else { + // If it's the last point, keep the rotation consistent with the end point's rotation + desiredRotation = endPoint.getRotation(); + } + + // Return the Pose2d (position + rotation) + return new Pose2d(currentPosition, desiredRotation); + } + /** * Generated by ChatGPT, this tells us where we SHOULD be at * @param points include: start, control points, end @@ -88,20 +119,4 @@ private Translation2d deCasteljau(List points, double lerpFractio return deCasteljau(newPoints, lerpFraction); } - - // ChatGPT said that this is better - private Translation2d deCasteljauIterative(List points, double lerpFraction) { - int n = points.size(); - List temp = new ArrayList<>(points); - - for (int level = 1; level < n; level++) { - for (int i = 0; i < n - level; i++) { - double x = (1 - lerpFraction) * temp.get(i).getX() + lerpFraction * temp.get(i + 1).getX(); - double y = (1 - lerpFraction) * temp.get(i).getY() + lerpFraction * temp.get(i + 1).getY(); - temp.set(i, new Translation2d(x, y)); // Update in place - } - } - - return temp.get(0); - } } From de944f8163c0708b17f9787147bd8f4b23a98367 Mon Sep 17 00:00:00 2001 From: Rong Date: Tue, 11 Feb 2025 20:33:26 -0800 Subject: [PATCH 6/8] Rotation follow lerp --- .../drive/SwerveBezierTrajectoryCommand.java | 25 ++++++------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index 75a2c6ff..4f4e4ddd 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -55,9 +55,14 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i allPoints.addAll(controlPoints); allPoints.add(endPoint.getTranslation()); + Rotation2d startingRotation = pose.getCurrentPose2d().getRotation(); for (int i = 1; i <= steps; i++) { double lerpFraction = i / (double) steps; - XbotSwervePoint point = new XbotSwervePoint(deCasteljauIterative(allPoints, lerpFraction), 10); + XbotSwervePoint point = new XbotSwervePoint( + deCasteljauIterative(allPoints, lerpFraction), + endPoint.getRotation().minus(startingRotation).div(lerpFraction), + 10 + ); bezierPoints.add(point); } @@ -65,7 +70,7 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i } // ChatGPT said that this is better - private Pose2d deCasteljauIterative(List points, double lerpFraction) { + private Translation2d deCasteljauIterative(List points, double lerpFraction) { int n = points.size(); List temp = new ArrayList<>(points); @@ -78,21 +83,7 @@ private Pose2d deCasteljauIterative(List points, double lerpFract } } - // After calculating the position, compute the desired rotation (angle to the next point) - Translation2d currentPosition = temp.get(0); - Translation2d nextPosition = (n > 1) ? temp.get(1) : currentPosition; // Use next point for rotation or current position - Rotation2d desiredRotation; - if (lerpFraction < 1) { - // Interpolate rotation between current and next points - desiredRotation = new Rotation2d(nextPosition.getX() - currentPosition.getX(), - nextPosition.getY() - currentPosition.getY()); - } else { - // If it's the last point, keep the rotation consistent with the end point's rotation - desiredRotation = endPoint.getRotation(); - } - - // Return the Pose2d (position + rotation) - return new Pose2d(currentPosition, desiredRotation); + return temp.get(0); } /** From 9fa81cca74c21280f64f3d6f30eb7e45390b0574 Mon Sep 17 00:00:00 2001 From: Rong Date: Tue, 11 Feb 2025 20:39:44 -0800 Subject: [PATCH 7/8] Fixed incorrect rotation calculations --- .../common/subsystems/drive/SwerveBezierTrajectoryCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index 4f4e4ddd..3396be5c 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -60,7 +60,7 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i double lerpFraction = i / (double) steps; XbotSwervePoint point = new XbotSwervePoint( deCasteljauIterative(allPoints, lerpFraction), - endPoint.getRotation().minus(startingRotation).div(lerpFraction), + startingRotation.plus((endPoint.getRotation().minus(startingRotation)).times(lerpFraction)), 10 ); bezierPoints.add(point); From 49b28cc287d074814eb4ffb759e30c5563f492f3 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Thu, 13 Feb 2025 22:34:47 -0800 Subject: [PATCH 8/8] Final clean up changes --- .../drive/SwerveBezierTrajectoryCommand.java | 47 +++++++------------ 1 file changed, 16 insertions(+), 31 deletions(-) diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java index 3396be5c..a45fff48 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java @@ -15,8 +15,9 @@ /** * A "not-so-simple" SwerveSimpleTrajectoryCommand that does a Bézier curve!~~ - * NOTE: Currently, rotation is neglected... - * IMPORTANT: This thing may get *expensive* the more control points you got + * TO USE: call setBezierConfiguration and pass it the needed params + * If you are EXTENDING this because you need some logic to compute control points: + * Remember to call setBezierConfiguration and super.initialize() */ public class SwerveBezierTrajectoryCommand extends SwerveSimpleTrajectoryCommand { @@ -48,7 +49,10 @@ public void setBezierConfiguration(List controlPoints, Pose2d end this.endPoint = endPoint; } - public void setBezierCurve(List controlPoints, Pose2d endPoint, int steps) { + /** + * Generates a Bézier curve for our command! + */ + private void setBezierCurve(List controlPoints, Pose2d endPoint, int steps) { List bezierPoints = new ArrayList<>(); List allPoints = new ArrayList<>(); allPoints.add(pose.getCurrentPose2d().getTranslation()); @@ -69,45 +73,26 @@ public void setBezierCurve(List controlPoints, Pose2d endPoint, i logic.setKeyPoints(bezierPoints); } - // ChatGPT said that this is better + /** + * Generated by some A.I., this tells us where we SHOULD be at while taking in consideration control points + * Apparently, "Iterative" potentially (not too-sure). + * @param points include: start, control points, end + * @param lerpFraction is our completion percentage + * @return our position during lerpFraction (progress of operation completion) + */ private Translation2d deCasteljauIterative(List points, double lerpFraction) { int n = points.size(); List temp = new ArrayList<>(points); - // Compute the position using de Casteljau's algorithm + // Compute the position using de Casteljau's algorithm (all we know is that it works) for (int level = 1; level < n; level++) { for (int i = 0; i < n - level; i++) { double x = (1 - lerpFraction) * temp.get(i).getX() + lerpFraction * temp.get(i + 1).getX(); double y = (1 - lerpFraction) * temp.get(i).getY() + lerpFraction * temp.get(i + 1).getY(); - temp.set(i, new Translation2d(x, y)); // Update in place + temp.set(i, new Translation2d(x, y)); } } return temp.get(0); } - - /** - * Generated by ChatGPT, this tells us where we SHOULD be at - * @param points include: start, control points, end - * @param lerpFraction is our completion percentage - * @return our position during lerpFraction (progress of operation completion) - */ - private Translation2d deCasteljau(List points, double lerpFraction) { - if (points.size() == 1) { - return points.get(0); - } - - List newPoints = new ArrayList<>(); - for (int i = 0; i < points.size() - 1; i++) { - Translation2d p1 = points.get(i); - Translation2d p2 = points.get(i + 1); - - double x = (1 - lerpFraction) * p1.getX() + lerpFraction * p2.getX(); - double y = (1 - lerpFraction) * p1.getY() + lerpFraction * p2.getY(); - - newPoints.add(new Translation2d(x, y)); - } - - return deCasteljau(newPoints, lerpFraction); - } }