clean up ScheduleGenerator

comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java

@@ -12,14 +12,14 @@
 import org.team100.lib.config.Feedforward100;
 import org.team100.lib.config.Identity;
 import org.team100.lib.config.PIDConstants;
-import org.team100.lib.geometry.GlobalAccelerationR3;
-import org.team100.lib.geometry.GlobalVelocityR3;
-import org.team100.lib.geometry.HolonomicPose2d;
+import org.team100.lib.geometry.AccelerationSE2;
+import org.team100.lib.geometry.VelocitySE2;
+import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.Level;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.LoggerFactory.ConfigLogger;
-import org.team100.lib.logging.LoggerFactory.GlobalAccelerationR3Logger;
-import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger;
+import org.team100.lib.logging.LoggerFactory.AccelerationSE2Logger;
+import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger;
 import org.team100.lib.logging.LoggerFactory.JointAccelerationsLogger;
 import org.team100.lib.logging.LoggerFactory.JointForceLogger;
 import org.team100.lib.logging.LoggerFactory.JointVelocitiesLogger;
@@ -105,8 +105,8 @@ public class CalgamesMech extends SubsystemBase implements Music, PositionSubsys
     private final JointForceLogger m_log_jointF;
 
     private final Pose2dLogger m_log_pose;
-    private final GlobalVelocityR3Logger m_log_cartesianV;
-    private final GlobalAccelerationR3Logger m_log_cartesianA;
+    private final VelocitySE2Logger m_log_cartesianV;
+    private final AccelerationSE2Logger m_log_cartesianA;
 
     private final LinearMechanism m_elevatorFront;
     private final LinearMechanism m_elevatorBack;
@@ -146,8 +146,8 @@ public CalgamesMech(
 
         LoggerFactory cartesianLog = parent.name("cartesian");
         m_log_pose = cartesianLog.pose2dLogger(Level.DEBUG, "pose");
-        m_log_cartesianV = cartesianLog.globalVelocityR3Logger(Level.DEBUG, "velocity");
-        m_log_cartesianA = cartesianLog.globalAccelerationR3Logger(Level.DEBUG, "accel");
+        m_log_cartesianV = cartesianLog.VelocitySE2Logger(Level.DEBUG, "velocity");
+        m_log_cartesianA = cartesianLog.AccelerationSE2Logger(Level.DEBUG, "accel");
 
         LoggerFactory elevatorbackLog = parent.name("elevatorBack");
         LoggerFactory elevatorfrontLog = parent.name("elevatorFront");
@@ -317,14 +317,14 @@ public ModelR3 getState() {
         EAWConfig c = getConfig();
         JointVelocities jv = getJointVelocity();
         Pose2d p = m_kinematics.forward(c);
-        GlobalVelocityR3 v = m_jacobian.forward(c, jv);
+        VelocitySE2 v = m_jacobian.forward(c, jv);
         return new ModelR3(p, v);
     }
 
     // for testing only
-    public void setVelocity(GlobalVelocityR3 v) {
+    public void setVelocity(VelocitySE2 v) {
         Pose2d pose = getState().pose();
-        GlobalAccelerationR3 a = new GlobalAccelerationR3(0, 0, 0);
+        AccelerationSE2 a = new AccelerationSE2(0, 0, 0);
         ControlR3 control = new ControlR3(pose, v, a);
 
         JointVelocities jv = m_jacobian.inverse(control.model());
@@ -490,87 +490,87 @@ public Command processorWithProfile() {
 
     public MoveAndHold homeToL1() {
         return m_transit.endless("homeToL1",
-                HolonomicPose2d.make(m_home, -1.5),
-                HolonomicPose2d.make(L2, -1.7));
+                WaypointSE2.irrotational(m_home, -1.5, 1.2),
+                WaypointSE2.irrotational(L2, -1.7, 1.2));
     }
 
     // NEVER CALL
     public Command l1ToHome() {
         return m_transit.terminal("l1ToHome",
-                HolonomicPose2d.make(L2, 1.3),
-                HolonomicPose2d.make(m_home, 1.5));
+                WaypointSE2.irrotational(L2, 1.3, 1.2),
+                WaypointSE2.irrotational(m_home, 1.5, 1.2));
     }
 
     public MoveAndHold homeToL2() {
         return m_transit.endless("homeToL2",
-                HolonomicPose2d.make(m_home, 1.5),
-                HolonomicPose2d.make(L2, 1.5));
+                WaypointSE2.irrotational(m_home, 1.5, 1.2),
+                WaypointSE2.irrotational(L2, 1.5, 1.2));
     }
 
     public Command l2ToHome() {
         return m_transit.terminal("l2ToHome",
-                HolonomicPose2d.make(L2, -1.5),
-                HolonomicPose2d.make(m_home, -1.5));
+                WaypointSE2.irrotational(L2, -1.5, 1.2),
+                WaypointSE2.irrotational(m_home, -1.5, 1.2));
     }
 
     public MoveAndHold homeToL3() {
         return m_transit.endless("homeToL3",
-                HolonomicPose2d.make(m_home, 0.8),
-                HolonomicPose2d.make(L3, 1.5));
+                WaypointSE2.irrotational(m_home, 0.8, 1.2),
+                WaypointSE2.irrotational(L3, 1.5, 1.2));
     }
 
     public Command l3ToHome() {
         return m_transit.terminal("l3ToHome",
-                HolonomicPose2d.make(L3, -1.5),
-                HolonomicPose2d.make(m_home, -2.3));
+                WaypointSE2.irrotational(L3, -1.5, 1.2),
+                WaypointSE2.irrotational(m_home, -2.3, 1.2));
     }
 
     public MoveAndHold homeToL4() {
         return m_transit.endless("homeToL4",
-                HolonomicPose2d.make(m_home, 0.1),
-                HolonomicPose2d.make(L4, 1.5));
+                WaypointSE2.irrotational(m_home, 0.1, 1.2),
+                WaypointSE2.irrotational(L4, 1.5, 1.2));
     }
 
     public MoveAndHold homeToL4Back() {
         return m_transit.endless("homeToL4",
-                HolonomicPose2d.make(m_home, 0.1),
-                HolonomicPose2d.make(L4_BACK, -1.5));
+                WaypointSE2.irrotational(m_home, 0.1, 1.2),
+                WaypointSE2.irrotational(L4_BACK, -1.5, 1.2));
     }
 
     public Command l4ToHome() {
         return m_transit.terminal("l4ToHome",
-                HolonomicPose2d.make(L4, -1.5),
-                HolonomicPose2d.make(m_home, -3));
+                WaypointSE2.irrotational(L4, -1.5, 1.2),
+                WaypointSE2.irrotational(m_home, -3, 1.2));
     }
 
     public Command l4BackToHome() {
         return m_transit.terminal("l4ToHome",
-                HolonomicPose2d.make(L4_BACK, 1.5),
-                HolonomicPose2d.make(m_home, -3));
+                WaypointSE2.irrotational(L4_BACK, 1.5, 1.2),
+                WaypointSE2.irrotational(m_home, -3, 1.2));
     }
 
     public Command homeToAlgaeL2() {
         return m_transit.endless("homeToAlgaeL2",
-                HolonomicPose2d.make(m_home, 1.5),
-                HolonomicPose2d.make(ALGAE_L2, 1.5));
+                WaypointSE2.irrotational(m_home, 1.5, 1.2),
+                WaypointSE2.irrotational(ALGAE_L2, 1.5, 1.2));
     }
 
     public Command homeToAlgaeL3() {
         return m_transit.endless("homeToAlgaeL3",
-                HolonomicPose2d.make(m_home, 0),
-                HolonomicPose2d.make(ALGAE_L3, 1.5));
+                WaypointSE2.irrotational(m_home, 0, 1.2),
+                WaypointSE2.irrotational(ALGAE_L3, 1.5, 1.2));
     }
 
     public Command algaeL2ToHome() {
         return m_transit.terminal("homeToAlgaeL2",
-                HolonomicPose2d.make(ALGAE_L2, -1.0),
-                HolonomicPose2d.make(m_home, Math.PI));
+                WaypointSE2.irrotational(ALGAE_L2, -1.0, 1.2),
+                WaypointSE2.irrotational(m_home, Math.PI, 1.2));
     }
 
     public Command algaeL3ToHome() {
         return m_transit.terminal("homeToAlgaeL3",
-                HolonomicPose2d.make(ALGAE_L3, -1.0),
-                HolonomicPose2d.make(m_home, Math.PI));
+                WaypointSE2.irrotational(ALGAE_L3, -1.0, 1.2),
+                WaypointSE2.irrotational(m_home, Math.PI, 1.2));
     }
 
     /**
@@ -598,14 +598,14 @@ public Command algaeReefExit(Supplier<ScoringLevel> level) {
      */
     public MoveAndHold homeToBarge() {
         return m_transit.endless("homeToBarge",
-                HolonomicPose2d.make(m_home, 0),
-                HolonomicPose2d.make(BARGE, -1));
+                WaypointSE2.irrotational(m_home, 0, 1.2),
+                WaypointSE2.irrotational(BARGE, -1, 1.2));
     }
 
     public MoveAndHold bargeToHome() {
         return m_transit.endless("bargeToHome",
-                HolonomicPose2d.make(BARGE, 2.5),
-                HolonomicPose2d.make(m_home, Math.PI));
+                WaypointSE2.irrotational(BARGE, 2.5, 1.2),
+                WaypointSE2.irrotational(m_home, Math.PI, 1.2));
 
     }
 
@@ -672,8 +672,8 @@ private void logConfig(EAWConfig c, JointVelocities jv, JointAccelerations ja, J
         m_log_jointA.log(() -> ja);
         m_log_jointF.log(() -> jf);
         Pose2d p = m_kinematics.forward(c);
-        GlobalVelocityR3 v = m_jacobian.forward(c, jv);
-        GlobalAccelerationR3 a = m_jacobian.forwardA(c, jv, ja);
+        VelocitySE2 v = m_jacobian.forward(c, jv);
+        AccelerationSE2 a = m_jacobian.forwardA(c, jv, ja);
         m_log_pose.log(() -> p);
         m_log_cartesianV.log(() -> v);
         m_log_cartesianA.log(() -> a);

comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java

@@ -4,16 +4,13 @@
 import java.util.List;
 
 import org.team100.lib.commands.MoveAndHold;
-import org.team100.lib.geometry.HolonomicPose2d;
+import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.prr.AnalyticalJacobian;
 import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics;
-import org.team100.lib.subsystems.prr.JointAccelerations;
-import org.team100.lib.subsystems.prr.JointVelocities;
 import org.team100.lib.subsystems.r3.commands.GoToPosePosition;
 import org.team100.lib.trajectory.TrajectoryPlanner;
 import org.team100.lib.trajectory.timing.ConstantConstraint;
-import org.team100.lib.trajectory.timing.JointConstraint;
 import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TorqueConstraint;
 import org.team100.lib.trajectory.timing.YawRateConstraint;
@@ -22,7 +19,6 @@
 
 /** Make a trajectory from the start to the end and follow it. */
 public class MechTrajectories extends Command {
-    private static final boolean USE_JOINT_CONSTRAINT = false;
 
     private final LoggerFactory m_log;
     private final CalgamesMech m_subsystem;
@@ -36,21 +32,12 @@ public MechTrajectories(
         m_log = parent.type(this);
         m_subsystem = mech;
         List<TimingConstraint> c = new ArrayList<>();
-        if (USE_JOINT_CONSTRAINT) {
-            // This is experimental, don't use it.
-            c.add(new JointConstraint(
-                    k,
-                    j,
-                    new JointVelocities(10, 10, 10),
-                    new JointAccelerations(10, 10, 10)));
 
-        } else {
-            // These are known to work, but suboptimal.
-            c.add(new ConstantConstraint(m_log, 10, 5));
-            c.add(new YawRateConstraint(m_log, 10, 5));
-            // This is new
-            c.add(new TorqueConstraint(20));
-        }
+        // These are known to work, but suboptimal.
+        c.add(new ConstantConstraint(m_log, 10, 5));
+        c.add(new YawRateConstraint(m_log, 10, 5));
+        // This is new
+        c.add(new TorqueConstraint(20));
 
         // ALERT!
         // The parameters here used to be double these values;
@@ -62,22 +49,22 @@ public MechTrajectories(
     }
 
     /** A command that goes from the start to the end and then finishes. */
-    public Command terminal(String name, HolonomicPose2d start, HolonomicPose2d end) {
+    public Command terminal(String name, WaypointSE2 start, WaypointSE2 end) {
 
         /** Use the start course and ignore the start pose for now */
         MoveAndHold f = new GoToPosePosition(
-                m_log, m_subsystem, start.course(), end, m_planner);
+                m_log, m_subsystem, start.course().toRotation(), end, m_planner);
         return f
                 .until(f::isDone)
                 .withName(name);
     }
 
     /** A command that goes from the start to the end and then waits forever. */
-    public MoveAndHold endless(String name, HolonomicPose2d start, HolonomicPose2d end) {
+    public MoveAndHold endless(String name, WaypointSE2 start, WaypointSE2 end) {
 
         /** Use the start course and ignore the start pose for now */
         GoToPosePosition c = new GoToPosePosition(
-                m_log, m_subsystem, start.course(), end, m_planner);
+                m_log, m_subsystem, start.course().toRotation(), end, m_planner);
         c.setName(name);
         return c;
 

comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java

@@ -0,0 +1,67 @@
+package org.team100.frc2025.Swerve.Auto;
+
+import java.util.List;
+import java.util.function.Function;
+
+import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.WaypointSE2;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
+import org.team100.lib.trajectory.Trajectory100;
+import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.timing.TimingConstraintFactory;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/** big looping trajectory for testing */
+public class BigLoop implements Function<Pose2d, Trajectory100> {
+    private final TrajectoryPlanner m_planner;
+
+    public BigLoop(
+            LoggerFactory log,
+            SwerveKinodynamics kinodynamics) {
+        m_planner = new TrajectoryPlanner(
+                new TimingConstraintFactory(kinodynamics).auto(log.type(this)));
+    }
+
+    @Override
+    public Trajectory100 apply(Pose2d p0) {
+        // field-relative
+        Pose2d p1 = new Pose2d(
+                p0.getX() + 2,
+                p0.getY() - 2,
+                p0.getRotation());
+        Pose2d p2 = new Pose2d(
+                p0.getX() + 4,
+                p0.getY(),
+                p0.getRotation());
+        Pose2d p3 = new Pose2d(
+                p0.getX() + 2,
+                p0.getY() + 2,
+                p0.getRotation());
+        List<WaypointSE2> waypoints = List.of(
+                new WaypointSE2(
+                        p0,
+                        new DirectionSE2(1, 0, 0),
+                        1),
+                new WaypointSE2(
+                        p1,
+                        new DirectionSE2(1, 0, 0),
+                        1),
+                new WaypointSE2(
+                        p2,
+                        new DirectionSE2(0, 1, 0),
+                        1),
+                new WaypointSE2(
+                        p3,
+                        new DirectionSE2(-1, 0, 0),
+                        1),
+                new WaypointSE2(
+                        p0,
+                        new DirectionSE2(-1, 0, 0),
+                        1)
+        //
+        );
+        return m_planner.restToRest(waypoints);
+    }
+}

comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java

@@ -6,7 +6,8 @@
 
 import org.team100.lib.field.FieldConstants;
 import org.team100.lib.field.FieldConstants.CoralStation;
-import org.team100.lib.geometry.HolonomicPose2d;
+import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.trajectory.Trajectory100;
@@ -51,9 +52,9 @@ public Trajectory100 apply(Pose2d currentPose) {
         Rotation2d newInitialSpline = FieldConstants.calculateDeltaSpline(
                 courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust);
 
-        List<HolonomicPose2d> waypoints = new ArrayList<>();
-        waypoints.add(new HolonomicPose2d(currTranslation, currentPose.getRotation(), newInitialSpline));
-        waypoints.add(new HolonomicPose2d(goal.getTranslation(), goal.getRotation(), courseToGoal));
+        List<WaypointSE2> waypoints = new ArrayList<>();
+        waypoints.add(new WaypointSE2(currentPose, DirectionSE2.irrotational(newInitialSpline), 1));
+        waypoints.add(new WaypointSE2(goal, DirectionSE2.irrotational(courseToGoal), 1));
 
         return m_planner.restToRest(waypoints);
     }