From c7e98ed1be0e45d72ae8451a9c881c6d42b266ea Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sun, 14 Dec 2025 19:19:28 -0800 Subject: [PATCH 01/42] working on 3d splines --- .../frc2025/CalgamesArm/CalgamesMech.java | 26 ++-- .../frc2025/CalgamesArm/MechTrajectories.java | 4 +- .../frc2025/Swerve/Auto/GoToCoralStation.java | 13 +- .../frc2025/Swerve/ManualWithBargeAssist.java | 10 +- .../Swerve/ManualWithProfiledReefLock.java | 10 +- .../org/team100/frc2025/robot/Prewarmer.java | 9 +- .../CalgamesArm/TrajectoryJointTest.java | 8 +- .../frc2025/CalgamesArm/TrajectoryTest.java | 52 +++++-- .../lib/controller/r3/ControllerR3.java | 4 +- .../lib/controller/r3/ControllerR3Base.java | 36 ++--- .../r3/FeedforwardControllerR3.java | 16 +- .../controller/r3/FullStateControllerR3.java | 44 +++--- .../lib/controller/r3/NullControllerR3.java | 12 +- .../team100/lib/geometry/AccelerationSE2.java | 77 ++++++++++ .../{GlobalDeltaR3.java => DeltaSE2.java} | 33 +++-- .../org/team100/lib/geometry/DirectionR2.java | 40 +++++ .../org/team100/lib/geometry/DirectionR3.java | 43 ++++++ .../team100/lib/geometry/DirectionSE2.java | 49 ++++++ .../team100/lib/geometry/DirectionSE3.java | 46 ++++++ .../team100/lib/geometry/GeometryUtil.java | 11 +- .../lib/geometry/GlobalAccelerationR3.java | 79 ---------- .../team100/lib/geometry/GlobalJerkR3.java | 15 -- .../lib/geometry/GlobalVelocity3d.java | 4 +- .../lib/geometry/GlobalVelocityR2.java | 2 +- .../team100/lib/geometry/HolonomicPose2d.java | 27 +++- .../team100/lib/geometry/HolonomicPose3d.java | 11 +- .../org/team100/lib/geometry/JerkSE2.java | 20 +++ .../lib/geometry/Pose2dWithMotion.java | 3 +- .../java/org/team100/lib/geometry/README.md | 6 +- ...GlobalVelocityR3.java => VelocitySE2.java} | 65 ++++---- .../lib/localization/InterpolationRecord.java | 8 +- .../team100/lib/localization/ManualPose.java | 4 +- .../lib/localization/OdometryUpdater.java | 8 +- .../lib/localization/SwerveHistory.java | 6 +- .../team100/lib/logging/LoggerFactory.java | 38 ++--- .../java/org/team100/lib/state/ControlR3.java | 20 +-- .../java/org/team100/lib/state/ModelR3.java | 10 +- .../subsystems/mecanum/MecanumDrive100.java | 16 +- .../mecanum/commands/ManualMecanum.java | 4 +- .../subsystems/prr/AnalyticalJacobian.java | 18 +-- .../team100/lib/subsystems/prr/Jacobian.java | 8 +- .../subsystems/r3/VelocitySubsystemR3.java | 4 +- ...veToPoseWithTrajectoryAndExitVelocity.java | 13 +- .../r3/commands/GoToPosePosition.java | 4 +- .../r3/commands/ManualPosition.java | 4 +- .../r3/commands/VelocityFeedforwardOnly.java | 4 +- .../helper/PositionReferenceControllerR3.java | 4 +- .../helper/ReferenceControllerR3Base.java | 6 +- .../helper/VelocityReferenceControllerR3.java | 4 +- .../swerve/SwerveDriveSubsystem.java | 12 +- .../commands/manual/DriveManuallySimple.java | 8 +- .../commands/manual/FieldRelativeAdapter.java | 6 +- .../commands/manual/FieldRelativeDriver.java | 8 +- .../manual/ManualFieldRelativeSpeeds.java | 12 +- .../manual/ManualWithFullStateHeading.java | 8 +- .../manual/ManualWithOptionalTargetLock.java | 8 +- .../manual/ManualWithProfiledHeading.java | 10 +- .../commands/manual/ManualWithTargetLock.java | 12 +- .../kinodynamics/SwerveKinodynamics.java | 8 +- .../FieldRelativeAccelerationLimiter.java | 28 ++-- .../limiter/FieldRelativeCapsizeLimiter.java | 30 ++-- .../limiter/FieldRelativeVelocityLimiter.java | 24 +-- .../kinodynamics/limiter/SwerveDeadband.java | 6 +- .../kinodynamics/limiter/SwerveLimiter.java | 16 +- .../kinodynamics/limiter/SwerveUtil.java | 14 +- .../subsystems/tank/commands/TankManual.java | 4 +- .../lib/subsystems/test/OffsetDrivetrain.java | 10 +- .../test/OffsetDrivetrainWithBoost.java | 12 +- .../lib/subsystems/test/OffsetUtil.java | 14 +- .../subsystems/test/TrivialDrivetrain.java | 10 +- .../org/team100/lib/targeting/TargetUtil.java | 4 +- .../lib/trajectory/TrajectoryPlanner.java | 38 +++-- .../lib/trajectory/path/PathFactory.java | 11 +- .../path/spline/HolonomicSpline.java | 62 ++++---- .../path/spline/HolonomicSpline3d.java | 93 +++++++++--- .../trajectory/timing/DiamondConstraint.java | 2 +- .../trajectory/timing/JointConstraint.java | 16 +- .../r3/FeedforwardControllerR3Test.java | 9 +- .../r3/FullStateControllerR3Test.java | 100 ++++++------- .../r3/ReferenceControllerR3Test.java | 17 ++- .../SwerveDrivePoseEstimator100Test.java | 4 +- .../lib/profile/r3/HolonomicProfileTest.java | 10 +- .../prr/AnalyticalJacobianTest.java | 44 +++--- .../lib/subsystems/prr/JacobianTest.java | 10 +- .../lib/subsystems/r3/MockSubsystemR3.java | 8 +- .../subsystems/swerve/SwerveControlTest.java | 8 +- .../subsystems/swerve/SwerveModelTest.java | 8 +- .../manual/ManualFieldRelativeSpeedsTest.java | 6 +- .../ManualWithFullStateHeadingTest.java | 30 ++-- .../manual/ManualWithProfiledHeadingTest.java | 40 ++--- .../kinodynamics/SwerveKinodynamicsTest.java | 10 +- .../FieldRelativeAccelerationLimiterTest.java | 40 ++--- .../FieldRelativeCapsizeLimiterTest.java | 32 ++-- .../FieldRelativeVelocityLimiterTest.java | 50 +++---- .../limiter/SimulatedDrivingTest.java | 14 +- .../limiter/SwerveDeadbandTest.java | 10 +- .../limiter/SwerveLimiterTest.java | 70 ++++----- .../kinodynamics/limiter/SwerveUtilTest.java | 22 +-- .../swerve/state/FieldRelativeDeltaTest.java | 8 +- .../team100/lib/targeting/TargetUtilTest.java | 20 +-- .../lib/trajectory/Trajectory100Test.java | 58 ++++++-- .../lib/trajectory/TrajectoryPlannerTest.java | 45 ++++-- .../lib/trajectory/path/Path100Test.java | 9 +- .../lib/trajectory/path/PathFactoryTest.java | 106 ++++++++++--- .../path/spline/HolonomicSpline3dTest.java | 13 +- .../path/spline/HolonomicSplineTest.java | 140 ++++++++++++++---- .../spline/QuinticHermiteOptimizerTest.java | 97 +++++++++--- .../trajectory/path/spline/SplineR1Test.java | 19 ++- .../timing/ScheduleGeneratorTest.java | 11 +- 109 files changed, 1564 insertions(+), 948 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java rename lib/src/main/java/org/team100/lib/geometry/{GlobalDeltaR3.java => DeltaSE2.java} (69%) create mode 100644 lib/src/main/java/org/team100/lib/geometry/DirectionR2.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/DirectionR3.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/DirectionSE3.java delete mode 100644 lib/src/main/java/org/team100/lib/geometry/GlobalAccelerationR3.java delete mode 100644 lib/src/main/java/org/team100/lib/geometry/GlobalJerkR3.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/JerkSE2.java rename lib/src/main/java/org/team100/lib/geometry/{GlobalVelocityR3.java => VelocitySE2.java} (51%) diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java index d48920018..13f5ace62 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java +++ b/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.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.HolonomicPose2d; 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()); @@ -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); diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java index d2cd8199e..ca02cae4d 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -66,7 +66,7 @@ public Command terminal(String name, HolonomicPose2d start, HolonomicPose2d 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); @@ -77,7 +77,7 @@ public MoveAndHold endless(String name, HolonomicPose2d start, HolonomicPose2d e /** 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; diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index d758dd5de..8ce44cfed 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -6,6 +6,7 @@ import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.CoralStation; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; @@ -52,8 +53,16 @@ public Trajectory100 apply(Pose2d currentPose) { courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust); List waypoints = new ArrayList<>(); - waypoints.add(new HolonomicPose2d(currTranslation, currentPose.getRotation(), newInitialSpline)); - waypoints.add(new HolonomicPose2d(goal.getTranslation(), goal.getRotation(), courseToGoal)); + waypoints.add( + new HolonomicPose2d( + currTranslation, + currentPose.getRotation(), + DirectionSE2.fromRotation(newInitialSpline))); + waypoints.add( + new HolonomicPose2d( + goal.getTranslation(), + goal.getRotation(), + DirectionSE2.fromRotation(courseToGoal))); return m_planner.restToRest(waypoints); } diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java index c774f0073..e498ea574 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java @@ -4,7 +4,7 @@ import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -112,10 +112,10 @@ public void reset(ModelR3 state) { * @return feasible field-relative velocity in m/s and rad/s */ @Override - public GlobalVelocityR3 apply( + public VelocitySE2 apply( final ModelR3 state, final Velocity twist1_1) { - final GlobalVelocityR3 control = clipAndScale(twist1_1); + final VelocitySE2 control = clipAndScale(twist1_1); final double currentVelocity = state.velocity().norm(); @@ -169,7 +169,7 @@ public GlobalVelocityR3 apply( thetaFF + thetaFB, -m_swerveKinodynamics.getMaxAngleSpeedRad_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); - GlobalVelocityR3 twistWithSnapM_S = new GlobalVelocityR3(control.x(), control.y(), omega); + VelocitySE2 twistWithSnapM_S = new VelocitySE2(control.x(), control.y(), omega); m_log_mode.log(() -> "snap"); m_log_goal_theta.log(m_goal::getRadians); @@ -185,7 +185,7 @@ public GlobalVelocityR3 apply( * Slow down when driving toward the barge scoring location, so you don't hit * it. */ - public GlobalVelocityR3 clipAndScale(Velocity twist1_1) { + public VelocitySE2 clipAndScale(Velocity twist1_1) { // clip the input to the unit circle final Velocity clipped = twist1_1.clip(1.0); diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java index c1238cb0e..153bee59b 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java @@ -5,7 +5,7 @@ import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.field.FieldConstants; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -102,10 +102,10 @@ public void reset(ModelR3 state) { * @return feasible field-relative velocity in m/s and rad/s */ @Override - public GlobalVelocityR3 apply( + public VelocitySE2 apply( final ModelR3 state, final Velocity twist1_1) { - final GlobalVelocityR3 control = clipAndScale(twist1_1); + final VelocitySE2 control = clipAndScale(twist1_1); if (!m_lockToReef.get()) { // not locked, just return the input. @@ -145,7 +145,7 @@ public GlobalVelocityR3 apply( -m_swerveKinodynamics.getMaxAngleSpeedRad_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); - GlobalVelocityR3 twistWithSnapM_S = new GlobalVelocityR3(control.x(), control.y(), omega); + VelocitySE2 twistWithSnapM_S = new VelocitySE2(control.x(), control.y(), omega); m_log_snap_mode.log(() -> true); m_log_goal_theta.log(m_goal::getRadians); @@ -157,7 +157,7 @@ public GlobalVelocityR3 apply( return twistWithSnapM_S; } - public GlobalVelocityR3 clipAndScale(Velocity twist1_1) { + public VelocitySE2 clipAndScale(Velocity twist1_1) { // clip the input to the unit circle final Velocity clipped = twist1_1.clip(1.0); diff --git a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java index 2336ede87..ea7063a74 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java @@ -4,7 +4,8 @@ import java.util.List; import org.team100.lib.coherence.Takt; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; @@ -31,17 +32,17 @@ public static void init(Machinery machinery) { waypoints.add(new HolonomicPose2d( new Translation2d(), Rotation2d.kZero, - Rotation2d.kZero)); + DirectionSE2.TO_X)); waypoints.add(new HolonomicPose2d( new Translation2d(1, 0), Rotation2d.kZero, - Rotation2d.kZero)); + DirectionSE2.TO_X)); TrajectoryPlanner planner = new TrajectoryPlanner( new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger)); planner.restToRest(waypoints); // Exercise the drive motors. - machinery.m_drive.setVelocity(new GlobalVelocityR3(0, 0, 0)); + machinery.m_drive.setVelocity(new VelocitySE2(0, 0, 0)); // Exercise some mechanism commands. Command c = machinery.m_mech.homeToL4(); diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java index e125487b1..19e23c96f 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -3,8 +3,8 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -62,8 +62,8 @@ void homeToL4() { for (double tt = 0; tt < t.duration(); tt += 0.02) { ControlR3 m = ControlR3.fromTimedPose(t.sample(tt)); Pose2d p = m.pose(); - GlobalVelocityR3 v = m.velocity(); - GlobalAccelerationR3 a = m.acceleration(); + VelocitySE2 v = m.velocity(); + AccelerationSE2 a = m.acceleration(); EAWConfig q = k.inverse(p); JointVelocities jv = J.inverse(m.model()); JointAccelerations ja = J.inverseA(m); diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java index c09eb12be..808907c52 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java @@ -2,6 +2,7 @@ import java.util.List; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -57,9 +58,14 @@ void testCurved() throws InterruptedException { new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); List waypoints = List.of( - new HolonomicPose2d(new Translation2d(1, 1), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(9, 9), new Rotation2d(-Math.PI / 2), - new Rotation2d(Math.PI / 2))); + new HolonomicPose2d( + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(9, 9), + new Rotation2d(-Math.PI / 2), + DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); new TrajectoryPlotter(0.1).plot(t, "simple"); Thread.sleep(5000); @@ -78,11 +84,17 @@ void testMultipleWaypoints() throws InterruptedException { TrajectoryPlanner p = new TrajectoryPlanner(c); List waypoints = List.of( new HolonomicPose2d( - new Translation2d(1, 1), new Rotation2d(), new Rotation2d()), + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_X), new HolonomicPose2d( - new Translation2d(5, 5), new Rotation2d(-2), new Rotation2d()), + new Translation2d(5, 5), + new Rotation2d(-2), + DirectionSE2.TO_X), new HolonomicPose2d( - new Translation2d(9, 9), new Rotation2d(-Math.PI / 2), new Rotation2d(Math.PI / 2))); + new Translation2d(9, 9), + new Rotation2d(-Math.PI / 2), + DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); new TrajectoryPlotter(0.1).plot(t, "simple"); Thread.sleep(5000); @@ -98,12 +110,18 @@ void testPickupToPlace() { List waypoints = List.of( // pickup new HolonomicPose2d( - new Translation2d(1, 0.1), new Rotation2d(-Math.PI), new Rotation2d(Math.PI / 2)), + new Translation2d(1, 0.1), + new Rotation2d(-Math.PI), + DirectionSE2.TO_Y), // place for gateway point? new HolonomicPose2d( - new Translation2d(3, 7), new Rotation2d(Math.PI / 2), new Rotation2d()), + new Translation2d(3, 7), + new Rotation2d(Math.PI / 2), + DirectionSE2.TO_X), new HolonomicPose2d( - new Translation2d(6, 9), new Rotation2d(-((7 * Math.PI) / 36)), new Rotation2d(Math.PI / 2))); + new Translation2d(6, 9), + new Rotation2d(-((7 * Math.PI) / 36)), + DirectionSE2.TO_Y)); @SuppressWarnings("unused") Trajectory100 t = p.restToRest(waypoints); // TrajectoryPlotter.plot(t, "simple"); @@ -118,15 +136,23 @@ void testSingularityDemo() { List waypoints = List.of( // pickup new HolonomicPose2d( - new Translation2d(1, 0.1), new Rotation2d(-Math.PI), new Rotation2d(Math.PI / 2)), + new Translation2d(1, 0.1), + new Rotation2d(-Math.PI), + DirectionSE2.TO_Y), // place for gateway point new HolonomicPose2d( - new Translation2d(0.75, 3), new Rotation2d(-Math.PI), new Rotation2d(Math.PI / 2)), + new Translation2d(0.75, 3), + new Rotation2d(-Math.PI), + DirectionSE2.TO_Y), // place for gateway point new HolonomicPose2d( - new Translation2d(3, 7), new Rotation2d(Math.PI / 2), new Rotation2d()), + new Translation2d(3, 7), + new Rotation2d(Math.PI / 2), + DirectionSE2.TO_X), new HolonomicPose2d( - new Translation2d(6, 9), new Rotation2d(-((7 * Math.PI) / 36)), new Rotation2d(Math.PI / 2))); + new Translation2d(6, 9), + new Rotation2d(-((7 * Math.PI) / 36)), + DirectionSE2.TO_Y)); @SuppressWarnings("unused") Trajectory100 t = p.restToRest(waypoints); // TrajectoryPlotter.plot(t, "simple"); diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java index 798a42f1a..cadc6d60c 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java @@ -1,6 +1,6 @@ package org.team100.lib.controller.r3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ControlR3; import org.team100.lib.state.ModelR3; @@ -27,7 +27,7 @@ public interface ControllerR3 { * Give this to SwerveDriveSubsystem.driveInFieldCoords() or something * similar. */ - GlobalVelocityR3 calculate( + VelocitySE2 calculate( ModelR3 measurement, ModelR3 currentReference, ControlR3 nextReference); diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java index 250991e4b..b2fe643e2 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java @@ -1,14 +1,14 @@ package org.team100.lib.controller.r3; -import org.team100.lib.geometry.GlobalDeltaR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.DeltaSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; import org.team100.lib.logging.LoggerFactory.ControlR3Logger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.logging.LoggerFactory.GlobaDeltaR3Logger; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.logging.LoggerFactory.ModelR3Logger; import org.team100.lib.state.ControlR3; import org.team100.lib.state.ModelR3; @@ -27,7 +27,7 @@ public abstract class ControllerR3Base implements ControllerR3 { private final ControlR3Logger m_log_nextReference; private final GlobaDeltaR3Logger m_log_position_error; - private final GlobalVelocityR3Logger m_log_velocity_error; + private final VelocitySE2Logger m_log_velocity_error; /** Error in cartesian distance, i.e. hypot(x, y). */ private final DoubleLogger m_log_cartesianPositionError; /** Error in cartesian velocity, i.e. hypot(vx, vy). */ @@ -43,9 +43,9 @@ public abstract class ControllerR3Base implements ControllerR3 { private final double m_omegaTolerance; /** The position error calculated in the most-recent call to calculate. */ - private GlobalDeltaR3 m_positionError; + private DeltaSE2 m_positionError; /** The velocity error calculated in the most-recent call to calculate. */ - private GlobalVelocityR3 m_velocityError; + private VelocitySE2 m_velocityError; public ControllerR3Base( LoggerFactory parent, @@ -59,8 +59,8 @@ public ControllerR3Base( m_log_currentReference = log.modelR3Logger(Level.DEBUG, "current reference"); m_log_nextReference = log.controlR3Logger(Level.DEBUG, "next reference"); - m_log_position_error = log.globalDeltaR3Logger(Level.TRACE, "position error"); - m_log_velocity_error = log.globalVelocityR3Logger(Level.TRACE, "velocity error"); + m_log_position_error = log.DeltaSE2Logger(Level.TRACE, "position error"); + m_log_velocity_error = log.VelocitySE2Logger(Level.TRACE, "velocity error"); m_log_cartesianPositionError = log.doubleLogger(Level.TRACE, "cartesian position error"); m_log_cartesianVelocityError = log.doubleLogger(Level.TRACE, "cartesian velocity error "); @@ -75,7 +75,7 @@ public ControllerR3Base( } @Override - public GlobalVelocityR3 calculate( + public VelocitySE2 calculate( ModelR3 measurement, ModelR3 currentReference, ControlR3 nextReference) { @@ -93,9 +93,9 @@ public GlobalVelocityR3 calculate( * @param nextReference velocity for dt from now * @returns control output for the period during dt */ - public abstract GlobalVelocityR3 calculate100( - GlobalDeltaR3 positionError, - GlobalVelocityR3 velocityError, + public abstract VelocitySE2 calculate100( + DeltaSE2 positionError, + VelocitySE2 velocityError, ControlR3 nextReference); /** @@ -111,7 +111,7 @@ public boolean atReference() { } /** True if cartesian and rotation position errors are within tolerance. */ - boolean positionOK(GlobalDeltaR3 positionError) { + boolean positionOK(DeltaSE2 positionError) { double cartesian = positionError.getTranslation().getNorm(); m_log_cartesianPositionError.log(() -> cartesian); double rotation = Math.abs(positionError.getRotation().getRadians()); @@ -121,7 +121,7 @@ boolean positionOK(GlobalDeltaR3 positionError) { } /** True if cartesian and rotation velocity errors are within tolerance. */ - boolean velocityOK(GlobalVelocityR3 velocityError) { + boolean velocityOK(VelocitySE2 velocityError) { double cartesian = velocityError.norm(); m_log_cartesianVelocityError.log(() -> cartesian); double rotation = Math.abs(velocityError.angle().orElse(Rotation2d.kZero).getRadians()); @@ -133,8 +133,8 @@ boolean velocityOK(GlobalVelocityR3 velocityError) { /** * Wraps heading. */ - GlobalDeltaR3 positionError(ModelR3 measurement, ModelR3 currentReference) { - GlobalDeltaR3 err = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); + DeltaSE2 positionError(ModelR3 measurement, ModelR3 currentReference) { + DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); m_log_position_error.log(() -> err); return err; } @@ -142,8 +142,8 @@ GlobalDeltaR3 positionError(ModelR3 measurement, ModelR3 currentReference) { /** * Velocity does not wrap. */ - GlobalVelocityR3 velocityError(ModelR3 measurement, ModelR3 currentReference) { - GlobalVelocityR3 err = currentReference.velocity().minus(measurement.velocity()); + VelocitySE2 velocityError(ModelR3 measurement, ModelR3 currentReference) { + VelocitySE2 err = currentReference.velocity().minus(measurement.velocity()); m_log_velocity_error.log(() -> err); return err; } diff --git a/lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java b/lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java index 1d44fecfa..10afa4b65 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java @@ -1,10 +1,10 @@ package org.team100.lib.controller.r3; -import org.team100.lib.geometry.GlobalDeltaR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.DeltaSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.state.ControlR3; /** @@ -12,7 +12,7 @@ * is appropriate if feedback control is outboard. */ public class FeedforwardControllerR3 extends ControllerR3Base { - private final GlobalVelocityR3Logger m_log_u_FF; + private final VelocitySE2Logger m_log_u_FF; public FeedforwardControllerR3( LoggerFactory parent, @@ -22,14 +22,14 @@ public FeedforwardControllerR3( double omegaTolerance) { super(parent, xTolerance, thetaTolerance, xDotTolerance, omegaTolerance); LoggerFactory log = parent.type(this); - m_log_u_FF = log.globalVelocityR3Logger(Level.TRACE, "feedforward"); + m_log_u_FF = log.VelocitySE2Logger(Level.TRACE, "feedforward"); } @Override - public GlobalVelocityR3 calculate100( - GlobalDeltaR3 positionError, - GlobalVelocityR3 velocityError, + public VelocitySE2 calculate100( + DeltaSE2 positionError, + VelocitySE2 velocityError, ControlR3 nextReference) { m_log_u_FF.log(() -> nextReference.velocity()); return nextReference.velocity(); diff --git a/lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java b/lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java index 8e79240a9..d18df6b9f 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java @@ -1,10 +1,10 @@ package org.team100.lib.controller.r3; -import org.team100.lib.geometry.GlobalDeltaR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.DeltaSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.state.ControlR3; /** @@ -12,9 +12,9 @@ */ public class FullStateControllerR3 extends ControllerR3Base { - private final GlobalVelocityR3Logger m_log_u_FF; - private final GlobalVelocityR3Logger m_log_u_FB; - private final GlobalVelocityR3Logger m_log_u_VFB; + private final VelocitySE2Logger m_log_u_FF; + private final VelocitySE2Logger m_log_u_FB; + private final VelocitySE2Logger m_log_u_VFB; private final double m_kPCart; private final double m_kPTheta; @@ -34,9 +34,9 @@ public FullStateControllerR3( super(parent, xTolerance, thetaTolerance, xDotTolerance, omegaTolerance); LoggerFactory log = parent.type(this); - m_log_u_FF = log.globalVelocityR3Logger(Level.TRACE, "feedforward"); - m_log_u_FB = log.globalVelocityR3Logger(Level.TRACE, "position feedback"); - m_log_u_VFB = log.globalVelocityR3Logger(Level.TRACE, "velocity feedback"); + m_log_u_FF = log.VelocitySE2Logger(Level.TRACE, "feedforward"); + m_log_u_FB = log.VelocitySE2Logger(Level.TRACE, "position feedback"); + m_log_u_VFB = log.VelocitySE2Logger(Level.TRACE, "velocity feedback"); m_kPCart = kPCart; m_kPTheta = kPTheta; @@ -45,12 +45,12 @@ public FullStateControllerR3( } @Override - public GlobalVelocityR3 calculate100( - GlobalDeltaR3 positionError, - GlobalVelocityR3 velocityError, + public VelocitySE2 calculate100( + DeltaSE2 positionError, + VelocitySE2 velocityError, ControlR3 nextReference) { - GlobalVelocityR3 u_FF = feedforward(nextReference); - GlobalVelocityR3 u_FB = fullFeedback(positionError, velocityError); + VelocitySE2 u_FF = feedforward(nextReference); + VelocitySE2 u_FB = fullFeedback(positionError, velocityError); return u_FF.plus(u_FB); } @@ -58,22 +58,22 @@ public GlobalVelocityR3 calculate100( // // package-private for testing - GlobalVelocityR3 feedforward(ControlR3 nextReference) { + VelocitySE2 feedforward(ControlR3 nextReference) { m_log_u_FF.log(() -> nextReference.velocity()); return nextReference.velocity(); } - GlobalVelocityR3 fullFeedback(GlobalDeltaR3 positionError, GlobalVelocityR3 velocityError) { - GlobalVelocityR3 u_XFB = positionFeedback(positionError); - GlobalVelocityR3 u_VFB = velocityFeedback(velocityError); + VelocitySE2 fullFeedback(DeltaSE2 positionError, VelocitySE2 velocityError) { + VelocitySE2 u_XFB = positionFeedback(positionError); + VelocitySE2 u_VFB = velocityFeedback(velocityError); return u_XFB.plus(u_VFB); } /** * Returns position feedback proportional to position error. */ - GlobalVelocityR3 positionFeedback(GlobalDeltaR3 positionError) { - GlobalVelocityR3 u_FB = new GlobalVelocityR3( + VelocitySE2 positionFeedback(DeltaSE2 positionError) { + VelocitySE2 u_FB = new VelocitySE2( m_kPCart * positionError.getX(), m_kPCart * positionError.getY(), m_kPTheta * positionError.getRotation().getRadians()); @@ -84,8 +84,8 @@ GlobalVelocityR3 positionFeedback(GlobalDeltaR3 positionError) { /** * Returns velocity feedback proportional to velocity error. */ - GlobalVelocityR3 velocityFeedback(GlobalVelocityR3 velocityError) { - GlobalVelocityR3 u_VFB = new GlobalVelocityR3( + VelocitySE2 velocityFeedback(VelocitySE2 velocityError) { + VelocitySE2 u_VFB = new VelocitySE2( m_kPCartV * velocityError.x(), m_kPCartV * velocityError.y(), m_kPThetaV * velocityError.theta()); diff --git a/lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java b/lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java index 00c23b074..82fad0df0 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java @@ -1,7 +1,7 @@ package org.team100.lib.controller.r3; -import org.team100.lib.geometry.GlobalDeltaR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.DeltaSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.state.ControlR3; @@ -21,11 +21,11 @@ public NullControllerR3( } @Override - public GlobalVelocityR3 calculate100( - GlobalDeltaR3 positionError, - GlobalVelocityR3 velocityError, + public VelocitySE2 calculate100( + DeltaSE2 positionError, + VelocitySE2 velocityError, ControlR3 nextReference) { - return GlobalVelocityR3.ZERO; + return VelocitySE2.ZERO; } } diff --git a/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java b/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java new file mode 100644 index 000000000..33053e034 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java @@ -0,0 +1,77 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** + * The second derivative of Pose2d with respect to time. + * + * Units are meters, radians, and seconds. + * + * This uses the R3 tangent space, not the SE(2) manifold; see README.md for + * details. + */ +public record AccelerationSE2(double x, double y, double theta) { + public VelocitySE2 integrate(double dtSec) { + return new VelocitySE2(x * dtSec, y * dtSec, theta * dtSec); + } + + public AccelerationSE2 plus(AccelerationSE2 other) { + return new AccelerationSE2(x + other.x, y + other.y, theta + other.theta); + } + + public AccelerationSE2 minus(AccelerationSE2 other) { + return new AccelerationSE2(x - other.x, y - other.y, theta - other.theta); + } + + public JerkSE2 jerk(AccelerationSE2 previous, double dt) { + AccelerationSE2 v = minus(previous).div(dt); + return new JerkSE2(v.x(), v.y(), v.theta()); + } + + public AccelerationSE2 times(double scalar) { + return new AccelerationSE2(x * scalar, y * scalar, theta * scalar); + } + + public AccelerationSE2 div(double scalar) { + return new AccelerationSE2(x / scalar, y / scalar, theta / scalar); + } + + public double norm() { + return Math.hypot(x, y); + } + + public static AccelerationSE2 diff( + VelocitySE2 v1, + VelocitySE2 v2, + double dtSec) { + VelocitySE2 dv = v2.minus(v1); + return new AccelerationSE2(dv.x() / dtSec, dv.y() / dtSec, dv.theta() / dtSec); + } + + public AccelerationSE2 clamp(double maxAccel, double maxAlpha) { + double norm = Math.hypot(x, y); + double ratio = 1.0; + if (norm > 1e-3 && norm > maxAccel) { + ratio = maxAccel / norm; + } + return new AccelerationSE2(ratio * x, ratio * y, MathUtil.clamp(theta, -maxAlpha, maxAlpha)); + } + + public static AccelerationSE2 fromVector(Matrix v) { + return new AccelerationSE2(v.get(0, 0), v.get(1, 0), v.get(2, 0)); + } + + public Vector toVector() { + return VecBuilder.fill(x, y, theta); + } + + @Override + public String toString() { + return String.format("(%5.2f, %5.2f, %5.2f)", x, y, theta); + } +} diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalDeltaR3.java b/lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java similarity index 69% rename from lib/src/main/java/org/team100/lib/geometry/GlobalDeltaR3.java rename to lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java index 1d1deae73..f3101e0f7 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalDeltaR3.java +++ b/lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java @@ -6,13 +6,14 @@ import edu.wpi.first.math.geometry.Translation2d; /** - * This is just a container for the difference between two poses, treating the - * dimensions of each pose as independent. That is, it is a difference in R3, - * not SE(2). + * This is just a container for the difference between two poses. + * + * This treats the dimensions as independent, i.e. in the R3 tangent space, + * not the SE(2) manifold. * * The SE(2) difference represents a *geodesic* in SE(2), and for differences - * that include rotation, this will appear as a curved path -- usually not what is - * desired. + * that include rotation, this will appear as a curved path -- usually not what + * is desired. * * The R3 difference represents a straight line in every axis. * @@ -20,24 +21,24 @@ * independent, e.g. if you have three separate proportional feedback * controllers, or if you want to interpolate the axes separately. */ -public class GlobalDeltaR3 { +public class DeltaSE2 { private final Translation2d m_translation; private final Rotation2d m_rotation; - public GlobalDeltaR3(Translation2d translation, Rotation2d rotation) { + public DeltaSE2(Translation2d translation, Rotation2d rotation) { m_translation = translation; m_rotation = rotation; } - /** Return a delta from start to end. Wraps heading. */ - public static GlobalDeltaR3 delta(Pose2d start, Pose2d end) { + /** Return a delta from start to end. Wraps heading. */ + public static DeltaSE2 delta(Pose2d start, Pose2d end) { Translation2d t = end.getTranslation().minus(start.getTranslation()); Rotation2d r = end.getRotation().minus(start.getRotation()); - return new GlobalDeltaR3(t, r); + return new DeltaSE2(t, r); } - public GlobalDeltaR3 limit(double cartesian, double rotation) { - return new GlobalDeltaR3( + public DeltaSE2 limit(double cartesian, double rotation) { + return new DeltaSE2( new Translation2d( MathUtil.clamp(m_translation.getX(), -cartesian, cartesian), MathUtil.clamp(m_translation.getY(), -cartesian, cartesian)), @@ -45,12 +46,12 @@ public GlobalDeltaR3 limit(double cartesian, double rotation) { MathUtil.clamp(m_rotation.getRadians(), -rotation, rotation))); } - public GlobalDeltaR3 times(double scalar) { - return new GlobalDeltaR3(m_translation.times(scalar), m_rotation.times(scalar)); + public DeltaSE2 times(double scalar) { + return new DeltaSE2(m_translation.times(scalar), m_rotation.times(scalar)); } - public GlobalDeltaR3 div(double scalar) { - return new GlobalDeltaR3(m_translation.div(scalar), m_rotation.div(scalar)); + public DeltaSE2 div(double scalar) { + return new DeltaSE2(m_translation.div(scalar), m_rotation.div(scalar)); } public double getX() { diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java new file mode 100644 index 000000000..6413ae6cf --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java @@ -0,0 +1,40 @@ +package org.team100.lib.geometry; + +/** + * Represents a direction in a 2d plane as a unit vector in R2. + * + * The WPI class, Rotation2d, is used to represent both transformations (from + * one direction to another) and directions themselves, using a "zero" direction + * of (1,0). + * + * It would be better to use distinct types to represent direction (this class) + * differently from transformation (Rotation2d). + * + * Possible parameterizations: + * + * * angle relative to (1,0) (this is what Rotation2d does) + * * sin and cos (also like Rotation2d) + * * direction cosines with each axis (x, y), L2 norm = 1 + * + * The direction cosine is just a unit vector, so we'll do that. + * + * @see https://en.wikipedia.org/wiki/Direction_cosine + */ +public class DirectionR2 { + public final double x; + public final double y; + + public DirectionR2(double px, double py) { + double h = Math.sqrt(px * px + py * py); + x = px / h; + y = py / h; + } + + @Override + public boolean equals(Object obj) { + return obj instanceof DirectionR2 other + && Math.abs(other.x - x) < 1E-9 + && Math.abs(other.y - y) < 1E-9; + } + +} diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionR3.java b/lib/src/main/java/org/team100/lib/geometry/DirectionR3.java new file mode 100644 index 000000000..08bf67fa4 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionR3.java @@ -0,0 +1,43 @@ +package org.team100.lib.geometry; + +/** + * Represents a direction in 3d space as a unit vector in R3. + * + * This is a different idea than Rotation3d, which specifies an angular + * *transformation* in 3d space. + * + * Possible parameterizations: + * + * * theta (equatorial), phi (polar) + * * elevation (above horizontal), azimuth (equatorial) + * * direction cosines (3 parameters with L2 norm = 1) + * + * First let's try direction cosines. + * + * l = cos(alpha) + * m = cos(beta) + * n = cos(gamma) + * + * @see https://en.wikipedia.org/wiki/Direction_cosine + */ +public class DirectionR3 { + public final double x; + public final double y; + public final double z; + + public DirectionR3(double px, double py, double pz) { + double h = Math.sqrt(px * px + py * py + pz * pz); + x = px / h; + y = py / h; + z = pz / h; + } + + @Override + public boolean equals(Object obj) { + return obj instanceof DirectionR3 other + && Math.abs(other.x - x) < 1E-9 + && Math.abs(other.y - y) < 1E-9 + && Math.abs(other.z - z) < 1E-9; + } + +} diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java new file mode 100644 index 000000000..b01d0c89c --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -0,0 +1,49 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; + +/** + * A direction (i.e. unit-length vector) in the SE2 manifold, + * rigid transformations in two dimensions (which have three dimensions). + * + * This is useful for representing spline controls for Pose2d. + * + * It is exactly a unit-length Twist2d. + */ +public class DirectionSE2 { + public final double x; + public final double y; + public final double theta; + + public DirectionSE2(double px, double py, double ptheta) { + double h = Math.sqrt(px * px + py * py + ptheta * ptheta); + x = px / h; + y = py / h; + theta = ptheta / h; + } + + /** Cartesian part of direction, as an old-fashioned Rotation2d */ + public Rotation2d toRotation() { + return new Rotation2d(x, y); + } + + public static final DirectionSE2 TO_X = new DirectionSE2(1, 0, 0); + public static final DirectionSE2 MINUS_X = new DirectionSE2(-1, 0, 0); + public static final DirectionSE2 TO_Y = new DirectionSE2(0, 1, 0); + public static final DirectionSE2 MINUS_Y = new DirectionSE2(0, -1, 0); + public static final DirectionSE2 SPIN = new DirectionSE2(0, 0, 1); + + /** Adapter for rotation-free directions */ + public static final DirectionSE2 fromRotation(Rotation2d r) { + return new DirectionSE2(r.getCos(), r.getSin(), 0); + } + + @Override + public boolean equals(Object obj) { + return obj instanceof DirectionSE2 other + && Math.abs(other.x - x) < 1E-9 + && Math.abs(other.y - y) < 1E-9 + && Math.abs(other.theta - theta) < 1E-9; + } +} diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE3.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE3.java new file mode 100644 index 000000000..76145b6a6 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE3.java @@ -0,0 +1,46 @@ +package org.team100.lib.geometry; + +/** + * A direction (i.e. unit-length vector) in the SE3 manifold, + * rigid transformations in three dimensions (which have six dimensions). + * + * This is useful for representing spline controls for Pose3d. + * + * It is exactly a unit-length Twist3d. + */ +public class DirectionSE3 { + public final double x; + public final double y; + public final double z; + public final double roll; + public final double pitch; + public final double yaw; + + public DirectionSE3( + double px, double py, double pz, + double proll, double ppitch, double pyaw) { + double h = Math.sqrt( + px * px + py * py + pz * pz + + proll * proll + ppitch * ppitch + pyaw * pyaw); + x = px / h; + y = py / h; + z = pz / h; + roll = proll / h; + pitch = ppitch / h; + yaw = pyaw / h; + } + + @Override + public boolean equals(Object obj) { + return obj instanceof DirectionSE3 other + && Math.abs(other.x - x) < 1E-9 + && Math.abs(other.y - y) < 1E-9 + && Math.abs(other.z - z) < 1E-9 + && Math.abs(other.roll - roll) < 1E-9 + && Math.abs(other.pitch - pitch) < 1E-9 + && Math.abs(other.yaw - yaw) < 1E-9 + + ; + } + +} diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 22d576478..132f9e34c 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -55,7 +55,7 @@ public static double dot(Translation2d a, Translation2d b) { return a.getX() * b.getX() + a.getY() * b.getY(); } - public static double dot(Translation2d a, GlobalVelocityR3 b) { + public static double dot(Translation2d a, VelocitySE2 b) { return a.getX() * b.x() + a.getY() * b.y(); } @@ -71,8 +71,8 @@ public static Twist2d scale(Twist2d twist, double scale) { return new Twist2d(twist.dx * scale, twist.dy * scale, twist.dtheta * scale); } - public static GlobalVelocityR3 scale(GlobalVelocityR3 v, double scale) { - return new GlobalVelocityR3(v.x() * scale, v.y() * scale, v.theta() * scale); + public static VelocitySE2 scale(VelocitySE2 v, double scale) { + return new VelocitySE2(v.x() * scale, v.y() * scale, v.theta() * scale); } public static Pose2d transformBy(Pose2d a, Pose2d b) { @@ -204,11 +204,14 @@ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { return new Pose2d(lerpT, lerpR); } + // TODO: fix interpolation public static HolonomicPose2d interpolate(HolonomicPose2d a, HolonomicPose2d b, double x) { return new HolonomicPose2d( a.translation().interpolate(b.translation(), x), interpolate2(a.heading(), b.heading(), x), - interpolate2(a.course(), b.course(), x)); + DirectionSE2.fromRotation(interpolate2( + a.course().toRotation(), + b.course().toRotation(), x))); } /** diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalAccelerationR3.java b/lib/src/main/java/org/team100/lib/geometry/GlobalAccelerationR3.java deleted file mode 100644 index fb8e8e2ad..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalAccelerationR3.java +++ /dev/null @@ -1,79 +0,0 @@ -package org.team100.lib.geometry; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; - -/** - * This is acceleration in the global reference frame, the second derivative of - * Pose2d. This means "field relative" for robot navigation and control. It is - * also useful for purposes other than navigation, e.g. planar mechanism - * kinematics. - * - * Units are meters, radians, and seconds. - * - * This implements acceleration in R3, not SE(2); see README.md for details. - */ -public record GlobalAccelerationR3(double x, double y, double theta) { - public GlobalVelocityR3 integrate(double dtSec) { - return new GlobalVelocityR3(x * dtSec, y * dtSec, theta * dtSec); - } - - public GlobalAccelerationR3 plus(GlobalAccelerationR3 other) { - return new GlobalAccelerationR3(x + other.x, y + other.y, theta + other.theta); - } - - public GlobalAccelerationR3 minus(GlobalAccelerationR3 other) { - return new GlobalAccelerationR3(x - other.x, y - other.y, theta - other.theta); - } - - public GlobalJerkR3 jerk(GlobalAccelerationR3 previous, double dt) { - GlobalAccelerationR3 v = minus(previous).div(dt); - return new GlobalJerkR3(v.x(), v.y(), v.theta()); - } - - public GlobalAccelerationR3 times(double scalar) { - return new GlobalAccelerationR3(x * scalar, y * scalar, theta * scalar); - } - - public GlobalAccelerationR3 div(double scalar) { - return new GlobalAccelerationR3(x / scalar, y / scalar, theta / scalar); - } - - public double norm() { - return Math.hypot(x, y); - } - - public static GlobalAccelerationR3 diff( - GlobalVelocityR3 v1, - GlobalVelocityR3 v2, - double dtSec) { - GlobalVelocityR3 dv = v2.minus(v1); - return new GlobalAccelerationR3(dv.x() / dtSec, dv.y() / dtSec, dv.theta() / dtSec); - } - - public GlobalAccelerationR3 clamp(double maxAccel, double maxAlpha) { - double norm = Math.hypot(x, y); - double ratio = 1.0; - if (norm > 1e-3 && norm > maxAccel) { - ratio = maxAccel / norm; - } - return new GlobalAccelerationR3(ratio * x, ratio * y, MathUtil.clamp(theta, -maxAlpha, maxAlpha)); - } - - public static GlobalAccelerationR3 fromVector(Matrix v) { - return new GlobalAccelerationR3(v.get(0, 0), v.get(1, 0), v.get(2, 0)); - } - - public Vector toVector() { - return VecBuilder.fill(x, y, theta); - } - - @Override - public String toString() { - return String.format("(%5.2f, %5.2f, %5.2f)", x, y, theta); - } -} diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalJerkR3.java b/lib/src/main/java/org/team100/lib/geometry/GlobalJerkR3.java deleted file mode 100644 index 5bc25a01d..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalJerkR3.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.team100.lib.geometry; - -public record GlobalJerkR3(double x, double y, double theta) { - public double norm() { - return Math.hypot(x, y); - } - - public GlobalJerkR3 times(double scalar) { - return new GlobalJerkR3(x * scalar, y * scalar, theta * scalar); - } - - public GlobalAccelerationR3 integrate(double dtSec) { - return new GlobalAccelerationR3(x * dtSec, y * dtSec, theta * dtSec); - } -} diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java index 8aeced8df..a2d98ff78 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java +++ b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java @@ -5,7 +5,7 @@ /** * Velocity in three dimensions, companion to Translation3d. * - * This is different from GlobalVelocityR3, which is the companion to Pose2d, + * This is different from VelocitySE2, which is the companion to Pose2d, * i.e. velocity of planar rigid transforms. */ public record GlobalVelocity3d(double x, double y, double z) { @@ -19,7 +19,7 @@ public static GlobalVelocity3d fromPolar( } /** Pick up the translation component of v, in the XY plane. */ - public static GlobalVelocity3d fromSe2(GlobalVelocityR3 v) { + public static GlobalVelocity3d fromSe2(VelocitySE2 v) { return new GlobalVelocity3d(v.x(), v.y(), 0); } diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR2.java b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR2.java index 00aa09562..b77a86687 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR2.java +++ b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR2.java @@ -20,7 +20,7 @@ public static GlobalVelocityR2 fromPolar(Rotation2d angle, double speed) { } /** Pick up the translation component of v. */ - public static GlobalVelocityR2 fromSe2(GlobalVelocityR3 v) { + public static GlobalVelocityR2 fromSe2(VelocitySE2 v) { return new GlobalVelocityR2(v.x(), v.y()); } diff --git a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java index 0dddbbf8e..fec2c2ca2 100644 --- a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java +++ b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java @@ -7,39 +7,49 @@ /** * For constructing splines, paths, and trajectories. * + * Together, translation and heading constitute Pose2d. + * * @param translation * @param heading where the front of the robot is facing - * @param course the direction the robot is moving + * @param course direction of travel, including rotation + * + * TODO: use Pose2d here */ public record HolonomicPose2d( Translation2d translation, Rotation2d heading, - Rotation2d course) { + DirectionSE2 course) { public Pose2d pose() { return new Pose2d(translation, heading); } - public static HolonomicPose2d make(Pose2d p, Rotation2d course) { + public static HolonomicPose2d make(Pose2d p, DirectionSE2 course) { return new HolonomicPose2d(p.getTranslation(), p.getRotation(), course); } + /** Course without rotation */ public static HolonomicPose2d make(Pose2d p, double course) { return new HolonomicPose2d( p.getTranslation(), p.getRotation(), - new Rotation2d(course)); + new DirectionSE2(Math.cos(course), Math.sin(course), 0)); } + /** Course without rotation */ public static HolonomicPose2d make(double x, double y, double heading, double course) { return new HolonomicPose2d( - new Translation2d(x, y), new Rotation2d(heading), new Rotation2d(course)); + new Translation2d(x, y), + new Rotation2d(heading), + new DirectionSE2(Math.cos(course), Math.sin(course), 0)); } /** For tank drive, heading and course are the same. */ public static HolonomicPose2d tank(double x, double y, double heading) { return new HolonomicPose2d( - new Translation2d(x, y), new Rotation2d(heading), new Rotation2d(heading)); + new Translation2d(x, y), + new Rotation2d(heading), + new DirectionSE2(Math.cos(heading), Math.sin(heading), 0)); } /** @@ -47,7 +57,10 @@ public static HolonomicPose2d tank(double x, double y, double heading) { * trajectory. */ public static HolonomicPose2d tank(Pose2d p) { + Rotation2d r = p.getRotation(); return new HolonomicPose2d( - p.getTranslation(), p.getRotation(), p.getRotation()); + p.getTranslation(), + r, + new DirectionSE2(r.getCos(), r.getSin(), 0)); } } diff --git a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java index 35a0eb532..0bb6b1a7c 100644 --- a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java +++ b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java @@ -1,18 +1,25 @@ package org.team100.lib.geometry; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; /** * For constructing 3d splines, paths, and trajectories. * + * Together, translation and heading constitude Pose3d. + * * @param translation * @param heading where we're facing, can include roll - * @param course where we're going, ignores roll. + * @param course where we're going */ public record HolonomicPose3d( Translation3d translation, Rotation3d heading, - Rotation3d course) { + DirectionR3 course) { + + public Pose3d pose() { + return new Pose3d(translation, heading); + } } diff --git a/lib/src/main/java/org/team100/lib/geometry/JerkSE2.java b/lib/src/main/java/org/team100/lib/geometry/JerkSE2.java new file mode 100644 index 000000000..efaee487e --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/JerkSE2.java @@ -0,0 +1,20 @@ +package org.team100.lib.geometry; + +/** + * Third time-derivative of Pose2d. + * + * Uses R3 tangent, not SE(2) manifold. + */ +public record JerkSE2(double x, double y, double theta) { + public double norm() { + return Math.hypot(x, y); + } + + public JerkSE2 times(double scalar) { + return new JerkSE2(x * scalar, y * scalar, theta * scalar); + } + + public AccelerationSE2 integrate(double dtSec) { + return new AccelerationSE2(x * dtSec, y * dtSec, theta * dtSec); + } +} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index d90ede1e9..d8feff0ab 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -44,8 +44,9 @@ public HolonomicPose2d getPose() { return m_pose; } + // TODO: change to DirectionSE2 public Rotation2d getCourse() { - return m_pose.course(); + return m_pose.course().toRotation(); } /** diff --git a/lib/src/main/java/org/team100/lib/geometry/README.md b/lib/src/main/java/org/team100/lib/geometry/README.md index aaa8925e3..de9b8d259 100644 --- a/lib/src/main/java/org/team100/lib/geometry/README.md +++ b/lib/src/main/java/org/team100/lib/geometry/README.md @@ -14,12 +14,12 @@ and the correct derivative takes this into account: see `Pose2d.exp()` and `Pose2d.log()`. We mostly do not use `exp()` and `log()`. We treat global velocity and -acceleration as if all the components were independent, i.e. using the R3 vector space, -not the SE(2) Lie group. +acceleration as if all the components were independent, +i.e. using the R3 vector space, not the SE(2) Lie group. Why? Because at large scales, we think in R3, e.g. we define trajectories and means to follow them without worrying about the coupling -in SE(2). We *do* handle SE(2) correct at the smallest scale, one +in SE(2). We *do* handle SE(2) correctly at the smallest scale, one robot-clock step at a time, where, for example, drivetrain actuation needs to be correctly "discretized" so that the constant-twist paths the robot follows (approximately) during a single time step end up diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java b/lib/src/main/java/org/team100/lib/geometry/VelocitySE2.java similarity index 51% rename from lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java rename to lib/src/main/java/org/team100/lib/geometry/VelocitySE2.java index 4e5bd6eee..47010a61f 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java +++ b/lib/src/main/java/org/team100/lib/geometry/VelocitySE2.java @@ -13,22 +13,21 @@ import edu.wpi.first.math.numbers.N3; /** - * This is velocity in the global reference frame, the first derivative of - * Pose2d. This means "field relative" for robot navigation and control. It is - * similar to WPI ChassisSpeeds, but field-relative. It is also useful for - * purposes other than navigation, e.g. planar mechanism kinematics. + * The first derivative of Pose2d with respect to time. * * Units are meters, radians, and seconds. * - * This implements velocity in R3, not SE(2); see README.md for details. + * Everything here is in the R3 tangent space to SE(2). + * + * See README.md for details. */ -public record GlobalVelocityR3(double x, double y, double theta) { +public record VelocitySE2(double x, double y, double theta) { - public static final GlobalVelocityR3 ZERO = new GlobalVelocityR3(0, 0, 0); + public static final VelocitySE2 ZERO = new VelocitySE2(0, 0, 0); - public static GlobalVelocityR3 velocity(Pose2d start, Pose2d end, double dt) { - GlobalDeltaR3 d = GlobalDeltaR3.delta(start, end); - return new GlobalVelocityR3(d.getX(), d.getY(), d.getRotation().getRadians()).div(dt); + public static VelocitySE2 velocity(Pose2d start, Pose2d end, double dt) { + DeltaSE2 d = DeltaSE2.delta(start, end); + return new VelocitySE2(d.getX(), d.getY(), d.getRotation().getRadians()).div(dt); } /** The cartesian part only */ @@ -36,11 +35,11 @@ public double norm() { return Math.hypot(x, y); } - public GlobalVelocityR3 normalize() { + public VelocitySE2 normalize() { double norm = norm(); if (norm < 1e-6) return ZERO; - return new GlobalVelocityR3(x, y, theta).times(1.0 / norm); + return new VelocitySE2(x, y, theta).times(1.0 / norm); } /** Field-relative course, or empty if slower than 1 micron/sec. */ @@ -50,13 +49,13 @@ public Optional angle() { return Optional.of(new Rotation2d(x, y)); } - public GlobalVelocityR3 plus(GlobalVelocityR3 other) { - return new GlobalVelocityR3(x + other.x, y + other.y, theta + other.theta); + public VelocitySE2 plus(VelocitySE2 other) { + return new VelocitySE2(x + other.x, y + other.y, theta + other.theta); } /** The return type here isn't really right. */ - public GlobalVelocityR3 minus(GlobalVelocityR3 other) { - return new GlobalVelocityR3(x - other.x, y - other.y, theta - other.theta); + public VelocitySE2 minus(VelocitySE2 other) { + return new VelocitySE2(x - other.x, y - other.y, theta - other.theta); } /** Integrate the velocity from the initial pose for time dt */ @@ -67,25 +66,25 @@ public Pose2d integrate(Pose2d initial, double dt) { initial.getRotation().plus(new Rotation2d(theta * dt))); } - public GlobalAccelerationR3 accel(GlobalVelocityR3 previous, double dt) { - GlobalVelocityR3 v = minus(previous).div(dt); - return new GlobalAccelerationR3(v.x(), v.y(), v.theta()); + public AccelerationSE2 accel(VelocitySE2 previous, double dt) { + VelocitySE2 v = minus(previous).div(dt); + return new AccelerationSE2(v.x(), v.y(), v.theta()); } - public GlobalVelocityR3 times(double scalar) { - return new GlobalVelocityR3(x * scalar, y * scalar, theta * scalar); + public VelocitySE2 times(double scalar) { + return new VelocitySE2(x * scalar, y * scalar, theta * scalar); } - public GlobalVelocityR3 div(double scalar) { - return new GlobalVelocityR3(x / scalar, y / scalar, theta / scalar); + public VelocitySE2 div(double scalar) { + return new VelocitySE2(x / scalar, y / scalar, theta / scalar); } - public GlobalVelocityR3 times(double cartesian, double angular) { - return new GlobalVelocityR3(x * cartesian, y * cartesian, theta * angular); + public VelocitySE2 times(double cartesian, double angular) { + return new VelocitySE2(x * cartesian, y * cartesian, theta * angular); } /** Dot product of translational part. */ - public double dot(GlobalVelocityR3 other) { + public double dot(VelocitySE2 other) { return x * other.x + y * other.y; } @@ -94,17 +93,17 @@ public Translation2d stopping(double accel) { double speed = norm(); double decelTime = speed / accel; // the unit here is wrong - GlobalVelocityR3 dx = normalize().times(0.5 * speed * decelTime); + VelocitySE2 dx = normalize().times(0.5 * speed * decelTime); return new Translation2d(dx.x, dx.y); } - public GlobalVelocityR3 clamp(double maxVelocity, double maxOmega) { + public VelocitySE2 clamp(double maxVelocity, double maxOmega) { double norm = Math.hypot(x, y); double ratio = 1.0; if (norm > 1e-3 && norm > maxVelocity) { ratio = maxVelocity / norm; } - return new GlobalVelocityR3(ratio * x, ratio * y, MathUtil.clamp(theta, -maxOmega, maxOmega)); + return new VelocitySE2(ratio * x, ratio * y, MathUtil.clamp(theta, -maxOmega, maxOmega)); } @Override @@ -112,12 +111,12 @@ public String toString() { return String.format("(%5.2f, %5.2f, %5.2f)", x, y, theta); } - public static GlobalVelocityR3 fromVector(Vector v) { - return new GlobalVelocityR3(v.get(0), v.get(1), v.get(2)); + public static VelocitySE2 fromVector(Vector v) { + return new VelocitySE2(v.get(0), v.get(1), v.get(2)); } - public static GlobalVelocityR3 fromVector(Matrix v) { - return new GlobalVelocityR3(v.get(0, 0), v.get(1, 0), v.get(2, 0)); + public static VelocitySE2 fromVector(Matrix v) { + return new VelocitySE2(v.get(0, 0), v.get(1, 0), v.get(2, 0)); } public Vector toVector() { diff --git a/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java b/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java index 3bea96559..c260f7620 100644 --- a/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java +++ b/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java @@ -2,7 +2,7 @@ import java.util.Objects; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveDriveKinematics100; import org.team100.lib.subsystems.swerve.module.state.SwerveModuleDeltas; @@ -72,9 +72,9 @@ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { Pose2d pose = m_state.pose().exp(twist); // these lerps are wrong but maybe close enough - GlobalVelocityR3 startVelocity = m_state.velocity(); - GlobalVelocityR3 endVelocity = endValue.m_state.velocity(); - GlobalVelocityR3 velocity = startVelocity.plus(endVelocity.minus(startVelocity).times(t)); + VelocitySE2 startVelocity = m_state.velocity(); + VelocitySE2 endVelocity = endValue.m_state.velocity(); + VelocitySE2 velocity = startVelocity.plus(endVelocity.minus(startVelocity).times(t)); ModelR3 newState = new ModelR3(pose, velocity); return new InterpolationRecord(m_kinematics, newState, wheelLerp); diff --git a/lib/src/main/java/org/team100/lib/localization/ManualPose.java b/lib/src/main/java/org/team100/lib/localization/ManualPose.java index bd0556832..34baa5f49 100644 --- a/lib/src/main/java/org/team100/lib/localization/ManualPose.java +++ b/lib/src/main/java/org/team100/lib/localization/ManualPose.java @@ -5,7 +5,7 @@ import org.team100.lib.coherence.Cache; import org.team100.lib.coherence.ObjectCache; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -73,7 +73,7 @@ private ModelR3 update() { m_state.pose().getX() + vx * DT, m_state.pose().getY() + vy * DT, m_state.pose().getRotation().plus(new Rotation2d(omega * DT))), - new GlobalVelocityR3(vx, vy, omega)); + new VelocitySE2(vx, vy, omega)); return m_state; } } diff --git a/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java b/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java index 9a3540bbf..f4f5e2837 100644 --- a/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java +++ b/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java @@ -5,8 +5,8 @@ import java.util.function.Supplier; import org.team100.lib.coherence.Takt; -import org.team100.lib.geometry.GlobalDeltaR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.DeltaSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.sensor.gyro.Gyro; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; @@ -154,12 +154,12 @@ private void put( } // this is the backward finite difference velocity from odometry - GlobalDeltaR3 odoVelo = GlobalDeltaR3.delta( + DeltaSE2 odoVelo = DeltaSE2.delta( previousState.pose(), newPose) .div(dt); // use the gyro rate instead of the odometry-derived rate - GlobalVelocityR3 velocity = new GlobalVelocityR3( + VelocitySE2 velocity = new VelocitySE2( odoVelo.getX(), odoVelo.getY(), gyroRateRad_SNWU); diff --git a/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java b/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java index 777d12cfc..1147295f0 100644 --- a/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java +++ b/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java @@ -4,7 +4,7 @@ import java.util.SortedMap; import java.util.function.DoubleFunction; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; @@ -59,7 +59,7 @@ public SwerveHistory( m_kinodynamics.getKinematics(), new ModelR3( initialPoseMeters, - new GlobalVelocityR3(0, 0, 0)), + new VelocitySE2(0, 0, 0)), modulePositions)); } @@ -82,7 +82,7 @@ void reset( timestampSeconds, new InterpolationRecord( m_kinodynamics.getKinematics(), - new ModelR3(pose, new GlobalVelocityR3(0, 0, 0)), + new ModelR3(pose, new VelocitySE2(0, 0, 0)), modulePositions)); } diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java index 03fbc6f72..68dba295c 100644 --- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java +++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java @@ -7,12 +7,12 @@ import java.util.function.LongSupplier; import java.util.function.Supplier; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalDeltaR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.GlobalVelocityR2; -import org.team100.lib.geometry.GlobalVelocityR3; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.localization.Blip24; import org.team100.lib.logging.primitive.PrimitiveLogger; import org.team100.lib.reference.r1.SetpointsR1; @@ -517,7 +517,7 @@ public void log(Supplier vals) { return; HolonomicPose2d val = vals.get().getPose(); m_pose2dLogger.log(val::pose); - m_rotation2dLogger.log(val::course); + m_rotation2dLogger.log(() -> val.course().toRotation()); } } @@ -592,45 +592,45 @@ public class GlobaDeltaR3Logger { m_thetaLogger = doubleLogger(level, join(leaf, "theta rad")); } - public void log(Supplier vals) { + public void log(Supplier vals) { if (!allow(m_level)) return; - GlobalDeltaR3 val = vals.get(); + DeltaSE2 val = vals.get(); m_xLogger.log(val::getX); m_yLogger.log(val::getY); m_thetaLogger.log(val::getRadians); } } - public GlobaDeltaR3Logger globalDeltaR3Logger(Level level, String leaf) { + public GlobaDeltaR3Logger DeltaSE2Logger(Level level, String leaf) { return new GlobaDeltaR3Logger(level, leaf); } - public class GlobalVelocityR3Logger { + public class VelocitySE2Logger { private final Level m_level; private final DoubleLogger m_xLogger; private final DoubleLogger m_yLogger; private final DoubleLogger m_thetaLogger; - GlobalVelocityR3Logger(Level level, String leaf) { + VelocitySE2Logger(Level level, String leaf) { m_level = level; m_xLogger = doubleLogger(level, join(leaf, "x m_s")); m_yLogger = doubleLogger(level, join(leaf, "y m_s")); m_thetaLogger = doubleLogger(level, join(leaf, "theta rad_s")); } - public void log(Supplier vals) { + public void log(Supplier vals) { if (!allow(m_level)) return; - GlobalVelocityR3 val = vals.get(); + VelocitySE2 val = vals.get(); m_xLogger.log(val::x); m_yLogger.log(val::y); m_thetaLogger.log(val::theta); } } - public GlobalVelocityR3Logger globalVelocityR3Logger(Level level, String leaf) { - return new GlobalVelocityR3Logger(level, leaf); + public VelocitySE2Logger VelocitySE2Logger(Level level, String leaf) { + return new VelocitySE2Logger(level, leaf); } public class GlobalVelocityR2Logger { @@ -657,31 +657,31 @@ public GlobalVelocityR2Logger globalVelocityR2Logger(Level level, String leaf) { return new GlobalVelocityR2Logger(level, leaf); } - public class GlobalAccelerationR3Logger { + public class AccelerationSE2Logger { private final Level m_level; private final DoubleLogger m_xLogger; private final DoubleLogger m_yLogger; private final DoubleLogger m_thetaLogger; - GlobalAccelerationR3Logger(Level level, String leaf) { + AccelerationSE2Logger(Level level, String leaf) { m_level = level; m_xLogger = doubleLogger(level, join(leaf, "x m_s_s")); m_yLogger = doubleLogger(level, join(leaf, "y m_s_s")); m_thetaLogger = doubleLogger(level, join(leaf, "theta rad_s_s")); } - public void log(Supplier vals) { + public void log(Supplier vals) { if (!allow(m_level)) return; - GlobalAccelerationR3 val = vals.get(); + AccelerationSE2 val = vals.get(); m_xLogger.log(val::x); m_yLogger.log(val::y); m_thetaLogger.log(val::theta); } } - public GlobalAccelerationR3Logger globalAccelerationR3Logger(Level level, String leaf) { - return new GlobalAccelerationR3Logger(level, leaf); + public AccelerationSE2Logger AccelerationSE2Logger(Level level, String leaf) { + return new AccelerationSE2Logger(level, leaf); } public class Model100Logger { diff --git a/lib/src/main/java/org/team100/lib/state/ControlR3.java b/lib/src/main/java/org/team100/lib/state/ControlR3.java index b7684a7b8..5e9acb245 100644 --- a/lib/src/main/java/org/team100/lib/state/ControlR3.java +++ b/lib/src/main/java/org/team100/lib/state/ControlR3.java @@ -1,7 +1,7 @@ package org.team100.lib.state; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.trajectory.timing.TimedPose; @@ -33,14 +33,14 @@ public ControlR3(Control100 x, Control100 y, Control100 theta) { m_theta = theta; } - public ControlR3(Pose2d x, GlobalVelocityR3 v) { + public ControlR3(Pose2d x, VelocitySE2 v) { this( new Control100(x.getX(), v.x(), 0), new Control100(x.getY(), v.y(), 0), new Control100(x.getRotation().getRadians(), v.theta(), 0)); } - public ControlR3(Pose2d x, GlobalVelocityR3 v, GlobalAccelerationR3 a) { + public ControlR3(Pose2d x, VelocitySE2 v, AccelerationSE2 a) { this( new Control100(x.getX(), v.x(), a.x()), new Control100(x.getY(), v.y(), a.y()), @@ -48,7 +48,7 @@ public ControlR3(Pose2d x, GlobalVelocityR3 v, GlobalAccelerationR3 a) { } public ControlR3(Pose2d x) { - this(x, new GlobalVelocityR3(0, 0, 0)); + this(x, new VelocitySE2(0, 0, 0)); } public ControlR3(Rotation2d x) { @@ -94,8 +94,8 @@ public Rotation2d rotation() { return new Rotation2d(m_theta.x()); } - public GlobalVelocityR3 velocity() { - return new GlobalVelocityR3(m_x.v(), m_y.v(), m_theta.v()); + public VelocitySE2 velocity() { + return new VelocitySE2(m_x.v(), m_y.v(), m_theta.v()); } /** Robot-relative speeds */ @@ -103,8 +103,8 @@ public ChassisSpeeds chassisSpeeds() { return SwerveKinodynamics.toInstantaneousChassisSpeeds(velocity(), rotation()); } - public GlobalAccelerationR3 acceleration() { - return new GlobalAccelerationR3(m_x.a(), m_y.a(), m_theta.a()); + public AccelerationSE2 acceleration() { + return new AccelerationSE2(m_x.a(), m_y.a(), m_theta.a()); } public Control100 x() { @@ -130,7 +130,7 @@ public static ControlR3 fromTimedPose(TimedPose timedPose) { double thetax = timedPose.state().getPose().heading().getRadians(); double velocityM_s = timedPose.velocityM_S(); - Rotation2d course = timedPose.state().getPose().course(); + Rotation2d course = timedPose.state().getPose().course().toRotation(); double xv = course.getCos() * velocityM_s; double yv = course.getSin() * velocityM_s; double thetav = timedPose.state().getHeadingRateRad_M() * velocityM_s; diff --git a/lib/src/main/java/org/team100/lib/state/ModelR3.java b/lib/src/main/java/org/team100/lib/state/ModelR3.java index 388d73add..e113d66bc 100644 --- a/lib/src/main/java/org/team100/lib/state/ModelR3.java +++ b/lib/src/main/java/org/team100/lib/state/ModelR3.java @@ -1,7 +1,7 @@ package org.team100.lib.state; import org.team100.lib.geometry.GlobalVelocityR2; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.trajectory.timing.TimedPose; @@ -31,7 +31,7 @@ public ModelR3(Model100 x, Model100 y, Model100 theta) { m_theta = theta; } - public ModelR3(Pose2d x, GlobalVelocityR3 v) { + public ModelR3(Pose2d x, VelocitySE2 v) { this( new Model100(x.getX(), v.x()), new Model100(x.getY(), v.y()), @@ -40,7 +40,7 @@ public ModelR3(Pose2d x, GlobalVelocityR3 v) { /** Motionless with the specified pose */ public ModelR3(Pose2d x) { - this(x, new GlobalVelocityR3(0, 0, 0)); + this(x, new VelocitySE2(0, 0, 0)); } /** Motionless at the origin with the specified heading */ @@ -99,8 +99,8 @@ public Rotation2d rotation() { return new Rotation2d(m_theta.x()); } - public GlobalVelocityR3 velocity() { - return new GlobalVelocityR3(m_x.v(), m_y.v(), m_theta.v()); + public VelocitySE2 velocity() { + return new VelocitySE2(m_x.v(), m_y.v(), m_theta.v()); } public GlobalVelocityR2 velocityR2() { diff --git a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java index 18cab7907..ec8c03f18 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java +++ b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java @@ -1,10 +1,10 @@ package org.team100.lib.subsystems.mecanum; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.sensor.gyro.Gyro; import org.team100.lib.servo.OutboardLinearVelocityServo; import org.team100.lib.state.ModelR3; @@ -27,7 +27,7 @@ public class MecanumDrive100 extends SubsystemBase implements VelocitySubsystemR3 { private final DoubleArrayLogger m_log_field_robot; - private final GlobalVelocityR3Logger m_log_input; + private final VelocitySE2Logger m_log_input; /** May be null. */ private final Gyro m_gyro; private final double m_trackWidthM; @@ -40,7 +40,7 @@ public class MecanumDrive100 extends SubsystemBase implements VelocitySubsystemR private final MecanumKinematics100 m_kinematics; private MecanumDriveWheelPositions m_positions; - private GlobalVelocityR3 m_input; + private VelocitySE2 m_input; private Pose2d m_pose; private Rotation2d m_gyroOffset; @@ -60,7 +60,7 @@ public MecanumDrive100( OutboardLinearVelocityServo rearRight) { LoggerFactory log = parent.type(this); m_log_field_robot = fieldLogger.doubleArrayLogger(Level.COMP, "robot"); - m_log_input = log.globalVelocityR3Logger(Level.TRACE, "drive input"); + m_log_input = log.VelocitySE2Logger(Level.TRACE, "drive input"); m_gyro = gyro; m_trackWidthM = trackWidthM; m_wheelbaseM = wheelbaseM; @@ -75,7 +75,7 @@ public MecanumDrive100( new Translation2d(-m_wheelbaseM / 2, m_trackWidthM / 2), new Translation2d(-m_wheelbaseM / 2, -m_trackWidthM / 2)); m_positions = new MecanumDriveWheelPositions(); - m_input = new GlobalVelocityR3(0, 0, 0); + m_input = new VelocitySE2(0, 0, 0); m_pose = new Pose2d(); m_gyroOffset = new Rotation2d(); } @@ -92,7 +92,7 @@ public ModelR3 getState() { * @param nextV for the next timestep */ @Override - public void setVelocity(GlobalVelocityR3 nextV) { + public void setVelocity(VelocitySE2 nextV) { Rotation2d yaw = getYaw(); ChassisSpeeds speed = SwerveKinodynamics.toInstantaneousChassisSpeeds( nextV, yaw); @@ -118,7 +118,7 @@ public void stop() { } /** Set the field-relative velocity. */ - public Command driveWithGlobalVelocity(GlobalVelocityR3 v) { + public Command driveWithGlobalVelocity(VelocitySE2 v) { return run(() -> setVelocity(v)) .withName("drive with global velocity"); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/mecanum/commands/ManualMecanum.java b/lib/src/main/java/org/team100/lib/subsystems/mecanum/commands/ManualMecanum.java index b04ed3066..66c4bdd01 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/mecanum/commands/ManualMecanum.java +++ b/lib/src/main/java/org/team100/lib/subsystems/mecanum/commands/ManualMecanum.java @@ -4,7 +4,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.mecanum.MecanumDrive100; @@ -72,7 +72,7 @@ public void execute() { case CLIP -> input.diamond(1, y_x, poseRotation); case SQUASH -> input.squashedDiamond(1, y_x, poseRotation); }; - GlobalVelocityR3 scaled = FieldRelativeDriver.scale( + VelocitySE2 scaled = FieldRelativeDriver.scale( clippedOrSquashed, m_maxVX.getAsDouble(), m_maxOmega.getAsDouble()); // Apply field-relative limits. if (Experiments.instance.enabled(Experiment.UseSetpointGenerator)) { diff --git a/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java b/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java index 3599777e6..0e6ee3d5d 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java +++ b/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.prr; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ControlR3; import org.team100.lib.state.ModelR3; @@ -36,9 +36,9 @@ public AnalyticalJacobian(ElevatorArmWristKinematics k) { * * See doc/README.md equation 4 */ - public GlobalVelocityR3 forward(EAWConfig q, JointVelocities qdot) { + public VelocitySE2 forward(EAWConfig q, JointVelocities qdot) { Matrix j = getJ(q); - return GlobalVelocityR3.fromVector(j.times(qdot.toVector())); + return VelocitySE2.fromVector(j.times(qdot.toVector())); } /** @@ -50,7 +50,7 @@ public GlobalVelocityR3 forward(EAWConfig q, JointVelocities qdot) { */ public JointVelocities inverse(ModelR3 m) { Pose2d x = m.pose(); - GlobalVelocityR3 xdot = m.velocity(); + VelocitySE2 xdot = m.velocity(); EAWConfig q = m_k.inverse(x); Matrix Jinv = getJinv(q); return JointVelocities.fromVector(Jinv.times(xdot.toVector())); @@ -63,11 +63,11 @@ public JointVelocities inverse(ModelR3 m) { * * See doc/README.md equation 6 */ - public GlobalAccelerationR3 forwardA( + public AccelerationSE2 forwardA( EAWConfig q, JointVelocities qdot, JointAccelerations qddot) { Matrix J = getJ(q); Matrix Jdot = getJdot(q, qdot); - return GlobalAccelerationR3.fromVector( + return AccelerationSE2.fromVector( Jdot.times(qdot.toVector()).plus(J.times(qddot.toVector()))); } @@ -80,8 +80,8 @@ public GlobalAccelerationR3 forwardA( */ public JointAccelerations inverseA(ControlR3 m) { Pose2d x = m.pose(); - GlobalVelocityR3 xdot = m.velocity(); - GlobalAccelerationR3 xddot = m.acceleration(); + VelocitySE2 xdot = m.velocity(); + AccelerationSE2 xddot = m.acceleration(); EAWConfig q = m_k.inverse(x); Matrix Jinv = getJinv(q); JointVelocities qdot = JointVelocities.fromVector(Jinv.times(xdot.toVector())); diff --git a/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java b/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java index 20939b47c..87baed178 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java +++ b/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java @@ -2,7 +2,7 @@ import java.util.function.Function; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.optimization.NumericalJacobian100; import org.team100.lib.state.ModelR3; @@ -34,10 +34,10 @@ public Jacobian(ElevatorArmWristKinematics k) { /** * Given a joint configuration and velocities, what is the cartesian velocity? */ - public GlobalVelocityR3 forward(EAWConfig c, JointVelocities v) { + public VelocitySE2 forward(EAWConfig c, JointVelocities v) { Matrix j = NumericalJacobian100.numericalJacobian2( Nat.N3(), Nat.N3(), m_f, config(c)); - return GlobalVelocityR3.fromVector(j.times(v.toVector())); + return VelocitySE2.fromVector(j.times(v.toVector())); } /** @@ -46,7 +46,7 @@ public GlobalVelocityR3 forward(EAWConfig c, JointVelocities v) { */ public JointVelocities inverse(ModelR3 swerveModel) { Pose2d p = swerveModel.pose(); - GlobalVelocityR3 v = swerveModel.velocity(); + VelocitySE2 v = swerveModel.velocity(); EAWConfig c = m_k.inverse(p); Matrix j = NumericalJacobian100.numericalJacobian2( Nat.N3(), Nat.N3(), m_f, config(c)); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java index aa33464f5..3de2390c0 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java @@ -1,6 +1,6 @@ package org.team100.lib.subsystems.r3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; /** * A subsystem with three independent dimensions, controlled by velocity. @@ -15,5 +15,5 @@ public interface VelocitySubsystemR3 extends SubsystemR3 { * * @param nextV for the next timestep. */ - void setVelocity(GlobalVelocityR3 nextV); + void setVelocity(VelocitySE2 nextV); } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java index f1042c516..6c7020933 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java @@ -4,7 +4,8 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; @@ -24,7 +25,7 @@ public class DriveToPoseWithTrajectoryAndExitVelocity extends MoveAndHold { private final LoggerFactory m_log; private final Pose2d m_goal; - private final GlobalVelocityR3 m_endVelocity; + private final VelocitySE2 m_endVelocity; private final VelocitySubsystemR3 m_drive; private final ControllerR3 m_controller; private final TrajectoryVisualization m_viz; @@ -35,7 +36,7 @@ public class DriveToPoseWithTrajectoryAndExitVelocity extends MoveAndHold { public DriveToPoseWithTrajectoryAndExitVelocity( LoggerFactory parent, Pose2d goal, - GlobalVelocityR3 endVelocity, + VelocitySE2 endVelocity, VelocitySubsystemR3 drive, ControllerR3 controller, TrajectoryPlanner planner, @@ -54,15 +55,15 @@ public DriveToPoseWithTrajectoryAndExitVelocity( public void initialize() { Pose2d pose = m_drive.getState().pose(); Translation2d toGoal = m_goal.getTranslation().minus(pose.getTranslation()); - GlobalVelocityR3 startVelocity = m_drive.getState().velocity(); + VelocitySE2 startVelocity = m_drive.getState().velocity(); HolonomicPose2d startWaypoint = new HolonomicPose2d( pose.getTranslation(), pose.getRotation(), - startVelocity.angle().orElse(toGoal.getAngle())); + DirectionSE2.fromRotation(startVelocity.angle().orElse(toGoal.getAngle()))); HolonomicPose2d endWaypoint = new HolonomicPose2d( m_goal.getTranslation(), m_goal.getRotation(), - m_endVelocity.angle().orElse(toGoal.getAngle())); + DirectionSE2.fromRotation(m_endVelocity.angle().orElse(toGoal.getAngle()))); Trajectory100 trajectory = m_planner.generateTrajectory( List.of(startWaypoint, endWaypoint), startVelocity.norm(), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java index d31249342..b35365e80 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java @@ -3,6 +3,7 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; @@ -43,7 +44,8 @@ public GoToPosePosition( @Override public void initialize() { HolonomicPose2d m_currentPose = HolonomicPose2d.make( - m_subsystem.getState().pose(), m_course); + m_subsystem.getState().pose(), + DirectionSE2.fromRotation(m_course)); Trajectory100 m_trajectory = m_trajectoryPlanner.restToRest( List.of(m_currentPose, m_goal)); m_referenceController = new PositionReferenceControllerR3( diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java index dc13736d9..261abe7af 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java @@ -2,7 +2,7 @@ import java.util.function.Supplier; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.state.ControlR3; import org.team100.lib.subsystems.r3.PositionSubsystemR3; @@ -44,7 +44,7 @@ public void execute() { // control is velocity. // velocity in m/s and rad/s // we want full scale to be about 0.5 m/s and 0.5 rad/s - GlobalVelocityR3 jv = new GlobalVelocityR3( + VelocitySE2 jv = new VelocitySE2( input.x() * 1.5, input.y() * 1.5, input.theta() * 3); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java index cd3f86677..92a9d552c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.r3.commands; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.r3.ProfileR3; import org.team100.lib.reference.r3.ProfileReferenceR3; @@ -46,7 +46,7 @@ public void initialize() { @Override public void execute() { - GlobalVelocityR3 velocity = m_reference.next().velocity(); + VelocitySE2 velocity = m_reference.next().velocity(); if (DEBUG) System.out.printf("velocity %s\n", velocity); m_drive.setVelocity(velocity); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java index b4a680343..3259b0d72 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java @@ -2,7 +2,7 @@ import org.team100.lib.controller.r3.ControllerR3; import org.team100.lib.controller.r3.NullControllerR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.ReferenceR3; import org.team100.lib.state.ControlR3; @@ -36,7 +36,7 @@ public PositionReferenceControllerR3( * @param u ignored, since this uses outboard feedback only. */ @Override - void execute100(ControlR3 next, GlobalVelocityR3 u) { + void execute100(ControlR3 next, VelocitySE2 u) { m_subsystem.set(next); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java index a4be3e0d3..2a450f881 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.r3.commands.helper; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; @@ -52,7 +52,7 @@ public abstract class ReferenceControllerR3Base { * @param next The next control setpoint. * @param u The controller output for the next dt */ - abstract void execute100(ControlR3 next, GlobalVelocityR3 u); + abstract void execute100(ControlR3 next, VelocitySE2 u); /** * This should be called in Command.execute(). @@ -65,7 +65,7 @@ public void execute() { ModelR3 error = current.minus(measurement); // u represents the time from now until now+dt, so it's also // what the mechanism should be doing at the next time step - GlobalVelocityR3 u = m_controller.calculate(measurement, current, next); + VelocitySE2 u = m_controller.calculate(measurement, current, next); execute100(next, u); m_log_measurement.log(() -> measurement); m_log_current.log(() -> current); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java index 68b62f19f..a98cbd406 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.r3.commands.helper; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.ReferenceR3; import org.team100.lib.state.ControlR3; @@ -35,7 +35,7 @@ public VelocityReferenceControllerR3( * velocity to this */ @Override - void execute100(ControlR3 next, GlobalVelocityR3 u) { + void execute100(ControlR3 next, VelocitySE2 u) { m_subsystem.setVelocity(u); } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java index fb6800895..4a970f7e9 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java @@ -7,7 +7,7 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.config.DriverSkill; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.localization.FreshSwerveEstimate; import org.team100.lib.localization.OdometryUpdater; import org.team100.lib.logging.Level; @@ -15,7 +15,7 @@ import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.logging.LoggerFactory.EnumLogger; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.logging.LoggerFactory.ModelR3Logger; import org.team100.lib.music.Music; import org.team100.lib.music.Player; @@ -49,7 +49,7 @@ public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsy private final DoubleLogger m_log_turning; private final DoubleArrayLogger m_log_pose_array; private final EnumLogger m_log_skill; - private final GlobalVelocityR3Logger m_log_input; + private final VelocitySE2Logger m_log_input; private final List m_players; @@ -70,7 +70,7 @@ public SwerveDriveSubsystem( m_log_turning = log.doubleLogger(Level.TRACE, "Tur Deg"); m_log_pose_array = log.doubleArrayLogger(Level.COMP, "pose array"); m_log_skill = log.enumLogger(Level.TRACE, "skill level"); - m_log_input = log.globalVelocityR3Logger(Level.TRACE, "drive input"); + m_log_input = log.VelocitySE2Logger(Level.TRACE, "drive input"); m_players = m_swerveLocal.players(); } @@ -85,7 +85,7 @@ public SwerveDriveSubsystem( * @param nextV for the next timestep */ @Override - public void setVelocity(GlobalVelocityR3 nextV) { + public void setVelocity(VelocitySE2 nextV) { // keep the limiter up to date on what we're doing m_limiter.updateSetpoint(nextV); @@ -223,7 +223,7 @@ public Pose2d getPose() { } /** Return cached velocity. */ - public GlobalVelocityR3 getVelocity() { + public VelocitySE2 getVelocity() { return m_stateCache.get().velocity(); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java index 91630f972..44f8fe55b 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java @@ -7,7 +7,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -73,9 +73,9 @@ public void execute() { // switch modes m_alternativeDriver.reset(state); } - GlobalVelocityR3 v = desiredVelocity(state, m_twistSupplier.get()); + VelocitySE2 v = desiredVelocity(state, m_twistSupplier.get()); // scale for driver skill. - GlobalVelocityR3 scaled = GeometryUtil.scale(v, DriverSkill.level().scale()); + VelocitySE2 scaled = GeometryUtil.scale(v, DriverSkill.level().scale()); // Apply field-relative limits. if (Experiments.instance.enabled(Experiment.UseSetpointGenerator)) { @@ -91,7 +91,7 @@ public void end(boolean interrupted) { m_drive.stop(); } - private GlobalVelocityR3 desiredVelocity(ModelR3 state, Velocity input) { + private VelocitySE2 desiredVelocity(ModelR3 state, Velocity input) { if (m_useAlternative.get()) { return m_alternativeDriver.apply(state, input); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java index 16aeef0bb..5516b3967 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java @@ -4,7 +4,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -31,9 +31,9 @@ public void apply(ModelR3 s, Velocity t) { if (DEBUG) { System.out.printf("FieldRelativeDriver %s\n", t); } - GlobalVelocityR3 v = m_driver.apply(s, t); + VelocitySE2 v = m_driver.apply(s, t); // scale for driver skill. - GlobalVelocityR3 scaled = GeometryUtil.scale(v, DriverSkill.level().scale()); + VelocitySE2 scaled = GeometryUtil.scale(v, DriverSkill.level().scale()); // Apply field-relative limits. if (Experiments.instance.enabled(Experiment.UseSetpointGenerator)) { diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java index 26a74da0c..1fe5521c6 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java @@ -1,6 +1,6 @@ package org.team100.lib.subsystems.swerve.commands.manual; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.state.ModelR3; @@ -13,7 +13,7 @@ public interface FieldRelativeDriver { * @param input control units [-1,1] * @return feasible field-relative velocity in m/s and rad/s */ - GlobalVelocityR3 apply(ModelR3 state, Velocity input); + VelocitySE2 apply(ModelR3 state, Velocity input); void reset(ModelR3 state); @@ -27,8 +27,8 @@ public interface FieldRelativeDriver { * @param maxRot radians per second * @return meters and rad per second as specified by speed limits */ - public static GlobalVelocityR3 scale(Velocity twist, double maxSpeed, double maxRot) { - return new GlobalVelocityR3( + public static VelocitySE2 scale(Velocity twist, double maxSpeed, double maxRot) { + return new VelocitySE2( maxSpeed * MathUtil.clamp(twist.x(), -1, 1), maxSpeed * MathUtil.clamp(twist.y(), -1, 1), maxRot * MathUtil.clamp(twist.theta(), -1, 1)); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java index 76979bb36..ac575da1e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java @@ -1,10 +1,10 @@ package org.team100.lib.subsystems.swerve.commands.manual; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; @@ -16,11 +16,11 @@ public class ManualFieldRelativeSpeeds implements FieldRelativeDriver { private final SwerveKinodynamics m_swerveKinodynamics; // LOGGERS - private final GlobalVelocityR3Logger m_log_scaled; + private final VelocitySE2Logger m_log_scaled; public ManualFieldRelativeSpeeds(LoggerFactory parent, SwerveKinodynamics swerveKinodynamics) { LoggerFactory log = parent.type(this); - m_log_scaled = log.globalVelocityR3Logger(Level.TRACE, "scaled"); + m_log_scaled = log.VelocitySE2Logger(Level.TRACE, "scaled"); m_swerveKinodynamics = swerveKinodynamics; } @@ -29,12 +29,12 @@ public ManualFieldRelativeSpeeds(LoggerFactory parent, SwerveKinodynamics swerve * feasible) speeds. */ @Override - public GlobalVelocityR3 apply(ModelR3 state, Velocity input) { + public VelocitySE2 apply(ModelR3 state, Velocity input) { // clip the input to the unit circle final Velocity clipped = input.clip(1.0); // scale to max in both translation and rotation - final GlobalVelocityR3 scaled = FieldRelativeDriver.scale( + final VelocitySE2 scaled = FieldRelativeDriver.scale( clipped, m_swerveKinodynamics.getMaxDriveVelocityM_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java index e8ffa20fd..4bef49e4c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java @@ -5,7 +5,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -110,7 +110,7 @@ public void reset(ModelR3 state) { * @return feasible field-relative velocity in m/s and rad/s */ @Override - public GlobalVelocityR3 apply( + public VelocitySE2 apply( final ModelR3 state, final Velocity twist1_1) { final Model100 thetaState = state.theta(); @@ -120,7 +120,7 @@ public GlobalVelocityR3 apply( // clip the input to the unit circle final Velocity clipped = twist1_1.clip(1.0); // scale to max in both translation and rotation - final GlobalVelocityR3 scaled = FieldRelativeDriver.scale( + final VelocitySE2 scaled = FieldRelativeDriver.scale( clipped, m_swerveKinodynamics.getMaxDriveVelocityM_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); @@ -161,7 +161,7 @@ public GlobalVelocityR3 apply( -m_swerveKinodynamics.getMaxAngleSpeedRad_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); - final GlobalVelocityR3 withSnap = new GlobalVelocityR3(scaled.x(), scaled.y(), omega); + final VelocitySE2 withSnap = new VelocitySE2(scaled.x(), scaled.y(), omega); m_log_snap_mode.log(() -> true); m_log_goal_theta.log(m_goal::getRadians); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java index be70bd9f3..3ffe4d65e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java @@ -5,7 +5,7 @@ import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -90,7 +90,7 @@ public void reset(ModelR3 state) { * @return feasible field-relative velocity in m/s and rad/s */ @Override - public GlobalVelocityR3 apply( + public VelocitySE2 apply( final ModelR3 state, final Velocity input) { @@ -106,7 +106,7 @@ public GlobalVelocityR3 apply( // clip the input to the unit circle Velocity clipped = input.clip(1.0); Optional target = m_target.get(); - GlobalVelocityR3 scaledInput = FieldRelativeDriver.scale( + VelocitySE2 scaledInput = FieldRelativeDriver.scale( clipped, m_swerveKinodynamics.getMaxDriveVelocityM_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); @@ -151,7 +151,7 @@ public GlobalVelocityR3 apply( target.get().getY(), 0 }); - return new GlobalVelocityR3( + return new VelocitySE2( scaledInput.x(), scaledInput.y(), omega); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java index 3010b7d18..bf7548bd5 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java @@ -4,7 +4,7 @@ import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -114,10 +114,10 @@ public void reset(ModelR3 state) { * @return feasible field-relative velocity in m/s and rad/s */ @Override - public GlobalVelocityR3 apply( + public VelocitySE2 apply( final ModelR3 state, final Velocity twist1_1) { - final GlobalVelocityR3 control = clipAndScale(twist1_1); + final VelocitySE2 control = clipAndScale(twist1_1); final double currentVelocity = state.velocity().norm(); @@ -172,7 +172,7 @@ public GlobalVelocityR3 apply( thetaFF + thetaFB, -m_swerveKinodynamics.getMaxAngleSpeedRad_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); - GlobalVelocityR3 twistWithSnapM_S = new GlobalVelocityR3(control.x(), control.y(), omega); + VelocitySE2 twistWithSnapM_S = new VelocitySE2(control.x(), control.y(), omega); m_log_snap_mode.log(() -> true); m_log_goal_theta.log(m_goal::getRadians); @@ -184,7 +184,7 @@ public GlobalVelocityR3 apply( return twistWithSnapM_S; } - public GlobalVelocityR3 clipAndScale(Velocity twist1_1) { + public VelocitySE2 clipAndScale(Velocity twist1_1) { // clip the input to the unit circle final Velocity clipped = twist1_1.clip(1.0); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java index 42bee5ac9..79b8bdb33 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java @@ -4,7 +4,7 @@ import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -89,7 +89,7 @@ public void reset(ModelR3 state) { * @return feasible field-relative velocity in m/s and rad/s */ @Override - public GlobalVelocityR3 apply( + public VelocitySE2 apply( final ModelR3 state, final Velocity input) { @@ -130,9 +130,9 @@ public GlobalVelocityR3 apply( -m_swerveKinodynamics.getMaxAngleSpeedRad_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); - final GlobalVelocityR3 scaledInput = getScaledInput(input); + final VelocitySE2 scaledInput = getScaledInput(input); - final GlobalVelocityR3 twistWithLockM_S = new GlobalVelocityR3( + final VelocitySE2 twistWithLockM_S = new VelocitySE2( scaledInput.x(), scaledInput.y(), omega); m_log_apparent_motion.log(() -> targetMotion); @@ -144,11 +144,11 @@ public GlobalVelocityR3 apply( return twistWithLockM_S; } - private GlobalVelocityR3 getScaledInput(Velocity input) { + private VelocitySE2 getScaledInput(Velocity input) { // clip the input to the unit circle Velocity clipped = input.clip(1.0); // this is user input scaled to m/s and rad/s - GlobalVelocityR3 scaledInput = FieldRelativeDriver.scale( + VelocitySE2 scaledInput = FieldRelativeDriver.scale( clipped, m_swerveKinodynamics.getMaxDriveVelocityM_S(), m_swerveKinodynamics.getMaxAngleSpeedRad_S()); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java index 9ebb6ef24..27eb4e885 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java @@ -4,7 +4,7 @@ import org.team100.lib.framework.TimedRobot100; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.incremental.IncrementalProfile; import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile; @@ -287,7 +287,7 @@ public ChassisSpeeds toChassisSpeedsWithDiscretization( * This simply rotates the velocity from the field frame to the robot frame. */ public static ChassisSpeeds toInstantaneousChassisSpeeds( - GlobalVelocityR3 v, + VelocitySE2 v, Rotation2d theta) { return ChassisSpeeds.fromFieldRelativeSpeeds( v.x(), @@ -300,9 +300,9 @@ public static ChassisSpeeds toInstantaneousChassisSpeeds( * Field-relative speed, without discretization. * This simply rotates the velocity from the robot frame to the field frame. */ - public static GlobalVelocityR3 fromInstantaneousChassisSpeeds(ChassisSpeeds instantaneous, Rotation2d theta) { + public static VelocitySE2 fromInstantaneousChassisSpeeds(ChassisSpeeds instantaneous, Rotation2d theta) { ChassisSpeeds c = ChassisSpeeds.fromRobotRelativeSpeeds(instantaneous, theta); - return new GlobalVelocityR3(c.vxMetersPerSecond, c.vyMetersPerSecond, c.omegaRadiansPerSecond); + return new VelocitySE2(c.vxMetersPerSecond, c.vyMetersPerSecond, c.omegaRadiansPerSecond); } public SwerveDriveKinematics100 getKinematics() { diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiter.java index 8fe5a4fc3..51333bc84 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiter.java @@ -1,12 +1,12 @@ package org.team100.lib.subsystems.swerve.kinodynamics.limiter; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.logging.LoggerFactory.GlobalAccelerationR3Logger; +import org.team100.lib.logging.LoggerFactory.AccelerationSE2Logger; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; /** @@ -20,7 +20,7 @@ public class FieldRelativeAccelerationLimiter { private static final boolean DEBUG = false; private final DoubleLogger m_log_scale; - private final GlobalAccelerationR3Logger m_log_accel; + private final AccelerationSE2Logger m_log_accel; private final SwerveKinodynamics m_limits; private final double m_cartesianScale; private final double m_alphaScale; @@ -38,17 +38,17 @@ public FieldRelativeAccelerationLimiter( double alphaScale) { LoggerFactory log = parent.type(this); m_log_scale = log.doubleLogger(Level.TRACE, "scale"); - m_log_accel = log.globalAccelerationR3Logger(Level.TRACE, "accel"); + m_log_accel = log.AccelerationSE2Logger(Level.TRACE, "accel"); m_limits = limits; m_cartesianScale = cartesianScale; m_alphaScale = alphaScale; } - public GlobalVelocityR3 apply( - GlobalVelocityR3 prev, - GlobalVelocityR3 target) { + public VelocitySE2 apply( + VelocitySE2 prev, + VelocitySE2 target) { // Acceleration required to achieve the target. - GlobalAccelerationR3 accel = target.accel( + AccelerationSE2 accel = target.accel( prev, TimedRobot100.LOOP_PERIOD_S); m_log_accel.log(() -> accel); @@ -56,7 +56,7 @@ public GlobalVelocityR3 apply( double alphaScale = alphaScale(accel); double scale = Math.min(cartesianScale, alphaScale); m_log_scale.log(() -> scale); - GlobalVelocityR3 result = prev.plus(accel.times(scale).integrate(TimedRobot100.LOOP_PERIOD_S)); + VelocitySE2 result = prev.plus(accel.times(scale).integrate(TimedRobot100.LOOP_PERIOD_S)); if (DEBUG) { System.out.printf( "FieldRelativeAccelerationLimiter prev %s target %s accel %s cartesian scale %5.2f alpha scale %5.2f total scale %5.2f result %s\n", @@ -66,9 +66,9 @@ public GlobalVelocityR3 apply( } double cartesianScale( - GlobalVelocityR3 prev, - GlobalVelocityR3 target, - GlobalAccelerationR3 accel) { + VelocitySE2 prev, + VelocitySE2 target, + AccelerationSE2 accel) { double a = accel.norm(); if (Math.abs(a) < 1e-6) { // Avoid divide-by-zero. @@ -82,7 +82,7 @@ public GlobalVelocityR3 apply( return Math.min(1, accelLimit / a); } - private double alphaScale(GlobalAccelerationR3 accel) { + private double alphaScale(AccelerationSE2 accel) { double a = accel.theta(); if (Math.abs(a) < 1e-6) { // Avoid divide-by-zero. diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiter.java index e31f13e33..44ec44862 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiter.java @@ -1,13 +1,13 @@ package org.team100.lib.subsystems.swerve.kinodynamics.limiter; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -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.subsystems.swerve.kinodynamics.SwerveKinodynamics; /** @@ -17,9 +17,9 @@ public class FieldRelativeCapsizeLimiter { private static final boolean DEBUG = false; private final DoubleLogger m_log_scale; - private final GlobalAccelerationR3Logger m_log_accel; - private final GlobalVelocityR3Logger m_log_prev; - private final GlobalVelocityR3Logger m_log_target; + private final AccelerationSE2Logger m_log_accel; + private final VelocitySE2Logger m_log_prev; + private final VelocitySE2Logger m_log_target; private final SwerveKinodynamics limits; @@ -28,19 +28,19 @@ public FieldRelativeCapsizeLimiter( SwerveKinodynamics m_limits) { LoggerFactory log = parent.type(this); m_log_scale = log.doubleLogger(Level.TRACE, "scale"); - m_log_accel = log.globalAccelerationR3Logger(Level.TRACE, "accel"); - m_log_prev = log.globalVelocityR3Logger(Level.TRACE, "prev"); - m_log_target = log.globalVelocityR3Logger(Level.TRACE, "target"); + m_log_accel = log.AccelerationSE2Logger(Level.TRACE, "accel"); + m_log_prev = log.VelocitySE2Logger(Level.TRACE, "prev"); + m_log_target = log.VelocitySE2Logger(Level.TRACE, "target"); limits = m_limits; } - public GlobalVelocityR3 apply( - GlobalVelocityR3 prev, - GlobalVelocityR3 target) { + public VelocitySE2 apply( + VelocitySE2 prev, + VelocitySE2 target) { m_log_prev.log(() -> prev); m_log_target.log(() -> target); // Acceleration required to achieve the target. - GlobalAccelerationR3 accel = target.accel( + AccelerationSE2 accel = target.accel( prev, TimedRobot100.LOOP_PERIOD_S); m_log_accel.log(() -> accel); @@ -51,7 +51,7 @@ public GlobalVelocityR3 apply( } double scale = scale(a); m_log_scale.log(() -> scale); - GlobalVelocityR3 result = prev.plus(accel.times(scale).integrate(TimedRobot100.LOOP_PERIOD_S)); + VelocitySE2 result = prev.plus(accel.times(scale).integrate(TimedRobot100.LOOP_PERIOD_S)); if (DEBUG) { System.out.printf("FieldRelativeCapsizeLimiter prev %s target %s accel %s scale %5.2f result %s\n", prev, target, accel, scale, result); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiter.java index ca478fb03..febb5f974 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiter.java @@ -2,7 +2,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; @@ -24,14 +24,14 @@ public FieldRelativeVelocityLimiter( m_limits = limit; } - public GlobalVelocityR3 apply(GlobalVelocityR3 target) { + public VelocitySE2 apply(VelocitySE2 target) { if (Experiments.instance.enabled(Experiment.LimitsPreferRotation)) return preferRotation(target); return proportional(target); } /** Maintain translation and rotation proportionality. */ - GlobalVelocityR3 proportional(GlobalVelocityR3 target) { + VelocitySE2 proportional(VelocitySE2 target) { if (DEBUG) { System.out.printf("proportional %s\n", target); } @@ -63,8 +63,8 @@ GlobalVelocityR3 proportional(GlobalVelocityR3 target) { } /** Both rotation and translation, scale proportionally. */ - private GlobalVelocityR3 proportional( - GlobalVelocityR3 target, + private VelocitySE2 proportional( + VelocitySE2 target, final double maxV, final double maxOmega, double xySpeed) { @@ -74,41 +74,41 @@ private GlobalVelocityR3 proportional( if (DEBUG) { System.out.printf("FieldRelativeVelocityLimiter proportional scale %.5f\n", scale); } - return new GlobalVelocityR3( + return new VelocitySE2( scale * target.x(), scale * target.y(), scale * target.theta()); } /** No rotation at all, so use maxV. */ - private GlobalVelocityR3 translateOnly(GlobalVelocityR3 target, double maxV, double xySpeed) { + private VelocitySE2 translateOnly(VelocitySE2 target, double maxV, double xySpeed) { double xyAngle = Math.atan2(target.y(), target.x()); double scale = Math.abs(maxV / xySpeed); m_log_scale.log(() -> scale); if (DEBUG) { System.out.printf("max v %s\n", target); } - return new GlobalVelocityR3( + return new VelocitySE2( maxV * Math.cos(xyAngle), maxV * Math.sin(xyAngle), 0); } /** Spinning in place, faster than is possible, so use maxOmega. */ - private GlobalVelocityR3 spinOnly(GlobalVelocityR3 target, double maxOmega) { + private VelocitySE2 spinOnly(VelocitySE2 target, double maxOmega) { double scale = Math.abs(maxOmega / target.theta()); m_log_scale.log(() -> scale); if (DEBUG) { System.out.printf("max omega %s\n", target); } - return new GlobalVelocityR3( + return new VelocitySE2( 0, 0, Math.signum(target.theta()) * maxOmega); } /** Scales translation to accommodate the rotation. */ - GlobalVelocityR3 preferRotation(GlobalVelocityR3 speeds) { + VelocitySE2 preferRotation(VelocitySE2 speeds) { double omegaRatio = Math.min(1, speeds.theta() / m_limits.getMaxAngleSpeedRad_S()); double xySpeed = speeds.norm(); double maxV = m_limits.getMaxDriveVelocityM_S(); @@ -125,7 +125,7 @@ GlobalVelocityR3 preferRotation(GlobalVelocityR3 speeds) { System.out.printf("FieldRelativeVelocityLimiter rotation ratio %.5f\n", ratio); } - return new GlobalVelocityR3( + return new VelocitySE2( ratio * maxV * Math.cos(xyAngle), ratio * maxV * Math.sin(xyAngle), speeds.theta()); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadband.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadband.java index 0d6576cab..da00939b4 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadband.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadband.java @@ -1,6 +1,6 @@ package org.team100.lib.subsystems.swerve.kinodynamics.limiter; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; @@ -29,7 +29,7 @@ public SwerveDeadband(LoggerFactory parent) { m_log_scale = log.doubleLogger(Level.TRACE, "scale"); } - public GlobalVelocityR3 apply(GlobalVelocityR3 target) { + public VelocitySE2 apply(VelocitySE2 target) { if (Math.abs(target.x()) > m_translationLimit || Math.abs(target.y()) > m_translationLimit || Math.abs(target.theta()) > m_omegaLimit) { @@ -37,6 +37,6 @@ public GlobalVelocityR3 apply(GlobalVelocityR3 target) { return target; } m_log_scale.log(() -> 0.0); - return GlobalVelocityR3.ZERO; + return VelocitySE2.ZERO; } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiter.java index 7892008d4..9b63c502e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiter.java @@ -4,11 +4,11 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; /** @@ -23,20 +23,20 @@ public class SwerveLimiter { private final DoubleLogger m_log_norm; private final DoubleLogger m_log_normIn; - private final GlobalVelocityR3Logger m_log_next; + private final VelocitySE2Logger m_log_next; private final FieldRelativeVelocityLimiter m_velocityLimiter; private final FieldRelativeCapsizeLimiter m_capsizeLimiter; private final FieldRelativeAccelerationLimiter m_accelerationLimiter; private final SwerveDeadband m_deadband; // Velocity expected at the current time, i.e. the previous time step's desire. - private GlobalVelocityR3 m_current; + private VelocitySE2 m_current; public SwerveLimiter(LoggerFactory parent, SwerveKinodynamics dynamics, DoubleSupplier voltage) { LoggerFactory log = parent.type(this); m_log_norm = log.doubleLogger(Level.TRACE, "norm"); m_log_normIn = log.doubleLogger(Level.TRACE, "norm in"); - m_log_next = log.globalVelocityR3Logger(Level.TRACE, "next"); + m_log_next = log.VelocitySE2Logger(Level.TRACE, "next"); BatterySagSpeedLimit limit = new BatterySagSpeedLimit(log, dynamics, voltage); m_velocityLimiter = new FieldRelativeVelocityLimiter(log, limit); @@ -56,7 +56,7 @@ public SwerveLimiter(LoggerFactory parent, SwerveKinodynamics dynamics, DoubleSu * Find a feasible setpoint in the direction of the target, and remember it for * next time. */ - public GlobalVelocityR3 apply(GlobalVelocityR3 nextReference) { + public VelocitySE2 apply(VelocitySE2 nextReference) { m_log_next.log(() -> nextReference); m_log_normIn.log(nextReference::norm); if (DEBUG) { @@ -66,7 +66,7 @@ public GlobalVelocityR3 apply(GlobalVelocityR3 nextReference) { m_current = nextReference; // First, limit the goal to a feasible velocity. - GlobalVelocityR3 result = m_velocityLimiter.apply(nextReference); + VelocitySE2 result = m_velocityLimiter.apply(nextReference); if (DEBUG) { System.out.printf("velocity limited %s\n", result); } @@ -102,7 +102,7 @@ public GlobalVelocityR3 apply(GlobalVelocityR3 nextReference) { * Set the current setpoint to the current velocity measurement. * This is required to make resumption of manual control smooth. */ - public void updateSetpoint(GlobalVelocityR3 setpoint) { + public void updateSetpoint(VelocitySE2 setpoint) { m_current = setpoint; } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java index 7fe24ec71..65208de7d 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java @@ -1,8 +1,8 @@ package org.team100.lib.subsystems.swerve.kinodynamics.limiter; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.util.Math100; @@ -21,8 +21,8 @@ public static double getAccelLimit( SwerveKinodynamics m_limits, double vScale, double aScale, - GlobalVelocityR3 prev, - GlobalVelocityR3 desired) { + VelocitySE2 prev, + VelocitySE2 desired) { if (isAccel(prev, desired)) { return minAccel(m_limits, vScale, aScale, prev.norm()); } @@ -53,9 +53,9 @@ public static double minAccel(SwerveKinodynamics m_limits, double vScale, double * This correctly captures sharp turns as decelerations; simply comparing the * magnitudes of initial and final velocities is not correct. */ - static boolean isAccel(GlobalVelocityR3 prev, - GlobalVelocityR3 target) { - GlobalAccelerationR3 accel = target.accel(prev, TimedRobot100.LOOP_PERIOD_S); + static boolean isAccel(VelocitySE2 prev, + VelocitySE2 target) { + AccelerationSE2 accel = target.accel(prev, TimedRobot100.LOOP_PERIOD_S); double dot = prev.x() * accel.x() + prev.y() * accel.y(); return dot >= 0; } diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/TankManual.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/TankManual.java index d68f68643..73bdb9859 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/TankManual.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/TankManual.java @@ -4,7 +4,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; @@ -59,7 +59,7 @@ public void execute() { m_logTranslation.log(() -> translationM_S); m_logRotation.log(() -> rotationRad_S); Rotation2d currentRotation = m_drive.getPose().getRotation(); - GlobalVelocityR3 v = SwerveKinodynamics.fromInstantaneousChassisSpeeds( + VelocitySE2 v = SwerveKinodynamics.fromInstantaneousChassisSpeeds( new ChassisSpeeds(translationM_S, 0, rotationRad_S), currentRotation); if (Experiments.instance.enabled(Experiment.UseSetpointGenerator)) { v = m_limiter.apply(v); diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java index 8ed815d6b..d1a6ea49a 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -49,12 +49,12 @@ public ModelR3 getState() { * @param nextV toolpoint velocity for the next timestep */ @Override - public void setVelocity(GlobalVelocityR3 nextV) { + public void setVelocity(VelocitySE2 nextV) { // the component of the rotation part that tries to move the // delegate in x and y // respecting 100% of this velocity will keep the toolpoint // where it wants to go (if the delegate responds perfectly) - GlobalVelocityR3 tangentialVelocity = OffsetUtil.tangentialVelocity( + VelocitySE2 tangentialVelocity = OffsetUtil.tangentialVelocity( OffsetUtil.omega(nextV), r(m_offset.unaryMinus())); m_delegate.setVelocity(nextV.plus(tangentialVelocity)); @@ -76,8 +76,8 @@ private Pose2d toolpointPose() { /** * Computes toolpoint velocity from delegate velocity, pose, and offset. */ - private GlobalVelocityR3 toolpointVelocity() { - GlobalVelocityR3 delegateVelocity = m_delegate.getState().velocity(); + private VelocitySE2 toolpointVelocity() { + VelocitySE2 delegateVelocity = m_delegate.getState().velocity(); return delegateVelocity.plus( OffsetUtil.tangentialVelocity( OffsetUtil.omega(delegateVelocity), r(m_offset))); diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java index 86cb688a7..cd1eb671c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -59,19 +59,19 @@ public ModelR3 getState() { * @param nextV toolpoint velocity for the next timestep */ @Override - public void setVelocity(GlobalVelocityR3 nextV) { + public void setVelocity(VelocitySE2 nextV) { // the component of the cartesian part that tries to spin // the delegate // adding some of this will make the toolpoint move more rapidly // towards the cartesian goal, while injecting theta error. - GlobalVelocityR3 perpendicularOmega = OffsetUtil.omega( + VelocitySE2 perpendicularOmega = OffsetUtil.omega( r(m_offset), OffsetUtil.velocity(nextV)); // the component of the rotation part that tries to move the // delegate in x and y // respecting 100% of this velocity will keep the toolpoint // where it wants to go (if the delegate responds perfectly) - GlobalVelocityR3 tangentialVelocity = OffsetUtil.tangentialVelocity( + VelocitySE2 tangentialVelocity = OffsetUtil.tangentialVelocity( OffsetUtil.omega(nextV), r(m_offset.unaryMinus())); m_delegate.setVelocity(nextV @@ -95,8 +95,8 @@ private Pose2d toolpointPose() { /** * Computes toolpoint velocity from delegate velocity, pose, and offset. */ - private GlobalVelocityR3 toolpointVelocity() { - GlobalVelocityR3 delegateVelocity = m_delegate.getState().velocity(); + private VelocitySE2 toolpointVelocity() { + VelocitySE2 delegateVelocity = m_delegate.getState().velocity(); return delegateVelocity.plus( OffsetUtil.tangentialVelocity( OffsetUtil.omega(delegateVelocity), r(m_offset))); diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetUtil.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetUtil.java index c9010fa8e..f985ce9e4 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetUtil.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetUtil.java @@ -1,6 +1,6 @@ package org.team100.lib.subsystems.test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import edu.wpi.first.math.Vector; import edu.wpi.first.math.numbers.N3; @@ -14,8 +14,8 @@ public class OffsetUtil { * * Cartesian components are always zero. */ - static GlobalVelocityR3 omega(Vector r, Vector v) { - return GlobalVelocityR3.fromVector( + static VelocitySE2 omega(Vector r, Vector v) { + return VelocitySE2.fromVector( Vector.cross(r, v).div(r.norm() * r.norm())); } @@ -27,23 +27,23 @@ static GlobalVelocityR3 omega(Vector r, Vector v) { * * Omega component is always zero. */ - static GlobalVelocityR3 tangentialVelocity( + static VelocitySE2 tangentialVelocity( Vector omega, Vector r) { - return GlobalVelocityR3.fromVector( + return VelocitySE2.fromVector( Vector.cross(omega, r)); } /** * Cartesian component of velocity. */ - static Vector velocity(GlobalVelocityR3 v) { + static Vector velocity(VelocitySE2 v) { return v.vVector(); } /** * Omega component of the velocity */ - static Vector omega(GlobalVelocityR3 v) { + static Vector omega(VelocitySE2 v) { return v.omegaVector(); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java index bf7faf8c6..f44a87dc5 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java @@ -3,7 +3,7 @@ import org.team100.lib.coherence.Cache; import org.team100.lib.coherence.ObjectCache; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -16,11 +16,11 @@ public class TrivialDrivetrain implements VelocitySubsystemR3 { private static final double DT = TimedRobot100.LOOP_PERIOD_S; private final ObjectCache m_stateCache; /** setpoint for the next timestep. used only by update(). */ - private GlobalVelocityR3 m_setpoint; + private VelocitySE2 m_setpoint; private ModelR3 m_state; public TrivialDrivetrain(Pose2d initial) { - m_setpoint = new GlobalVelocityR3(0, 0, 0); + m_setpoint = new VelocitySE2(0, 0, 0); m_state = new ModelR3(initial); m_stateCache = Cache.of(this::update); } @@ -32,11 +32,11 @@ public ModelR3 getState() { @Override public void stop() { - m_setpoint = new GlobalVelocityR3(0, 0, 0); + m_setpoint = new VelocitySE2(0, 0, 0); } @Override - public void setVelocity(GlobalVelocityR3 setpoint) { + public void setVelocity(VelocitySE2 setpoint) { m_setpoint = setpoint; } diff --git a/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java b/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java index f6288e226..c1c5cf91f 100644 --- a/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java +++ b/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java @@ -1,6 +1,6 @@ package org.team100.lib.targeting; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import edu.wpi.first.math.geometry.Rotation2d; @@ -37,7 +37,7 @@ public static Rotation2d absoluteBearing(Translation2d robot, Translation2d targ * @return apparent rotation of the target around the robot, rad/s */ public static double targetMotion(ModelR3 state, Translation2d target) { - GlobalVelocityR3 velocity = state.velocity(); + VelocitySE2 velocity = state.velocity(); if (velocity.angle().isEmpty()) { // If there's no robot motion, there's no target motion. return 0; diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 2d3936c74..9125cd8bd 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -3,7 +3,8 @@ import java.util.List; import java.util.function.Function; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.path.Path100; @@ -115,10 +116,10 @@ public Trajectory100 movingToRest(ModelR3 startState, Pose2d end) { public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { Translation2d startTranslation = startState.translation(); - GlobalVelocityR3 startVelocity = startState.velocity(); + VelocitySE2 startVelocity = startState.velocity(); Translation2d endTranslation = endState.translation(); - GlobalVelocityR3 endVelocity = endState.velocity(); + VelocitySE2 endVelocity = endState.velocity(); // should work with out this. if (startVelocity.norm() < VELOCITY_EPSILON && endVelocity.norm() < VELOCITY_EPSILON) { @@ -140,11 +141,11 @@ public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { new HolonomicPose2d( startTranslation, startState.rotation(), - startingAngle), + DirectionSE2.fromRotation(startingAngle)), new HolonomicPose2d( endTranslation, endState.rotation(), - courseToGoal)), + DirectionSE2.fromRotation(courseToGoal))), startVelocity.norm(), endVelocity.norm(), magicNumbers); @@ -154,12 +155,13 @@ public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { } } - public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, double splineEntranceVelocity, ModelR3 endState, Rotation2d endCourse, double splineExitVelocity) { + public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, double splineEntranceVelocity, + ModelR3 endState, Rotation2d endCourse, double splineExitVelocity) { Translation2d startTranslation = startState.translation(); - GlobalVelocityR3 startVelocity = startState.velocity(); + VelocitySE2 startVelocity = startState.velocity(); Translation2d endTranslation = endState.translation(); - GlobalVelocityR3 endVelocity = endState.velocity(); + VelocitySE2 endVelocity = endState.velocity(); // should work with out this. if (startVelocity.norm() < VELOCITY_EPSILON && endVelocity.norm() < VELOCITY_EPSILON) { @@ -179,11 +181,11 @@ public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, new HolonomicPose2d( startTranslation, startState.rotation(), - startCourse), + DirectionSE2.fromRotation(startCourse)), new HolonomicPose2d( endTranslation, endState.rotation(), - endCourse)), + DirectionSE2.fromRotation(endCourse))), splineEntranceVelocity, splineExitVelocity, magicNumbers); @@ -193,8 +195,10 @@ public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, } } - public Trajectory100 movingToRest(ModelR3 startState, Rotation2d startCourse, double splineEntranceVelocity, Pose2d end, Rotation2d endCourse, double splineExitVelocity) { - return movingToMoving(startState, startCourse,splineEntranceVelocity, new ModelR3(end), endCourse,splineExitVelocity); + public Trajectory100 movingToRest(ModelR3 startState, Rotation2d startCourse, double splineEntranceVelocity, + Pose2d end, Rotation2d endCourse, double splineExitVelocity) { + return movingToMoving(startState, startCourse, splineEntranceVelocity, new ModelR3(end), endCourse, + splineExitVelocity); } /** @@ -209,8 +213,14 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { try { return restToRest( List.of( - new HolonomicPose2d(startTranslation, start.getRotation(), courseToGoal), - new HolonomicPose2d(endTranslation, end.getRotation(), courseToGoal))); + new HolonomicPose2d( + startTranslation, + start.getRotation(), + DirectionSE2.fromRotation(courseToGoal)), + new HolonomicPose2d( + endTranslation, + end.getRotation(), + DirectionSE2.fromRotation(courseToGoal)))); } catch (TrajectoryGenerationException e) { return null; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 45314b42a..2d7adbb03 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import java.util.List; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; @@ -28,7 +29,7 @@ public static Path100 pathFromWaypoints( new HolonomicSpline( waypoints.get(i - 1), waypoints.get(i), - mN.get(i-1), + mN.get(i - 1), mN.get(i))); } // does not force C1, theta responds too much @@ -68,8 +69,12 @@ public static Path100 withoutControlPoints( Translation2d p1 = waypoints.get(i).getTranslation(); Rotation2d course = p1.minus(p0).getAngle(); splines.add(new HolonomicSpline( - new HolonomicPose2d(p0, waypoints.get(i - 1).getRotation(), course), - new HolonomicPose2d(p1, waypoints.get(i).getRotation(), course))); + new HolonomicPose2d( + p0, waypoints.get(i - 1).getRotation(), + DirectionSE2.fromRotation(course)), + new HolonomicPose2d( + p1, waypoints.get(i).getRotation(), + DirectionSE2.fromRotation(course)))); } // then adjust the control points to make it C1 smooth SplineUtil.forceC1(splines); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 5cab58027..28d7b2859 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -2,6 +2,7 @@ import java.util.Optional; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; @@ -33,12 +34,12 @@ public class HolonomicSpline { private final SplineR1 m_x; private final SplineR1 m_y; - private final SplineR1 m_theta; + private final SplineR1 m_heading; /** - * Offset for rotational spline: the rotational spline doesn't include the + * Offset for heading spline: the heading spline doesn't include the * starting point in order to correctly handle wrapping. */ - private final Rotation2d m_r0; + private final Rotation2d m_heading0; /** * The theta endpoint derivative is just the average theta rate, which is new, @@ -78,14 +79,17 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl if (DEBUG) { System.out.printf("scale %f %f\n", scale0, scale1); } - Translation2d course0 = new Translation2d(1,0).rotateBy(p0.course()); - Translation2d course1 = new Translation2d(1,0).rotateBy(p1.course()); - + + // Translation2d course0 = new Translation2d(1,0).rotateBy(p0.course()); + // Translation2d course1 = new Translation2d(1,0).rotateBy(p1.course()); + double x0 = p0.translation().getX(); double x1 = p1.translation().getX(); // first derivatives are just the course - double dx0 = course0.getX() * scale0; - double dx1 = course1.getX() * scale1; + // double dx0 = course0.getX() * scale0; + double dx0 = p0.course().x * scale0; + // double dx1 = course1.getX() * scale1; + double dx1 = p1.course().x * scale1; // second derivatives are zero at the ends double ddx0 = 0; double ddx1 = 0; @@ -93,8 +97,10 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl double y0 = p0.translation().getY(); double y1 = p1.translation().getY(); // first derivatives are just the course - double dy0 = course0.getY() * scale0; - double dy1 = course1.getY() * scale1; + // double dy0 = course0.getY() * scale0; + double dy0 = p0.course().y * scale0; + // double dy1 = course1.getY() * scale1; + double dy1 = p1.course().y * scale1; // second derivatives are zero at the ends double ddy0 = 0; double ddy1 = 0; @@ -102,7 +108,7 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl m_x = SplineR1.get(x0, x1, dx0, dx1, ddx0, ddx1); m_y = SplineR1.get(y0, y1, dy0, dy1, ddy0, ddy1); - m_r0 = p0.heading(); + m_heading0 = p0.heading(); double delta = p1.heading().minus(p0.heading()).getRadians(); // previously dtheta at the endpoints was zero, which is bad: it meant the omega @@ -117,12 +123,12 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl // second derivatives are zero at the ends double ddtheta0 = 0; double ddtheta1 = 0; - m_theta = SplineR1.get(0.0, delta, dtheta0, dtheta1, ddtheta0, ddtheta1); + m_heading = SplineR1.get(0.0, delta, dtheta0, dtheta1, ddtheta0, ddtheta1); } @Override public String toString() { - return "HolonomicSpline [m_x=" + m_x + ", m_y=" + m_y + ", m_theta=" + m_theta + ", m_r0=" + m_r0 + "]"; + return "HolonomicSpline [m_x=" + m_x + ", m_y=" + m_y + ", m_theta=" + m_heading + ", m_r0=" + m_heading0 + "]"; } /** This is used by various optimization steps. */ @@ -133,8 +139,8 @@ private HolonomicSpline( Rotation2d r0) { m_x = x; m_y = y; - m_theta = theta; - m_r0 = r0; + m_heading = theta; + m_heading0 = r0; } public Pose2dWithMotion getPose2dWithMotion(double p) { @@ -142,7 +148,7 @@ public Pose2dWithMotion getPose2dWithMotion(double p) { new HolonomicPose2d( getPoint(p), getHeading(p), - getCourse(p).orElseThrow()), + DirectionSE2.fromRotation(getCourse(p).orElseThrow())), getDHeadingDs(p), getCurvature(p), getDCurvatureDs(p)); @@ -171,11 +177,11 @@ public Pose2d getPose2d(double p) { //////////////////////////////////////////////////////////////////////// protected Rotation2d getHeading(double t) { - return m_r0.rotateBy(Rotation2d.fromRadians(m_theta.getPosition(t))); + return m_heading0.rotateBy(Rotation2d.fromRadians(m_heading.getPosition(t))); } protected double getDHeading(double t) { - return m_theta.getVelocity(t); + return m_heading.getVelocity(t); } HolonomicSpline replaceFirstDerivatives( @@ -201,13 +207,13 @@ HolonomicSpline replaceFirstDerivatives( m_y.getAcceleration(0), m_y.getAcceleration(1)), SplineR1.get( - m_theta.getPosition(0), - m_theta.getPosition(1), + m_heading.getPosition(0), + m_heading.getPosition(1), dtheta0, dtheta1, - m_theta.getAcceleration(0), - m_theta.getAcceleration(1)), - m_r0); + m_heading.getAcceleration(0), + m_heading.getAcceleration(1)), + m_heading0); } /** @@ -222,8 +228,8 @@ HolonomicSpline addToSecondDerivatives( return new HolonomicSpline( m_x.addCoefs(SplineR1.get(0, 0, 0, 0, ddx0_sub, ddx1_sub)), m_y.addCoefs(SplineR1.get(0, 0, 0, 0, ddy0_sub, ddy1_sub)), - m_theta, - m_r0); + m_heading, + m_heading0); } /** @@ -302,7 +308,7 @@ protected Translation2d getPoint(double t) { } double dtheta(double t) { - return m_theta.getVelocity(t); + return m_heading.getVelocity(t); } double ddx(double t) { @@ -314,7 +320,7 @@ protected Translation2d getPoint(double t) { } double ddtheta(double t) { - return m_theta.getAcceleration(t); + return m_heading.getAcceleration(t); } double dddx(double t) { @@ -326,7 +332,7 @@ protected Translation2d getPoint(double t) { } double dddtheta(double t) { - return m_theta.getJerk(t); + return m_heading.getJerk(t); } /** diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java index 87f540d12..0448b319b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java @@ -1,8 +1,12 @@ package org.team100.lib.trajectory.path.spline; +import java.util.Optional; + +import org.team100.lib.geometry.DirectionR3; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.HolonomicPose3d; import org.team100.lib.geometry.Pose3dWithMotion; +import org.team100.lib.util.Math100; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -25,11 +29,16 @@ public class HolonomicSpline3d { // low. most splines go between 0.5 and 5 meters so this is steps of 2 to 20 cm. private static final int SAMPLES = 25; + // these are for position private final SplineR1 m_x; private final SplineR1 m_y; private final SplineR1 m_z; + + // these are for heading + private final SplineR1 m_roll; private final SplineR1 m_pitch; private final SplineR1 m_yaw; + private final Rotation2d m_roll0; private final Rotation2d m_pitch0; private final Rotation2d m_yaw0; @@ -41,14 +50,14 @@ public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, dou double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); - Translation3d course0 = new Translation3d(1, 0, 0).rotateBy(p0.course()); - Translation3d course1 = new Translation3d(1, 0, 0).rotateBy(p1.course()); + DirectionR3 course0 = p0.course(); + DirectionR3 course1 = p1.course(); double x0 = p0.translation().getX(); double x1 = p1.translation().getX(); // first derivatives are just the course - double dx0 = course0.getX() * scale0; - double dx1 = course1.getX() * scale1; + double dx0 = course0.x * scale0; + double dx1 = course1.x * scale1; // second derivatives are zero at the ends double ddx0 = 0; double ddx1 = 0; @@ -56,8 +65,8 @@ public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, dou double y0 = p0.translation().getY(); double y1 = p1.translation().getY(); // first derivatives are just the course - double dy0 = course0.getY() * scale0; - double dy1 = course1.getY() * scale1; + double dy0 = course0.y * scale0; + double dy1 = course1.y * scale1; // second derivatives are zero at the ends double ddy0 = 0; double ddy1 = 0; @@ -65,8 +74,8 @@ public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, dou double z0 = p0.translation().getZ(); double z1 = p1.translation().getZ(); // first derivatives are just the course - double dz0 = course0.getZ() * scale0; - double dz1 = course1.getZ() * scale1; + double dz0 = course0.z * scale0; + double dz1 = course1.z * scale1; // second derivatives are zero at the ends double ddz0 = 0; double ddz1 = 0; @@ -75,13 +84,24 @@ public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, dou m_y = SplineR1.get(y0, y1, dy0, dy1, ddy0, ddy1); m_z = SplineR1.get(z0, z1, dz0, dz1, ddz0, ddz1); + m_roll0 = new Rotation2d(p0.heading().getX()); m_pitch0 = new Rotation2d(p0.heading().getY()); m_yaw0 = new Rotation2d(p0.heading().getZ()); Rotation3d headingDelta = p1.heading().minus(p0.heading()); + double rollDelta = headingDelta.getX(); double pitchDelta = headingDelta.getY(); double yawDelta = headingDelta.getZ(); + // first derivative is the average + double droll0 = rollDelta * mN0; + double droll1 = rollDelta * mN1; + // second derivatives are zero at the ends + double ddroll0 = 0; + double ddroll1 = 0; + m_roll = SplineR1.get(0.0, rollDelta, droll0, droll1, ddroll0, ddroll1); + + // first derivative is the average double dpitch0 = pitchDelta * mN0; double dpitch1 = pitchDelta * mN1; // second derivatives are zero at the ends @@ -89,12 +109,14 @@ public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, dou double ddpitch1 = 0; m_pitch = SplineR1.get(0.0, pitchDelta, dpitch0, dpitch1, ddpitch0, ddpitch1); + // first derivative is the average double dyaw0 = yawDelta * mN0; double dyaw1 = yawDelta * mN1; // second derivatives are zero at the ends double ddyaw0 = 0; double ddyaw1 = 0; m_yaw = SplineR1.get(0.0, yawDelta, dyaw0, dyaw1, ddyaw0, ddyaw1); + } /** @@ -119,16 +141,28 @@ protected Translation3d getPoint(double t) { return m_z.getPosition(t); } - public Pose3dWithMotion getPose3dWithMotion(double p) { - return null; - // return new Pose3dWithMotion( - // new HolonomicPose3d( - // getPoint(p), - // getHeading(p), - // getCourse(p).orElseThrow()), - // getDHeadingDs(p), - // getCurvature(p), - // getDCurvatureDs(p)); + // public Pose3dWithMotion getPose3dWithMotion(double p) { + // return new Pose3dWithMotion( + // new HolonomicPose3d( + // getPoint(p), + // getHeading(p), + // getCourse(p).orElseThrow()), + // getDHeadingDs(p), + // getCurvature(p), + // getDCurvatureDs(p)); + // } + + public Optional getCourse(double t) { + double dx = dx(t); + double dy = dy(t); + double dz = dz(t); + if (Math100.epsilonEquals(dx, 0.0) + && Math100.epsilonEquals(dy, 0.0) + && Math100.epsilonEquals(dz, 0.0)) { + // rotation below would be garbage so give up + return Optional.empty(); + } + return Optional.of(new DirectionR3(dx, dy, dz)); } protected Rotation3d getHeading(double t) { @@ -136,4 +170,27 @@ protected Rotation3d getHeading(double t) { // return m_r0.rotateBy(Rotation2d.fromRadians(m_theta.getPosition(t))); } + double dx(double t) { + return m_x.getVelocity(t); + } + + double dy(double t) { + return m_y.getVelocity(t); + } + + double dz(double t) { + return m_z.getVelocity(t); + } + + /** + * Velocity is the change in position per parameter, p: ds/dp (meters per p). + * Since p is not time, it is not "velocity" in the usual sense. + */ + protected double getVelocity(double t) { + double dx = dx(t); + double dy = dy(t); + double dz = dy(t); + return Math.sqrt(dx * dx + dy * dy + dz * dz); + } + } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java index 8242e1185..a13a258f8 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java @@ -34,7 +34,7 @@ public DiamondConstraint(LoggerFactory parent, double maxVX, double maxVY, doubl @Override public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - Rotation2d course = state.getPose().course(); + Rotation2d course = state.getPose().course().toRotation(); Rotation2d heading = state.getPose().heading(); Rotation2d strafe = course.minus(heading); // a rhombus is a superellipse with exponent 1 diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java index ae448a422..686715284 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java @@ -1,7 +1,7 @@ package org.team100.lib.trajectory.timing; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ControlR3; @@ -48,7 +48,7 @@ public JointConstraint( public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { HolonomicPose2d pose = state.getPose(); // Velocity if translation speed were 1.0 m/s. - GlobalVelocityR3 v = new GlobalVelocityR3( + VelocitySE2 v = new VelocitySE2( state.getCourse().getCos(), state.getCourse().getSin(), state.getHeadingRateRad_M()); @@ -69,7 +69,7 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { EAWConfig q = m_k.inverse(pose.pose()); - GlobalVelocityR3 maxV = m_j.forward(q, maxQdotInMotionDirection); + VelocitySE2 maxV = m_j.forward(q, maxQdotInMotionDirection); double norm = maxV.norm(); if (Double.isNaN(norm)) return new NonNegativeDouble(0); @@ -80,7 +80,7 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { public MinMaxAcceleration getMinMaxAcceleration( Pose2dWithMotion state, double velocityM_S) { Pose2d pose = state.getPose().pose(); - Rotation2d course2 = state.getPose().course(); + Rotation2d course2 = state.getPose().course().toRotation(); double c = course2.getCos(); double s = course2.getSin(); @@ -90,14 +90,14 @@ public MinMaxAcceleration getMinMaxAcceleration( double omega = velocityM_S * r; // actual cartesian velocity - GlobalVelocityR3 v = new GlobalVelocityR3(vx, vy, omega); + VelocitySE2 v = new VelocitySE2(vx, vy, omega); EAWConfig q = m_k.inverse(pose); // actual qdot JointVelocities qdot = m_j.inverse(new ModelR3(pose, v)); // find accel in motion - GlobalAccelerationR3 unitA = new GlobalAccelerationR3(c, s, r); + AccelerationSE2 unitA = new AccelerationSE2(c, s, r); ControlR3 sc = new ControlR3(pose, v, unitA); // corresponding a vector in joint space JointAccelerations qddot = m_j.inverseA(sc); @@ -112,7 +112,7 @@ public MinMaxAcceleration getMinMaxAcceleration( // scale qddot to the nearest maximum JointAccelerations maxQddotInMotionDirection = qddot.times(1 / maxScale); - GlobalAccelerationR3 fa = m_j.forwardA(q, qdot, maxQddotInMotionDirection); + AccelerationSE2 fa = m_j.forwardA(q, qdot, maxQddotInMotionDirection); double norm = fa.norm(); if (Double.isNaN(norm)) diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java index dc8c39a35..e7e72ef4a 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java @@ -5,7 +5,8 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -22,7 +23,7 @@ public class FeedforwardControllerR3Test { void testMotionless() { FeedforwardControllerR3 c = new FeedforwardControllerR3(logger, 0.01, 0.01, 0.01, 0.01); assertFalse(c.atReference()); - GlobalVelocityR3 v = c.calculate( + VelocitySE2 v = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -45,7 +46,7 @@ void testMotionless() { void testNotAtReference() { FeedforwardControllerR3 c = new FeedforwardControllerR3(logger, 0.01, 0.01, 0.01, 0.01); assertFalse(c.atReference()); - GlobalVelocityR3 v = c.calculate( + VelocitySE2 v = c.calculate( new ModelR3( new Model100(1, 0), new Model100(0, 0), @@ -68,7 +69,7 @@ void testNotAtReference() { void testFeedforward() { FeedforwardControllerR3 c = new FeedforwardControllerR3(logger, 0.01, 0.01, 0.01, 0.01); assertFalse(c.atReference()); - GlobalVelocityR3 v = c.calculate( + VelocitySE2 v = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java index 2650571f7..32c1b4c9c 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java @@ -5,10 +5,10 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalDeltaR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -30,7 +30,7 @@ class FullStateControllerR3Test implements Timeless { void testAtRest() { FullStateControllerR3 c = ControllerFactoryR3.test2(logger); assertFalse(c.atReference()); - GlobalVelocityR3 t = c.calculate( + VelocitySE2 t = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -54,7 +54,7 @@ void testFar() { FullStateControllerR3 c = ControllerFactoryR3.test2(logger); assertFalse(c.atReference()); // no velocity, no feedforward - GlobalVelocityR3 t = c.calculate( + VelocitySE2 t = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -78,7 +78,7 @@ void testFar() { void testFast() { FullStateControllerR3 c = ControllerFactoryR3.test2(logger); assertFalse(c.atReference()); - GlobalVelocityR3 t = c.calculate( + VelocitySE2 t = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -103,7 +103,7 @@ void testFast() { void testOnTrack() { FullStateControllerR3 c = ControllerFactoryR3.test2(logger); assertFalse(c.atReference()); - GlobalVelocityR3 t = c.calculate( + VelocitySE2 t = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -128,7 +128,7 @@ void testAllAxes() { FullStateControllerR3 c = ControllerFactoryR3.test2(logger); assertFalse(c.atReference()); // none of these have any velocity so there's no feedforward. - GlobalVelocityR3 t = c.calculate( + VelocitySE2 t = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -152,7 +152,7 @@ void testAllAxes() { void testRotation() { FullStateControllerR3 c = ControllerFactoryR3.test2(logger); assertFalse(c.atReference()); - GlobalVelocityR3 t = c.calculate( + VelocitySE2 t = c.calculate( new ModelR3( new Model100(0, 0), new Model100(0, 0), @@ -182,7 +182,7 @@ void testErrZero() { new Pose2dWithMotion( HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(0, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); assertEquals(0, err.getRotation().getRadians(), 0.001); @@ -196,7 +196,7 @@ void testErrXAhead() { ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); assertEquals(0, err.getRotation().getRadians(), 0.001); @@ -210,7 +210,7 @@ void testErrXBehind() { ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( HolonomicPose2d.make(1, 0, 0, 0), 0, 0, 0), 0, 0, 0)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); assertEquals(0, err.getRotation().getRadians(), 0.001); @@ -225,7 +225,7 @@ void testErrXAheadWithRotation() { ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( HolonomicPose2d.make(0, 0, 1, 0), 0, 0, 0), 0, 0, 0)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); assertEquals(0, err.getRotation().getRadians(), 0.001); @@ -252,7 +252,7 @@ void testErrorAhead() { double acceleration = 0; // we're exactly on the setpoint so zero error ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 positionError = controller.positionError(measurement, currentReference); + DeltaSE2 positionError = controller.positionError(measurement, currentReference); assertEquals(0, positionError.getX(), DELTA); assertEquals(0, positionError.getY(), DELTA); assertEquals(0, positionError.getRadians(), DELTA); @@ -276,7 +276,7 @@ void testErrorSideways() { // constant speed double acceleration = 0; ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 positionError = controller.positionError(measurement, currentReference); + DeltaSE2 positionError = controller.positionError(measurement, currentReference); assertEquals(1, positionError.getX(), DELTA); assertEquals(0, positionError.getY(), DELTA); assertEquals(0, positionError.getRadians(), DELTA); @@ -289,7 +289,7 @@ void testVelocityErrorZero() { // measurement position doesn't matter, rotation here matches velocity below ModelR3 measurement = new ModelR3( new Pose2d(1, 2, new Rotation2d(Math.PI)), - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( @@ -302,7 +302,7 @@ void testVelocityErrorZero() { // constant speed double acceleration = 0; ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalVelocityR3 error = controller.velocityError(measurement, currentReference); + VelocitySE2 error = controller.velocityError(measurement, currentReference); // we're exactly on the setpoint so zero error assertEquals(0, error.x(), DELTA); assertEquals(0, error.y(), DELTA); @@ -317,7 +317,7 @@ void testVelocityErrorAhead() { // measurement is the wrong velocity ModelR3 measurement = new ModelR3( new Pose2d(), - new GlobalVelocityR3(0, 1, 0)); + new VelocitySE2(0, 1, 0)); // motion is in a straight line, down the x axis // at the origin Pose2dWithMotion state = new Pose2dWithMotion( @@ -332,7 +332,7 @@ void testVelocityErrorAhead() { double acceleration = 0; ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalVelocityR3 error = controller.velocityError(measurement, currentReference); + VelocitySE2 error = controller.velocityError(measurement, currentReference); // error should include both components assertEquals(1, error.x(), DELTA); assertEquals(-1, error.y(), DELTA); @@ -357,7 +357,7 @@ void testFeedForwardAhead() { TimedPose setpoint = new TimedPose(state, t, velocity, acceleration); // feedforward should be straight ahead, no rotation. ControlR3 nextReference = ControlR3.fromTimedPose(setpoint); - GlobalVelocityR3 speeds = controller.feedforward(nextReference); + VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); assertEquals(0, speeds.theta(), DELTA); @@ -381,7 +381,7 @@ void testFeedForwardSideways() { TimedPose setpoint = new TimedPose(state, t, velocity, acceleration); // feedforward should be -y, robot relative, no rotation. ControlR3 nextReference = ControlR3.fromTimedPose(setpoint); - GlobalVelocityR3 speeds = controller.feedforward(nextReference); + VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); assertEquals(0, speeds.theta(), DELTA); @@ -404,7 +404,7 @@ void testFeedForwardTurning() { // constant speed double acceleration = 0; ControlR3 nextReference = ControlR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalVelocityR3 speeds = controller.feedforward(nextReference); + VelocitySE2 speeds = controller.feedforward(nextReference); // feedforward should be ahead and rotating. assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -436,11 +436,11 @@ void testFeedbackAhead() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 err = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 speeds = controller.positionFeedback(err); + DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 speeds = controller.positionFeedback(err); // we're exactly on the setpoint so zero feedback assertEquals(0, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -472,11 +472,11 @@ void testFeedbackAheadPlusY() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 err = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 speeds = controller.positionFeedback(err); + DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 speeds = controller.positionFeedback(err); // setpoint should be negative y assertEquals(0, speeds.x(), DELTA); assertEquals(-1, speeds.y(), DELTA); @@ -508,11 +508,11 @@ void testFeedbackAheadPlusTheta() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 err = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 speeds = controller.positionFeedback(err); + DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 speeds = controller.positionFeedback(err); // robot is on the setpoint in translation // but needs negative rotation // setpoint should be negative theta @@ -547,10 +547,10 @@ void testFeedbackSideways() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 err = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 speeds = controller.positionFeedback(err); + DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 speeds = controller.positionFeedback(err); // on target assertEquals(0, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -582,10 +582,10 @@ void testFeedbackSidewaysPlusY() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 err = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 speeds = controller.positionFeedback(err); + DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 speeds = controller.positionFeedback(err); // feedback is -y field relative assertEquals(0, speeds.x(), DELTA); assertEquals(-1, speeds.y(), DELTA); @@ -618,11 +618,11 @@ void testFullFeedbackAhead() { // motion is on setpoint ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 perr = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 verr = currentReference.velocity().minus(measurement.velocity()); - GlobalVelocityR3 speeds = controller.fullFeedback(perr, verr); + DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); + VelocitySE2 speeds = controller.fullFeedback(perr, verr); // we're exactly on the setpoint so zero feedback assertEquals(0, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -656,11 +656,11 @@ void testFullFeedbackSideways() { // measurement is too slow ModelR3 measurement = new ModelR3( currentPose, - new GlobalVelocityR3(0.5, 0, 0)); + new VelocitySE2(0.5, 0, 0)); ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 perr = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 verr = currentReference.velocity().minus(measurement.velocity()); - GlobalVelocityR3 speeds = controller.fullFeedback(perr, verr); + DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); + VelocitySE2 speeds = controller.fullFeedback(perr, verr); // speed up assertEquals(0.5, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -677,7 +677,7 @@ void testFullFeedbackSidewaysWithRotation() { ModelR3 measurement = new ModelR3( new Pose2d(0.1, 0.1, Rotation2d.kCCW_Pi_2.plus(new Rotation2d(0.1))), - new GlobalVelocityR3(0.5, 0, 0)); + new VelocitySE2(0.5, 0, 0)); // setpoint postion is ahead in x and y and theta HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, Math.PI / 2, 0); @@ -700,9 +700,9 @@ void testFullFeedbackSidewaysWithRotation() { ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); // feedforward should be straight ahead, no rotation. - GlobalDeltaR3 perr = GlobalDeltaR3.delta(measurement.pose(), currentReference.pose()); - GlobalVelocityR3 verr = currentReference.velocity().minus(measurement.velocity()); - GlobalVelocityR3 positionFeedback = controller.positionFeedback(perr); + DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); + VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); + VelocitySE2 positionFeedback = controller.positionFeedback(perr); // field-relative x is ahead assertEquals(-0.1, positionFeedback.x(), DELTA); // field-relative y is ahead @@ -710,13 +710,13 @@ void testFullFeedbackSidewaysWithRotation() { // pull back theta assertEquals(-0.1, positionFeedback.theta(), DELTA); - GlobalVelocityR3 velocityFeedback = controller.velocityFeedback(verr); + VelocitySE2 velocityFeedback = controller.velocityFeedback(verr); assertEquals(0.5, velocityFeedback.x(), DELTA); assertEquals(0, velocityFeedback.y(), DELTA); assertEquals(0, velocityFeedback.theta(), DELTA); - GlobalVelocityR3 speeds = controller.fullFeedback(perr, verr); + VelocitySE2 speeds = controller.fullFeedback(perr, verr); // this is just the sum assertEquals(0.4, speeds.x(), DELTA); assertEquals(-0.1, speeds.y(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java index 917033c21..371e16d3e 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java @@ -9,7 +9,8 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -131,11 +132,17 @@ void testTrajectoryDone() { void testFieldRelativeTrajectory() { List waypoints = new ArrayList<>(); waypoints.add(new HolonomicPose2d( - new Translation2d(), Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(0))); + new Translation2d(), + Rotation2d.fromDegrees(180), + DirectionSE2.TO_X)); waypoints.add(new HolonomicPose2d( - new Translation2d(100, 4), Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(0))); + new Translation2d(100, 4), + Rotation2d.fromDegrees(180), + DirectionSE2.TO_X)); waypoints.add(new HolonomicPose2d( - new Translation2d(196, 13), Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0))); + new Translation2d(196, 13), + Rotation2d.fromDegrees(0), + DirectionSE2.TO_X)); double start_vel = 0.0; double end_vel = 0.0; @@ -166,7 +173,7 @@ void testFieldRelativeTrajectory() { logger, drive, swerveController, reference); Pose2d pose = trajectory.sample(0).state().getPose().pose(); - GlobalVelocityR3 velocity = GlobalVelocityR3.ZERO; + VelocitySE2 velocity = VelocitySE2.ZERO; double mDt = 0.02; int i = 0; diff --git a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java index 3358df0a6..8b6f1c24f 100644 --- a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java +++ b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java @@ -11,7 +11,7 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -59,7 +59,7 @@ private static void verify(double x, ModelR3 state) { } private static void verifyVelocity(double xV, ModelR3 state) { - GlobalVelocityR3 v = state.velocity(); + VelocitySE2 v = state.velocity(); assertEquals(xV, v.x(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java index a75a5d77e..c60616304 100644 --- a/lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java +++ b/lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.coherence.Takt; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -24,9 +24,9 @@ class HolonomicProfileTest implements Timeless { void testSolve() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); ModelR3 i = new ModelR3( - new Pose2d(0, 0, Rotation2d.kZero), new GlobalVelocityR3(1, 0, 0)); + new Pose2d(0, 0, Rotation2d.kZero), new VelocitySE2(1, 0, 0)); ModelR3 g = new ModelR3( - new Pose2d(0, 2, Rotation2d.kZero), new GlobalVelocityR3(0, 0, 0)); + new Pose2d(0, 2, Rotation2d.kZero), new VelocitySE2(0, 0, 0)); hp.solve(i, g); // scale factors assertEquals(0.8125, hp.sx, DELTA); @@ -77,7 +77,7 @@ void test2dExp() { @Test void test2dWithEntrySpeed() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ModelR3 i = new ModelR3(new Pose2d(), new GlobalVelocityR3(1, 0, 0)); + ModelR3 i = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); ModelR3 g = new ModelR3(new Pose2d(0, 1, Rotation2d.kZero)); hp.solve(i, g); ControlR3 s = i.control(); @@ -104,7 +104,7 @@ void test2dWithEntrySpeed() { @Test void testSolvePerformance() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ModelR3 i = new ModelR3(new Pose2d(), new GlobalVelocityR3(1, 0, 0)); + ModelR3 i = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); ModelR3 g = new ModelR3(new Pose2d(0, 1, Rotation2d.kZero)); int N = 10000; double t0 = Takt.actual(); diff --git a/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java b/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java index 649015598..d69e55936 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java @@ -5,8 +5,8 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -33,7 +33,7 @@ void testForward() { EAWConfig q = new EAWConfig(0, 0, 0); // extended and motionless JointVelocities jv = new JointVelocities(0, 0, 0); - GlobalVelocityR3 v = j.forward(q, jv); + VelocitySE2 v = j.forward(q, jv); assertEquals(0, v.x(), DELTA); assertEquals(0, v.y(), DELTA); assertEquals(0, v.theta(), DELTA); @@ -87,21 +87,21 @@ void testInverse() { assertEquals(0, jv.wrist(), DELTA); // +x - v = new ModelR3(p, new GlobalVelocityR3(1, 0, 0)); + v = new ModelR3(p, new VelocitySE2(1, 0, 0)); jv = j.inverse(v); assertEquals(1, jv.elevator(), DELTA); assertEquals(0, jv.shoulder(), DELTA); assertEquals(0, jv.wrist(), DELTA); // +y - v = new ModelR3(p, new GlobalVelocityR3(0, 1, 0)); + v = new ModelR3(p, new VelocitySE2(0, 1, 0)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(0.5, jv.shoulder(), DELTA); assertEquals(-0.5, jv.wrist(), DELTA); // +theta - v = new ModelR3(p, new GlobalVelocityR3(0, 0, 1)); + v = new ModelR3(p, new VelocitySE2(0, 0, 1)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(-0.5, jv.shoulder(), DELTA); @@ -116,7 +116,7 @@ void testForwardA() { // extended, motionless JointVelocities qdot = new JointVelocities(0, 0, 0); JointAccelerations qddot = new JointAccelerations(0, 0, 0); - GlobalAccelerationR3 a = j.forwardA(q, qdot, qddot); + AccelerationSE2 a = j.forwardA(q, qdot, qddot); assertEquals(0, a.x(), DELTA); assertEquals(0, a.y(), DELTA); assertEquals(0, a.theta(), DELTA); @@ -175,8 +175,8 @@ void testInverseA() { // extended, motionless EAWConfig c = new EAWConfig(0, 0, 0); Pose2d p = k.forward(c); - GlobalVelocityR3 v = new GlobalVelocityR3(0, 0, 0); - ControlR3 m = new ControlR3(p, v, new GlobalAccelerationR3(0, 0, 0)); + VelocitySE2 v = new VelocitySE2(0, 0, 0); + ControlR3 m = new ControlR3(p, v, new AccelerationSE2(0, 0, 0)); JointAccelerations ja = j.inverseA(m); assertEquals(0, ja.elevator(), DELTA); assertEquals(0, ja.shoulder(), DELTA); @@ -185,8 +185,8 @@ void testInverseA() { // +x => +elevator c = new EAWConfig(0, 0, 0); p = k.forward(c); - v = new GlobalVelocityR3(0, 0, 0); - m = new ControlR3(p, v, new GlobalAccelerationR3(1, 0, 0)); + v = new VelocitySE2(0, 0, 0); + m = new ControlR3(p, v, new AccelerationSE2(1, 0, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0, ja.shoulder(), DELTA); @@ -195,8 +195,8 @@ void testInverseA() { // +y => +shoulder and -wrist (because zero theta) c = new EAWConfig(0, 0, 0); p = k.forward(c); - v = new GlobalVelocityR3(0, 0, 0); - m = new ControlR3(p, v, new GlobalAccelerationR3(0, 1, 0)); + v = new VelocitySE2(0, 0, 0); + m = new ControlR3(p, v, new AccelerationSE2(0, 1, 0)); ja = j.inverseA(m); assertEquals(0, ja.elevator(), DELTA); assertEquals(0.5, ja.shoulder(), DELTA); @@ -205,8 +205,8 @@ void testInverseA() { // +theta => -shoulder, +wrist (because no translation) c = new EAWConfig(0, 0, 0); p = k.forward(c); - v = new GlobalVelocityR3(0, 0, 0); - m = new ControlR3(p, v, new GlobalAccelerationR3(0, 0, 1)); + v = new VelocitySE2(0, 0, 0); + m = new ControlR3(p, v, new AccelerationSE2(0, 0, 1)); ja = j.inverseA(m); assertEquals(0, ja.elevator(), DELTA); assertEquals(-0.5, ja.shoulder(), DELTA); @@ -216,8 +216,8 @@ void testInverseA() { // using 45 deg because of singularity at 90 c = new EAWConfig(0, Math.PI / 4, 0); p = k.forward(c); - v = new GlobalVelocityR3(0, 0, 0); - m = new ControlR3(p, v, new GlobalAccelerationR3(1, 0, 0)); + v = new VelocitySE2(0, 0, 0); + m = new ControlR3(p, v, new AccelerationSE2(1, 0, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0, ja.shoulder(), DELTA); @@ -227,8 +227,8 @@ void testInverseA() { c = new EAWConfig(0, Math.PI / 4, 0); // using 45 deg because of singularity at 90 p = k.forward(c); - v = new GlobalVelocityR3(0, 0, 0); - m = new ControlR3(p, v, new GlobalAccelerationR3(0, 1, 0)); + v = new VelocitySE2(0, 0, 0); + m = new ControlR3(p, v, new AccelerationSE2(0, 1, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0.707, ja.shoulder(), DELTA); @@ -238,8 +238,8 @@ void testInverseA() { c = new EAWConfig(0, Math.PI / 4, Math.PI / 4); // using 45 deg because of singularity at 90 p = k.forward(c); - v = new GlobalVelocityR3(0, 0, 0); - m = new ControlR3(p, v, new GlobalAccelerationR3(0, 1, 0)); + v = new VelocitySE2(0, 0, 0); + m = new ControlR3(p, v, new AccelerationSE2(0, 1, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0.707, ja.shoulder(), DELTA); @@ -261,7 +261,7 @@ void testTrajectory() { TimedPose tp = t.sample(time); ModelR3 sm = ModelR3.fromTimedPose(tp); Pose2d p = sm.pose(); - GlobalVelocityR3 v = sm.velocity(); + VelocitySE2 v = sm.velocity(); EAWConfig c = k.inverse(p); JointVelocities jv = j.inverse(sm); if (DEBUG) diff --git a/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java b/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java index bb70b9288..19f472d5f 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java @@ -7,7 +7,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -138,21 +138,21 @@ void test05() { assertEquals(0, jv.wrist(), DELTA); // +x - v = new ModelR3(p, new GlobalVelocityR3(1, 0, 0)); + v = new ModelR3(p, new VelocitySE2(1, 0, 0)); jv = j.inverse(v); assertEquals(1, jv.elevator(), DELTA); assertEquals(0, jv.shoulder(), DELTA); assertEquals(0, jv.wrist(), DELTA); // +y - v = new ModelR3(p, new GlobalVelocityR3(0, 1, 0)); + v = new ModelR3(p, new VelocitySE2(0, 1, 0)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(0.5, jv.shoulder(), DELTA); assertEquals(-0.5, jv.wrist(), DELTA); // +theta - v = new ModelR3(p, new GlobalVelocityR3(0, 0, 1)); + v = new ModelR3(p, new VelocitySE2(0, 0, 1)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(-0.5, jv.shoulder(), DELTA); @@ -238,7 +238,7 @@ void testTrajectory() { TimedPose tp = t.sample(time); ModelR3 sm = ModelR3.fromTimedPose(tp); Pose2d p = sm.pose(); - GlobalVelocityR3 v = sm.velocity(); + VelocitySE2 v = sm.velocity(); EAWConfig c = k.inverse(p); JointVelocities jv = j.inverse(sm); if (DEBUG) diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java b/lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java index 4cf04b5d5..559982057 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java @@ -1,11 +1,11 @@ package org.team100.lib.subsystems.r3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; public class MockSubsystemR3 implements VelocitySubsystemR3 { - public GlobalVelocityR3 m_setpoint; - public GlobalVelocityR3 m_recentSetpoint; + public VelocitySE2 m_setpoint; + public VelocitySE2 m_recentSetpoint; public ModelR3 m_state; public MockSubsystemR3(ModelR3 initial) { @@ -23,7 +23,7 @@ public void stop() { } @Override - public void setVelocity(GlobalVelocityR3 setpoint) { + public void setVelocity(VelocitySE2 setpoint) { m_setpoint = setpoint; m_recentSetpoint = setpoint; } diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index e5cd84cf4..f25dee561 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ControlR3; @@ -20,7 +20,7 @@ class SwerveControlTest { @Test void testTransform() { Pose2d p = new Pose2d(new Translation2d(1, 1), new Rotation2d(1)); - GlobalVelocityR3 t = new GlobalVelocityR3(1, 1, 1); + VelocitySE2 t = new VelocitySE2(1, 1, 1); ControlR3 s = new ControlR3(p, t); assertEquals(1, s.x().x(), DELTA); } @@ -91,7 +91,7 @@ void testTimedPose4() { void testChassisSpeeds0() { ControlR3 state = new ControlR3( new Pose2d(new Translation2d(0, 0), Rotation2d.kPi), - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(-1, speeds.vxMetersPerSecond, DELTA); assertEquals(0, speeds.vyMetersPerSecond, DELTA); @@ -102,7 +102,7 @@ void testChassisSpeeds0() { void testChassisSpeeds1() { ControlR3 state = new ControlR3( new Pose2d(new Translation2d(0, 0), Rotation2d.kCCW_Pi_2), - new GlobalVelocityR3(1, 0, 1)); + new VelocitySE2(1, 0, 1)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(0, speeds.vxMetersPerSecond, DELTA); assertEquals(-1, speeds.vyMetersPerSecond, DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index e13604e47..760352346 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -4,7 +4,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ModelR3; @@ -21,7 +21,7 @@ class ModelR3Test { @Test void testTransform() { Pose2d p = new Pose2d(new Translation2d(1, 1), new Rotation2d(1)); - GlobalVelocityR3 t = new GlobalVelocityR3(1, 1, 1); + VelocitySE2 t = new VelocitySE2(1, 1, 1); ModelR3 s = new ModelR3(p, t); assertEquals(1, s.x().x(), DELTA); } @@ -91,7 +91,7 @@ void testTimedPose4() { void testChassisSpeeds0() { ModelR3 state = new ModelR3( new Pose2d(new Translation2d(0, 0), Rotation2d.kPi), - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(1, 0, 0)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(-1, speeds.vxMetersPerSecond, DELTA); assertEquals(0, speeds.vyMetersPerSecond, DELTA); @@ -102,7 +102,7 @@ void testChassisSpeeds0() { void testChassisSpeeds1() { ModelR3 state = new ModelR3( new Pose2d(new Translation2d(0, 0), Rotation2d.kCCW_Pi_2), - new GlobalVelocityR3(1, 0, 1)); + new VelocitySE2(1, 0, 1)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(0, speeds.vxMetersPerSecond, DELTA); assertEquals(-1, speeds.vyMetersPerSecond, DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java index d1ed3a0b5..f2847aeec 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -22,7 +22,7 @@ void testTwistZero() { ManualFieldRelativeSpeeds manual = new ManualFieldRelativeSpeeds(logger, limits); Velocity input = new Velocity(0, 0, 0); ModelR3 s = new ModelR3(); - GlobalVelocityR3 twist = manual.apply(s, input); + VelocitySE2 twist = manual.apply(s, input); assertEquals(0, twist.x(), DELTA); assertEquals(0, twist.y(), DELTA); assertEquals(0, twist.theta(), DELTA); @@ -35,7 +35,7 @@ void testTwistNonzero() { // these inputs are clipped Velocity input = new Velocity(1, 2, 3); ModelR3 s = new ModelR3(); - GlobalVelocityR3 twist = manual.apply(s, input); + VelocitySE2 twist = manual.apply(s, input); assertEquals(0.447, twist.x(), DELTA); assertEquals(0.894, twist.y(), DELTA); assertEquals(2.828, twist.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java index 54b97ca7f..4aedfff5c 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java @@ -9,7 +9,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -47,7 +47,7 @@ void testModeSwitching() { Velocity twist1_1 = new Velocity(0, 0, 0); - GlobalVelocityR3 twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); + VelocitySE2 twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); verify(0, 0, 0, twistM_S); // with a non-null desired rotation we're in snap mode @@ -79,7 +79,7 @@ void testNotSnapMode() { Velocity twist1_1 = new Velocity(0, 0, 1); - GlobalVelocityR3 twistM_S = m_manualWithHeading.apply( + VelocitySE2 twistM_S = m_manualWithHeading.apply( new ModelR3(), twist1_1); @@ -116,7 +116,7 @@ void testSnapMode() { // no user input final Velocity zeroVelocity = new Velocity(0, 0, 0); - GlobalVelocityR3 twistM_S = m_manualWithHeading.apply( + VelocitySE2 twistM_S = m_manualWithHeading.apply( new ModelR3(), zeroVelocity); // in snap mode @@ -188,8 +188,8 @@ void testSnapHeld() { // no stick input Velocity twist1_1 = new Velocity(0, 0, 0); - GlobalVelocityR3 v = m_manualWithHeading.apply( - new ModelR3(currentPose, new GlobalVelocityR3(0, 0, 0)), + VelocitySE2 v = m_manualWithHeading.apply( + new ModelR3(currentPose, new VelocitySE2(0, 0, 0)), twist1_1); // in snap mode @@ -248,11 +248,11 @@ void testStickyHeading() { ModelR3 currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 0)); + new VelocitySE2(0, 0, 0)); // no POV desiredRotation = null; - GlobalVelocityR3 v = m_manualWithHeading.apply(currentState, twist1_1); + VelocitySE2 v = m_manualWithHeading.apply(currentState, twist1_1); // scale 1.0 input to max omega assertNull(m_manualWithHeading.m_goal); assertNull(m_manualWithHeading.m_thetaSetpoint); @@ -261,7 +261,7 @@ void testStickyHeading() { // already going full speed: currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, twist1_1); @@ -273,7 +273,7 @@ void testStickyHeading() { twist1_1 = new Velocity(0, 0, 0); currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, twist1_1); @@ -304,11 +304,11 @@ void testStickyHeading2() { ModelR3 currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 0)); + new VelocitySE2(0, 0, 0)); // no POV desiredRotation = null; - GlobalVelocityR3 v = m_manualWithHeading.apply(currentState, twist1_1); + VelocitySE2 v = m_manualWithHeading.apply(currentState, twist1_1); // scale 1.0 input to max omega assertNull(m_manualWithHeading.m_goal); assertNull(m_manualWithHeading.m_thetaSetpoint); @@ -317,7 +317,7 @@ void testStickyHeading2() { // already going full speed: currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, twist1_1); @@ -329,7 +329,7 @@ void testStickyHeading2() { twist1_1 = new Velocity(0, 0, 0); currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, twist1_1); @@ -373,7 +373,7 @@ void testProfile() { } } - private void verify(double vx, double vy, double omega, GlobalVelocityR3 v) { + private void verify(double vx, double vy, double omega, VelocitySE2 v) { assertEquals(vx, v.x(), DELTA); assertEquals(vy, v.y(), DELTA); assertEquals(omega, v.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java index f13e51b3e..d2e1f4a84 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java @@ -11,7 +11,7 @@ import org.team100.lib.controller.r1.PIDFeedback; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -51,7 +51,7 @@ void testModeSwitching() { Velocity twist1_1 = new Velocity(0, 0, 0); - GlobalVelocityR3 twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); + VelocitySE2 twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); verify(0, 0, 0, twistM_S); // with a non-null desired rotation we're in snap mode @@ -85,7 +85,7 @@ void testNotSnapMode() { Velocity twist1_1 = new Velocity(0, 0, 1); - GlobalVelocityR3 twistM_S = m_manualWithHeading.apply( + VelocitySE2 twistM_S = m_manualWithHeading.apply( new ModelR3(), twist1_1); @@ -124,7 +124,7 @@ void testSnapMode() { final Velocity twist1_1 = new Velocity(0, 0, 0); // initial state is motionless - GlobalVelocityR3 twistM_S = m_manualWithHeading.apply( + VelocitySE2 twistM_S = m_manualWithHeading.apply( new ModelR3(), twist1_1); // in snap mode @@ -147,7 +147,7 @@ void testSnapMode() { twistM_S = m_manualWithHeading.apply( new ModelR3( new Pose2d(0, 0, new Rotation2d(0.5)), - new GlobalVelocityR3(0, 0, 0.1)), + new VelocitySE2(0, 0, 0.1)), twist1_1); assertEquals(1.017, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -158,7 +158,7 @@ void testSnapMode() { twistM_S = m_manualWithHeading.apply( new ModelR3( new Pose2d(0, 0, new Rotation2d(1.55)), - new GlobalVelocityR3(0, 0, 0.2)), + new VelocitySE2(0, 0, 0.2)), twist1_1); assertEquals(0.183, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -170,7 +170,7 @@ void testSnapMode() { twistM_S = m_manualWithHeading.apply( new ModelR3( new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), - new GlobalVelocityR3(0, 0, 0)), + new VelocitySE2(0, 0, 0)), twist1_1); assertNotNull(m_manualWithHeading.m_goal); @@ -202,7 +202,7 @@ void testSnapHeld() { // no stick input final Velocity twist1_1 = new Velocity(0, 0, 0); - GlobalVelocityR3 v = m_manualWithHeading.apply( + VelocitySE2 v = m_manualWithHeading.apply( new ModelR3(), twist1_1); @@ -217,7 +217,7 @@ void testSnapHeld() { v = m_manualWithHeading.apply( new ModelR3( new Pose2d(0, 0, new Rotation2d(0.5)), - new GlobalVelocityR3(0, 0, 1)), + new VelocitySE2(0, 0, 1)), twist1_1); assertEquals(1.017, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -228,7 +228,7 @@ void testSnapHeld() { v = m_manualWithHeading.apply( new ModelR3( new Pose2d(0, 0, new Rotation2d(1.555)), - new GlobalVelocityR3(0, 0, 0.2)), + new VelocitySE2(0, 0, 0.2)), twist1_1); assertEquals(0.183, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -241,7 +241,7 @@ void testSnapHeld() { v = m_manualWithHeading.apply( new ModelR3( new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), - new GlobalVelocityR3(0, 0, 0)), + new VelocitySE2(0, 0, 0)), twist1_1); assertNotNull(m_manualWithHeading.m_goal); // there should be no more profile to follow @@ -270,11 +270,11 @@ void testStickyHeading() { ModelR3 currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 0)); + new VelocitySE2(0, 0, 0)); // no POV desiredRotation = null; - GlobalVelocityR3 v = m_manualWithHeading.apply(currentState, control); + VelocitySE2 v = m_manualWithHeading.apply(currentState, control); // scale 1.0 input to max omega assertNull(m_manualWithHeading.m_goal); assertNull(m_manualWithHeading.m_thetaSetpoint); @@ -283,7 +283,7 @@ void testStickyHeading() { // already going full speed: currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, control); @@ -295,7 +295,7 @@ void testStickyHeading() { control = new Velocity(0, 0, 0); currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; @@ -341,11 +341,11 @@ void testStickyHeading2() { ModelR3 currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 0)); + new VelocitySE2(0, 0, 0)); // no POV desiredRotation = null; - GlobalVelocityR3 v = m_manualWithHeading.apply(currentState, twist1_1); + VelocitySE2 v = m_manualWithHeading.apply(currentState, twist1_1); // scale 1.0 input to max omega assertNull(m_manualWithHeading.m_goal); assertNull(m_manualWithHeading.m_thetaSetpoint); @@ -354,7 +354,7 @@ void testStickyHeading2() { // already going full speed: currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, twist1_1); @@ -366,7 +366,7 @@ void testStickyHeading2() { twist1_1 = new Velocity(0, 0, 0); currentState = new ModelR3( Pose2d.kZero, - new GlobalVelocityR3(0, 0, 2.828)); + new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; v = m_manualWithHeading.apply(currentState, twist1_1); @@ -411,7 +411,7 @@ void testProfile() { } } - private void verify(double vx, double vy, double omega, GlobalVelocityR3 v) { + private void verify(double vx, double vy, double omega, VelocitySE2 v) { assertEquals(vx, v.x(), DELTA); assertEquals(vy, v.y(), DELTA); assertEquals(omega, v.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamicsTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamicsTest.java index d084b0a89..c0752d4e0 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamicsTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamicsTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -21,12 +21,12 @@ class SwerveKinodynamicsTest implements Timeless { @Test void testRoundTripMotionless() { SwerveKinodynamics unlimited = SwerveKinodynamicsFactory.unlimited(logger); - GlobalVelocityR3 v = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 v = new VelocitySE2(0, 0, 0); Rotation2d theta = new Rotation2d(); ChassisSpeeds instantaneous = SwerveKinodynamics.toInstantaneousChassisSpeeds(v, theta); SwerveModuleStates states = unlimited.toSwerveModuleStates(instantaneous, 0.02); ChassisSpeeds implied = unlimited.toChassisSpeedsWithDiscretization(states, 0.02); - GlobalVelocityR3 result = SwerveKinodynamics.fromInstantaneousChassisSpeeds(implied, theta); + VelocitySE2 result = SwerveKinodynamics.fromInstantaneousChassisSpeeds(implied, theta); assertEquals(0, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -36,12 +36,12 @@ void testRoundTripMotionless() { @Test void testRoundTripDriveAndSpin() { SwerveKinodynamics unlimited = SwerveKinodynamicsFactory.unlimited(logger); - GlobalVelocityR3 v = new GlobalVelocityR3(5, 0, 25); + VelocitySE2 v = new VelocitySE2(5, 0, 25); Rotation2d theta = new Rotation2d(); ChassisSpeeds instantaneous = SwerveKinodynamics.toInstantaneousChassisSpeeds(v, theta); SwerveModuleStates states = unlimited.toSwerveModuleStates(instantaneous, 0.02); ChassisSpeeds implied = unlimited.toChassisSpeedsWithDiscretization(states, 0.02); - GlobalVelocityR3 result = SwerveKinodynamics.fromInstantaneousChassisSpeeds(implied, theta); + VelocitySE2 result = SwerveKinodynamics.fromInstantaneousChassisSpeeds(implied, theta); assertEquals(5, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(25, result.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiterTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiterTest.java index 186cf12c7..1c7583ce1 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiterTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeAccelerationLimiterTest.java @@ -4,8 +4,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -21,9 +21,9 @@ public class FieldRelativeAccelerationLimiterTest implements Timeless { void testMotionless() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeAccelerationLimiter limiter = new FieldRelativeAccelerationLimiter(logger, limits, 1, 1); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(0, 0, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(0, 0, 0)); assertEquals(0, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -33,9 +33,9 @@ void testMotionless() { void testConstrained() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeAccelerationLimiter limiter = new FieldRelativeAccelerationLimiter(logger, limits, 1, 1); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(1, 0, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(1, 0, 0)); assertEquals(0.02, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -45,9 +45,9 @@ void testConstrained() { void testCartesianScale() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeAccelerationLimiter limiter = new FieldRelativeAccelerationLimiter(logger, limits, 1, 1); - GlobalVelocityR3 prev = new GlobalVelocityR3(0, 0, 0); - GlobalVelocityR3 target = new GlobalVelocityR3(1, 0, 0); - GlobalAccelerationR3 accel = target.accel( + VelocitySE2 prev = new VelocitySE2(0, 0, 0); + VelocitySE2 target = new VelocitySE2(1, 0, 0); + AccelerationSE2 accel = target.accel( prev, TimedRobot100.LOOP_PERIOD_S); assertEquals(50, accel.x(), DELTA); @@ -60,9 +60,9 @@ void testCartesianScale() { void testAlpha() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeAccelerationLimiter limiter = new FieldRelativeAccelerationLimiter(logger, limits, 1, 1); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(1, 0, 1)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(1, 0, 1)); assertEquals(0.02, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0.02, result.theta(), DELTA); @@ -72,9 +72,9 @@ void testAlpha() { void testAlphaRatio() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeAccelerationLimiter limiter = new FieldRelativeAccelerationLimiter(logger, limits, 1, 1); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(1, 0, 10)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(1, 0, 10)); assertEquals(0.017, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0.170, result.theta(), DELTA); @@ -84,9 +84,9 @@ void testAlphaRatio() { void testPureAlpha() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeAccelerationLimiter limiter = new FieldRelativeAccelerationLimiter(logger, limits, 1, 1); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(0, 0, 1)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(0, 0, 1)); assertEquals(0, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0.170, result.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiterTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiterTest.java index 2589a6092..3170b653c 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiterTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeCapsizeLimiterTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -35,9 +35,9 @@ void testScale() { void testUnconstrained() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); FieldRelativeCapsizeLimiter limiter = new FieldRelativeCapsizeLimiter(logger, limits); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(0, 0, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(0, 0, 0)); assertEquals(0, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -48,9 +48,9 @@ void testInline() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); assertEquals(8.166, limits.getMaxCapsizeAccelM_S2(), DELTA); FieldRelativeCapsizeLimiter limiter = new FieldRelativeCapsizeLimiter(logger, limits); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(1, 0, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(0, 0, 0), + new VelocitySE2(1, 0, 0)); // 0.163 is 8.166 * 0.02 assertEquals(0.163, result.x(), DELTA); assertEquals(0, result.y(), DELTA); @@ -68,9 +68,9 @@ void testConstrained() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); assertEquals(8.166, limits.getMaxCapsizeAccelM_S2(), DELTA); FieldRelativeCapsizeLimiter limiter = new FieldRelativeCapsizeLimiter(logger, limits); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(1, 0, 0), - new GlobalVelocityR3(0, 1, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(1, 0, 0), + new VelocitySE2(0, 1, 0)); assertEquals(0.884, result.x(), DELTA); assertEquals(0.115, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -81,9 +81,9 @@ void testLowCentripetal() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.lowCapsize(logger); assertEquals(1.225, limits.getMaxCapsizeAccelM_S2(), DELTA); FieldRelativeCapsizeLimiter limiter = new FieldRelativeCapsizeLimiter(logger, limits); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(1, 0, 0), - new GlobalVelocityR3(0, 1, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(1, 0, 0), + new VelocitySE2(0, 1, 0)); assertEquals(0.982, result.x(), DELTA); assertEquals(0.017, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -94,9 +94,9 @@ void testOverspeedCentripetal() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); assertEquals(8.166, limits.getMaxCapsizeAccelM_S2(), DELTA); FieldRelativeCapsizeLimiter limiter = new FieldRelativeCapsizeLimiter(logger, limits); - GlobalVelocityR3 result = limiter.apply( - new GlobalVelocityR3(5, 0, 0), - new GlobalVelocityR3(0, 5, 0)); + VelocitySE2 result = limiter.apply( + new VelocitySE2(5, 0, 0), + new VelocitySE2(0, 5, 0)); assertEquals(4.884, result.x(), DELTA); assertEquals(0.115, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiterTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiterTest.java index fdd0b6b0e..64aa3f166 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiterTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/FieldRelativeVelocityLimiterTest.java @@ -4,7 +4,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -22,8 +22,8 @@ void testDrive() { BatterySagSpeedLimit limit = new BatterySagSpeedLimit(logger, k, () -> 12); FieldRelativeVelocityLimiter limiter = new FieldRelativeVelocityLimiter(logger, limit); assertEquals(5, k.getMaxDriveVelocityM_S(), DELTA); - GlobalVelocityR3 s = new GlobalVelocityR3(10, 0, 0); - GlobalVelocityR3 i = limiter.proportional(s); + VelocitySE2 s = new VelocitySE2(10, 0, 0); + VelocitySE2 i = limiter.proportional(s); assertEquals(5, i.x(), DELTA); assertEquals(0, i.y(), DELTA); assertEquals(0, i.theta(), DELTA); @@ -35,8 +35,8 @@ void testSpin() { BatterySagSpeedLimit limit = new BatterySagSpeedLimit(logger, k, () -> 12); FieldRelativeVelocityLimiter limiter = new FieldRelativeVelocityLimiter(logger, limit); assertEquals(14.142, k.getMaxAngleSpeedRad_S(), DELTA); - GlobalVelocityR3 target = new GlobalVelocityR3(0, 0, -20); - GlobalVelocityR3 i = limiter.proportional(target); + VelocitySE2 target = new VelocitySE2(0, 0, -20); + VelocitySE2 i = limiter.proportional(target); assertEquals(0, i.x(), DELTA); assertEquals(0, i.y(), DELTA); assertEquals(-14.142, i.theta(), DELTA); @@ -49,8 +49,8 @@ void testMotionless() { BatterySagSpeedLimit limit = new BatterySagSpeedLimit(logger, k, () -> 12); FieldRelativeVelocityLimiter l = new FieldRelativeVelocityLimiter(logger, limit); assertEquals(14.142, k.getMaxAngleSpeedRad_S(), DELTA); - GlobalVelocityR3 t = new GlobalVelocityR3(0, 0, 0); - GlobalVelocityR3 i = l.preferRotation(t); + VelocitySE2 t = new VelocitySE2(0, 0, 0); + VelocitySE2 i = l.preferRotation(t); assertEquals(0, i.x(), DELTA); assertEquals(0, i.y(), DELTA); assertEquals(0, i.theta(), DELTA); @@ -64,24 +64,24 @@ void testPreferRotation2() { FieldRelativeVelocityLimiter l = new FieldRelativeVelocityLimiter(logger, limit); { // inside the envelope => no change - GlobalVelocityR3 target = new GlobalVelocityR3(1, 0, 1); - GlobalVelocityR3 i = l.preferRotation(target); + VelocitySE2 target = new VelocitySE2(1, 0, 1); + VelocitySE2 i = l.preferRotation(target); assertEquals(1, i.x(), DELTA); assertEquals(0, i.y(), DELTA); assertEquals(1, i.theta(), DELTA); } { // full v, half omega => half v - GlobalVelocityR3 target = new GlobalVelocityR3(5, 0, 7.05); - GlobalVelocityR3 i = l.preferRotation(target); + VelocitySE2 target = new VelocitySE2(5, 0, 7.05); + VelocitySE2 i = l.preferRotation(target); assertEquals(2.507, i.x(), DELTA); assertEquals(0, i.y(), DELTA); assertEquals(7.05, i.theta(), DELTA); } { // full v, full omega => zero v, sorry - GlobalVelocityR3 target = new GlobalVelocityR3(5, 0, 14.142); - GlobalVelocityR3 i = l.preferRotation(target); + VelocitySE2 target = new VelocitySE2(5, 0, 14.142); + VelocitySE2 i = l.preferRotation(target); assertEquals(0, i.x(), DELTA); assertEquals(0, i.y(), DELTA); assertEquals(14.142, i.theta(), DELTA); @@ -94,8 +94,8 @@ void testBrownout() { SwerveKinodynamics k = SwerveKinodynamicsFactory.forRealisticTest(logger); BatterySagSpeedLimit limit = new BatterySagSpeedLimit(logger, k, () -> 6); FieldRelativeVelocityLimiter limiter = new FieldRelativeVelocityLimiter(logger, limit); - GlobalVelocityR3 target = new GlobalVelocityR3(1, 0, 0); - GlobalVelocityR3 limited = limiter.apply(target); + VelocitySE2 target = new VelocitySE2(1, 0, 0); + VelocitySE2 limited = limiter.apply(target); assertEquals(0, limited.x(), DELTA); assertEquals(0, limited.y(), DELTA); assertEquals(0, limited.theta(), DELTA); @@ -109,35 +109,35 @@ void testDesaturationCourseInvariant() { FieldRelativeVelocityLimiter l = new FieldRelativeVelocityLimiter(logger, limit); { // both motionless - GlobalVelocityR3 target = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 target = new VelocitySE2(0, 0, 0); assertTrue(target.angle().isEmpty()); - GlobalVelocityR3 desaturated = l.apply(target); + VelocitySE2 desaturated = l.apply(target); assertTrue(desaturated.angle().isEmpty()); } { // translating ahead - GlobalVelocityR3 target = new GlobalVelocityR3(10, 0, 0); + VelocitySE2 target = new VelocitySE2(10, 0, 0); assertEquals(0, target.angle().get().getRadians(), DELTA); assertEquals(10, target.norm(), DELTA); - GlobalVelocityR3 desaturated = l.apply(target); + VelocitySE2 desaturated = l.apply(target); assertEquals(0, desaturated.angle().get().getRadians(), DELTA); assertEquals(5, desaturated.norm(), DELTA); } { // translating ahead and spinning - GlobalVelocityR3 target = new GlobalVelocityR3(10, 0, 10); + VelocitySE2 target = new VelocitySE2(10, 0, 10); assertEquals(0, target.angle().get().getRadians(), DELTA); assertEquals(10, target.norm(), DELTA); assertEquals(10, target.theta(), DELTA); - GlobalVelocityR3 desaturated = l.apply(target); + VelocitySE2 desaturated = l.apply(target); assertEquals(0, desaturated.angle().get().getRadians(), DELTA); assertEquals(3.694, desaturated.norm(), DELTA); assertEquals(3.694, desaturated.theta(), DELTA); } { // translating 45 to the left and spinning - GlobalVelocityR3 target = new GlobalVelocityR3(10 / Math.sqrt(2), 10 / Math.sqrt(2), 10); + VelocitySE2 target = new VelocitySE2(10 / Math.sqrt(2), 10 / Math.sqrt(2), 10); assertEquals(Math.PI / 4, target.angle().get().getRadians(), DELTA); assertEquals(10, target.norm(), DELTA); assertEquals(10, target.theta(), DELTA); - GlobalVelocityR3 desaturated = l.apply(target); + VelocitySE2 desaturated = l.apply(target); assertEquals(Math.PI / 4, desaturated.angle().get().getRadians(), DELTA); assertEquals(2.612, desaturated.x(), DELTA); assertEquals(2.612, desaturated.y(), DELTA); @@ -152,13 +152,13 @@ void driveAndSpinLimited() { SwerveKinodynamics k = SwerveKinodynamicsFactory.limiting(logger); BatterySagSpeedLimit limit = new BatterySagSpeedLimit(logger, k, () -> 12); FieldRelativeVelocityLimiter limiter = new FieldRelativeVelocityLimiter(logger, limit); - GlobalVelocityR3 target = new GlobalVelocityR3(5, 0, 25); + VelocitySE2 target = new VelocitySE2(5, 0, 25); assertEquals(5, target.x(), DELTA); assertEquals(0, target.y(), DELTA); assertEquals(25, target.theta(), DELTA); // the spin is 5x the drive, as requested. // use the target as the previous setpoint - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertEquals(1.806, setpoint.x(), DELTA); assertEquals(0, setpoint.y(), DELTA); assertEquals(9.032, setpoint.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java index b57b6feea..9472d38eb 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java @@ -9,7 +9,7 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.localization.AprilTagRobotLocalizer; import org.team100.lib.localization.FreshSwerveEstimate; @@ -87,7 +87,7 @@ public class SimulatedDrivingTest implements Timeless { @Test void testSteps() { - GlobalVelocityR3 input = new GlobalVelocityR3(2, 0, 3.5); + VelocitySE2 input = new VelocitySE2(2, 0, 3.5); Rotation2d theta = new Rotation2d(); ChassisSpeeds targetChassisSpeeds = SwerveKinodynamics.toInstantaneousChassisSpeeds(input, theta); SwerveModuleStates states = swerveKinodynamics.toSwerveModuleStates(targetChassisSpeeds); @@ -139,7 +139,7 @@ void testSteps() { void testStraight() { // just +x collection.reset(); - GlobalVelocityR3 input = new GlobalVelocityR3(2, 0, 0); + VelocitySE2 input = new VelocitySE2(2, 0, 0); double start = Takt.get(); for (int i = 0; i < 100; ++i) { stepTime(); @@ -154,7 +154,7 @@ void testStraightVerbatim() { // just +x // this accelerates infinitely, immediately to the requested speed. collection.reset(); - GlobalVelocityR3 input = new GlobalVelocityR3(2, 0, 0); + VelocitySE2 input = new VelocitySE2(2, 0, 0); double start = Takt.get(); for (int i = 0; i < 100; ++i) { stepTime(); @@ -175,7 +175,7 @@ void testVeering() { Experiments.instance.testOverride(Experiment.UseSetpointGenerator, true); collection.reset(); // +x and spinning. course is always zero. - GlobalVelocityR3 input = new GlobalVelocityR3(2, 0, 3.5); + VelocitySE2 input = new VelocitySE2(2, 0, 3.5); for (int i = 0; i < 50; ++i) { if (DEBUG) System.out.printf("\nstep time ...\n"); @@ -193,7 +193,7 @@ void testVeering() { void testVeeringVerbatim() { collection.reset(); // +x and spinning - GlobalVelocityR3 input = new GlobalVelocityR3(2, 0, 3.5); + VelocitySE2 input = new VelocitySE2(2, 0, 3.5); for (int i = 0; i < 100; ++i) { if (DEBUG) System.out.printf("\nstep time ...\n"); @@ -208,7 +208,7 @@ void testVeeringVerbatim() { @Test void testGyro() { // spin fast - GlobalVelocityR3 input = new GlobalVelocityR3(0, 0, 4); + VelocitySE2 input = new VelocitySE2(0, 0, 4); if (DEBUG) System.out.printf("pose %s, gyro %s, rate %f\n", drive.getPose(), diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadbandTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadbandTest.java index 4167f66dc..c6e5fba76 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadbandTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveDeadbandTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -15,9 +15,9 @@ public class SwerveDeadbandTest { @Test void testLargeInput() { // large input should be unaffected. - GlobalVelocityR3 input = new GlobalVelocityR3(1, 0, 0); + VelocitySE2 input = new VelocitySE2(1, 0, 0); SwerveDeadband deadband = new SwerveDeadband(logger); - GlobalVelocityR3 result = deadband.apply(input); + VelocitySE2 result = deadband.apply(input); assertEquals(1, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); @@ -27,9 +27,9 @@ void testLargeInput() { void testSmallInput() { // small input should be ignored. // 5 mm/s is quite slow, maybe too slow? - GlobalVelocityR3 input = new GlobalVelocityR3(0.005, 0, 0); + VelocitySE2 input = new VelocitySE2(0.005, 0, 0); SwerveDeadband deadband = new SwerveDeadband(logger); - GlobalVelocityR3 result = deadband.apply(input); + VelocitySE2 result = deadband.apply(input); assertEquals(0, result.x(), DELTA); assertEquals(0, result.y(), DELTA); assertEquals(0, result.theta(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java index e702a1dc4..509f7ef0e 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java @@ -5,7 +5,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -27,23 +27,23 @@ public class SwerveLimiterTest implements Timeless { /** The setpoint generator never changes the field-relative course. */ @Test void courseInvariant() { - GlobalVelocityR3 target = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 target = new VelocitySE2(0, 0, 0); SwerveLimiter limiter = new SwerveLimiter(logger, KINEMATIC_LIMITS, () -> 12); { // motionless - GlobalVelocityR3 prevSetpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 prevSetpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(prevSetpoint); - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertTrue(prevSetpoint.angle().isEmpty()); assertTrue(setpoint.angle().isEmpty()); } { // at max speed, 45 to the left and spinning - GlobalVelocityR3 speed = new GlobalVelocityR3(2.640, 2.640, 3.733); - GlobalVelocityR3 prevSetpoint = speed; + VelocitySE2 speed = new VelocitySE2(2.640, 2.640, 3.733); + VelocitySE2 prevSetpoint = speed; limiter.updateSetpoint(prevSetpoint); - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertEquals(Math.PI / 4, prevSetpoint.angle().get().getRadians(), 1e-12); assertEquals(3.733, prevSetpoint.norm(), DELTA); assertEquals(3.733, prevSetpoint.theta(), DELTA); @@ -58,12 +58,12 @@ void courseInvariant() { /** This is pulled from SimulatedDrivingTest, to isolate the problem. */ @Test void courseInvariantRealistic() { - GlobalVelocityR3 targetSpeed = new GlobalVelocityR3(2, 0, 3.5); + VelocitySE2 targetSpeed = new VelocitySE2(2, 0, 3.5); // not going very fast. note the previous instantaneous robot-relative speed has // no "y" component at all, because at the previous time, we had heading of zero // (and no speed either). - GlobalVelocityR3 prevSpeed = new GlobalVelocityR3(0.16333333, 0, 0.28583333); + VelocitySE2 prevSpeed = new VelocitySE2(0.16333333, 0, 0.28583333); // the previous course is exactly zero: this is the first time step after // starting. @@ -82,7 +82,7 @@ void courseInvariantRealistic() { SwerveLimiter limiter = new SwerveLimiter(logger, KINEMATIC_LIMITS, () -> 12); limiter.updateSetpoint(prevSpeed); - GlobalVelocityR3 setpoint = limiter.apply(targetSpeed); + VelocitySE2 setpoint = limiter.apply(targetSpeed); assertEquals(0, setpoint.angle().get().getRadians(), 1e-12); assertEquals(0.3266666633333334, setpoint.norm(), 1e-12); @@ -95,15 +95,15 @@ void motionlessNoOp() { SwerveKinodynamics unlimited = SwerveKinodynamicsFactory.unlimited(logger); SwerveLimiter limiter = new SwerveLimiter(logger, unlimited, () -> 12); - GlobalVelocityR3 target = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 target = new VelocitySE2(0, 0, 0); assertEquals(0, target.x(), DELTA); assertEquals(0, target.y(), DELTA); assertEquals(0, target.theta(), DELTA); - GlobalVelocityR3 prevSetpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 prevSetpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(prevSetpoint); - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertEquals(0, setpoint.x(), DELTA); assertEquals(0, setpoint.y(), DELTA); assertEquals(0, setpoint.theta(), DELTA); @@ -115,15 +115,15 @@ void driveNoOp() { SwerveKinodynamics unlimited = SwerveKinodynamicsFactory.unlimited(logger); SwerveLimiter limiter = new SwerveLimiter(logger, unlimited, () -> 12); - GlobalVelocityR3 target = new GlobalVelocityR3(1, 0, 0); + VelocitySE2 target = new VelocitySE2(1, 0, 0); assertEquals(1, target.x(), DELTA); assertEquals(0, target.y(), DELTA); assertEquals(0, target.theta(), DELTA); - GlobalVelocityR3 prevSetpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 prevSetpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(prevSetpoint); - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertEquals(1, setpoint.x(), DELTA); assertEquals(0, setpoint.y(), DELTA); assertEquals(0, setpoint.theta(), DELTA); @@ -135,15 +135,15 @@ void spinNoOp() { SwerveKinodynamics unlimited = SwerveKinodynamicsFactory.unlimited(logger); SwerveLimiter limiter = new SwerveLimiter(logger, unlimited, () -> 12); - GlobalVelocityR3 target = new GlobalVelocityR3(0, 0, 1); + VelocitySE2 target = new VelocitySE2(0, 0, 1); assertEquals(0, target.x(), DELTA); assertEquals(0, target.y(), DELTA); assertEquals(1, target.theta(), DELTA); - GlobalVelocityR3 prevSetpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 prevSetpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(prevSetpoint); - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertEquals(0, setpoint.x(), DELTA); assertEquals(0, setpoint.y(), DELTA); assertEquals(1, setpoint.theta(), DELTA); @@ -155,16 +155,16 @@ void driveAndSpin() { SwerveLimiter limiter = new SwerveLimiter(logger, unlimited, () -> 12); // spin fast to make the discretization effect larger - GlobalVelocityR3 target = new GlobalVelocityR3(5, 0, 25); + VelocitySE2 target = new VelocitySE2(5, 0, 25); assertEquals(5, target.x(), DELTA); assertEquals(0, target.y(), DELTA); assertEquals(25, target.theta(), DELTA); // this should do nothing since the limits are so high - GlobalVelocityR3 prevSetpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 prevSetpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(prevSetpoint); - GlobalVelocityR3 setpoint = limiter.apply(target); + VelocitySE2 setpoint = limiter.apply(target); assertEquals(5, setpoint.x(), DELTA); assertEquals(0, setpoint.y(), DELTA); assertEquals(25, setpoint.theta(), DELTA); @@ -181,12 +181,12 @@ void testAccel() { SwerveLimiter limiter = new SwerveLimiter(logger, limits, () -> 12); // initially at rest, wheels facing forward. - GlobalVelocityR3 setpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 setpoint = new VelocitySE2(0, 0, 0); // initial setpoint steering is at angle zero // desired speed +x - GlobalVelocityR3 desiredSpeeds = new GlobalVelocityR3(10, 0, 0); + VelocitySE2 desiredSpeeds = new VelocitySE2(10, 0, 0); // the first setpoint should be accel limited: 10 m/s^2, 0.02 sec, // so v = 0.2 m/s @@ -215,10 +215,10 @@ void testNotLimiting() { SwerveLimiter limiter = new SwerveLimiter(logger, limits, () -> 12); // initially at rest. - GlobalVelocityR3 setpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 setpoint = new VelocitySE2(0, 0, 0); // desired speed is feasible, max accel = 10 * dt = 0.02 => v = 0.2 - GlobalVelocityR3 desiredSpeeds = new GlobalVelocityR3(0.2, 0, 0); + VelocitySE2 desiredSpeeds = new VelocitySE2(0.2, 0, 0); limiter.updateSetpoint(setpoint); setpoint = limiter.apply(desiredSpeeds); @@ -234,11 +234,11 @@ void testLimitingALittle() { SwerveLimiter limiter = new SwerveLimiter(logger, limits, () -> 12); // initially at rest. - GlobalVelocityR3 setpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 setpoint = new VelocitySE2(0, 0, 0); // desired speed is double the feasible accel so we should reach it in two // iterations. - GlobalVelocityR3 desiredSpeeds = new GlobalVelocityR3(0.4, 0, 0); + VelocitySE2 desiredSpeeds = new VelocitySE2(0.4, 0, 0); limiter.updateSetpoint(setpoint); setpoint = limiter.apply(desiredSpeeds); @@ -258,10 +258,10 @@ void testCase4() { SwerveLimiter limiter = new SwerveLimiter(logger, limits, () -> 12); // initially moving 0.5 +y - GlobalVelocityR3 setpoint = new GlobalVelocityR3(0, 0.5, 0); + VelocitySE2 setpoint = new VelocitySE2(0, 0.5, 0); // desired state is 1 +x - final GlobalVelocityR3 desiredSpeeds = new GlobalVelocityR3(1, 0, 0); + final VelocitySE2 desiredSpeeds = new VelocitySE2(1, 0, 0); limiter.updateSetpoint(setpoint); setpoint = limiter.apply(desiredSpeeds); @@ -289,9 +289,9 @@ void testSweep() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.likeComp25(logger); SwerveLimiter limiter = new SwerveLimiter(logger, limits, () -> 12); // target is infeasible and constant - final GlobalVelocityR3 target = new GlobalVelocityR3(5, 0, 0); + final VelocitySE2 target = new VelocitySE2(5, 0, 0); // start is motionless - GlobalVelocityR3 setpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 setpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(setpoint); for (int i = 0; i < 100; ++i) { if (DEBUG) @@ -337,15 +337,15 @@ void testProfile() { final SwerveLimiter limiter = new SwerveLimiter(logger, limits, () -> 12); Control100 profileTarget = initial.control(); - GlobalVelocityR3 target = new GlobalVelocityR3(profileTarget.v(), 0, 0); + VelocitySE2 target = new VelocitySE2(profileTarget.v(), 0, 0); // start is motionless - GlobalVelocityR3 setpoint = new GlobalVelocityR3(0, 0, 0); + VelocitySE2 setpoint = new VelocitySE2(0, 0, 0); limiter.updateSetpoint(setpoint); for (int i = 0; i < 81; ++i) { double accelLimit = SwerveUtil.getAccelLimit(limits, 1, 1, setpoint, target); profileTarget = profile.calculate(TimedRobot100.LOOP_PERIOD_S, profileTarget, goal); - target = new GlobalVelocityR3(profileTarget.v(), 0, 0); + target = new VelocitySE2(profileTarget.v(), 0, 0); setpoint = limiter.apply(target); if (DEBUG) System.out.printf("i %d accelLimit %5.2f setpoint %5.2f target %5.2f\n", diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java index 6ed0534ba..738acbce7 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java @@ -7,7 +7,7 @@ import java.io.IOException; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -25,12 +25,12 @@ class SwerveUtilTest implements Timeless { void testIsAccel() { // hard left turn is not accel assertFalse(SwerveUtil.isAccel( - new GlobalVelocityR3(1, 0, 0), - new GlobalVelocityR3(0, 1, 0))); + new VelocitySE2(1, 0, 0), + new VelocitySE2(0, 1, 0))); // speed up veering left assertTrue(SwerveUtil.isAccel( - new GlobalVelocityR3(0.5, 0.5, 0), - new GlobalVelocityR3(0, 1, 0))); + new VelocitySE2(0.5, 0.5, 0), + new VelocitySE2(0, 1, 0))); } @Test @@ -46,8 +46,8 @@ void testAccelLimit1() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); assertEquals(10, limits.getMaxDriveAccelerationM_S2(), DELTA); double accelLimit = SwerveUtil.getAccelLimit(limits, 1, 1, - new GlobalVelocityR3(0, 0, 0), - new GlobalVelocityR3(1, 0, 0)); + new VelocitySE2(0, 0, 0), + new VelocitySE2(1, 0, 0)); // low speed, current limited. assertEquals(10, accelLimit, DELTA); } @@ -58,8 +58,8 @@ void testAccelLimit2() { assertEquals(10, limits.getMaxDriveAccelerationM_S2(), DELTA); assertEquals(5, limits.getMaxDriveVelocityM_S(), DELTA); double accelLimit = SwerveUtil.getAccelLimit(limits, 1, 1, - new GlobalVelocityR3(4.9, 0, 0), - new GlobalVelocityR3(5, 0, 0)); + new VelocitySE2(4.9, 0, 0), + new VelocitySE2(5, 0, 0)); // near top speed, EMF-limited assertEquals(0.2, accelLimit, DELTA); } @@ -71,8 +71,8 @@ void testGetAccelLimit() throws IOException { SwerveKinodynamics limits = new Fixture().swerveKinodynamics; assertEquals(1, limits.getMaxDriveAccelerationM_S2(), DELTA); double accelLimit = SwerveUtil.getAccelLimit(limits, 1, 1, - new GlobalVelocityR3(0.92, 0, 0), - new GlobalVelocityR3(0.94, 0, 0)); + new VelocitySE2(0.92, 0, 0), + new VelocitySE2(0.94, 0, 0)); assertEquals(0.8, accelLimit, DELTA); } diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/state/FieldRelativeDeltaTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/state/FieldRelativeDeltaTest.java index cc33b6b77..34ab28380 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/state/FieldRelativeDeltaTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/state/FieldRelativeDeltaTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalDeltaR3; +import org.team100.lib.geometry.DeltaSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -14,7 +14,7 @@ void testPolarity() { // the delta sign is correct Pose2d start = new Pose2d(); Pose2d end = new Pose2d(1, 0, new Rotation2d()); - GlobalDeltaR3 d = GlobalDeltaR3.delta(start, end); + DeltaSE2 d = DeltaSE2.delta(start, end); assertEquals(1, d.getTranslation().getX(), 0.01); assertEquals(0, d.getTranslation().getY(), 0.01); assertEquals(0, d.getRotation().getRadians(), 0.01); @@ -25,7 +25,7 @@ void testWithRotation() { // unlike Pose2d.minus(), the rotation is independent Pose2d start = new Pose2d(); Pose2d end = new Pose2d(1, 0, new Rotation2d(1)); - GlobalDeltaR3 d = GlobalDeltaR3.delta(start, end); + DeltaSE2 d = DeltaSE2.delta(start, end); assertEquals(1, d.getTranslation().getX(), 0.01); assertEquals(0, d.getTranslation().getY(), 0.01); assertEquals(1, d.getRotation().getRadians(), 0.01); @@ -36,7 +36,7 @@ void testWrapping() { // the delta sign is correct Pose2d start = new Pose2d(0, 0, new Rotation2d(3)); Pose2d end = new Pose2d(0, 0, new Rotation2d(-3)); - GlobalDeltaR3 d = GlobalDeltaR3.delta(start, end); + DeltaSE2 d = DeltaSE2.delta(start, end); assertEquals(0, d.getTranslation().getX(), 0.01); assertEquals(0, d.getTranslation().getY(), 0.01); assertEquals(0.283, d.getRotation().getRadians(), 0.01); diff --git a/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java b/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java index e40df8a2b..8f057a7f9 100644 --- a/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java +++ b/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import edu.wpi.first.math.geometry.Pose2d; @@ -46,7 +46,7 @@ void testBearing() { @Test void testTargetMotion() { // at the origin moving 1 m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(1, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(0, 1); // so it appears to move clockwise @@ -56,7 +56,7 @@ void testTargetMotion() { @Test void testTargetMotionFaster() { // at the origin moving 2 m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(2, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(2, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(0, 1); // so it appears to move clockwise @@ -66,7 +66,7 @@ void testTargetMotionFaster() { @Test void testTargetMotionElsewhere() { // somewhere else, moving 1 m/s +x - ModelR3 state = new ModelR3(new Pose2d(1, 1, new Rotation2d()), new GlobalVelocityR3(1, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(1, 1, new Rotation2d()), new VelocitySE2(1, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(1, 2); // so it appears to move clockwise @@ -76,7 +76,7 @@ void testTargetMotionElsewhere() { @Test void testTargetMotionReverse() { // at the origin, moving 1m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(1, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is 1m to the right Translation2d target = new Translation2d(0, -1); // so it appears to move counterclockwise @@ -86,7 +86,7 @@ void testTargetMotionReverse() { @Test void testTargetMotionAhead() { // at the origin, moving 1m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(1, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is dead ahead Translation2d target = new Translation2d(2, 0); // no apparent motion @@ -96,7 +96,7 @@ void testTargetMotionAhead() { @Test void testTargetMotionOblique() { // at the origin, moving 1m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(1, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is at 45 Translation2d target = new Translation2d(1, 1); // apparent motion is slower @@ -106,7 +106,7 @@ void testTargetMotionOblique() { @Test void testTargetMotionY() { // at the origin, moving 1m/s +y - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(0, 1, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(0, 1, 0)); // target is dead ahead Translation2d target = new Translation2d(1, 0); // target moves the other way @@ -118,7 +118,7 @@ void testTargetMotionYReversed() { // in front of the origin, facing back to it, moving 1m/s +y, ModelR3 state = new ModelR3( new Pose2d(1, 0, Rotation2d.kPi), - new GlobalVelocityR3(0, 1, 0)); + new VelocitySE2(0, 1, 0)); // target is dead ahead Translation2d target = new Translation2d(0, 0); // target appears to move counterclockwise @@ -128,7 +128,7 @@ void testTargetMotionYReversed() { @Test void testTargetMotionZero() { // not moving, no motion - ModelR3 state = new ModelR3(new Pose2d(), new GlobalVelocityR3(0, 0, 0)); + ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(0, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(0, 1); // it should not move diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index a2065ca53..ffff20931 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -5,6 +5,7 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -21,7 +22,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -class Trajectory100Test implements Timeless{ +class Trajectory100Test implements Timeless { private static final double DELTA = 0.001; private static final boolean DEBUG = false; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -37,8 +38,14 @@ void testPreviewAndAdvance() { Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); List waypoints = List.of( - new HolonomicPose2d(currentTranslation, start.getRotation(), angleToGoal), - new HolonomicPose2d(goalTranslation, end.getRotation(), angleToGoal)); + new HolonomicPose2d( + currentTranslation, + start.getRotation(), + DirectionSE2.fromRotation(angleToGoal)), + new HolonomicPose2d( + goalTranslation, + end.getRotation(), + DirectionSE2.fromRotation(angleToGoal))); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -69,8 +76,14 @@ void testSample() { Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); List waypoints = List.of( - new HolonomicPose2d(currentTranslation, start.getRotation(), angleToGoal), - new HolonomicPose2d(goalTranslation, end.getRotation(), angleToGoal)); + new HolonomicPose2d( + currentTranslation, + start.getRotation(), + DirectionSE2.fromRotation(angleToGoal)), + new HolonomicPose2d( + goalTranslation, + end.getRotation(), + DirectionSE2.fromRotation(angleToGoal))); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -93,8 +106,14 @@ void testSample() { void testSampleThoroughly() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kZero)); + new HolonomicPose2d( + new Translation2d(), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); @@ -124,8 +143,14 @@ void testSampleThoroughly() { @Test void testSampleThoroughlyWithRotation() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kCCW_Pi_2, Rotation2d.kZero)); + new HolonomicPose2d( + new Translation2d(), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kCCW_Pi_2, + DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); @@ -161,9 +186,18 @@ void testSampleThoroughlyWithRotation() { // @Test void testSamplePerformance() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(10, 0), Rotation2d.kCCW_Pi_2, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(10, 10), Rotation2d.kPi, Rotation2d.kZero)); + new HolonomicPose2d( + new Translation2d(), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(10, 0), + Rotation2d.kCCW_Pi_2, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(10, 10), + Rotation2d.kPi, + DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 9d8d9a5f9..e06df0d6c 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -7,7 +7,8 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -33,13 +34,17 @@ class TrajectoryPlannerTest implements Timeless { private static final double DELTA = 0.01; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); - - @Test void testLinear() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); @@ -52,8 +57,14 @@ void testLinear() { @Test void testBackingUp() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(0, 0), Rotation2d.kZero, new Rotation2d(Math.PI)), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kZero)); + new HolonomicPose2d( + new Translation2d(0, 0), + Rotation2d.kZero, + DirectionSE2.MINUS_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); // these are the same as StraightLineTrajectoryTest. @@ -81,8 +92,14 @@ void testBackingUp() { @Test void testPerformance() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 1), new Rotation2d(), new Rotation2d(Math.PI / 2))); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_Y)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); long startTimeNs = System.nanoTime(); @@ -105,14 +122,12 @@ void testPerformance() { assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } - - @Test void testRestToRest() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - ModelR3 start = new ModelR3(Pose2d.kZero, new GlobalVelocityR3(0, 0, 0)); + ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 trajectory = planner.restToRest(start.pose(), end); assertEquals(1.565, trajectory.duration(), DELTA); @@ -140,7 +155,7 @@ void testMovingToRest() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - ModelR3 start = new ModelR3(Pose2d.kZero, new GlobalVelocityR3(1, 0, 0)); + ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(1, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); assertEquals(1.176, traj.duration(), DELTA); @@ -151,7 +166,7 @@ void testBackingUp2() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - ModelR3 start = new ModelR3(Pose2d.kZero, new GlobalVelocityR3(-1, 0, 0)); + ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(-1, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); assertEquals(1.176, traj.duration(), DELTA); @@ -162,7 +177,7 @@ void test2d() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - ModelR3 start = new ModelR3(Pose2d.kZero, new GlobalVelocityR3(0, 1, 0)); + ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 1, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); assertEquals(2.958, traj.duration(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 9d365edc8..26bf50e9d 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -9,6 +9,7 @@ import java.util.Optional; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; @@ -54,9 +55,13 @@ void testSimple() { // spline is in the x direction, no curvature. HolonomicSpline spline = new HolonomicSpline( new HolonomicPose2d( - new Translation2d(), new Rotation2d(), new Rotation2d()), + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), new HolonomicPose2d( - new Translation2d(1, 0), new Rotation2d(), new Rotation2d())) { + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X)) { @Override public Translation2d getPoint(double t) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 2b72c822d..b050c7030 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -7,6 +7,7 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; @@ -80,8 +81,14 @@ void testForced() throws TimingException { @Test void testBackingUp() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(0, 0), Rotation2d.kZero, new Rotation2d(Math.PI)), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kZero)); + new HolonomicPose2d( + new Translation2d(0, 0), + Rotation2d.kZero, + DirectionSE2.MINUS_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_X)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.0127, @@ -94,9 +101,18 @@ void testBackingUp() { @Test void testCorner() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(0, 0), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 1), new Rotation2d(), new Rotation2d(Math.PI / 2))); + new HolonomicPose2d( + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_Y)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); assertEquals(9, path.length()); @@ -113,8 +129,14 @@ void testCorner() { @Test void testLinear() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.01, 0.01, 0.1); assertEquals(2, path.length()); @@ -131,10 +153,22 @@ void testLinear() { @Test void testActualCorner() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(0, 0), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d(Math.PI / 2)), - new HolonomicPose2d(new Translation2d(1, 1), new Rotation2d(), new Rotation2d(Math.PI / 2))); + new HolonomicPose2d( + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_Y), + new HolonomicPose2d( + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_Y)); assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); } @@ -142,18 +176,36 @@ void testActualCorner() { @Test void testComposite() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(0, 0), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(1), new Rotation2d()), - new HolonomicPose2d(new Translation2d(2, 0), new Rotation2d(1), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(1), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(2, 0), + new Rotation2d(1), + DirectionSE2.TO_X)); assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); } @Test void test() { - HolonomicPose2d p1 = new HolonomicPose2d(new Translation2d(0, 0), Rotation2d.kZero, Rotation2d.kZero); - HolonomicPose2d p2 = new HolonomicPose2d(new Translation2d(15, 10), Rotation2d.kZero, new Rotation2d(1, 5)); + HolonomicPose2d p1 = new HolonomicPose2d( + new Translation2d(0, 0), + Rotation2d.kZero, + DirectionSE2.TO_X); + HolonomicPose2d p2 = new HolonomicPose2d( + new Translation2d(15, 10), + Rotation2d.kZero, + new DirectionSE2(1, 5, 0)); HolonomicSpline s = new HolonomicSpline(p1, p2); List samples = PathFactory.parameterizeSpline(s, 0.05, 0.05, 0.1, 0.0, 1.0); @@ -179,8 +231,14 @@ void test() { @Test void testDx() { HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(0, -1), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kCCW_90deg), + new HolonomicPose2d( + new Translation2d(0, -1), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_Y), 1.0, 1.0); List splines = List.of(s0); List motion = PathFactory.parameterizeSplines(splines, 0.001, 0.001, 0.001); @@ -198,8 +256,14 @@ void testDx() { @Test void testPerformance() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 1), new Rotation2d(), new Rotation2d(Math.PI / 2))); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_Y)); long startTimeNs = System.nanoTime(); Path100 t = new Path100(new ArrayList<>()); final int iterations = 100; diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java index 1451a60cb..1c089e565 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java @@ -3,6 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionR3; import org.team100.lib.geometry.HolonomicPose3d; import org.team100.lib.testing.Timeless; @@ -15,16 +16,24 @@ public class HolonomicSpline3dTest implements Timeless { @Test void testLinear() { HolonomicSpline3d s = new HolonomicSpline3d( - new HolonomicPose3d(new Translation3d(), new Rotation3d(), new Rotation3d()), - new HolonomicPose3d(new Translation3d(1, 0, 0), new Rotation3d(), new Rotation3d())); + new HolonomicPose3d( + new Translation3d(), + new Rotation3d(), + new DirectionR3(1, 0, 0)), + new HolonomicPose3d( + new Translation3d(1, 0, 0), + new Rotation3d(), + new DirectionR3(1, 0, 0))); Translation3d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); assertEquals(1, t.getX(), DELTA); + // Pose3dWithMotion p = s.getPose3dWithMotion(0); // assertEquals(0, p.getPose().translation().getX(), DELTA); // assertEquals(0, p.getPose().heading().getZ(), DELTA); // assertEquals(0, p.getHeadingYawRateRad_M(), DELTA); + // p = s.getPose3dWithMotion(1); // assertEquals(1, p.getPose().translation().getX(), DELTA); // assertEquals(0, p.getPose().heading().getZ(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 74e9b223e..859b33fb9 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -7,6 +7,7 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; @@ -31,11 +32,26 @@ class HolonomicSplineTest implements Timeless { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); + @Test + void testCourse() { + Rotation2d course = new Rotation2d(Math.PI / 4); + Translation2d t = new Translation2d(1, 0).rotateBy(course); + assertEquals(0.707, t.getX(), DELTA); + assertEquals(0.707, t.getY(), DELTA); + + } + @Test void testLinear() { HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(), + DirectionSE2.TO_X)); Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); @@ -53,8 +69,14 @@ void testLinear() { @Test void testLinear2() { HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(2, 0), new Rotation2d(), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(2, 0), + new Rotation2d(), + DirectionSE2.TO_X)); Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); @@ -72,8 +94,14 @@ void testLinear2() { @Test void testRotation() { HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(1), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(1), + DirectionSE2.TO_X)); if (DEBUG) { System.out.println("d, x, y, heading, course"); @@ -110,8 +138,14 @@ void testRotation() { void testRotation2() { // Make sure the rotation goes over +/- pi HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(), new Rotation2d(2.5), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1,0), new Rotation2d(-2.5), new Rotation2d())); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(2.5), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(-2.5), + DirectionSE2.TO_X)); if (DEBUG) { System.out.println("d, x, y, heading, course"); for (double tt = 0; tt <= 1.0; tt += 0.01) { @@ -134,20 +168,44 @@ void testCircle() { // magic number of 1 makes something about 1.5% from a circle, close enough. double magicNumber = 1.0; HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.k180deg, Rotation2d.kCCW_90deg), - new HolonomicPose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg, Rotation2d.k180deg), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.k180deg, + DirectionSE2.TO_Y), + new HolonomicPose2d( + new Translation2d(0, 1), + Rotation2d.kCW_90deg, + DirectionSE2.MINUS_X), magicNumber, magicNumber); HolonomicSpline s1 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg, Rotation2d.k180deg), - new HolonomicPose2d(new Translation2d(-1, 0), Rotation2d.kZero, Rotation2d.kCW_90deg), + new HolonomicPose2d( + new Translation2d(0, 1), + Rotation2d.kCW_90deg, + DirectionSE2.MINUS_X), + new HolonomicPose2d( + new Translation2d(-1, 0), + Rotation2d.kZero, + DirectionSE2.MINUS_Y), magicNumber, magicNumber); HolonomicSpline s2 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(-1, 0), Rotation2d.kZero, Rotation2d.kCW_90deg), - new HolonomicPose2d(new Translation2d(0, -1), Rotation2d.kCCW_90deg, Rotation2d.kZero), + new HolonomicPose2d( + new Translation2d(-1, 0), + Rotation2d.kZero, + DirectionSE2.MINUS_Y), + new HolonomicPose2d( + new Translation2d(0, -1), + Rotation2d.kCCW_90deg, + DirectionSE2.TO_X), magicNumber, magicNumber); HolonomicSpline s3 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(0, -1), Rotation2d.kCCW_90deg, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.k180deg, Rotation2d.kCCW_90deg), + new HolonomicPose2d( + new Translation2d(0, -1), + Rotation2d.kCCW_90deg, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.k180deg, + DirectionSE2.TO_Y), magicNumber, magicNumber); List splines = new ArrayList<>(); splines.add(s0); @@ -205,13 +263,25 @@ void testMismatchedThetaDerivatives() { double magicNumber = 1.0; // turn a bit to the left HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(0, 0), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(1), Rotation2d.kZero), + new HolonomicPose2d( + new Translation2d(0, 0), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(1), + DirectionSE2.TO_X), magicNumber, magicNumber); // turn much more to the left HolonomicSpline s1 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(1), Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(2, 0), Rotation2d.k180deg, Rotation2d.kZero), + new HolonomicPose2d( + new Translation2d(1, 0), + new Rotation2d(1), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(2, 0), + Rotation2d.k180deg, + DirectionSE2.TO_X), magicNumber, magicNumber); List splines = new ArrayList<>(); splines.add(s0); @@ -257,14 +327,26 @@ void testMismatchedXYDerivatives() { // this goes straight ahead to (1,0) // derivatives point straight ahead HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(0, 0), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kZero), + new HolonomicPose2d( + new Translation2d(0, 0), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_X), magicNumber, magicNumber); // this is a sharp turn to the left // derivatives point to the left HolonomicSpline s1 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kCCW_90deg), - new HolonomicPose2d(new Translation2d(1, 1), Rotation2d.kZero, Rotation2d.kCCW_90deg), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_Y), + new HolonomicPose2d( + new Translation2d(1, 1), + Rotation2d.kZero, + DirectionSE2.TO_Y), magicNumber, magicNumber); List splines = new ArrayList<>(); splines.add(s0); @@ -323,8 +405,14 @@ void testEntryVelocity() { // radius is 1 m. HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d(new Translation2d(0, -1), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kCCW_90deg), + new HolonomicPose2d( + new Translation2d(0, -1), + Rotation2d.kZero, + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 0), + Rotation2d.kZero, + DirectionSE2.TO_Y), 1.2, 1.2); if (DEBUG) { for (double t = 0; t < 1; t += 0.03) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java index 52863d539..b13bbda08 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java @@ -7,6 +7,7 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.HolonomicPose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,11 +19,17 @@ class QuinticHermiteOptimizerTest { @Test void test() { HolonomicPose2d a = new HolonomicPose2d( - new Translation2d(0, 100), new Rotation2d(), Rotation2d.fromDegrees(270)); + new Translation2d(0, 100), + new Rotation2d(), + DirectionSE2.MINUS_Y); HolonomicPose2d b = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); + new Translation2d(50, 0), + new Rotation2d(), + DirectionSE2.TO_X); HolonomicPose2d c = new HolonomicPose2d( - new Translation2d(100, 100), new Rotation2d(), Rotation2d.fromDegrees(90)); + new Translation2d(100, 100), + new Rotation2d(), + DirectionSE2.TO_Y); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); @@ -31,13 +38,21 @@ void test() { assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); HolonomicPose2d d = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(90)); + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_Y); HolonomicPose2d e = new HolonomicPose2d( - new Translation2d(0, 50), new Rotation2d(), Rotation2d.fromDegrees(0)); + new Translation2d(0, 50), + new Rotation2d(), + DirectionSE2.TO_X); HolonomicPose2d f = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(), Rotation2d.fromDegrees(-90)); + new Translation2d(100, 50), + new Rotation2d(), + DirectionSE2.MINUS_Y); HolonomicPose2d g = new HolonomicPose2d( - new Translation2d(100, 0), new Rotation2d(), Rotation2d.fromDegrees(-180)); + new Translation2d(100, 0), + new Rotation2d(), + DirectionSE2.MINUS_X); List splines1 = new ArrayList<>(); splines1.add(new HolonomicSpline(d, e)); @@ -47,15 +62,25 @@ void test() { assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); HolonomicPose2d h = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_X); HolonomicPose2d i = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); + new Translation2d(50, 0), + new Rotation2d(), + DirectionSE2.TO_X); HolonomicPose2d j = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(), Rotation2d.fromDegrees(45)); + new Translation2d(100, 50), + new Rotation2d(), + new DirectionSE2(1, 1, 0)); HolonomicPose2d k = new HolonomicPose2d( - new Translation2d(150, 0), new Rotation2d(), Rotation2d.fromDegrees(270)); + new Translation2d(150, 0), + new Rotation2d(), + DirectionSE2.MINUS_Y); HolonomicPose2d l = new HolonomicPose2d( - new Translation2d(150, -50), new Rotation2d(), Rotation2d.fromDegrees(270)); + new Translation2d(150, -50), + new Rotation2d(), + DirectionSE2.MINUS_Y); List splines2 = new ArrayList<>(); splines2.add(new HolonomicSpline(h, i)); @@ -71,11 +96,17 @@ void test() { @Test void testHolonomic() { HolonomicPose2d a = new HolonomicPose2d( - new Translation2d(0, 100), new Rotation2d(), Rotation2d.fromDegrees(270)); + new Translation2d(0, 100), + new Rotation2d(), + DirectionSE2.MINUS_Y); HolonomicPose2d b = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(0)); + new Translation2d(50, 0), + new Rotation2d(Math.PI / 2), + DirectionSE2.TO_X); HolonomicPose2d c = new HolonomicPose2d( - new Translation2d(100, 100), new Rotation2d(Math.PI), Rotation2d.fromDegrees(90)); + new Translation2d(100, 100), + new Rotation2d(Math.PI), + DirectionSE2.TO_Y); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); @@ -84,13 +115,21 @@ void testHolonomic() { assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); HolonomicPose2d d = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(90)); + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_Y); HolonomicPose2d e = new HolonomicPose2d( - new Translation2d(0, 50), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(0)); + new Translation2d(0, 50), + new Rotation2d(Math.PI / 2), + DirectionSE2.TO_X); HolonomicPose2d f = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(Math.PI), Rotation2d.fromDegrees(-90)); + new Translation2d(100, 50), + new Rotation2d(Math.PI), + DirectionSE2.MINUS_Y); HolonomicPose2d g = new HolonomicPose2d( - new Translation2d(100, 0), new Rotation2d(), Rotation2d.fromDegrees(-180)); + new Translation2d(100, 0), + new Rotation2d(), + DirectionSE2.MINUS_X); List splines1 = new ArrayList<>(); splines1.add(new HolonomicSpline(d, e)); @@ -100,15 +139,25 @@ void testHolonomic() { assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); HolonomicPose2d h = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); + new Translation2d(0, 0), + new Rotation2d(), + DirectionSE2.TO_X); HolonomicPose2d i = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(0)); + new Translation2d(50, 0), + new Rotation2d(Math.PI / 2), + DirectionSE2.TO_X); HolonomicPose2d j = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(Math.PI), Rotation2d.fromDegrees(45)); + new Translation2d(100, 50), + new Rotation2d(Math.PI), + new DirectionSE2(1, 1, 0)); HolonomicPose2d k = new HolonomicPose2d( - new Translation2d(150, 0), new Rotation2d(), Rotation2d.fromDegrees(270)); + new Translation2d(150, 0), + new Rotation2d(), + DirectionSE2.MINUS_Y); HolonomicPose2d l = new HolonomicPose2d( - new Translation2d(150, -50), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(270)); + new Translation2d(150, -50), + new Rotation2d(Math.PI / 2), + DirectionSE2.MINUS_Y); List splines2 = new ArrayList<>(); splines2.add(new HolonomicSpline(h, i)); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java index 4438fd3f1..27716aa87 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java @@ -13,7 +13,17 @@ * */ public class SplineR1Test { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; + + /** + * Making the end derivatives the average, with zero second derivative, makes + * the spline always just a straight line + */ + @Test + void testEnds() { + SplineR1 spline = SplineR1.get(0, 1, 1, 1, 0, 0); + show(spline); + } /** Look at an example */ @Test @@ -23,13 +33,18 @@ void testSample() { // is not useful without modifying the schedule. // Spline1d spline = Spline1d.get(0, 1, 0, 0, 0, 0); SplineR1 spline = SplineR1.viaMatrix(0, 1, 0, 0, 0, 0); + show(spline); + } + + private void show(SplineR1 spline) { + System.out.println("t, x, v, a, j"); for (double t = 0; t <= 1; t += 0.01) { double x = spline.getPosition(t); double v = spline.getVelocity(t); double a = spline.getAcceleration(t); double j = spline.getJerk(t); if (DEBUG) - System.out.printf("%8.3f %8.3f %8.3f %8.3f %8.3f\n", + System.out.printf("%8.3f, %8.3f, %8.3f, %8.3f, %8.3f\n", t, x, v, a, j); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index 56b207d34..5c3d4dbd9 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -11,6 +11,7 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.HolonomicPose2d; import org.team100.lib.geometry.Pose2dWithMotion; @@ -265,8 +266,14 @@ void testV1() { @Test void testPerformance() { List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 1), new Rotation2d(), new Rotation2d(Math.PI / 2))); + new HolonomicPose2d( + new Translation2d(), + new Rotation2d(), + DirectionSE2.TO_X), + new HolonomicPose2d( + new Translation2d(1, 1), + new Rotation2d(), + DirectionSE2.TO_Y)); long startTimeNs = System.nanoTime(); final int iterations = 100; final double SPLINE_SAMPLE_TOLERANCE_M = 0.05; From 0f7405457c642e41e8adeb4faef3bf160a2d9d6b Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 15 Dec 2025 09:47:21 -0800 Subject: [PATCH 02/42] 3d trajectory WIP --- .../frc2025/CalgamesArm/CalgamesMech.java | 66 +++---- .../frc2025/CalgamesArm/MechTrajectories.java | 6 +- .../frc2025/Swerve/Auto/GoToCoralStation.java | 14 +- .../team100/frc2025/robot/LolipopAuto.java | 6 +- .../org/team100/frc2025/robot/Prewarmer.java | 17 +- .../CalgamesArm/TrajectoryJointTest.java | 6 +- .../frc2025/CalgamesArm/TrajectoryTest.java | 96 +++++---- .../CalgamesArm/TrajectoryToVectorSeries.java | 6 +- .../team100/lib/geometry/DirectionSE2.java | 1 - .../team100/lib/geometry/GeometryUtil.java | 7 +- .../team100/lib/geometry/HolonomicPose2d.java | 66 ------- .../team100/lib/geometry/HolonomicPose3d.java | 25 --- .../lib/geometry/Pose2dWithDirection.java | 62 ++++++ .../lib/geometry/Pose2dWithMotion.java | 10 +- .../lib/geometry/Pose3dWithDirection.java | 24 +++ .../lib/geometry/Pose3dWithMotion.java | 8 +- .../team100/lib/logging/LoggerFactory.java | 4 +- ...veToPoseWithTrajectoryAndExitVelocity.java | 12 +- .../r3/commands/GoToPosePosition.java | 8 +- .../tank/commands/ToPoseWithTrajectory.java | 6 +- .../lib/trajectory/TrajectoryPlanner.java | 40 ++-- .../lib/trajectory/path/PathFactory.java | 20 +- .../path/spline/HolonomicSpline.java | 11 +- .../path/spline/HolonomicSpline3d.java | 28 +-- .../trajectory/timing/JointConstraint.java | 4 +- .../TrajectoryVisualization.java | 4 +- .../r3/FullStateControllerR3Test.java | 40 ++-- .../r3/ReferenceControllerR3Test.java | 25 +-- .../DriveToPoseWithTrajectoryTest.java | 4 +- .../DriveWithTrajectoryFunctionTest.java | 6 +- .../subsystems/swerve/SwerveControlTest.java | 10 +- .../subsystems/swerve/SwerveModelTest.java | 10 +- .../lib/trajectory/Trajectory100Test.java | 81 ++++---- .../lib/trajectory/TrajectoryPlannerTest.java | 50 ++--- .../lib/trajectory/path/Path100Test.java | 34 ++-- .../lib/trajectory/path/PathFactoryTest.java | 161 ++++++++------- .../path/spline/HolonomicSpline3dTest.java | 23 ++- .../path/spline/HolonomicSplineTest.java | 184 ++++++++++-------- .../spline/QuinticHermiteOptimizerTest.java | 171 +++++++++------- ...CentripetalAccelerationConstraintTest.java | 12 +- .../timing/ConstantConstraintTest.java | 6 +- .../timing/DiamondConstraintTest.java | 14 +- .../timing/JointConstraintTest.java | 12 +- .../timing/ScheduleGeneratorTest.java | 31 +-- .../SwerveDriveDynamicsConstraintTest.java | 12 +- .../lib/trajectory/timing/TimedStateTest.java | 6 +- .../timing/TorqueConstraintTest.java | 14 +- .../timing/TrajectoryVelocityProfileTest.java | 6 +- .../VelocityLimitRegionConstraintTest.java | 6 +- .../timing/YawRateConstraintTest.java | 10 +- 50 files changed, 785 insertions(+), 700 deletions(-) delete mode 100644 lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java delete mode 100644 lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/Pose3dWithDirection.java diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java index 13f5ace62..f54e734ae 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java @@ -14,7 +14,7 @@ import org.team100.lib.config.PIDConstants; import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.ConfigLogger; @@ -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)); + Pose2dWithDirection.make(m_home, -1.5), + Pose2dWithDirection.make(L2, -1.7)); } // NEVER CALL public Command l1ToHome() { return m_transit.terminal("l1ToHome", - HolonomicPose2d.make(L2, 1.3), - HolonomicPose2d.make(m_home, 1.5)); + Pose2dWithDirection.make(L2, 1.3), + Pose2dWithDirection.make(m_home, 1.5)); } public MoveAndHold homeToL2() { return m_transit.endless("homeToL2", - HolonomicPose2d.make(m_home, 1.5), - HolonomicPose2d.make(L2, 1.5)); + Pose2dWithDirection.make(m_home, 1.5), + Pose2dWithDirection.make(L2, 1.5)); } public Command l2ToHome() { return m_transit.terminal("l2ToHome", - HolonomicPose2d.make(L2, -1.5), - HolonomicPose2d.make(m_home, -1.5)); + Pose2dWithDirection.make(L2, -1.5), + Pose2dWithDirection.make(m_home, -1.5)); } public MoveAndHold homeToL3() { return m_transit.endless("homeToL3", - HolonomicPose2d.make(m_home, 0.8), - HolonomicPose2d.make(L3, 1.5)); + Pose2dWithDirection.make(m_home, 0.8), + Pose2dWithDirection.make(L3, 1.5)); } public Command l3ToHome() { return m_transit.terminal("l3ToHome", - HolonomicPose2d.make(L3, -1.5), - HolonomicPose2d.make(m_home, -2.3)); + Pose2dWithDirection.make(L3, -1.5), + Pose2dWithDirection.make(m_home, -2.3)); } public MoveAndHold homeToL4() { return m_transit.endless("homeToL4", - HolonomicPose2d.make(m_home, 0.1), - HolonomicPose2d.make(L4, 1.5)); + Pose2dWithDirection.make(m_home, 0.1), + Pose2dWithDirection.make(L4, 1.5)); } public MoveAndHold homeToL4Back() { return m_transit.endless("homeToL4", - HolonomicPose2d.make(m_home, 0.1), - HolonomicPose2d.make(L4_BACK, -1.5)); + Pose2dWithDirection.make(m_home, 0.1), + Pose2dWithDirection.make(L4_BACK, -1.5)); } public Command l4ToHome() { return m_transit.terminal("l4ToHome", - HolonomicPose2d.make(L4, -1.5), - HolonomicPose2d.make(m_home, -3)); + Pose2dWithDirection.make(L4, -1.5), + Pose2dWithDirection.make(m_home, -3)); } public Command l4BackToHome() { return m_transit.terminal("l4ToHome", - HolonomicPose2d.make(L4_BACK, 1.5), - HolonomicPose2d.make(m_home, -3)); + Pose2dWithDirection.make(L4_BACK, 1.5), + Pose2dWithDirection.make(m_home, -3)); } public Command homeToAlgaeL2() { return m_transit.endless("homeToAlgaeL2", - HolonomicPose2d.make(m_home, 1.5), - HolonomicPose2d.make(ALGAE_L2, 1.5)); + Pose2dWithDirection.make(m_home, 1.5), + Pose2dWithDirection.make(ALGAE_L2, 1.5)); } public Command homeToAlgaeL3() { return m_transit.endless("homeToAlgaeL3", - HolonomicPose2d.make(m_home, 0), - HolonomicPose2d.make(ALGAE_L3, 1.5)); + Pose2dWithDirection.make(m_home, 0), + Pose2dWithDirection.make(ALGAE_L3, 1.5)); } public Command algaeL2ToHome() { return m_transit.terminal("homeToAlgaeL2", - HolonomicPose2d.make(ALGAE_L2, -1.0), - HolonomicPose2d.make(m_home, Math.PI)); + Pose2dWithDirection.make(ALGAE_L2, -1.0), + Pose2dWithDirection.make(m_home, Math.PI)); } public Command algaeL3ToHome() { return m_transit.terminal("homeToAlgaeL3", - HolonomicPose2d.make(ALGAE_L3, -1.0), - HolonomicPose2d.make(m_home, Math.PI)); + Pose2dWithDirection.make(ALGAE_L3, -1.0), + Pose2dWithDirection.make(m_home, Math.PI)); } /** @@ -598,14 +598,14 @@ public Command algaeReefExit(Supplier level) { */ public MoveAndHold homeToBarge() { return m_transit.endless("homeToBarge", - HolonomicPose2d.make(m_home, 0), - HolonomicPose2d.make(BARGE, -1)); + Pose2dWithDirection.make(m_home, 0), + Pose2dWithDirection.make(BARGE, -1)); } public MoveAndHold bargeToHome() { return m_transit.endless("bargeToHome", - HolonomicPose2d.make(BARGE, 2.5), - HolonomicPose2d.make(m_home, Math.PI)); + Pose2dWithDirection.make(BARGE, 2.5), + Pose2dWithDirection.make(m_home, Math.PI)); } diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java index ca02cae4d..0de6d0537 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -4,7 +4,7 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; @@ -62,7 +62,7 @@ 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, Pose2dWithDirection start, Pose2dWithDirection end) { /** Use the start course and ignore the start pose for now */ MoveAndHold f = new GoToPosePosition( @@ -73,7 +73,7 @@ public Command terminal(String name, HolonomicPose2d start, HolonomicPose2d end) } /** 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, Pose2dWithDirection start, Pose2dWithDirection end) { /** Use the start course and ignore the start pose for now */ GoToPosePosition c = new GoToPosePosition( diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index 8ce44cfed..ef516ac17 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -7,7 +7,7 @@ import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.CoralStation; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.trajectory.Trajectory100; @@ -52,16 +52,14 @@ public Trajectory100 apply(Pose2d currentPose) { Rotation2d newInitialSpline = FieldConstants.calculateDeltaSpline( courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust); - List waypoints = new ArrayList<>(); + List waypoints = new ArrayList<>(); waypoints.add( - new HolonomicPose2d( - currTranslation, - currentPose.getRotation(), + new Pose2dWithDirection( + currentPose, DirectionSE2.fromRotation(newInitialSpline))); waypoints.add( - new HolonomicPose2d( - goal.getTranslation(), - goal.getRotation(), + new Pose2dWithDirection( + goal, DirectionSE2.fromRotation(courseToGoal))); return m_planner.restToRest(waypoints); diff --git a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java index 746d3c550..9e87f483a 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java +++ b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java @@ -12,7 +12,7 @@ import org.team100.lib.controller.r3.FullStateControllerR3; import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.ReefPoint; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.r3.ProfileR3; import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; @@ -51,8 +51,8 @@ public Command get() { DriveWithTrajectoryFunction toReefTrajectory = new DriveWithTrajectoryFunction( m_log, m_machinery.m_drive, m_autoController, m_machinery.m_trajectoryViz, (p) -> m_planner.restToRest(List.of( - HolonomicPose2d.make(m_machinery.m_drive.getPose(), Math.PI), - HolonomicPose2d.make(3, 5, 0, -2)))); + Pose2dWithDirection.make(m_machinery.m_drive.getPose(), Math.PI), + Pose2dWithDirection.make(3, 5, 0, -2)))); DriveToPoseWithProfile toReefA = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, diff --git a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java index ea7063a74..8e3a2f606 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java @@ -4,14 +4,15 @@ import java.util.List; import org.team100.lib.coherence.Takt; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; @@ -28,14 +29,12 @@ public static void init(Machinery machinery) { double startS = Takt.actual(); // Exercise the trajectory planner. - List waypoints = new ArrayList<>(); - waypoints.add(new HolonomicPose2d( - new Translation2d(), - Rotation2d.kZero, + List waypoints = new ArrayList<>(); + waypoints.add(new Pose2dWithDirection( + new Pose2d(new Translation2d(), Rotation2d.kZero), DirectionSE2.TO_X)); - waypoints.add(new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + waypoints.add(new Pose2dWithDirection( + new Pose2d(new Translation2d(1, 0), Rotation2d.kZero), DirectionSE2.TO_X)); TrajectoryPlanner planner = new TrajectoryPlanner( new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger)); diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java index 19e23c96f..8f8742455 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -5,7 +5,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -49,8 +49,8 @@ void homeToL4() { TrajectoryPlanner m_planner = new TrajectoryPlanner(c); Trajectory100 t = m_planner.restToRest(List.of( - HolonomicPose2d.make(1, 0, 0, 0), - HolonomicPose2d.make(1.9, 0.5, 2.5, 2))); + Pose2dWithDirection.make(1, 0, 0, 0), + Pose2dWithDirection.make(1.9, 0.5, 2.5, 2))); ElevatorArmWristKinematics k = new ElevatorArmWristKinematics( 0.5, 0.3); diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java index 808907c52..0365c8aee 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java @@ -3,7 +3,7 @@ import java.util.List; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -45,7 +45,7 @@ void testSimple() throws InterruptedException { /** * Yields a curve. * - * A HolonomicPose2d allows separate specification of heading (which way the + * A Pose2dWithDirection allows separate specification of heading (which way the * front of the robot is facing) and course (which way the robot is moving). * * In this case, is facing +x, and moving +x, and it ends up moving +y but @@ -57,14 +57,16 @@ void testCurved() throws InterruptedException { new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(9, 9), - new Rotation2d(-Math.PI / 2), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(9, 9), + new Rotation2d(-Math.PI / 2)), DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); new TrajectoryPlotter(0.1).plot(t, "simple"); @@ -82,18 +84,21 @@ void testMultipleWaypoints() throws InterruptedException { new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(5, 5), - new Rotation2d(-2), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(5, 5), + new Rotation2d(-2)), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(9, 9), - new Rotation2d(-Math.PI / 2), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(9, 9), + new Rotation2d(-Math.PI / 2)), DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); new TrajectoryPlotter(0.1).plot(t, "simple"); @@ -107,20 +112,23 @@ void testPickupToPlace() { new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( + List waypoints = List.of( // pickup - new HolonomicPose2d( - new Translation2d(1, 0.1), - new Rotation2d(-Math.PI), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0.1), + new Rotation2d(-Math.PI)), DirectionSE2.TO_Y), // place for gateway point? - new HolonomicPose2d( - new Translation2d(3, 7), - new Rotation2d(Math.PI / 2), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(3, 7), + new Rotation2d(Math.PI / 2)), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(6, 9), - new Rotation2d(-((7 * Math.PI) / 36)), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(6, 9), + new Rotation2d(-((7 * Math.PI) / 36))), DirectionSE2.TO_Y)); @SuppressWarnings("unused") Trajectory100 t = p.restToRest(waypoints); @@ -133,25 +141,29 @@ void testSingularityDemo() { new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( + List waypoints = List.of( // pickup - new HolonomicPose2d( - new Translation2d(1, 0.1), - new Rotation2d(-Math.PI), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0.1), + new Rotation2d(-Math.PI)), DirectionSE2.TO_Y), // place for gateway point - new HolonomicPose2d( - new Translation2d(0.75, 3), - new Rotation2d(-Math.PI), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0.75, 3), + new Rotation2d(-Math.PI)), DirectionSE2.TO_Y), // place for gateway point - new HolonomicPose2d( - new Translation2d(3, 7), - new Rotation2d(Math.PI / 2), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(3, 7), + new Rotation2d(Math.PI / 2)), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(6, 9), - new Rotation2d(-((7 * Math.PI) / 36)), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(6, 9), + new Rotation2d(-((7 * Math.PI) / 36))), DirectionSE2.TO_Y)); @SuppressWarnings("unused") Trajectory100 t = p.restToRest(waypoints); diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java index 729c961ad..1035fab3a 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java @@ -1,7 +1,7 @@ package org.team100.frc2025.CalgamesArm; import org.jfree.data.xy.VectorSeries; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.timing.TimedPose; @@ -24,7 +24,7 @@ public VectorSeries convert(Trajectory100 t) { double duration = t.duration(); for (double time = 0; time < duration; time += DT) { TimedPose p = t.sample(time); - HolonomicPose2d pp = p.state().getPose(); + Pose2dWithDirection pp = p.state().getPose(); double x = pp.translation().getX(); double y = pp.translation().getY(); Rotation2d heading = pp.heading(); @@ -41,7 +41,7 @@ public VectorSeries convertRotated(Trajectory100 t) { double duration = t.duration(); for (double time = 0; time < duration; time += DT) { TimedPose p = t.sample(time); - HolonomicPose2d pp = p.state().getPose(); + Pose2dWithDirection pp = p.state().getPose(); double y = pp.translation().getX(); double x = -pp.translation().getY(); Rotation2d heading = pp.heading(); diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index b01d0c89c..d608c5e27 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -1,7 +1,6 @@ package org.team100.lib.geometry; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; /** * A direction (i.e. unit-length vector) in the SE2 manifold, diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 132f9e34c..0ffdfbfbf 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -205,10 +205,9 @@ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { } // TODO: fix interpolation - public static HolonomicPose2d interpolate(HolonomicPose2d a, HolonomicPose2d b, double x) { - return new HolonomicPose2d( - a.translation().interpolate(b.translation(), x), - interpolate2(a.heading(), b.heading(), x), + public static Pose2dWithDirection interpolate(Pose2dWithDirection a, Pose2dWithDirection b, double x) { + return new Pose2dWithDirection( + interpolate(a.pose(), b.pose(), x), DirectionSE2.fromRotation(interpolate2( a.course().toRotation(), b.course().toRotation(), x))); diff --git a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java deleted file mode 100644 index fec2c2ca2..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.team100.lib.geometry; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * For constructing splines, paths, and trajectories. - * - * Together, translation and heading constitute Pose2d. - * - * @param translation - * @param heading where the front of the robot is facing - * @param course direction of travel, including rotation - * - * TODO: use Pose2d here - */ -public record HolonomicPose2d( - Translation2d translation, - Rotation2d heading, - DirectionSE2 course) { - - public Pose2d pose() { - return new Pose2d(translation, heading); - } - - public static HolonomicPose2d make(Pose2d p, DirectionSE2 course) { - return new HolonomicPose2d(p.getTranslation(), p.getRotation(), course); - } - - /** Course without rotation */ - public static HolonomicPose2d make(Pose2d p, double course) { - return new HolonomicPose2d( - p.getTranslation(), - p.getRotation(), - new DirectionSE2(Math.cos(course), Math.sin(course), 0)); - } - - /** Course without rotation */ - public static HolonomicPose2d make(double x, double y, double heading, double course) { - return new HolonomicPose2d( - new Translation2d(x, y), - new Rotation2d(heading), - new DirectionSE2(Math.cos(course), Math.sin(course), 0)); - } - - /** For tank drive, heading and course are the same. */ - public static HolonomicPose2d tank(double x, double y, double heading) { - return new HolonomicPose2d( - new Translation2d(x, y), - new Rotation2d(heading), - new DirectionSE2(Math.cos(heading), Math.sin(heading), 0)); - } - - /** - * For tank drive, heading and course are the same. This is like the WPI - * trajectory. - */ - public static HolonomicPose2d tank(Pose2d p) { - Rotation2d r = p.getRotation(); - return new HolonomicPose2d( - p.getTranslation(), - r, - new DirectionSE2(r.getCos(), r.getSin(), 0)); - } -} diff --git a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java deleted file mode 100644 index 0bb6b1a7c..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.team100.lib.geometry; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; - -/** - * For constructing 3d splines, paths, and trajectories. - * - * Together, translation and heading constitude Pose3d. - * - * @param translation - * @param heading where we're facing, can include roll - * @param course where we're going - */ -public record HolonomicPose3d( - Translation3d translation, - Rotation3d heading, - DirectionR3 course) { - - public Pose3d pose() { - return new Pose3d(translation, heading); - } - -} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java new file mode 100644 index 000000000..6be881a33 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java @@ -0,0 +1,62 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * For constructing splines, paths, and trajectories. + * + * This is similar to ModelR3, except it doesn't have the notion of velocity, + * only direction. + * + * @param pose location and orientation + * @param course direction of travel (includes rotation) + */ +public record Pose2dWithDirection( + Pose2d pose, + DirectionSE2 course) { + + public Translation2d translation() { + return pose.getTranslation(); + } + + public Rotation2d heading() { + return pose.getRotation(); + } + + public static Pose2dWithDirection make(Pose2d p, DirectionSE2 course) { + return new Pose2dWithDirection(p, course); + } + + /** Course without rotation */ + public static Pose2dWithDirection make(Pose2d p, double course) { + return new Pose2dWithDirection( + p, + new DirectionSE2(Math.cos(course), Math.sin(course), 0)); + } + + /** Course without rotation */ + public static Pose2dWithDirection make(double x, double y, double heading, double course) { + return new Pose2dWithDirection( + new Pose2d(new Translation2d(x, y), new Rotation2d(heading)), + new DirectionSE2(Math.cos(course), Math.sin(course), 0)); + } + + /** For tank drive, heading and course are the same. */ + public static Pose2dWithDirection tank(double x, double y, double heading) { + return new Pose2dWithDirection( + new Pose2d(new Translation2d(x, y), new Rotation2d(heading)), + new DirectionSE2(Math.cos(heading), Math.sin(heading), 0)); + } + + /** + * For tank drive, heading and course are the same. This is like the WPI + * trajectory. + */ + public static Pose2dWithDirection tank(Pose2d p) { + Rotation2d r = p.getRotation(); + return new Pose2dWithDirection( + p, new DirectionSE2(r.getCos(), r.getSin(), 0)); + } +} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index d8feff0ab..8a5e29e86 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -6,15 +6,15 @@ import edu.wpi.first.math.geometry.Rotation2d; /** - * HolonomicPose2d with: + * Pose2dWithDirection with: * * * the spatial rate of change in heading * * the spatial rate of change in course * * the spatial rate of change in course curvature */ public class Pose2dWithMotion { - /** Position, heading and course. */ - private final HolonomicPose2d m_pose; + /** Pose and course. */ + private final Pose2dWithDirection m_pose; /** Change in heading per meter of motion, rad/m. */ private final double m_headingRate; /** Change in course per change in distance, rad/m. */ @@ -30,7 +30,7 @@ public class Pose2dWithMotion { * @param dCurvatureDsRad_M2 acceleration in course per meter traveled squared. */ public Pose2dWithMotion( - HolonomicPose2d pose, + Pose2dWithDirection pose, double headingRate, double curvatureRad_M, double dCurvatureDsRad_M2) { @@ -40,7 +40,7 @@ public Pose2dWithMotion( m_dCurvatureDsRad_M2 = dCurvatureDsRad_M2; } - public HolonomicPose2d getPose() { + public Pose2dWithDirection getPose() { return m_pose; } diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose3dWithDirection.java b/lib/src/main/java/org/team100/lib/geometry/Pose3dWithDirection.java new file mode 100644 index 000000000..83b671c66 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/Pose3dWithDirection.java @@ -0,0 +1,24 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +/** + * For constructing 3d splines, paths, and trajectories. + * + * @param pose location and orientation + * @param course direction of travel (including rotation) + */ +public record Pose3dWithDirection( + Pose3d pose, + DirectionSE3 course) { + + public Translation3d translation() { + return pose.getTranslation(); + } + + public Rotation3d heading() { + return pose.getRotation(); + } +} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java index 8b17c48a6..d9daa2b3a 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java @@ -8,19 +8,19 @@ * * the spatial rate of change in course curvature */ public class Pose3dWithMotion { - /** Position, heading and course. */ - private final HolonomicPose3d m_pose; + /** Pose and course. */ + private final Pose3dWithDirection m_pose; /** Change in yaw per meter of motion, rad/m. */ private final double m_headingYawRate; public Pose3dWithMotion( - HolonomicPose3d pose, + Pose3dWithDirection pose, double headingYawRate) { m_pose = pose; m_headingYawRate = headingYawRate; } - public HolonomicPose3d getPose() { + public Pose3dWithDirection getPose() { return m_pose; } diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java index 68dba295c..e5229a6bf 100644 --- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java +++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java @@ -10,7 +10,7 @@ import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.GlobalVelocityR2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.localization.Blip24; @@ -515,7 +515,7 @@ public class Pose2dWithMotionLogger { public void log(Supplier vals) { if (!allow(m_level)) return; - HolonomicPose2d val = vals.get().getPose(); + Pose2dWithDirection val = vals.get().getPose(); m_pose2dLogger.log(val::pose); m_rotation2dLogger.log(() -> val.course().toRotation()); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java index 6c7020933..5aa700533 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java @@ -6,7 +6,7 @@ import org.team100.lib.controller.r3.ControllerR3; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -56,13 +56,11 @@ public void initialize() { Pose2d pose = m_drive.getState().pose(); Translation2d toGoal = m_goal.getTranslation().minus(pose.getTranslation()); VelocitySE2 startVelocity = m_drive.getState().velocity(); - HolonomicPose2d startWaypoint = new HolonomicPose2d( - pose.getTranslation(), - pose.getRotation(), + Pose2dWithDirection startWaypoint = new Pose2dWithDirection( + pose, DirectionSE2.fromRotation(startVelocity.angle().orElse(toGoal.getAngle()))); - HolonomicPose2d endWaypoint = new HolonomicPose2d( - m_goal.getTranslation(), - m_goal.getRotation(), + Pose2dWithDirection endWaypoint = new Pose2dWithDirection( + m_goal, DirectionSE2.fromRotation(m_endVelocity.angle().orElse(toGoal.getAngle()))); Trajectory100 trajectory = m_planner.generateTrajectory( List.of(startWaypoint, endWaypoint), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java index b35365e80..2f77c81cc 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java @@ -4,7 +4,7 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; import org.team100.lib.subsystems.r3.PositionSubsystemR3; @@ -21,7 +21,7 @@ public class GoToPosePosition extends MoveAndHold { private final LoggerFactory m_log; private final PositionSubsystemR3 m_subsystem; - private final HolonomicPose2d m_goal; + private final Pose2dWithDirection m_goal; private final Rotation2d m_course; private final TrajectoryPlanner m_trajectoryPlanner; @@ -31,7 +31,7 @@ public GoToPosePosition( LoggerFactory parent, PositionSubsystemR3 subsystem, Rotation2d course, - HolonomicPose2d goal, + Pose2dWithDirection goal, TrajectoryPlanner trajectoryPlanner) { m_log = parent.type(this); m_subsystem = subsystem; @@ -43,7 +43,7 @@ public GoToPosePosition( @Override public void initialize() { - HolonomicPose2d m_currentPose = HolonomicPose2d.make( + Pose2dWithDirection m_currentPose = Pose2dWithDirection.make( m_subsystem.getState().pose(), DirectionSE2.fromRotation(m_course)); Trajectory100 m_trajectory = m_trajectoryPlanner.restToRest( diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java index 6d28540b5..4e72ca4e8 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java @@ -4,7 +4,7 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.tank.TankDrive; import org.team100.lib.trajectory.Trajectory100; @@ -49,8 +49,8 @@ public void initialize() { m_startTimeS = Takt.get(); // may return null m_trajectory = m_planner.restToRest(List.of( - HolonomicPose2d.tank(m_drive.getPose()), - HolonomicPose2d.tank(m_goal))); + Pose2dWithDirection.tank(m_drive.getPose()), + Pose2dWithDirection.tank(m_goal))); m_viz.setViz(m_trajectory); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 9125cd8bd..366f4b794 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -5,7 +5,7 @@ import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; @@ -102,11 +102,11 @@ public Trajectory100 line(Pose2d initial) { initial.plus(new Transform2d(1, 0, Rotation2d.kZero))); } - public Trajectory100 restToRest(List waypoints) { + public Trajectory100 restToRest(List waypoints) { return generateTrajectory(waypoints, 0.0, 0.0); } - public Trajectory100 restToRest(List waypoints, List mN) { + public Trajectory100 restToRest(List waypoints, List mN) { return generateTrajectory(waypoints, 0.0, 0.0, mN); } @@ -138,13 +138,11 @@ public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { try { return generateTrajectory( List.of( - new HolonomicPose2d( - startTranslation, - startState.rotation(), + new Pose2dWithDirection( + startState.pose(), DirectionSE2.fromRotation(startingAngle)), - new HolonomicPose2d( - endTranslation, - endState.rotation(), + new Pose2dWithDirection( + endState.pose(), DirectionSE2.fromRotation(courseToGoal))), startVelocity.norm(), endVelocity.norm(), @@ -178,13 +176,11 @@ public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, try { return generateTrajectory( List.of( - new HolonomicPose2d( - startTranslation, - startState.rotation(), + new Pose2dWithDirection( + startState.pose(), DirectionSE2.fromRotation(startCourse)), - new HolonomicPose2d( - endTranslation, - endState.rotation(), + new Pose2dWithDirection( + endState.pose(), DirectionSE2.fromRotation(endCourse))), splineEntranceVelocity, splineExitVelocity, @@ -213,13 +209,11 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { try { return restToRest( List.of( - new HolonomicPose2d( - startTranslation, - start.getRotation(), + new Pose2dWithDirection( + start, DirectionSE2.fromRotation(courseToGoal)), - new HolonomicPose2d( - endTranslation, - end.getRotation(), + new Pose2dWithDirection( + end, DirectionSE2.fromRotation(courseToGoal)))); } catch (TrajectoryGenerationException e) { return null; @@ -230,7 +224,7 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { * The shape of the spline accommodates the start and end velocities. */ public Trajectory100 generateTrajectory( - List waypoints, + List waypoints, double start_vel, double end_vel) { try { @@ -257,7 +251,7 @@ public Trajectory100 generateTrajectory( } public Trajectory100 generateTrajectory( - List waypoints, + List waypoints, double start_vel, double end_vel, List mN) { diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 2d7adbb03..3cea9ea49 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -5,7 +5,7 @@ import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.path.spline.SplineUtil; @@ -18,7 +18,7 @@ public class PathFactory { public static Path100 pathFromWaypoints( - List waypoints, + List waypoints, double maxDx, double maxDy, double maxDTheta, @@ -39,7 +39,7 @@ public static Path100 pathFromWaypoints( } public static Path100 pathFromWaypoints( - List waypoints, + List waypoints, double maxDx, double maxDy, double maxDTheta) { @@ -65,15 +65,15 @@ public static Path100 withoutControlPoints( List splines = new ArrayList<>(waypoints.size() - 1); // first make a series of straight lines, with corners at the waypoints for (int i = 1; i < waypoints.size(); ++i) { - Translation2d p0 = waypoints.get(i - 1).getTranslation(); - Translation2d p1 = waypoints.get(i).getTranslation(); - Rotation2d course = p1.minus(p0).getAngle(); + Pose2d pose0 = waypoints.get(i - 1); + Pose2d pose1 = waypoints.get(i); + Rotation2d course = pose1.getTranslation().minus(pose0.getTranslation()).getAngle(); splines.add(new HolonomicSpline( - new HolonomicPose2d( - p0, waypoints.get(i - 1).getRotation(), + new Pose2dWithDirection( + pose0, DirectionSE2.fromRotation(course)), - new HolonomicPose2d( - p1, waypoints.get(i).getRotation(), + new Pose2dWithDirection( + pose1, DirectionSE2.fromRotation(course)))); } // then adjust the control points to make it C1 smooth diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 28d7b2859..7d594e8aa 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -4,7 +4,7 @@ import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.util.Math100; @@ -48,7 +48,7 @@ public class HolonomicSpline { * You'll probably want to call SplineUtil.forceC1() and * SplineUtil.optimizeSpline() after creating these segments. */ - public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1) { + public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1) { this(p0, p1, 1.2, 1.2); } @@ -73,7 +73,7 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1) { * @param mN0 starting magic number * @param mN1 ending magic number */ - public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, double mN1) { + public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1, double mN0, double mN1) { double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); if (DEBUG) { @@ -145,9 +145,8 @@ private HolonomicSpline( public Pose2dWithMotion getPose2dWithMotion(double p) { return new Pose2dWithMotion( - new HolonomicPose2d( - getPoint(p), - getHeading(p), + new Pose2dWithDirection( + new Pose2d(getPoint(p), getHeading(p)), DirectionSE2.fromRotation(getCourse(p).orElseThrow())), getDHeadingDs(p), getCurvature(p), diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java index 0448b319b..3036b996a 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java @@ -3,9 +3,9 @@ import java.util.Optional; import org.team100.lib.geometry.DirectionR3; +import org.team100.lib.geometry.DirectionSE3; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose3d; -import org.team100.lib.geometry.Pose3dWithMotion; +import org.team100.lib.geometry.Pose3dWithDirection; import org.team100.lib.util.Math100; import edu.wpi.first.math.geometry.Rotation2d; @@ -42,16 +42,16 @@ public class HolonomicSpline3d { private final Rotation2d m_pitch0; private final Rotation2d m_yaw0; - public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1) { + public HolonomicSpline3d(Pose3dWithDirection p0, Pose3dWithDirection p1) { this(p0, p1, 1.2, 1.2); } - public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, double mN1) { + public HolonomicSpline3d(Pose3dWithDirection p0, Pose3dWithDirection p1, double mN0, double mN1) { double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); - DirectionR3 course0 = p0.course(); - DirectionR3 course1 = p1.course(); + DirectionSE3 course0 = p0.course(); + DirectionSE3 course1 = p1.course(); double x0 = p0.translation().getX(); double x1 = p1.translation().getX(); @@ -142,14 +142,14 @@ protected Translation3d getPoint(double t) { } // public Pose3dWithMotion getPose3dWithMotion(double p) { - // return new Pose3dWithMotion( - // new HolonomicPose3d( - // getPoint(p), - // getHeading(p), - // getCourse(p).orElseThrow()), - // getDHeadingDs(p), - // getCurvature(p), - // getDCurvatureDs(p)); + // return new Pose3dWithMotion( + // new HolonomicPose3d( + // getPoint(p), + // getHeading(p), + // getCourse(p).orElseThrow()), + // getDHeadingDs(p), + // getCurvature(p), + // getDCurvatureDs(p)); // } public Optional getCourse(double t) { diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java index 686715284..17cf57d8b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java @@ -2,7 +2,7 @@ import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ControlR3; import org.team100.lib.state.ModelR3; @@ -46,7 +46,7 @@ public JointConstraint( @Override public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - HolonomicPose2d pose = state.getPose(); + Pose2dWithDirection pose = state.getPose(); // Velocity if translation speed were 1.0 m/s. VelocitySE2 v = new VelocitySE2( state.getCourse().getCos(), diff --git a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java index bee8616c9..81c6b030c 100644 --- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java +++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java @@ -2,7 +2,7 @@ import java.util.List; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; @@ -32,7 +32,7 @@ private static double[] fromTrajectory100(Trajectory100 m_trajectory) { double[] arr = new double[m_trajectory.length() * 3]; int ndx = 0; for (TimedPose p : m_trajectory.getPoints()) { - HolonomicPose2d pose = p.state().getPose(); + Pose2dWithDirection pose = p.state().getPose(); arr[ndx + 0] = pose.translation().getX(); arr[ndx + 1] = pose.translation().getY(); arr[ndx + 2] = pose.heading().getDegrees(); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java index 32c1b4c9c..c2ef28f42 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java @@ -6,7 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DeltaSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; @@ -180,7 +180,7 @@ void testErrZero() { ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(0, err.getX(), 0.001); @@ -195,7 +195,7 @@ void testErrXAhead() { ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -209,7 +209,7 @@ void testErrXBehind() { ModelR3 measurement = new ModelR3(new Pose2d(0, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - HolonomicPose2d.make(1, 0, 0, 0), 0, 0, 0), 0, 0, 0)); + Pose2dWithDirection.make(1, 0, 0, 0), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -224,7 +224,7 @@ void testErrXAheadWithRotation() { ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d(1))); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 1, 0), 0, 0, 0), 0, 0, 0)); + Pose2dWithDirection.make(0, 0, 1, 0), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -241,7 +241,7 @@ void testErrorAhead() { // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, // no curvature 0); // no change in curvature @@ -267,7 +267,7 @@ void testErrorSideways() { // motion is in a straight line, down the x axis // setpoint is +x, facing down y Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(1, 0, Math.PI / 2, 0), 0, + Pose2dWithDirection.make(1, 0, Math.PI / 2, 0), 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -293,7 +293,7 @@ void testVelocityErrorZero() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -321,7 +321,7 @@ void testVelocityErrorAhead() { // motion is in a straight line, down the x axis // at the origin Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, // no curvature 0); // no change in curvature @@ -346,7 +346,7 @@ void testFeedForwardAhead() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, // no curvature 0);// no change in curvature double t = 0; @@ -370,7 +370,7 @@ void testFeedForwardSideways() { // motion is in a straight line, down the x axis // setpoint is the same Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, Math.PI / 2, 0), 0, + Pose2dWithDirection.make(0, 0, Math.PI / 2, 0), 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -394,7 +394,7 @@ void testFeedForwardTurning() { // motion is tangential to the x axis but turning left // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 1, 1, // driving and turning 0); // no change in curvature @@ -418,7 +418,7 @@ void testFeedbackAhead() { // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); // setpoint is also at the origin - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -454,7 +454,7 @@ void testFeedbackAheadPlusY() { // measurement is plus-Y, facing ahead Pose2d currentState = new Pose2d(0, 1, Rotation2d.kZero); // setpoint is at the origin - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -490,7 +490,7 @@ void testFeedbackAheadPlusTheta() { // measurement is plus-theta Pose2d currentState = new Pose2d(0, 0, new Rotation2d(1.0)); // setpoint is also at the origin - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -529,7 +529,7 @@ void testFeedbackSideways() { // measurement is at the origin, facing down the y axis Pose2d currentState = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint is the same - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -564,7 +564,7 @@ void testFeedbackSidewaysPlusY() { // measurement is plus-y, facing down the y axis Pose2d currentState = new Pose2d(0, 1, Rotation2d.kCCW_Pi_2); // setpoint is parallel at the origin - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -599,7 +599,7 @@ void testFullFeedbackAhead() { // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); // setpoint is also at the origin - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -637,7 +637,7 @@ void testFullFeedbackSideways() { // measurement is at the origin, facing +y Pose2d currentPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint postion is the same - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -680,7 +680,7 @@ void testFullFeedbackSidewaysWithRotation() { new VelocitySE2(0.5, 0, 0)); // setpoint postion is ahead in x and y and theta - HolonomicPose2d setpointPose = HolonomicPose2d.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; diff --git a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java index 371e16d3e..4e44f8eba 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java @@ -11,7 +11,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -130,18 +130,21 @@ void testTrajectoryDone() { @Test void testFieldRelativeTrajectory() { - List waypoints = new ArrayList<>(); - waypoints.add(new HolonomicPose2d( - new Translation2d(), - Rotation2d.fromDegrees(180), + List waypoints = new ArrayList<>(); + waypoints.add(new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + Rotation2d.fromDegrees(180)), DirectionSE2.TO_X)); - waypoints.add(new HolonomicPose2d( - new Translation2d(100, 4), - Rotation2d.fromDegrees(180), + waypoints.add(new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 4), + Rotation2d.fromDegrees(180)), DirectionSE2.TO_X)); - waypoints.add(new HolonomicPose2d( - new Translation2d(196, 13), - Rotation2d.fromDegrees(0), + waypoints.add(new Pose2dWithDirection( + new Pose2d( + new Translation2d(196, 13), + Rotation2d.fromDegrees(0)), DirectionSE2.TO_X)); double start_vel = 0.0; diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java index ff91daee8..13b08a762 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java @@ -8,7 +8,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.controller.r3.ControllerFactoryR3; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.logging.LoggerFactory; @@ -57,7 +57,7 @@ void testSimple() throws IOException { List.of( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0))), controller, viz); diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java index a7d24549d..94a0ca722 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java @@ -5,7 +5,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.controller.r3.ControllerFactoryR3; import org.team100.lib.controller.r3.FullStateControllerR3; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -46,8 +46,8 @@ public class DriveWithTrajectoryFunctionTest implements Timeless { Trajectory100 makeTrajectory(Pose2d startingPose) { return planner.restToRest( List.of( - HolonomicPose2d.make(startingPose, 0), - HolonomicPose2d.make(1, 2, Math.PI / 2, Math.PI / 2))); + Pose2dWithDirection.make(startingPose, 0), + Pose2dWithDirection.make(1, 2, Math.PI / 2, Math.PI / 2))); } @Test diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index f25dee561..d2f72fc5c 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ControlR3; import org.team100.lib.trajectory.timing.TimedPose; @@ -30,7 +30,7 @@ void testTimedPose() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -45,7 +45,7 @@ void testTimedPose2() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -60,7 +60,7 @@ void testTimedPose3() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -76,7 +76,7 @@ void testTimedPose4() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index 760352346..0579f8fcd 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -5,7 +5,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.timing.TimedPose; @@ -31,7 +31,7 @@ void testTimedPose() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -46,7 +46,7 @@ void testTimedPose2() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -61,7 +61,7 @@ void testTimedPose3() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -77,7 +77,7 @@ void testTimedPose4() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index ffff20931..7f5f2491e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -6,7 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -37,14 +37,12 @@ void testPreviewAndAdvance() { Translation2d goalTranslation = end.getTranslation(); Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); - List waypoints = List.of( - new HolonomicPose2d( - currentTranslation, - start.getRotation(), + List waypoints = List.of( + new Pose2dWithDirection( + start, DirectionSE2.fromRotation(angleToGoal)), - new HolonomicPose2d( - goalTranslation, - end.getRotation(), + new Pose2dWithDirection( + end, DirectionSE2.fromRotation(angleToGoal))); List constraints = new TimingConstraintFactory(limits).fast(logger); @@ -75,14 +73,12 @@ void testSample() { Translation2d goalTranslation = end.getTranslation(); Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); - List waypoints = List.of( - new HolonomicPose2d( - currentTranslation, - start.getRotation(), + List waypoints = List.of( + new Pose2dWithDirection( + start, DirectionSE2.fromRotation(angleToGoal)), - new HolonomicPose2d( - goalTranslation, - end.getRotation(), + new Pose2dWithDirection( + end, DirectionSE2.fromRotation(angleToGoal))); List constraints = new TimingConstraintFactory(limits).fast(logger); @@ -105,14 +101,16 @@ void testSample() { @Test void testSampleThoroughly() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - Rotation2d.kZero, + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); @@ -142,14 +140,16 @@ void testSampleThoroughly() { @Test void testSampleThoroughlyWithRotation() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - Rotation2d.kZero, + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kCCW_Pi_2, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kCCW_Pi_2), DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); @@ -185,18 +185,21 @@ void testSampleThoroughlyWithRotation() { // There's no need to run this all the time // @Test void testSamplePerformance() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - Rotation2d.kZero, + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(10, 0), - Rotation2d.kCCW_Pi_2, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(10, 0), + Rotation2d.kCCW_Pi_2), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(10, 10), - Rotation2d.kPi, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(10, 10), + Rotation2d.kPi), DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index e06df0d6c..4f1e30077 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -9,7 +9,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -36,14 +36,16 @@ class TrajectoryPlannerTest implements Timeless { @Test void testLinear() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -56,14 +58,16 @@ void testLinear() { @Test void testBackingUp() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(0, 0), - Rotation2d.kZero, + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), DirectionSE2.MINUS_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_X)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); @@ -91,14 +95,16 @@ void testBackingUp() { */ @Test void testPerformance() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_Y)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 26bf50e9d..2fe4e530f 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -11,7 +11,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; @@ -32,13 +32,13 @@ class Path100Test { private static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(24, 0, Math.toRadians(30), 0), 0, 0, 0), + Pose2dWithDirection.make(24, 0, Math.toRadians(30), 0), 0, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(36, 12, Math.toRadians(60), 0), 0, 0, 0), + Pose2dWithDirection.make(36, 12, Math.toRadians(60), 0), 0, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(60, 12, Math.toRadians(90), 0), 0, 0, 0)); + Pose2dWithDirection.make(60, 12, Math.toRadians(90), 0), 0, 0, 0)); @Test void testEmpty() { @@ -54,13 +54,15 @@ void testEmpty() { void testSimple() { // spline is in the x direction, no curvature. HolonomicSpline spline = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X)) { @Override @@ -137,19 +139,19 @@ void testStateAccessors() { void test() throws TimingException { List waypoints = Arrays.asList( new Pose2dWithMotion( - HolonomicPose2d.make(0.0, 0.0, Math.toRadians(0), 0), + Pose2dWithDirection.make(0.0, 0.0, Math.toRadians(0), 0), 0.1, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(24.0, 0.0, Math.toRadians(30), 0), + Pose2dWithDirection.make(24.0, 0.0, Math.toRadians(30), 0), 0.1, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(36.0, 0.0, Math.toRadians(60), Math.PI / 2), + Pose2dWithDirection.make(36.0, 0.0, Math.toRadians(60), Math.PI / 2), 1e6, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(36.0, 24.0, Math.toRadians(60), 0), + Pose2dWithDirection.make(36.0, 24.0, Math.toRadians(60), 0), 0.1, 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(60.0, 24.0, Math.toRadians(180), 0), + Pose2dWithDirection.make(60.0, 24.0, Math.toRadians(180), 0), 0.1, 0, 0)); // Create the reference trajectory (straight line motion between waypoints). diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index b050c7030..9d6fb49fc 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -9,7 +9,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -80,14 +80,16 @@ void testForced() throws TimingException { @Test void testBackingUp() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(0, 0), - Rotation2d.kZero, + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), DirectionSE2.MINUS_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_X)); Path100 path = PathFactory.pathFromWaypoints( waypoints, @@ -100,18 +102,21 @@ void testBackingUp() { /** Preserves the tangent at the corner and so makes a little "S" */ @Test void testCorner() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_Y)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); @@ -128,14 +133,16 @@ void testCorner() { @Test void testLinear() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.01, 0.01, 0.1); @@ -152,22 +159,26 @@ void testLinear() { @Test void testActualCorner() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_Y), - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_Y)); assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); @@ -175,22 +186,26 @@ void testActualCorner() { @Test void testComposite() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(1), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(2, 0), - new Rotation2d(1), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(2, 0), + new Rotation2d(1)), DirectionSE2.TO_X)); assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); @@ -198,13 +213,15 @@ void testComposite() { @Test void test() { - HolonomicPose2d p1 = new HolonomicPose2d( - new Translation2d(0, 0), - Rotation2d.kZero, + Pose2dWithDirection p1 = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), DirectionSE2.TO_X); - HolonomicPose2d p2 = new HolonomicPose2d( - new Translation2d(15, 10), - Rotation2d.kZero, + Pose2dWithDirection p2 = new Pose2dWithDirection( + new Pose2d( + new Translation2d(15, 10), + Rotation2d.kZero), new DirectionSE2(1, 5, 0)); HolonomicSpline s = new HolonomicSpline(p1, p2); @@ -231,13 +248,15 @@ void test() { @Test void testDx() { HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(0, -1), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, -1), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_Y), 1.0, 1.0); List splines = List.of(s0); @@ -255,14 +274,16 @@ void testDx() { */ @Test void testPerformance() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_Y)); long startTimeNs = System.nanoTime(); Path100 t = new Path100(new ArrayList<>()); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java index 1c089e565..ad87c336c 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java @@ -3,10 +3,11 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.DirectionR3; -import org.team100.lib.geometry.HolonomicPose3d; +import org.team100.lib.geometry.DirectionSE3; +import org.team100.lib.geometry.Pose3dWithDirection; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; @@ -16,14 +17,16 @@ public class HolonomicSpline3dTest implements Timeless { @Test void testLinear() { HolonomicSpline3d s = new HolonomicSpline3d( - new HolonomicPose3d( - new Translation3d(), - new Rotation3d(), - new DirectionR3(1, 0, 0)), - new HolonomicPose3d( - new Translation3d(1, 0, 0), - new Rotation3d(), - new DirectionR3(1, 0, 0))); + new Pose3dWithDirection( + new Pose3d( + new Translation3d(), + new Rotation3d()), + new DirectionSE3(1, 0, 0, 0, 0, 0)), + new Pose3dWithDirection( + new Pose3d( + new Translation3d(1, 0, 0), + new Rotation3d()), + new DirectionSE3(1, 0, 0, 0, 0, 0))); Translation3d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 859b33fb9..b1b3bb055 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -8,7 +8,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -44,13 +44,15 @@ void testCourse() { @Test void testLinear() { HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), DirectionSE2.TO_X)); Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); @@ -69,13 +71,15 @@ void testLinear() { @Test void testLinear2() { HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(2, 0), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(2, 0), + new Rotation2d()), DirectionSE2.TO_X)); Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); @@ -94,13 +98,15 @@ void testLinear2() { @Test void testRotation() { HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(1), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), DirectionSE2.TO_X)); if (DEBUG) { @@ -138,13 +144,15 @@ void testRotation() { void testRotation2() { // Make sure the rotation goes over +/- pi HolonomicSpline s = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(2.5), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d(2.5)), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(-2.5), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(-2.5)), DirectionSE2.TO_X)); if (DEBUG) { System.out.println("d, x, y, heading, course"); @@ -168,43 +176,51 @@ void testCircle() { // magic number of 1 makes something about 1.5% from a circle, close enough. double magicNumber = 1.0; HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.k180deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.k180deg), DirectionSE2.TO_Y), - new HolonomicPose2d( - new Translation2d(0, 1), - Rotation2d.kCW_90deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 1), + Rotation2d.kCW_90deg), DirectionSE2.MINUS_X), magicNumber, magicNumber); HolonomicSpline s1 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(0, 1), - Rotation2d.kCW_90deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 1), + Rotation2d.kCW_90deg), DirectionSE2.MINUS_X), - new HolonomicPose2d( - new Translation2d(-1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(-1, 0), + Rotation2d.kZero), DirectionSE2.MINUS_Y), magicNumber, magicNumber); HolonomicSpline s2 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(-1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(-1, 0), + Rotation2d.kZero), DirectionSE2.MINUS_Y), - new HolonomicPose2d( - new Translation2d(0, -1), - Rotation2d.kCCW_90deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, -1), + Rotation2d.kCCW_90deg), DirectionSE2.TO_X), magicNumber, magicNumber); HolonomicSpline s3 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(0, -1), - Rotation2d.kCCW_90deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, -1), + Rotation2d.kCCW_90deg), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.k180deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.k180deg), DirectionSE2.TO_Y), magicNumber, magicNumber); List splines = new ArrayList<>(); @@ -263,24 +279,28 @@ void testMismatchedThetaDerivatives() { double magicNumber = 1.0; // turn a bit to the left HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(0, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(1), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), DirectionSE2.TO_X), magicNumber, magicNumber); // turn much more to the left HolonomicSpline s1 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(1, 0), - new Rotation2d(1), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(2, 0), - Rotation2d.k180deg, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(2, 0), + Rotation2d.k180deg), DirectionSE2.TO_X), magicNumber, magicNumber); List splines = new ArrayList<>(); @@ -327,25 +347,29 @@ void testMismatchedXYDerivatives() { // this goes straight ahead to (1,0) // derivatives point straight ahead HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(0, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_X), magicNumber, magicNumber); // this is a sharp turn to the left // derivatives point to the left HolonomicSpline s1 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_Y), - new HolonomicPose2d( - new Translation2d(1, 1), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + Rotation2d.kZero), DirectionSE2.TO_Y), magicNumber, magicNumber); List splines = new ArrayList<>(); @@ -405,13 +429,15 @@ void testEntryVelocity() { // radius is 1 m. HolonomicSpline s0 = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(0, -1), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, -1), + Rotation2d.kZero), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), DirectionSE2.TO_Y), 1.2, 1.2); if (DEBUG) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java index b13bbda08..283d19585 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java @@ -8,8 +8,9 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -18,17 +19,20 @@ class QuinticHermiteOptimizerTest { @Test void test() { - HolonomicPose2d a = new HolonomicPose2d( - new Translation2d(0, 100), - new Rotation2d(), + Pose2dWithDirection a = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 100), + new Rotation2d()), DirectionSE2.MINUS_Y); - HolonomicPose2d b = new HolonomicPose2d( - new Translation2d(50, 0), - new Rotation2d(), + Pose2dWithDirection b = new Pose2dWithDirection( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d()), DirectionSE2.TO_X); - HolonomicPose2d c = new HolonomicPose2d( - new Translation2d(100, 100), - new Rotation2d(), + Pose2dWithDirection c = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 100), + new Rotation2d()), DirectionSE2.TO_Y); List splines = new ArrayList<>(); @@ -37,21 +41,25 @@ void test() { assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); - HolonomicPose2d d = new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + Pose2dWithDirection d = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_Y); - HolonomicPose2d e = new HolonomicPose2d( - new Translation2d(0, 50), - new Rotation2d(), + Pose2dWithDirection e = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 50), + new Rotation2d()), DirectionSE2.TO_X); - HolonomicPose2d f = new HolonomicPose2d( - new Translation2d(100, 50), - new Rotation2d(), + Pose2dWithDirection f = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d()), DirectionSE2.MINUS_Y); - HolonomicPose2d g = new HolonomicPose2d( - new Translation2d(100, 0), - new Rotation2d(), + Pose2dWithDirection g = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 0), + new Rotation2d()), DirectionSE2.MINUS_X); List splines1 = new ArrayList<>(); @@ -61,25 +69,30 @@ void test() { assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); - HolonomicPose2d h = new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + Pose2dWithDirection h = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_X); - HolonomicPose2d i = new HolonomicPose2d( - new Translation2d(50, 0), - new Rotation2d(), + Pose2dWithDirection i = new Pose2dWithDirection( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d()), DirectionSE2.TO_X); - HolonomicPose2d j = new HolonomicPose2d( - new Translation2d(100, 50), - new Rotation2d(), + Pose2dWithDirection j = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d()), new DirectionSE2(1, 1, 0)); - HolonomicPose2d k = new HolonomicPose2d( - new Translation2d(150, 0), - new Rotation2d(), + Pose2dWithDirection k = new Pose2dWithDirection( + new Pose2d( + new Translation2d(150, 0), + new Rotation2d()), DirectionSE2.MINUS_Y); - HolonomicPose2d l = new HolonomicPose2d( - new Translation2d(150, -50), - new Rotation2d(), + Pose2dWithDirection l = new Pose2dWithDirection( + new Pose2d( + new Translation2d(150, -50), + new Rotation2d()), DirectionSE2.MINUS_Y); List splines2 = new ArrayList<>(); @@ -95,17 +108,20 @@ void test() { @Test void testHolonomic() { - HolonomicPose2d a = new HolonomicPose2d( - new Translation2d(0, 100), - new Rotation2d(), + Pose2dWithDirection a = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 100), + new Rotation2d()), DirectionSE2.MINUS_Y); - HolonomicPose2d b = new HolonomicPose2d( - new Translation2d(50, 0), - new Rotation2d(Math.PI / 2), + Pose2dWithDirection b = new Pose2dWithDirection( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d(Math.PI / 2)), DirectionSE2.TO_X); - HolonomicPose2d c = new HolonomicPose2d( - new Translation2d(100, 100), - new Rotation2d(Math.PI), + Pose2dWithDirection c = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 100), + new Rotation2d(Math.PI)), DirectionSE2.TO_Y); List splines = new ArrayList<>(); @@ -114,21 +130,25 @@ void testHolonomic() { assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); - HolonomicPose2d d = new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + Pose2dWithDirection d = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_Y); - HolonomicPose2d e = new HolonomicPose2d( - new Translation2d(0, 50), - new Rotation2d(Math.PI / 2), + Pose2dWithDirection e = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 50), + new Rotation2d(Math.PI / 2)), DirectionSE2.TO_X); - HolonomicPose2d f = new HolonomicPose2d( - new Translation2d(100, 50), - new Rotation2d(Math.PI), + Pose2dWithDirection f = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d(Math.PI)), DirectionSE2.MINUS_Y); - HolonomicPose2d g = new HolonomicPose2d( - new Translation2d(100, 0), - new Rotation2d(), + Pose2dWithDirection g = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 0), + new Rotation2d()), DirectionSE2.MINUS_X); List splines1 = new ArrayList<>(); @@ -138,25 +158,30 @@ void testHolonomic() { assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); - HolonomicPose2d h = new HolonomicPose2d( - new Translation2d(0, 0), - new Rotation2d(), + Pose2dWithDirection h = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), DirectionSE2.TO_X); - HolonomicPose2d i = new HolonomicPose2d( - new Translation2d(50, 0), - new Rotation2d(Math.PI / 2), + Pose2dWithDirection i = new Pose2dWithDirection( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d(Math.PI / 2)), DirectionSE2.TO_X); - HolonomicPose2d j = new HolonomicPose2d( - new Translation2d(100, 50), - new Rotation2d(Math.PI), + Pose2dWithDirection j = new Pose2dWithDirection( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d(Math.PI)), new DirectionSE2(1, 1, 0)); - HolonomicPose2d k = new HolonomicPose2d( - new Translation2d(150, 0), - new Rotation2d(), + Pose2dWithDirection k = new Pose2dWithDirection( + new Pose2d( + new Translation2d(150, 0), + new Rotation2d()), DirectionSE2.MINUS_Y); - HolonomicPose2d l = new HolonomicPose2d( - new Translation2d(150, -50), - new Rotation2d(Math.PI / 2), + Pose2dWithDirection l = new Pose2dWithDirection( + new Pose2d( + new Translation2d(150, -50), + new Rotation2d(Math.PI / 2)), DirectionSE2.MINUS_Y); List splines2 = new ArrayList<>(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java index e6eb35a34..83afb193e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -26,7 +26,7 @@ void testSimple() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); // motionless, so 100% of the capsize accel is available assertEquals(-8.166, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(8.166, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); @@ -43,7 +43,7 @@ void testSimpleMoving() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); // moving, only some of the capsize accel is available assertEquals(-5.257, c.getMinMaxAcceleration(p, 2.5).getMinAccel(), DELTA); assertEquals(5.257, c.getMinMaxAcceleration(p, 2.5).getMaxAccel(), DELTA); @@ -60,7 +60,7 @@ void testSimpleOverspeed() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); // above the velocity limit assertEquals(-1, c.getMinMaxAcceleration(p, 3).getMinAccel(), DELTA); assertEquals(0, c.getMinMaxAcceleration(p, 3).getMaxAccel(), DELTA); @@ -76,7 +76,7 @@ void testSimple2() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(2.021, c.getMaxVelocity(p).getValue(), DELTA); @@ -91,7 +91,7 @@ void testStraightLine() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java index 00398eb5a..c16c45742 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -18,7 +18,7 @@ public class ConstantConstraintTest implements Timeless { void testVelocity() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); } @@ -26,7 +26,7 @@ void testVelocity() { void testAccel() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); assertEquals(-3, c.getMinMaxAcceleration(state, 1).getMinAccel(), DELTA); assertEquals(3, c.getMinMaxAcceleration(state, 1).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java index 365bdba2d..6c1381869 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -19,15 +19,15 @@ void testSquare() { // here the two speeds are the same DiamondConstraint c = new DiamondConstraint(logger, 1, 1, 4); Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); // moving purely in x, get the x number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, Math.PI / 2), 0, 0, 0); // moving purely in y, get the y number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, Math.PI / 4), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, Math.PI / 4), 0, 0, 0); // moving diagonally, get less. assertEquals(0.707, c.getMaxVelocity(state).getValue(), DELTA); } @@ -36,15 +36,15 @@ void testSquare() { void testVelocity() { DiamondConstraint c = new DiamondConstraint(logger, 2, 3, 4); Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); // moving purely in x, get the x number assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, Math.PI / 2), 0, 0, 0); // moving purely in y, get the y number assertEquals(3, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, Math.PI / 4), 0, 0, 0); + Pose2dWithDirection.make(0, 0, 0, Math.PI / 4), 0, 0, 0); // moving diagonally, get less. assertEquals(1.697, c.getMaxVelocity(state).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java index 6e18e2cf0..9babf9ef2 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; @@ -22,7 +22,7 @@ void test1() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // motion +x, limiter is elevator. Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(3, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(3, 0, 0, 0), 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -37,7 +37,7 @@ void test2() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // motion +y, shoulder and wrist have same constraint Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(3, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(3, 0, 0, Math.PI / 2), 0, 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -52,7 +52,7 @@ void test3() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist, motion +x, bend doesn't matter. Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(2, 1, Math.PI / 2, 0), 0, 0, 0); + Pose2dWithDirection.make(2, 1, Math.PI / 2, 0), 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -67,7 +67,7 @@ void test4() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist, motion +y, shoulder is the limiter Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(2, 1, Math.PI / 2, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(2, 1, Math.PI / 2, Math.PI / 2), 0, 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -82,7 +82,7 @@ void test5() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist and shoulder, arm at 45, motion +y, Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(2, 1 + Math.sqrt(2), Math.PI / 2, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(2, 1 + Math.sqrt(2), Math.PI / 2, Math.PI / 2), 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index 5c3d4dbd9..504fc66c5 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -13,7 +13,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -25,6 +25,7 @@ import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.TimingConstraint.MinMaxAcceleration; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -35,10 +36,10 @@ public class ScheduleGeneratorTest { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); public static final List WAYPOINTS = Arrays.asList( - new Pose2dWithMotion(HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(HolonomicPose2d.make(24.0, 0.0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(HolonomicPose2d.make(36, 12, 0, 0), 0, 0, 0), - new Pose2dWithMotion(HolonomicPose2d.make(60, 12, 0, 0), 0, 0, 0)); + new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make(24.0, 0.0, 0, 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make(36, 12, 0, 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make(60, 12, 0, 0), 0, 0, 0)); public static final List HEADINGS = List.of( GeometryUtil.fromDegrees(0), @@ -104,8 +105,8 @@ public void checkTrajectory( @Test void testJustTurningInPlace() { Path100 path = new Path100(Arrays.asList( - new Pose2dWithMotion(HolonomicPose2d.make(0, 0, 0, 0), 1, 0, 0), - new Pose2dWithMotion(HolonomicPose2d.make(0, 0, Math.PI, 0), 1, 0, 0))); + new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, 0, 0), 1, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, Math.PI, 0), 1, 0, 0))); // Triangle profile. assertThrows(IllegalArgumentException.class, @@ -265,14 +266,16 @@ void testV1() { */ @Test void testPerformance() { - List waypoints = List.of( - new HolonomicPose2d( - new Translation2d(), - new Rotation2d(), + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), DirectionSE2.TO_X), - new HolonomicPose2d( - new Translation2d(1, 1), - new Rotation2d(), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), DirectionSE2.TO_Y)); long startTimeNs = System.nanoTime(); final int iterations = 100; diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java index 8cd74a668..3cbcbf4c4 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -23,19 +23,19 @@ void testVelocity() { // motionless double m = c.getMaxVelocity(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0)).getValue(); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0)).getValue(); assertEquals(5, m, DELTA); // moving in +x, no curvature, no rotation m = c.getMaxVelocity(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0)).getValue(); // max allowed velocity is full speed assertEquals(5, m, DELTA); // moving in +x, 5 rad/meter m = c.getMaxVelocity(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 5, 0, 0)).getValue(); // at 5 rad/m with 0.5m sides the fastest you can go is 1.55 m/s. assertEquals(1.925, m, DELTA); @@ -46,7 +46,7 @@ void testVelocity() { // traveling 1 m/s, there are 4 m/s available for the fastest wheel // which means 11.314 rad/s, and also 11.314 rad/m since we're going 1 m/s. Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 11.313708, 0, 0); m = c.getMaxVelocity( state) @@ -64,7 +64,7 @@ void testAccel() { SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1); // this is constant MinMaxAcceleration m = c.getMinMaxAcceleration(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), 0); + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0); assertEquals(-20, m.getMinAccel(), DELTA); assertEquals(10, m.getMaxAccel(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index 6d14b5b5a..b4f38d5e4 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; class TimedStateTest { @@ -14,13 +14,13 @@ void test() { // At (0,0,0), t=0, v=0, acceleration=1 TimedPose start_state = new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0.0, 0.0, 1.0); // At (.5,0,0), t=1, v=1, acceleration=0 TimedPose end_state = new TimedPose( new Pose2dWithMotion( - HolonomicPose2d.make(0.5, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make(0.5, 0, 0, 0), 0, 0, 0), 1.0, 1.0, 0.0); TimedPose i0 = start_state.interpolate2(end_state, 0.0); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java index 4e03a0f72..71b2a0dc8 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; public class TorqueConstraintTest { @@ -14,7 +14,7 @@ void testRadial() { TorqueConstraint jc = new TorqueConstraint(6); // moving +x at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(1, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make(1, 0, 0, 0), 0, 0, 0); // no tangential motion => no limit assertEquals(Double.NEGATIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -25,7 +25,7 @@ void testTangential() { TorqueConstraint jc = new TorqueConstraint(6); // at (1,0,0), moving (0,1,0) Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(1, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(1, 0, 0, Math.PI / 2), 0, 0, 0); // tangential motion at 1 m assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -36,7 +36,7 @@ void testInclined() { TorqueConstraint jc = new TorqueConstraint(6); // moving +x+y at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(1, 0, 0, Math.PI / 4), 0, 0, 0); + Pose2dWithDirection.make(1, 0, 0, Math.PI / 4), 0, 0, 0); // motion at 45 deg => higher limit assertEquals(-1.414, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1.414, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -47,7 +47,7 @@ void testFar() { TorqueConstraint jc = new TorqueConstraint(6); // moving +y at (2,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(2, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(2, 0, 0, Math.PI / 2), 0, 0, 0); // more r => lower limit assertEquals(-0.5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(0.5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -58,7 +58,7 @@ void testFar2() { TorqueConstraint jc = new TorqueConstraint(6); // moving +y at (3,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(3, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(3, 0, 0, Math.PI / 2), 0, 0, 0); // more r => lower limit assertEquals(-0.333, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(0.333, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -69,7 +69,7 @@ void testRealistic() { TorqueConstraint jc = new TorqueConstraint(30); // moving +y at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(1, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make(1, 0, 0, Math.PI / 2), 0, 0, 0); // should match the constant constraint at around 1 m assertEquals(-5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 3c2c7a6cf..c5bfccaea 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -6,7 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -31,8 +31,8 @@ public class TrajectoryVelocityProfileTest implements Timeless { // A five-meter straight line. public static final List WAYPOINTS = Arrays.asList( - new Pose2dWithMotion(HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(HolonomicPose2d.make(5, 0, 0, 0), 0, 0, 0)); + new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make(5, 0, 0, 0), 0, 0, 0)); // No rotation. public static final List HEADINGS = List.of( diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java index f2cdb912a..fd070dde9 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import edu.wpi.first.math.geometry.Translation2d; @@ -17,7 +17,7 @@ void testOutside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(-1, -1, 0, 0), + Pose2dWithDirection.make(-1, -1, 0, 0), 0, // spatial, so rad/m 0, 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -31,7 +31,7 @@ void testInside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0.5, 0.5, 0, 0), + Pose2dWithDirection.make(0.5, 0.5, 0, 0), 0, // spatial, so rad/m 0, 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java index dd198b645..c5e8a0286 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -25,7 +25,7 @@ void testNormal() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 1, // spatial, so rad/m 0, 0); assertEquals(-8.485, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -39,7 +39,7 @@ void testVelocity2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest2(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 1, // spatial, so rad/m + Pose2dWithDirection.make(0, 0, 0, 0), 1, // spatial, so rad/m 0, 0); assertEquals(5.656, c.getMaxVelocity(p).getValue(), DELTA); } @@ -52,7 +52,7 @@ void testAccel() { YAW_RATE_SCALE); // driving and spinning Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 1, + Pose2dWithDirection.make(0, 0, 0, 0), 1, 0, 0); // there is an accel limit. assertEquals(-8.485, @@ -68,7 +68,7 @@ void testAccel2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forRealisticTest(logger), scale); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + Pose2dWithDirection.make(0, 0, 0, 0), 1, // spatial, so rad/m 0, 0); // this number is still quite high even with a low scale. From 45e5dde66e07884cf23aa843d5c4f1586d5983a1 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 15 Dec 2025 12:05:19 -0800 Subject: [PATCH 03/42] 3d trajectory WIP --- .../frc2025/Swerve/Auto/GoToCoralStation.java | 7 +- .../team100/frc2025/robot/LolipopAuto.java | 4 +- .../CalgamesArm/TrajectoryJointTest.java | 9 ++- .../lib/controller/r3/ControllerR3Base.java | 2 +- .../org/team100/lib/geometry/DirectionR2.java | 6 ++ .../team100/lib/geometry/DirectionSE2.java | 9 ++- .../team100/lib/geometry/GeometryUtil.java | 8 ++- .../lib/geometry/Pose2dWithDirection.java | 18 ----- ...veToPoseWithTrajectoryAndExitVelocity.java | 11 ++- .../r3/commands/GoToPosePosition.java | 6 +- .../lib/trajectory/TrajectoryPlanner.java | 21 ++++-- .../lib/trajectory/path/PathFactory.java | 8 ++- .../path/spline/HolonomicSpline.java | 4 +- .../r3/FullStateControllerR3Test.java | 63 ++++++++++++----- .../DriveToPoseWithTrajectoryTest.java | 4 +- .../DriveWithTrajectoryFunctionTest.java | 4 +- .../subsystems/swerve/SwerveControlTest.java | 17 +++-- .../subsystems/swerve/SwerveModelTest.java | 18 +++-- .../team100/lib/targeting/TargetsTest.java | 9 +-- .../lib/trajectory/Trajectory100Test.java | 13 ++-- .../lib/trajectory/path/Path100Test.java | 31 ++++++--- ...CentripetalAccelerationConstraintTest.java | 23 +++++-- .../timing/ConstantConstraintTest.java | 11 ++- .../timing/DiamondConstraintTest.java | 27 ++++++-- .../timing/JointConstraintTest.java | 23 +++++-- .../timing/ScheduleGeneratorTest.java | 18 +++-- .../SwerveDriveDynamicsConstraintTest.java | 20 ++++-- .../lib/trajectory/timing/TimedStateTest.java | 11 ++- .../timing/TorqueConstraintTest.java | 27 ++++++-- .../timing/TrajectoryVelocityProfileTest.java | 7 +- .../VelocityLimitRegionConstraintTest.java | 8 ++- .../timing/YawRateConstraintTest.java | 16 +++-- .../main/java/org/team100/frc2025/Autons.java | 27 ++++---- .../org/team100/frc2025/SingleIntake.java | 1 - .../team100/frc2025/SingleShooterFactory.java | 1 - .../main/java/org/team100/frc2025/Autons.java | 69 ++++++++++--------- 36 files changed, 372 insertions(+), 189 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index ef516ac17..6e3ca15e3 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -6,6 +6,7 @@ import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.CoralStation; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; @@ -56,11 +57,13 @@ public Trajectory100 apply(Pose2d currentPose) { waypoints.add( new Pose2dWithDirection( currentPose, - DirectionSE2.fromRotation(newInitialSpline))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(newInitialSpline), 0))); waypoints.add( new Pose2dWithDirection( goal, - DirectionSE2.fromRotation(courseToGoal))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(courseToGoal), 0))); return m_planner.restToRest(waypoints); } diff --git a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java index 9e87f483a..ea8903eea 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java +++ b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java @@ -20,6 +20,7 @@ import org.team100.lib.subsystems.r3.commands.DriveWithTrajectoryFunction; import org.team100.lib.trajectory.TrajectoryPlanner; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; @@ -52,7 +53,8 @@ public Command get() { m_log, m_machinery.m_drive, m_autoController, m_machinery.m_trajectoryViz, (p) -> m_planner.restToRest(List.of( Pose2dWithDirection.make(m_machinery.m_drive.getPose(), Math.PI), - Pose2dWithDirection.make(3, 5, 0, -2)))); + Pose2dWithDirection.make( + new Pose2d(3, 5, new Rotation2d(0)), -2)))); DriveToPoseWithProfile toReefA = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java index 8f8742455..342e4d0bf 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -4,8 +4,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.AccelerationSE2; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -22,6 +22,7 @@ import org.team100.lib.trajectory.timing.YawRateConstraint; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; /** How do the joints respond to trajectories? */ public class TrajectoryJointTest { @@ -49,8 +50,10 @@ void homeToL4() { TrajectoryPlanner m_planner = new TrajectoryPlanner(c); Trajectory100 t = m_planner.restToRest(List.of( - Pose2dWithDirection.make(1, 0, 0, 0), - Pose2dWithDirection.make(1.9, 0.5, 2.5, 2))); + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(0)), 0), + Pose2dWithDirection.make( + new Pose2d(1.9, 0.5, new Rotation2d(2.5)), 2))); ElevatorArmWristKinematics k = new ElevatorArmWristKinematics( 0.5, 0.3); diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java index b2fe643e2..76859f375 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java @@ -8,8 +8,8 @@ import org.team100.lib.logging.LoggerFactory.ControlR3Logger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.logging.LoggerFactory.GlobaDeltaR3Logger; -import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.logging.LoggerFactory.ModelR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.state.ControlR3; import org.team100.lib.state.ModelR3; diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java index 6413ae6cf..f476fbfee 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java @@ -1,5 +1,7 @@ package org.team100.lib.geometry; +import edu.wpi.first.math.geometry.Rotation2d; + /** * Represents a direction in a 2d plane as a unit vector in R2. * @@ -30,6 +32,10 @@ public DirectionR2(double px, double py) { y = py / h; } + public static DirectionR2 fromRotation(Rotation2d r) { + return new DirectionR2(r.getCos(), r.getSin()); + } + @Override public boolean equals(Object obj) { return obj instanceof DirectionR2 other diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index d608c5e27..565c53bbd 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -33,9 +33,12 @@ public Rotation2d toRotation() { public static final DirectionSE2 MINUS_Y = new DirectionSE2(0, -1, 0); public static final DirectionSE2 SPIN = new DirectionSE2(0, 0, 1); - /** Adapter for rotation-free directions */ - public static final DirectionSE2 fromRotation(Rotation2d r) { - return new DirectionSE2(r.getCos(), r.getSin(), 0); + /** + * @param course cartesian course + * @param theta direction of rotation + */ + public static final DirectionSE2 fromDirections(DirectionR2 course, double theta) { + return new DirectionSE2(course.x, course.y, theta); } @Override diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 0ffdfbfbf..85bb0d1aa 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -208,9 +208,11 @@ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { public static Pose2dWithDirection interpolate(Pose2dWithDirection a, Pose2dWithDirection b, double x) { return new Pose2dWithDirection( interpolate(a.pose(), b.pose(), x), - DirectionSE2.fromRotation(interpolate2( - a.course().toRotation(), - b.course().toRotation(), x))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(interpolate2( + a.course().toRotation(), + b.course().toRotation(), x)), + 0)); } /** diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java index 6be881a33..f632256c5 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java @@ -25,10 +25,6 @@ public Rotation2d heading() { return pose.getRotation(); } - public static Pose2dWithDirection make(Pose2d p, DirectionSE2 course) { - return new Pose2dWithDirection(p, course); - } - /** Course without rotation */ public static Pose2dWithDirection make(Pose2d p, double course) { return new Pose2dWithDirection( @@ -36,20 +32,6 @@ public static Pose2dWithDirection make(Pose2d p, double course) { new DirectionSE2(Math.cos(course), Math.sin(course), 0)); } - /** Course without rotation */ - public static Pose2dWithDirection make(double x, double y, double heading, double course) { - return new Pose2dWithDirection( - new Pose2d(new Translation2d(x, y), new Rotation2d(heading)), - new DirectionSE2(Math.cos(course), Math.sin(course), 0)); - } - - /** For tank drive, heading and course are the same. */ - public static Pose2dWithDirection tank(double x, double y, double heading) { - return new Pose2dWithDirection( - new Pose2d(new Translation2d(x, y), new Rotation2d(heading)), - new DirectionSE2(Math.cos(heading), Math.sin(heading), 0)); - } - /** * For tank drive, heading and course are the same. This is like the WPI * trajectory. diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java index 5aa700533..8a2c9d47e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java @@ -5,6 +5,7 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.controller.r3.ControllerR3; import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; @@ -58,10 +59,16 @@ public void initialize() { VelocitySE2 startVelocity = m_drive.getState().velocity(); Pose2dWithDirection startWaypoint = new Pose2dWithDirection( pose, - DirectionSE2.fromRotation(startVelocity.angle().orElse(toGoal.getAngle()))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation( + startVelocity.angle().orElse(toGoal.getAngle())), + 0)); Pose2dWithDirection endWaypoint = new Pose2dWithDirection( m_goal, - DirectionSE2.fromRotation(m_endVelocity.angle().orElse(toGoal.getAngle()))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation( + m_endVelocity.angle().orElse(toGoal.getAngle())), + 0)); Trajectory100 trajectory = m_planner.generateTrajectory( List.of(startWaypoint, endWaypoint), startVelocity.norm(), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java index 2f77c81cc..42eeb1338 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java @@ -3,6 +3,7 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; @@ -43,9 +44,10 @@ public GoToPosePosition( @Override public void initialize() { - Pose2dWithDirection m_currentPose = Pose2dWithDirection.make( + Pose2dWithDirection m_currentPose = new Pose2dWithDirection( m_subsystem.getState().pose(), - DirectionSE2.fromRotation(m_course)); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(m_course), 0)); Trajectory100 m_trajectory = m_trajectoryPlanner.restToRest( List.of(m_currentPose, m_goal)); m_referenceController = new PositionReferenceControllerR3( diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 366f4b794..017cac682 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -3,9 +3,10 @@ import java.util.List; import java.util.function.Function; -import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; @@ -140,10 +141,12 @@ public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { List.of( new Pose2dWithDirection( startState.pose(), - DirectionSE2.fromRotation(startingAngle)), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(startingAngle), 0)), new Pose2dWithDirection( endState.pose(), - DirectionSE2.fromRotation(courseToGoal))), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(courseToGoal), 0))), startVelocity.norm(), endVelocity.norm(), magicNumbers); @@ -178,10 +181,12 @@ public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, List.of( new Pose2dWithDirection( startState.pose(), - DirectionSE2.fromRotation(startCourse)), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(startCourse), 0)), new Pose2dWithDirection( endState.pose(), - DirectionSE2.fromRotation(endCourse))), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(endCourse), 0))), splineEntranceVelocity, splineExitVelocity, magicNumbers); @@ -211,10 +216,12 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { List.of( new Pose2dWithDirection( start, - DirectionSE2.fromRotation(courseToGoal)), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(courseToGoal), 0)), new Pose2dWithDirection( end, - DirectionSE2.fromRotation(courseToGoal)))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(courseToGoal), 0)))); } catch (TrajectoryGenerationException e) { return null; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 3cea9ea49..b98ea7422 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import java.util.List; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithDirection; @@ -12,7 +13,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; public class PathFactory { @@ -71,10 +71,12 @@ public static Path100 withoutControlPoints( splines.add(new HolonomicSpline( new Pose2dWithDirection( pose0, - DirectionSE2.fromRotation(course)), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(course), 0)), new Pose2dWithDirection( pose1, - DirectionSE2.fromRotation(course)))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(course), 0)))); } // then adjust the control points to make it C1 smooth SplineUtil.forceC1(splines); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 7d594e8aa..a4a63f037 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -2,6 +2,7 @@ import java.util.Optional; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithDirection; @@ -147,7 +148,8 @@ public Pose2dWithMotion getPose2dWithMotion(double p) { return new Pose2dWithMotion( new Pose2dWithDirection( new Pose2d(getPoint(p), getHeading(p)), - DirectionSE2.fromRotation(getCourse(p).orElseThrow())), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(getCourse(p).orElseThrow()), 0)), getDHeadingDs(p), getCurvature(p), getDCurvatureDs(p)); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java index c2ef28f42..8a7eb165e 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java @@ -180,7 +180,9 @@ void testErrZero() { ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(0, err.getX(), 0.001); @@ -195,7 +197,9 @@ void testErrXAhead() { ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0, 0, 0)); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -209,7 +213,9 @@ void testErrXBehind() { ModelR3 measurement = new ModelR3(new Pose2d(0, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - Pose2dWithDirection.make(1, 0, 0, 0), 0, 0, 0), 0, 0, 0)); + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -224,7 +230,9 @@ void testErrXAheadWithRotation() { ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d(1))); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 1, 0), 0, 0, 0), 0, 0, 0)); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(1)), 0), + 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -241,7 +249,8 @@ void testErrorAhead() { // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, // no curvature 0); // no change in curvature @@ -267,7 +276,9 @@ void testErrorSideways() { // motion is in a straight line, down the x axis // setpoint is +x, facing down y Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(1, 0, Math.PI / 2, 0), 0, + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), 0), + 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -293,7 +304,7 @@ void testVelocityErrorZero() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, + Pose2dWithDirection.make(new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -321,7 +332,8 @@ void testVelocityErrorAhead() { // motion is in a straight line, down the x axis // at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, // no curvature 0); // no change in curvature @@ -346,7 +358,9 @@ void testFeedForwardAhead() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, // no curvature 0);// no change in curvature double t = 0; @@ -370,7 +384,9 @@ void testFeedForwardSideways() { // motion is in a straight line, down the x axis // setpoint is the same Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, Math.PI / 2, 0), 0, + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0), + 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -394,7 +410,8 @@ void testFeedForwardTurning() { // motion is tangential to the x axis but turning left // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 1, 1, // driving and turning 0); // no change in curvature @@ -418,7 +435,8 @@ void testFeedbackAhead() { // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); // setpoint is also at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -454,7 +472,8 @@ void testFeedbackAheadPlusY() { // measurement is plus-Y, facing ahead Pose2d currentState = new Pose2d(0, 1, Rotation2d.kZero); // setpoint is at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -490,7 +509,8 @@ void testFeedbackAheadPlusTheta() { // measurement is plus-theta Pose2d currentState = new Pose2d(0, 0, new Rotation2d(1.0)); // setpoint is also at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -529,7 +549,8 @@ void testFeedbackSideways() { // measurement is at the origin, facing down the y axis Pose2d currentState = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint is the same - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -564,7 +585,8 @@ void testFeedbackSidewaysPlusY() { // measurement is plus-y, facing down the y axis Pose2d currentState = new Pose2d(0, 1, Rotation2d.kCCW_Pi_2); // setpoint is parallel at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -599,7 +621,8 @@ void testFullFeedbackAhead() { // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); // setpoint is also at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, 0, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -637,7 +660,8 @@ void testFullFeedbackSideways() { // measurement is at the origin, facing +y Pose2d currentPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint postion is the same - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -680,7 +704,8 @@ void testFullFeedbackSidewaysWithRotation() { new VelocitySE2(0.5, 0, 0)); // setpoint postion is ahead in x and y and theta - Pose2dWithDirection setpointPose = Pose2dWithDirection.make(0, 0, Math.PI / 2, 0); + Pose2dWithDirection setpointPose = Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java index 13b08a762..1a80e80c5 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java @@ -57,7 +57,9 @@ void testSimple() throws IOException { List.of( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 0))), controller, viz); diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java index 94a0ca722..55a40f135 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java @@ -20,6 +20,7 @@ import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; /** @@ -47,7 +48,8 @@ Trajectory100 makeTrajectory(Pose2d startingPose) { return planner.restToRest( List.of( Pose2dWithDirection.make(startingPose, 0), - Pose2dWithDirection.make(1, 2, Math.PI / 2, Math.PI / 2))); + Pose2dWithDirection.make( + new Pose2d(1, 2, new Rotation2d(Math.PI / 2)), Math.PI / 2))); } @Test diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index d2f72fc5c..aa7df059f 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -3,9 +3,9 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ControlR3; import org.team100.lib.trajectory.timing.TimedPose; @@ -30,7 +30,9 @@ void testTimedPose() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -45,7 +47,9 @@ void testTimedPose2() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -60,7 +64,9 @@ void testTimedPose3() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -76,7 +82,8 @@ void testTimedPose4() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 1, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index 0579f8fcd..0a87cfea4 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -4,9 +4,9 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.timing.TimedPose; @@ -31,7 +31,9 @@ void testTimedPose() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -46,7 +48,9 @@ void testTimedPose2() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -61,7 +65,9 @@ void testTimedPose3() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -77,7 +83,9 @@ void testTimedPose4() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 1, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java index e410480ed..ab3f09d59 100644 --- a/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java +++ b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java @@ -58,8 +58,9 @@ void testTargets() throws InterruptedException { // tilt down 45 pub.set(new Rotation3d[] { new Rotation3d(0, Math.PI / 4, 0) }, (long) (Takt.get() * 1000000.0)); + // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); inst.flush(); stepTime(); t.update(); @@ -120,7 +121,7 @@ void testMultipleCameras() throws InterruptedException { ModelR3 p = new ModelR3(); Targets reader = new Targets(logger, logger, (x) -> p); - Thread.sleep(50); + Thread.sleep(100); SimulatedTargetWriter writer = new SimulatedTargetWriter( logger, List.of(Camera.TEST4, Camera.TEST5), @@ -128,13 +129,13 @@ void testMultipleCameras() throws InterruptedException { new Translation2d[] { new Translation2d(1, 0) }); // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); stepTime(); writer.update(); // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); stepTime(); reader.update(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 7f5f2491e..260122b0d 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -5,6 +5,7 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; @@ -40,10 +41,12 @@ void testPreviewAndAdvance() { List waypoints = List.of( new Pose2dWithDirection( start, - DirectionSE2.fromRotation(angleToGoal)), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(angleToGoal), 0)), new Pose2dWithDirection( end, - DirectionSE2.fromRotation(angleToGoal))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(angleToGoal), 0))); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -76,10 +79,12 @@ void testSample() { List waypoints = List.of( new Pose2dWithDirection( start, - DirectionSE2.fromRotation(angleToGoal)), + DirectionSE2.fromDirections( + DirectionR2.fromRotation(angleToGoal), 0)), new Pose2dWithDirection( end, - DirectionSE2.fromRotation(angleToGoal))); + DirectionSE2.fromDirections( + DirectionR2.fromRotation(angleToGoal), 0))); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 2fe4e530f..b78bdb681 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -32,13 +32,21 @@ class Path100Test { private static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(24, 0, Math.toRadians(30), 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(24, 0, new Rotation2d(Math.toRadians(30))), 0), + 0, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(36, 12, Math.toRadians(60), 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(36, 12, new Rotation2d(Math.toRadians(60))), 0), + 0, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(60, 12, Math.toRadians(90), 0), 0, 0, 0)); + Pose2dWithDirection.make( + new Pose2d(60, 12, new Rotation2d(Math.toRadians(90))), 0), + 0, 0, 0)); @Test void testEmpty() { @@ -139,19 +147,24 @@ void testStateAccessors() { void test() throws TimingException { List waypoints = Arrays.asList( new Pose2dWithMotion( - Pose2dWithDirection.make(0.0, 0.0, Math.toRadians(0), 0), + Pose2dWithDirection.make( + new Pose2d(0.0, 0.0, new Rotation2d(Math.toRadians(0))), 0), 0.1, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(24.0, 0.0, Math.toRadians(30), 0), + Pose2dWithDirection.make( + new Pose2d(24.0, 0.0, new Rotation2d(Math.toRadians(30))), 0), 0.1, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(36.0, 0.0, Math.toRadians(60), Math.PI / 2), + Pose2dWithDirection.make( + new Pose2d(36.0, 0.0, new Rotation2d(Math.toRadians(60))), Math.PI / 2), 1e6, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(36.0, 24.0, Math.toRadians(60), 0), + Pose2dWithDirection.make( + new Pose2d(36.0, 24.0, new Rotation2d(Math.toRadians(60))), 0), 0.1, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make(60.0, 24.0, Math.toRadians(180), 0), + Pose2dWithDirection.make( + new Pose2d(60.0, 24.0, new Rotation2d(Math.toRadians(180))), 0), 0.1, 0, 0)); // Create the reference trajectory (straight line motion between waypoints). diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java index 83afb193e..0745fc81e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java @@ -11,6 +11,9 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + class CentripetalAccelerationConstraintTest implements Timeless { private static final double DELTA = 0.001; private static final double CENTRIPETAL_SCALE = 1.0; @@ -26,7 +29,9 @@ void testSimple() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 1, 0); // motionless, so 100% of the capsize accel is available assertEquals(-8.166, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(8.166, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); @@ -43,7 +48,9 @@ void testSimpleMoving() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 1, 0); // moving, only some of the capsize accel is available assertEquals(-5.257, c.getMinMaxAcceleration(p, 2.5).getMinAccel(), DELTA); assertEquals(5.257, c.getMinMaxAcceleration(p, 2.5).getMaxAccel(), DELTA); @@ -60,7 +67,9 @@ void testSimpleOverspeed() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 1, 0); // above the velocity limit assertEquals(-1, c.getMinMaxAcceleration(p, 3).getMinAccel(), DELTA); assertEquals(0, c.getMinMaxAcceleration(p, 3).getMaxAccel(), DELTA); @@ -76,7 +85,9 @@ void testSimple2() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 1, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 1, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(2.021, c.getMaxVelocity(p).getValue(), DELTA); @@ -91,7 +102,9 @@ void testStraightLine() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java index c16c45742..f2a608d6a 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java @@ -10,6 +10,9 @@ import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public class ConstantConstraintTest implements Timeless { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -18,7 +21,9 @@ public class ConstantConstraintTest implements Timeless { void testVelocity() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0); assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); } @@ -26,7 +31,9 @@ void testVelocity() { void testAccel() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0); assertEquals(-3, c.getMinMaxAcceleration(state, 1).getMinAccel(), DELTA); assertEquals(3, c.getMinMaxAcceleration(state, 1).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java index 6c1381869..55fa6d57e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java @@ -10,6 +10,9 @@ import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public class DiamondConstraintTest implements Timeless { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -19,15 +22,21 @@ void testSquare() { // here the two speeds are the same DiamondConstraint c = new DiamondConstraint(logger, 1, 1, 4); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0); // moving purely in x, get the x number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); // moving purely in y, get the y number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, Math.PI / 4), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4), + 0, 0, 0); // moving diagonally, get less. assertEquals(0.707, c.getMaxVelocity(state).getValue(), DELTA); } @@ -36,15 +45,21 @@ void testSquare() { void testVelocity() { DiamondConstraint c = new DiamondConstraint(logger, 2, 3, 4); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0); // moving purely in x, get the x number assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); // moving purely in y, get the y number assertEquals(3, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, Math.PI / 4), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4), + 0, 0, 0); // moving diagonally, get less. assertEquals(1.697, c.getMaxVelocity(state).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java index 9babf9ef2..e3649a627 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java @@ -10,6 +10,9 @@ import org.team100.lib.subsystems.prr.JointAccelerations; import org.team100.lib.subsystems.prr.JointVelocities; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public class JointConstraintTest { private static final double DELTA = 0.001; @@ -22,7 +25,9 @@ void test1() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // motion +x, limiter is elevator. Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(3, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(3, 0, new Rotation2d(0)), 0), + 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -37,7 +42,9 @@ void test2() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // motion +y, shoulder and wrist have same constraint Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(3, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -52,7 +59,9 @@ void test3() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist, motion +x, bend doesn't matter. Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(2, 1, Math.PI / 2, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), 0), + 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -67,7 +76,9 @@ void test4() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist, motion +y, shoulder is the limiter Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(2, 1, Math.PI / 2, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), Math.PI / 2), + 0, 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -82,7 +93,9 @@ void test5() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist and shoulder, arm at 45, motion +y, Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(2, 1 + Math.sqrt(2), Math.PI / 2, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(2, 1 + Math.sqrt(2), new Rotation2d(Math.PI / 2)), Math.PI / 2), + 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index 504fc66c5..b93a580aa 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -36,10 +36,14 @@ public class ScheduleGeneratorTest { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); public static final List WAYPOINTS = Arrays.asList( - new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make(24.0, 0.0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make(36, 12, 0, 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make(60, 12, 0, 0), 0, 0, 0)); + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(24.0, 0.0, new Rotation2d(0)), 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(36, 12, new Rotation2d(0)), 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(60, 12, new Rotation2d(0)), 0), 0, 0, 0)); public static final List HEADINGS = List.of( GeometryUtil.fromDegrees(0), @@ -105,8 +109,10 @@ public void checkTrajectory( @Test void testJustTurningInPlace() { Path100 path = new Path100(Arrays.asList( - new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, 0, 0), 1, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, Math.PI, 0), 1, 0, 0))); + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 1, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(Math.PI)), 0), 1, 0, 0))); // Triangle profile. assertThrows(IllegalArgumentException.class, diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java index 3cbcbf4c4..3b070a599 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java @@ -12,6 +12,9 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.trajectory.timing.TimingConstraint.MinMaxAcceleration; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + class SwerveDriveDynamicsConstraintTest { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -23,19 +26,23 @@ void testVelocity() { // motionless double m = c.getMaxVelocity(new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0)).getValue(); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0)).getValue(); assertEquals(5, m, DELTA); // moving in +x, no curvature, no rotation m = c.getMaxVelocity(new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, 0)).getValue(); // max allowed velocity is full speed assertEquals(5, m, DELTA); // moving in +x, 5 rad/meter m = c.getMaxVelocity(new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 5, 0, 0)).getValue(); // at 5 rad/m with 0.5m sides the fastest you can go is 1.55 m/s. assertEquals(1.925, m, DELTA); @@ -46,7 +53,8 @@ void testVelocity() { // traveling 1 m/s, there are 4 m/s available for the fastest wheel // which means 11.314 rad/s, and also 11.314 rad/m since we're going 1 m/s. Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 11.313708, 0, 0); m = c.getMaxVelocity( state) @@ -64,7 +72,9 @@ void testAccel() { SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1); // this is constant MinMaxAcceleration m = c.getMinMaxAcceleration(new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), 0); + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0); assertEquals(-20, m.getMinAccel(), DELTA); assertEquals(10, m.getMaxAccel(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index b4f38d5e4..fa77e73b5 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -6,6 +6,9 @@ import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + class TimedStateTest { private static final double EPSILON = 1e-12; @@ -14,13 +17,17 @@ void test() { // At (0,0,0), t=0, v=0, acceleration=1 TimedPose start_state = new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 0, 0, 0), 0.0, 0.0, 1.0); // At (.5,0,0), t=1, v=1, acceleration=0 TimedPose end_state = new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make(0.5, 0, 0, 0), 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0.5, 0, new Rotation2d(0)), 0), + 0, 0, 0), 1.0, 1.0, 0.0); TimedPose i0 = start_state.interpolate2(end_state, 0.0); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java index 71b2a0dc8..c23f959db 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java @@ -6,6 +6,9 @@ import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public class TorqueConstraintTest { private static final double DELTA = 0.001; @@ -14,7 +17,9 @@ void testRadial() { TorqueConstraint jc = new TorqueConstraint(6); // moving +x at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(1, 0, 0, 0), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(0)), 0), + 0, 0, 0); // no tangential motion => no limit assertEquals(Double.NEGATIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -25,7 +30,9 @@ void testTangential() { TorqueConstraint jc = new TorqueConstraint(6); // at (1,0,0), moving (0,1,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(1, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); // tangential motion at 1 m assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -36,7 +43,9 @@ void testInclined() { TorqueConstraint jc = new TorqueConstraint(6); // moving +x+y at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(1, 0, 0, Math.PI / 4), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 4), + 0, 0, 0); // motion at 45 deg => higher limit assertEquals(-1.414, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1.414, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -47,7 +56,9 @@ void testFar() { TorqueConstraint jc = new TorqueConstraint(6); // moving +y at (2,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(2, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(2, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); // more r => lower limit assertEquals(-0.5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(0.5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -58,7 +69,9 @@ void testFar2() { TorqueConstraint jc = new TorqueConstraint(6); // moving +y at (3,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(3, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); // more r => lower limit assertEquals(-0.333, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(0.333, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -69,7 +82,9 @@ void testRealistic() { TorqueConstraint jc = new TorqueConstraint(30); // moving +y at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(1, 0, 0, Math.PI / 2), 0, 0, 0); + Pose2dWithDirection.make( + new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2), + 0, 0, 0); // should match the constant constraint at around 1 m assertEquals(-5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index c5bfccaea..0d5b545f2 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -17,6 +17,7 @@ import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.path.Path100; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; /** @@ -31,8 +32,10 @@ public class TrajectoryVelocityProfileTest implements Timeless { // A five-meter straight line. public static final List WAYPOINTS = Arrays.asList( - new Pose2dWithMotion(Pose2dWithDirection.make(0, 0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make(5, 0, 0, 0), 0, 0, 0)); + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, 0), + new Pose2dWithMotion(Pose2dWithDirection.make( + new Pose2d(5, 0, new Rotation2d(0)), 0), 0, 0, 0)); // No rotation. public static final List HEADINGS = List.of( diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java index fd070dde9..a66a0cb85 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java @@ -6,6 +6,8 @@ import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; class VelocityLimitRegionConstraintTest { @@ -17,7 +19,8 @@ void testOutside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(-1, -1, 0, 0), + Pose2dWithDirection.make( + new Pose2d(-1, -1, new Rotation2d(0)), 0), 0, // spatial, so rad/m 0, 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -31,7 +34,8 @@ void testInside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0.5, 0.5, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0.5, 0.5, new Rotation2d(0)), 0), 0, // spatial, so rad/m 0, 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java index c5e8a0286..efc3d3200 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java @@ -11,6 +11,9 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + class YawRateConstraintTest implements Timeless { private static final double DELTA = 0.001; // for testing, use the aboslute maximum. This shouldn't be used in a real @@ -25,7 +28,7 @@ void testNormal() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make(new Pose2d(0, 0, new Rotation2d(0)), 0), 1, // spatial, so rad/m 0, 0); assertEquals(-8.485, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -39,7 +42,9 @@ void testVelocity2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest2(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 1, // spatial, so rad/m + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 1, // spatial, so rad/m 0, 0); assertEquals(5.656, c.getMaxVelocity(p).getValue(), DELTA); } @@ -52,7 +57,9 @@ void testAccel() { YAW_RATE_SCALE); // driving and spinning Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), 1, + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), + 1, 0, 0); // there is an accel limit. assertEquals(-8.485, @@ -68,7 +75,8 @@ void testAccel2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forRealisticTest(logger), scale); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(0, 0, 0, 0), + Pose2dWithDirection.make( + new Pose2d(0, 0, new Rotation2d(0)), 0), 1, // spatial, so rad/m 0, 0); // this number is still quite high even with a low scale. diff --git a/studies/rookie_bot_1/src/main/java/org/team100/frc2025/Autons.java b/studies/rookie_bot_1/src/main/java/org/team100/frc2025/Autons.java index 6e6fde7eb..246700ba9 100644 --- a/studies/rookie_bot_1/src/main/java/org/team100/frc2025/Autons.java +++ b/studies/rookie_bot_1/src/main/java/org/team100/frc2025/Autons.java @@ -5,13 +5,12 @@ import org.team100.lib.config.AnnotatedCommand; import org.team100.lib.config.AutonChooser; import org.team100.lib.field.MechanicalMayhem2025; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.shooter.IndexerServo; import org.team100.lib.subsystems.shooter.SingleDrumShooter; import org.team100.lib.subsystems.tank.TankDrive; import org.team100.lib.subsystems.tank.commands.FixedTrajectory; -import org.team100.lib.subsystems.tank.commands.ToPoseWithTrajectory; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.ConstantConstraint; @@ -90,48 +89,48 @@ public AnnotatedCommand get() { private Trajectory100 redLeftTrajectory(TrajectoryPlanner planner) { // delaying construction allows trajectory constraints to be mutable return planner.restToRest(List.of( - HolonomicPose2d.tank(MechanicalMayhem2025.START_RED_LEFT), - HolonomicPose2d.tank(MechanicalMayhem2025.START_RED_LEFT + Pose2dWithDirection.tank(MechanicalMayhem2025.START_RED_LEFT), + Pose2dWithDirection.tank(MechanicalMayhem2025.START_RED_LEFT .plus(new Transform2d(1.5, 0, Rotation2d.kZero))))); } private Trajectory100 redRightTrajectory(TrajectoryPlanner planner) { // delaying construction allows trajectory constraints to be mutable return planner.restToRest(List.of( - HolonomicPose2d.tank(MechanicalMayhem2025.START_RED_RIGHT), - HolonomicPose2d.tank(MechanicalMayhem2025.START_RED_RIGHT + Pose2dWithDirection.tank(MechanicalMayhem2025.START_RED_RIGHT), + Pose2dWithDirection.tank(MechanicalMayhem2025.START_RED_RIGHT .plus(new Transform2d(1.5, 0, Rotation2d.kZero))))); } private Trajectory100 redCenterTrajectory(TrajectoryPlanner planner) { // delaying construction allows trajectory constraints to be mutable return planner.restToRest(List.of( - HolonomicPose2d.tank(MechanicalMayhem2025.START_RED_CENTER), - HolonomicPose2d.tank(MechanicalMayhem2025.START_RED_CENTER + Pose2dWithDirection.tank(MechanicalMayhem2025.START_RED_CENTER), + Pose2dWithDirection.tank(MechanicalMayhem2025.START_RED_CENTER .plus(new Transform2d(1.5, 0, Rotation2d.kZero))))); } private Trajectory100 blueLeftTrajectory(TrajectoryPlanner planner) { // delaying construction allows trajectory constraints to be mutable return planner.restToRest(List.of( - HolonomicPose2d.tank(MechanicalMayhem2025.START_BLUE_LEFT), - HolonomicPose2d.tank(MechanicalMayhem2025.START_BLUE_LEFT + Pose2dWithDirection.tank(MechanicalMayhem2025.START_BLUE_LEFT), + Pose2dWithDirection.tank(MechanicalMayhem2025.START_BLUE_LEFT .plus(new Transform2d(1.5, 0, Rotation2d.kZero))))); } private Trajectory100 blueRightTrajectory(TrajectoryPlanner planner) { // delaying construction allows trajectory constraints to be mutable return planner.restToRest(List.of( - HolonomicPose2d.tank(MechanicalMayhem2025.START_BLUE_RIGHT), - HolonomicPose2d.tank(MechanicalMayhem2025.START_BLUE_RIGHT + Pose2dWithDirection.tank(MechanicalMayhem2025.START_BLUE_RIGHT), + Pose2dWithDirection.tank(MechanicalMayhem2025.START_BLUE_RIGHT .plus(new Transform2d(1.5, 0, Rotation2d.kZero))))); } private Trajectory100 blueCenterTrajectory(TrajectoryPlanner planner) { // delaying construction allows trajectory constraints to be mutable return planner.restToRest(List.of( - HolonomicPose2d.tank(MechanicalMayhem2025.START_BLUE_CENTER), - HolonomicPose2d.tank(MechanicalMayhem2025.START_BLUE_CENTER + Pose2dWithDirection.tank(MechanicalMayhem2025.START_BLUE_CENTER), + Pose2dWithDirection.tank(MechanicalMayhem2025.START_BLUE_CENTER .plus(new Transform2d(1.5, 0, Rotation2d.kZero))))); } diff --git a/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleIntake.java b/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleIntake.java index 281cbab35..dfe2f263e 100644 --- a/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleIntake.java +++ b/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleIntake.java @@ -1,6 +1,5 @@ package org.team100.frc2025; -import org.team100.lib.subsystems.shooter.IndexerServo; import org.team100.lib.subsystems.shooter.SingleDrumShooter; import edu.wpi.first.wpilibj2.command.Command; diff --git a/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleShooterFactory.java b/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleShooterFactory.java index 9f059b7f7..a432b96e9 100644 --- a/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleShooterFactory.java +++ b/studies/rookie_bot_1/src/main/java/org/team100/frc2025/SingleShooterFactory.java @@ -5,7 +5,6 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motor.BareMotor; import org.team100.lib.motor.MotorPhase; -import org.team100.lib.motor.rev.Neo550CANSparkMotor; import org.team100.lib.motor.rev.NeoVortexCANSparkMotor; import org.team100.lib.servo.OutboardLinearVelocityServo; import org.team100.lib.subsystems.shooter.SingleDrumShooter; diff --git a/studies/rookie_bot_3/src/main/java/org/team100/frc2025/Autons.java b/studies/rookie_bot_3/src/main/java/org/team100/frc2025/Autons.java index dadc33a4b..a6d2e9a22 100644 --- a/studies/rookie_bot_3/src/main/java/org/team100/frc2025/Autons.java +++ b/studies/rookie_bot_3/src/main/java/org/team100/frc2025/Autons.java @@ -8,10 +8,11 @@ import org.team100.lib.controller.r3.ControllerFactoryR3; import org.team100.lib.controller.r3.ControllerR3; import org.team100.lib.field.MechanicalMayhem2025; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.HolonomicProfile; +import org.team100.lib.profile.r3.HolonomicProfile; +import org.team100.lib.profile.r3.HolonomicProfileFactory; import org.team100.lib.subsystems.mecanum.MecanumDrive100; import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; import org.team100.lib.subsystems.r3.commands.DriveWithTrajectoryFunction; @@ -59,7 +60,7 @@ public Autons( m_drive = drive; m_indexer = indexer; m_shooter = shooter; - m_profile = HolonomicProfile.wpi(4, 8, 3, 6); + m_profile = HolonomicProfileFactory.wpi(4, 8, 3, 6); List constraints = List.of( new DiamondConstraint(m_log, 2, 2, 2), new ConstantConstraint(m_log, 1, 1), @@ -82,7 +83,7 @@ public Autons( new AnnotatedCommand(two.until(two::isDone).withName("auto two"), null, null)); Command three = m_drive.driveWithGlobalVelocity( - new GlobalVelocityR3(1.5, 0, 0)).withTimeout(1.0); + new VelocitySE2(1.5, 0, 0)).withTimeout(1.0); m_autonChooser.add("three", new AnnotatedCommand(three.withName("auto three"), null, null)); @@ -120,25 +121,25 @@ public Autons( .withName("auto standard"), null, new Pose2d(.48, 0.53, Rotation2d.kZero))); - MoveAndHold standard4 = new DriveWithTrajectoryFunction( + MoveAndHold standard4 = new DriveWithTrajectoryFunction( m_log, drive, m_controller, m_viz, this::standard4); m_autonChooser.add("standard4", - new AnnotatedCommand( - standard4.until(standard4::isDone) - .andThen( - new Shoot(m_shooter, m_indexer, 8).withTimeout(2)) - .withName("auto standard"), - null, new Pose2d(.48, 0.53, Rotation2d.kZero))); - - MoveAndHold standard5 = new DriveWithTrajectoryFunction( + new AnnotatedCommand( + standard4.until(standard4::isDone) + .andThen( + new Shoot(m_shooter, m_indexer, 8).withTimeout(2)) + .withName("auto standard"), + null, new Pose2d(.48, 0.53, Rotation2d.kZero))); + + MoveAndHold standard5 = new DriveWithTrajectoryFunction( m_log, drive, m_controller, m_viz, this::standard5); m_autonChooser.add("standard5", - new AnnotatedCommand( - standard5.until(standard5::isDone) - .andThen( - new Shoot(m_shooter, m_indexer, 8).withTimeout(2)) - .withName("auto standard"), - null, new Pose2d(.48, 0.53, Rotation2d.kZero))); + new AnnotatedCommand( + standard5.until(standard5::isDone) + .andThen( + new Shoot(m_shooter, m_indexer, 8).withTimeout(2)) + .withName("auto standard"), + null, new Pose2d(.48, 0.53, Rotation2d.kZero))); // MoveAndHold calib = new DriveWithTrajectoryFunction( @@ -170,43 +171,43 @@ public AnnotatedCommand get() { private Trajectory100 middle(Pose2d p) { Pose2d end = new Pose2d(p.getX() + 1, p.getY() + 0, p.getRotation()); return m_planner.restToRest(List.of( - HolonomicPose2d.make(p, 0), - HolonomicPose2d.make(end, Math.toRadians(0)))); + Pose2dWithDirection.make(p, 0), + Pose2dWithDirection.make(end, Math.toRadians(0)))); } private Trajectory100 standard2(Pose2d p) { Pose2d end = new Pose2d(p.getX() + 1.4, p.getY() + 0, p.getRotation().plus(Rotation2d.fromDegrees(5))); return m_planner.restToRest(List.of( - HolonomicPose2d.make(p, 0), - HolonomicPose2d.make(end, Math.toRadians(0)))); + Pose2dWithDirection.make(p, 0), + Pose2dWithDirection.make(end, Math.toRadians(0)))); } private Trajectory100 standard3(Pose2d p) { Pose2d end = new Pose2d(p.getX() + 1.5, p.getY() + 0, p.getRotation().plus(Rotation2d.fromDegrees(-30))); return m_planner.restToRest(List.of( - HolonomicPose2d.make(p, 0), - HolonomicPose2d.make(end, Math.toRadians(0)))); + Pose2dWithDirection.make(p, 0), + Pose2dWithDirection.make(end, Math.toRadians(0)))); } private Trajectory100 standard4(Pose2d p) { Pose2d end = new Pose2d(p.getX() + 1.5, p.getY() + 0, p.getRotation().plus(Rotation2d.fromDegrees(-5))); return m_planner.restToRest(List.of( - HolonomicPose2d.make(p, 0), - HolonomicPose2d.make(end, Math.toRadians(0)))); + Pose2dWithDirection.make(p, 0), + Pose2dWithDirection.make(end, Math.toRadians(0)))); } private Trajectory100 standard5(Pose2d p) { Pose2d end = new Pose2d(p.getX() + 1.5, p.getY() + 0, p.getRotation().plus(Rotation2d.fromDegrees(30))); return m_planner.restToRest(List.of( - HolonomicPose2d.make(p, 0), - HolonomicPose2d.make(end, Math.toRadians(0)))); + Pose2dWithDirection.make(p, 0), + Pose2dWithDirection.make(end, Math.toRadians(0)))); } private Trajectory100 calib(Pose2d p) { Pose2d end = new Pose2d(p.getX(), p.getY() + 1, p.getRotation()); return m_planner.restToRest(List.of( - HolonomicPose2d.make(p, 0), - HolonomicPose2d.make(end, Math.PI / 2))); + Pose2dWithDirection.make(p, 0), + Pose2dWithDirection.make(end, Math.PI / 2))); } /* * private Trajectory100 swerve(Pose2d p) { @@ -225,8 +226,8 @@ private Command redRight() { m_controller, m_viz, (p) -> m_planner.restToRest(List.of( - HolonomicPose2d.make(MechanicalMayhem2025.START_RED_RIGHT, 0), - HolonomicPose2d.make(MechanicalMayhem2025.START_RED_RIGHT + Pose2dWithDirection.make(MechanicalMayhem2025.START_RED_RIGHT, 0), + Pose2dWithDirection.make(MechanicalMayhem2025.START_RED_RIGHT .plus(new Transform2d(1, 3, Rotation2d.kCCW_90deg)), 0)))); return cmd.until(cmd::isDone).withName("red right"); } From 1ab76f3745f66e827a8956aaf326c62b75b61a19 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 15 Dec 2025 12:29:44 -0800 Subject: [PATCH 04/42] fix up 2d trajectory plotting --- .../CalgamesArm/TrajectoryPlotter.java | 34 +++++++- .../frc2025/CalgamesArm/TrajectoryTest.java | 86 +++---------------- .../CalgamesArm/TrajectoryToVectorSeries.java | 18 ---- .../AnalyticLynxArmKinematicsTest.java | 9 +- 4 files changed, 48 insertions(+), 99 deletions(-) diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java index 8b9d38fff..237396f57 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java @@ -1,6 +1,9 @@ package org.team100.frc2025.CalgamesArm; import java.awt.Dimension; +import java.util.function.ToDoubleFunction; + +import javax.swing.WindowConstants; import org.jfree.chart.ChartFrame; import org.jfree.chart.JFreeChart; @@ -8,8 +11,10 @@ import org.jfree.chart.axis.NumberTickUnit; import org.jfree.chart.plot.XYPlot; import org.jfree.chart.renderer.xy.VectorRenderer; +import org.jfree.data.Range; import org.jfree.data.xy.VectorSeriesCollection; import org.team100.lib.trajectory.Trajectory100; +import org.team100.lib.trajectory.timing.TimedPose; public class TrajectoryPlotter { private final TrajectoryToVectorSeries converter; @@ -20,7 +25,7 @@ public TrajectoryPlotter(double arrowLength) { public void plot(Trajectory100 t, String name) { VectorSeriesCollection dataSet = new VectorSeriesCollection(); - dataSet.addSeries(converter.convertRotated(t)); + dataSet.addSeries(converter.convert(t)); XYPlot plot = new XYPlot( dataSet, new NumberAxis("X"), @@ -28,9 +33,8 @@ public void plot(Trajectory100 t, String name) { new VectorRenderer()); NumberAxis domain = (NumberAxis) plot.getDomainAxis(); NumberAxis range = (NumberAxis) plot.getRangeAxis(); - // make the x and y scales the same - domain.setRange(-1, 1); - range.setRange(0, 2); + domain.setRangeWithMargins(xRange(t)); + range.setRangeWithMargins(yRange(t)); domain.setTickUnit(new NumberTickUnit(1)); range.setTickUnit(new NumberTickUnit(1)); @@ -38,9 +42,31 @@ public void plot(Trajectory100 t, String name) { frame.setPreferredSize(new Dimension(500, 500)); frame.pack(); frame.setVisible(true); + frame.setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE); try { Thread.sleep(50000); } catch (InterruptedException e) { } } + + Range xRange(Trajectory100 t) { + return range(t, (p) -> p.state().getPose().pose().getX()); + } + + Range yRange(Trajectory100 t) { + return range(t, (p) -> p.state().getPose().pose().getY()); + } + + Range range(Trajectory100 t, ToDoubleFunction f) { + double max = Double.NEGATIVE_INFINITY; + double min = Double.POSITIVE_INFINITY; + for (TimedPose p : t.getPoints()) { + double x = f.applyAsDouble(p); + if (x > max) + max = x; + if (x < min) + min = x; + } + return new Range(min, max); + } } diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java index 0365c8aee..cd08f6c1b 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java @@ -2,6 +2,7 @@ import java.util.List; +import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.logging.LoggerFactory; @@ -21,6 +22,7 @@ * These pop up GUI windows, so leave them commented out when you check in. */ public class TrajectoryTest { + private static final boolean SHOW = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); /** @@ -29,7 +31,7 @@ public class TrajectoryTest { * TrajectoryPlanner.restToRest() has several overloads: the one that takes * two non-holonomic poses draws a straight line between them. */ - // @Test + @Test void testSimple() throws InterruptedException { List c = List.of( new ConstantConstraint(log, 1, 0.1), @@ -38,8 +40,8 @@ void testSimple() throws InterruptedException { Trajectory100 t = p.restToRest( new Pose2d(0, 0, new Rotation2d()), new Pose2d(10, 1, new Rotation2d())); - new TrajectoryPlotter(0.1).plot(t, "simple"); - Thread.sleep(5000); + if (SHOW) + new TrajectoryPlotter(0.1).plot(t, "simple"); } /** @@ -51,7 +53,7 @@ void testSimple() throws InterruptedException { * In this case, is facing +x, and moving +x, and it ends up moving +y but * facing the other way (i.e. backwards) */ - // @Test + @Test void testCurved() throws InterruptedException { List c = List.of( new ConstantConstraint(log, 2, 0.5), @@ -69,8 +71,8 @@ void testCurved() throws InterruptedException { new Rotation2d(-Math.PI / 2)), DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); - new TrajectoryPlotter(0.1).plot(t, "simple"); - Thread.sleep(5000); + if (SHOW) + new TrajectoryPlotter(0.1).plot(t, "simple"); } /** @@ -78,7 +80,7 @@ void testCurved() throws InterruptedException { * specifying many such points will make the curve harder to calculate and * harder to make smooth. */ - // @Test + @Test void testMultipleWaypoints() throws InterruptedException { List c = List.of( new ConstantConstraint(log, 2, 0.5), @@ -101,73 +103,7 @@ void testMultipleWaypoints() throws InterruptedException { new Rotation2d(-Math.PI / 2)), DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); - new TrajectoryPlotter(0.1).plot(t, "simple"); - Thread.sleep(5000); + if (SHOW) + new TrajectoryPlotter(0.1).plot(t, "simple"); } - - /** Example of using a trajectory library for 2025 scoring paths */ - // @Test - void testPickupToPlace() { - List c = List.of( - new ConstantConstraint(log, 2, 0.5), - new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( - // pickup - new Pose2dWithDirection( - new Pose2d( - new Translation2d(1, 0.1), - new Rotation2d(-Math.PI)), - DirectionSE2.TO_Y), - // place for gateway point? - new Pose2dWithDirection( - new Pose2d( - new Translation2d(3, 7), - new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X), - new Pose2dWithDirection( - new Pose2d( - new Translation2d(6, 9), - new Rotation2d(-((7 * Math.PI) / 36))), - DirectionSE2.TO_Y)); - @SuppressWarnings("unused") - Trajectory100 t = p.restToRest(waypoints); - // TrajectoryPlotter.plot(t, "simple"); - } - - // @Test - void testSingularityDemo() { - List c = List.of( - new ConstantConstraint(log, 2, 0.5), - new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( - // pickup - new Pose2dWithDirection( - new Pose2d( - new Translation2d(1, 0.1), - new Rotation2d(-Math.PI)), - DirectionSE2.TO_Y), - // place for gateway point - new Pose2dWithDirection( - new Pose2d( - new Translation2d(0.75, 3), - new Rotation2d(-Math.PI)), - DirectionSE2.TO_Y), - // place for gateway point - new Pose2dWithDirection( - new Pose2d( - new Translation2d(3, 7), - new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X), - new Pose2dWithDirection( - new Pose2d( - new Translation2d(6, 9), - new Rotation2d(-((7 * Math.PI) / 36))), - DirectionSE2.TO_Y)); - @SuppressWarnings("unused") - Trajectory100 t = p.restToRest(waypoints); - // TrajectoryPlotter.plot(t, "simple"); - } - } diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java index 1035fab3a..798201fbe 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java @@ -34,22 +34,4 @@ public VectorSeries convert(Trajectory100 t) { } return s; } - - /** Maps x to y, y to -x */ - public VectorSeries convertRotated(Trajectory100 t) { - VectorSeries s = new VectorSeries("trajectory"); - double duration = t.duration(); - for (double time = 0; time < duration; time += DT) { - TimedPose p = t.sample(time); - Pose2dWithDirection pp = p.state().getPose(); - double y = pp.translation().getX(); - double x = -pp.translation().getY(); - Rotation2d heading = pp.heading(); - double dy = m_scale * heading.getCos(); - double dx = -m_scale * heading.getSin(); - s.add(x, y, dx, dy); - } - return s; - } - } diff --git a/lib/src/test/java/org/team100/lib/subsystems/lynxmotion_arm/AnalyticLynxArmKinematicsTest.java b/lib/src/test/java/org/team100/lib/subsystems/lynxmotion_arm/AnalyticLynxArmKinematicsTest.java index 97c33c7ef..aedab0ecc 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/lynxmotion_arm/AnalyticLynxArmKinematicsTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/lynxmotion_arm/AnalyticLynxArmKinematicsTest.java @@ -117,7 +117,7 @@ void testWrist2() { } - // @Test + @Test void testBadRotation() { // stretched out along x but with the wrong end rotation AnalyticLynxArmKinematics k = AnalyticLynxArmKinematics.unit(); @@ -129,7 +129,12 @@ void testBadRotation() { new Pose3d(), new Pose3d(new Translation3d(2, 0, 1), new Rotation3d(0, 0, 1))); LynxArmConfig q = new LynxArmConfig(0, 0, 0, 0, 0); - assertThrows(IllegalArgumentException.class, () -> k.inverse(q, t.p6())); + LynxArmConfig foo = k.inverse(q, t.p6()); + assertEquals(0, foo.swing().getAsDouble(), DELTA); + assertEquals(-0.288, foo.boom(), DELTA); + assertEquals(0.576, foo.stick(), DELTA); + assertEquals(-0.288, foo.wrist(), DELTA); + assertEquals(0, foo.twist().getAsDouble(), DELTA); } @Test From a4ef525c6783ff63d4b50c0c6b4992816531a022 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 15 Dec 2025 15:42:27 -0800 Subject: [PATCH 05/42] remove spline optimization it's hard to understand when it breaks, and it doesn't help very much at all. --- .../CalgamesArm/TrajectoryPlotter.java | 72 ---- lib/build.gradle | 2 + .../lib/trajectory/path/PathFactory.java | 49 +-- .../trajectory/path/spline/SplineUtil.java | 408 ------------------ .../lib/trajectory/SplineToVectorSeries.java | 38 ++ .../lib/trajectory/TrajectoryPlotter.java | 153 +++++++ .../lib/trajectory}/TrajectoryTest.java | 10 +- .../trajectory}/TrajectoryToVectorSeries.java | 12 +- .../lib/trajectory/path/PathFactoryTest.java | 55 --- .../path/spline/HolonomicSplineTest.java | 262 +++++++---- .../spline/QuinticHermiteOptimizerTest.java | 73 ++-- 11 files changed, 422 insertions(+), 712 deletions(-) delete mode 100644 comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineUtil.java create mode 100644 lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java create mode 100644 lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java rename {comp/src/test/java/org/team100/frc2025/CalgamesArm => lib/src/test/java/org/team100/lib/trajectory}/TrajectoryTest.java (92%) rename {comp/src/test/java/org/team100/frc2025/CalgamesArm => lib/src/test/java/org/team100/lib/trajectory}/TrajectoryToVectorSeries.java (72%) diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java deleted file mode 100644 index 237396f57..000000000 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java +++ /dev/null @@ -1,72 +0,0 @@ -package org.team100.frc2025.CalgamesArm; - -import java.awt.Dimension; -import java.util.function.ToDoubleFunction; - -import javax.swing.WindowConstants; - -import org.jfree.chart.ChartFrame; -import org.jfree.chart.JFreeChart; -import org.jfree.chart.axis.NumberAxis; -import org.jfree.chart.axis.NumberTickUnit; -import org.jfree.chart.plot.XYPlot; -import org.jfree.chart.renderer.xy.VectorRenderer; -import org.jfree.data.Range; -import org.jfree.data.xy.VectorSeriesCollection; -import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.trajectory.timing.TimedPose; - -public class TrajectoryPlotter { - private final TrajectoryToVectorSeries converter; - - public TrajectoryPlotter(double arrowLength) { - converter = new TrajectoryToVectorSeries(arrowLength); - } - - public void plot(Trajectory100 t, String name) { - VectorSeriesCollection dataSet = new VectorSeriesCollection(); - dataSet.addSeries(converter.convert(t)); - XYPlot plot = new XYPlot( - dataSet, - new NumberAxis("X"), - new NumberAxis("Y"), - new VectorRenderer()); - NumberAxis domain = (NumberAxis) plot.getDomainAxis(); - NumberAxis range = (NumberAxis) plot.getRangeAxis(); - domain.setRangeWithMargins(xRange(t)); - range.setRangeWithMargins(yRange(t)); - domain.setTickUnit(new NumberTickUnit(1)); - range.setTickUnit(new NumberTickUnit(1)); - - ChartFrame frame = new ChartFrame(name, new JFreeChart(plot)); - frame.setPreferredSize(new Dimension(500, 500)); - frame.pack(); - frame.setVisible(true); - frame.setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE); - try { - Thread.sleep(50000); - } catch (InterruptedException e) { - } - } - - Range xRange(Trajectory100 t) { - return range(t, (p) -> p.state().getPose().pose().getX()); - } - - Range yRange(Trajectory100 t) { - return range(t, (p) -> p.state().getPose().pose().getY()); - } - - Range range(Trajectory100 t, ToDoubleFunction f) { - double max = Double.NEGATIVE_INFINITY; - double min = Double.POSITIVE_INFINITY; - for (TimedPose p : t.getPoints()) { - double x = f.applyAsDouble(p); - if (x > max) - max = x; - if (x < min) - min = x; - } - return new Range(min, max); - } -} diff --git a/lib/build.gradle b/lib/build.gradle index 3a947b78c..c794ac243 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -72,6 +72,8 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + testImplementation "org.jfree:jfreechart:1.5.3" } test { diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index b98ea7422..a76faea5c 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -3,13 +3,10 @@ import java.util.ArrayList; import java.util.List; -import org.team100.lib.geometry.DirectionR2; -import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.path.spline.HolonomicSpline; -import org.team100.lib.trajectory.path.spline.SplineUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,19 +19,16 @@ public static Path100 pathFromWaypoints( double maxDx, double maxDy, double maxDTheta, - final List mN) { + List magicNumbers) { List splines = new ArrayList<>(waypoints.size() - 1); for (int i = 1; i < waypoints.size(); ++i) { splines.add( new HolonomicSpline( waypoints.get(i - 1), waypoints.get(i), - mN.get(i - 1), - mN.get(i))); + magicNumbers.get(i - 1), + magicNumbers.get(i))); } - // does not force C1, theta responds too much - // SplineUtil.forceC1(splines); - SplineUtil.optimizeSpline(splines); return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); } @@ -45,43 +39,10 @@ public static Path100 pathFromWaypoints( double maxDTheta) { List splines = new ArrayList<>(waypoints.size() - 1); for (int i = 1; i < waypoints.size(); ++i) { - splines.add(new HolonomicSpline(waypoints.get(i - 1), waypoints.get(i))); - } - // does not force C1, theta responds too much - // SplineUtil.forceC1(splines); - SplineUtil.optimizeSpline(splines); - return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); - } - - /** - * Make a spline from points without any control points -- the spline will go - * through the points, computing appropriate (maybe) control points to do so. - */ - public static Path100 withoutControlPoints( - final List waypoints, - double maxDx, - double maxDy, - double maxDTheta) { - List splines = new ArrayList<>(waypoints.size() - 1); - // first make a series of straight lines, with corners at the waypoints - for (int i = 1; i < waypoints.size(); ++i) { - Pose2d pose0 = waypoints.get(i - 1); - Pose2d pose1 = waypoints.get(i); - Rotation2d course = pose1.getTranslation().minus(pose0.getTranslation()).getAngle(); splines.add(new HolonomicSpline( - new Pose2dWithDirection( - pose0, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(course), 0)), - new Pose2dWithDirection( - pose1, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(course), 0)))); + waypoints.get(i - 1), + waypoints.get(i))); } - // then adjust the control points to make it C1 smooth - SplineUtil.forceC1(splines); - // then try to make it C2 smooth - SplineUtil.optimizeSpline(splines); return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineUtil.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineUtil.java deleted file mode 100644 index 8ad44d86e..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineUtil.java +++ /dev/null @@ -1,408 +0,0 @@ -package org.team100.lib.trajectory.path.spline; - -import java.util.List; -import java.util.Optional; - -import org.team100.lib.geometry.GeometryUtil; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** Static utility methods for splines. */ -public class SplineUtil { - private static final boolean DEBUG = false; - private static final double EPSILON = 1e-5; - private static final double STEP_SIZE = 1.0; - private static final double MIN_DELTA = 0.001; - private static final int MAX_ITERATIONS = 100; - - /** - * Makes optimization code a little more readable - */ - static class ControlPoint { - double ddx; - double ddy; - } - - /** - * True if adjacent spline endpoints have (nearly) identical derivative terms. - */ - public static boolean verifyC1(List splines) { - if (splines.size() < 2) - return true; - for (int i = 0; i < splines.size() - 1; ++i) { - HolonomicSpline s0 = splines.get(i); - HolonomicSpline s1 = splines.get(i + 1); - if (!MathUtil.isNear(s0.dx(1), s1.dx(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad x C1 %f %f\n", s0.dx(1), s1.dx(0)); - } - return false; - } - if (!MathUtil.isNear(s0.dy(1), s1.dy(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad y C1 %f %f\n", s0.dy(1), s1.dy(0)); - } - return false; - } - if (!MathUtil.isNear(s0.dtheta(1), s1.dtheta(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad theta C1 %f %f\n", s0.dtheta(1), s1.dtheta(0)); - } - return false; - } - } - return true; - } - - /** - * True if adjacent spline endpoints have (nearly) identical second-derivative - * terms. - */ - public static boolean verifyC2(List splines) { - if (splines.size() < 2) - return true; - for (int i = 0; i < splines.size() - 1; ++i) { - HolonomicSpline s0 = splines.get(i); - HolonomicSpline s1 = splines.get(i + 1); - if (!MathUtil.isNear(s0.ddx(1), s1.ddx(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad x C2 %f %f\n", s0.ddx(1), s1.ddx(0)); - } - return false; - } - if (!MathUtil.isNear(s0.ddy(1), s1.ddy(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad y C2 %f %f\n", s0.ddy(1), s1.ddy(0)); - } - return false; - } - if (!MathUtil.isNear(s0.ddtheta(1), s1.ddtheta(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad theta C2 %f %f\n", s0.ddtheta(1), s1.ddtheta(0)); - } - return false; - } - } - return true; - } - - /** - * True if adjacent spline endpoints have (nearly) identical third-derivative - * terms. - */ - public static boolean verifyC3(List splines) { - if (splines.size() < 2) - return true; - for (int i = 0; i < splines.size() - 1; ++i) { - HolonomicSpline s0 = splines.get(i); - HolonomicSpline s1 = splines.get(i + 1); - if (!MathUtil.isNear(s0.dddx(1), s1.dddx(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad x C3 %f %f\n", s0.dddx(1), s1.dddx(0)); - } - return false; - } - if (!MathUtil.isNear(s0.dddy(1), s1.dddy(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad y C3 %f %f\n", s0.dddy(1), s1.dddy(0)); - } - return false; - } - if (!MathUtil.isNear(s0.dddtheta(1), s1.dddtheta(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad theta C3 %f %f\n", s0.dddtheta(1), s1.dddtheta(0)); - } - return false; - } - } - return true; - } - - /** - * Force derivatives to be the same at joints, by choosing the average. - * - * Note that it is easy to create large theta excursions with this method. - */ - public static void forceC1(List splines) { - if (splines.size() < 2) - return; - for (int i = 0; i < splines.size() - 1; ++i) { - HolonomicSpline s0 = splines.get(i); - HolonomicSpline s1 = splines.get(i + 1); - - // the derivatives need to be scaled by the distance of each spline - // since they are derivatives with respect to the parameter, - // not with respect to distance. - double d0 = Math.hypot(s0.x(1) - s0.x(0), s0.y(1) - s0.y(0)); - double d1 = Math.hypot(s1.x(1) - s1.x(0), s1.y(1) - s1.y(0)); - if (DEBUG) { - System.out.printf("d0 %f d1 %f\n", d0, d1); - } - // // note, sometimes these can be zero - if (d0 < 1e-3 || d1 < 1e-3) - return; - - double s0dx1 = s0.dx(1); - if (DEBUG) { - System.out.printf("s0dx1 %f\n", s0dx1); - } - double s1dx0 = s1.dx(0); - if (DEBUG) { - System.out.printf("s1dx0 %f\n", s1dx0); - } - double meanDx = 0.5 * (s0dx1 / d0 + s1dx0 / d1); - if (DEBUG) { - System.out.printf("mean dx %f\n", meanDx); - } - double s0dy1 = s0.dy(1); - double s1dy0 = s1.dy(0); - double meanDy = 0.5 * (s0dy1 / d0 + s1dy0 / d1); - double s0dtheta1 = s0.dtheta(1); - double s1dtheta0 = s1.dtheta(0); - double meanDtheta = 0.5 * (s0dtheta1 / d0 + s1dtheta0 / d1); - splines.set( - i, - s0.replaceFirstDerivatives( - s0.dx(0), meanDx * d0, - s0.dy(0), meanDy * d0, - s0.dtheta(0), meanDtheta * d0)); - splines.set( - i + 1, - s1.replaceFirstDerivatives( - meanDx * d1, s1.dx(1), - meanDy * d1, s1.dy(1), - meanDtheta * d1, s1.dtheta(1))); - - } - } - - /** - * Finds the optimal second derivative values for a set of splines to reduce the - * sum of the change in curvature squared over the path - * - * @param splines the list of splines to optimize - * @return the final sumDCurvature2 - */ - public static double optimizeSpline(List splines) { - // can't optimize anything with less than 2 splines - if (splines.size() <= 1) { - // we don't care about measuring the curvature in this case. - return 0; - } - int count = 0; - double prev = sumDCurvature2(splines); - while (count < MAX_ITERATIONS) { - runOptimizationIteration(splines); - double current = sumDCurvature2(splines); - if (prev - current < MIN_DELTA) - return current; - prev = current; - count++; - } - System.out.println("WARNING: Spline optimization failed"); - return prev; - } - - /** - * @return integral of dCurvature^2 over the length of multiple splines - */ - static double sumDCurvature2(List splines) { - double sum = 0; - for (HolonomicSpline s : splines) { - sum += s.sumDCurvature2(); - } - if (Double.isNaN(sum)) - throw new IllegalArgumentException(); - return sum; - } - - /** - * Runs a single optimization iteration, using Newton's method. - */ - static void runOptimizationIteration(List splines) { - // can't optimize anything with less than 2 splines - if (splines.size() <= 1) { - return; - } - - ControlPoint[] controlPoints = new ControlPoint[splines.size() - 1]; - - double magnitude = getControlPoints(splines, controlPoints); - - magnitude = Math.sqrt(magnitude); - if (Double.isNaN(magnitude)) - throw new IllegalArgumentException(); - - // minimize along the direction of the gradient - // first calculate 3 points along the direction of the gradient - - // middle point is at the current location - Translation2d p2 = new Translation2d(0, sumDCurvature2(splines)); - - // first point is offset from the middle location by -stepSize - for (int i = 0; i < splines.size() - 1; ++i) { - backwards(splines, controlPoints, magnitude, i); - } - - // last point is offset from the middle location by +stepSize - Translation2d p1 = new Translation2d(-STEP_SIZE, sumDCurvature2(splines)); - for (int i = 0; i < splines.size() - 1; ++i) { - forwards(splines, controlPoints, i); - } - - Translation2d p3 = new Translation2d(STEP_SIZE, sumDCurvature2(splines)); - // approximate step size to minimize sumDCurvature2 along the gradient - double stepSize = fitParabola(p1, p2, p3); - - for (int i = 0; i < splines.size() - 1; ++i) { - finish(splines, controlPoints, stepSize, i); - } - } - - static void finish( - List splines, - ControlPoint[] controlPoints, - double stepSize, - int i) { - Optional startPose = splines.get(i).getStartPose(); - Optional startPose2 = splines.get(i + 1).getStartPose(); - Optional endPose = splines.get(i).getEndPose(); - Optional endPose2 = splines.get(i + 1).getEndPose(); - if (startPose.isEmpty() || startPose2.isEmpty() || endPose.isEmpty() || endPose2.isEmpty()) { - throw new IllegalArgumentException(); - } - if (GeometryUtil.isColinear(startPose.get(), startPose2.get()) - || GeometryUtil.isColinear(endPose.get(), endPose2.get())) { - return; - } - - // why would this happen? - if (controlPoints[i] == null) - return; - - // move by the step size calculated by the parabola fit (+1 to offset for the - // final transformation to find p3) - controlPoints[i].ddx *= 1 + stepSize / STEP_SIZE; - controlPoints[i].ddy *= 1 + stepSize / STEP_SIZE; - - splines.set(i, splines.get(i).addToSecondDerivatives(0, controlPoints[i].ddx, 0, controlPoints[i].ddy)); - splines.set(i + 1, - splines.get(i + 1).addToSecondDerivatives(controlPoints[i].ddx, 0, controlPoints[i].ddy, 0)); - } - - static void forwards(List splines, ControlPoint[] controlPoints, int i) { - Optional startPose = splines.get(i).getStartPose(); - Optional startPose2 = splines.get(i + 1).getStartPose(); - Optional endPose = splines.get(i).getEndPose(); - Optional endPose2 = splines.get(i + 1).getEndPose(); - if (startPose.isEmpty() || startPose2.isEmpty() || endPose.isEmpty() || endPose2.isEmpty()) { - throw new IllegalArgumentException(); - } - if (GeometryUtil.isColinear(startPose.get(), startPose2.get()) - || GeometryUtil.isColinear(endPose.get(), endPose2.get())) { - return; - } - - // why would this happen? - if (controlPoints[i] == null) - return; - - // move along the gradient by 2 times the step size amount (to return to - // original location and move by 1 step) - splines.set(i, - splines.get(i).addToSecondDerivatives(0, 2 * controlPoints[i].ddx, 0, 2 * controlPoints[i].ddy)); - splines.set(i + 1, splines.get(i + 1).addToSecondDerivatives(2 * controlPoints[i].ddx, 0, - 2 * controlPoints[i].ddy, 0)); - } - - static void backwards( - List splines, - ControlPoint[] controlPoints, - double magnitude, - int i) { - Optional startPose = splines.get(i).getStartPose(); - Optional startPose2 = splines.get(i + 1).getStartPose(); - Optional endPose = splines.get(i).getEndPose(); - Optional endPose2 = splines.get(i + 1).getEndPose(); - if (startPose.isEmpty() || startPose2.isEmpty() || endPose.isEmpty() || endPose2.isEmpty()) { - throw new IllegalArgumentException(); - } - if (GeometryUtil.isColinear(startPose.get(), startPose2.get()) - || GeometryUtil.isColinear(endPose.get(), endPose2.get())) { - return; - } - - // why would this happen? - if (controlPoints[i] == null) - return; - - // normalize to step size - controlPoints[i].ddx *= STEP_SIZE / magnitude; - controlPoints[i].ddy *= STEP_SIZE / magnitude; - - // move opposite the gradient by step size amount - splines.set(i, splines.get(i).addToSecondDerivatives(0, -controlPoints[i].ddx, 0, -controlPoints[i].ddy)); - splines.set(i + 1, - splines.get(i + 1).addToSecondDerivatives(-controlPoints[i].ddx, 0, -controlPoints[i].ddy, 0)); - } - - /** - * Extract the control points from the list of splines. - * - * @param splines input splines - * @param controlPoints output control points - * @return sum of ddx^2+ddy^2 - */ - static double getControlPoints( - List splines, - ControlPoint[] controlPoints) { - double magnitude = 0; - for (int i = 0; i < splines.size() - 1; ++i) { - // don't try to optimize colinear points - Optional startPose = splines.get(i).getStartPose(); - Optional startPose2 = splines.get(i + 1).getStartPose(); - Optional endPose = splines.get(i).getEndPose(); - Optional endPose2 = splines.get(i + 1).getEndPose(); - if (startPose.isEmpty() || startPose2.isEmpty() || endPose.isEmpty() || endPose2.isEmpty()) { - throw new IllegalArgumentException(); - } - if (GeometryUtil.isColinear(startPose.get(), startPose2.get()) - || GeometryUtil.isColinear(endPose.get(), endPose2.get())) { - continue; - } - double original = sumDCurvature2(splines); - - // holds the gradient at a control point - controlPoints[i] = new ControlPoint(); - - // calculate partial derivatives of sumDCurvature2 - splines.set(i, splines.get(i).addToSecondDerivatives(0, EPSILON, 0, 0)); - splines.set(i + 1, splines.get(i + 1).addToSecondDerivatives(EPSILON, 0, 0, 0)); - controlPoints[i].ddx = (sumDCurvature2(splines) - original) / EPSILON; - - splines.set(i, splines.get(i).addToSecondDerivatives(0, 0, 0, EPSILON)); - splines.set(i + 1, splines.get(i + 1).addToSecondDerivatives(0, 0, EPSILON, 0)); - controlPoints[i].ddy = (sumDCurvature2(splines) - original) / EPSILON; - - magnitude += controlPoints[i].ddx * controlPoints[i].ddx + controlPoints[i].ddy * controlPoints[i].ddy; - } - return magnitude; - } - - /** - * fits a parabola to 3 points - * - * @return the x coordinate of the vertex of the parabola - */ - static double fitParabola(Translation2d p1, Translation2d p2, Translation2d p3) { - double A = (p3.getX() * (p2.getY() - p1.getY()) + p2.getX() * (p1.getY() - p3.getY()) - + p1.getX() * (p3.getY() - p2.getY())); - double B = (p3.getX() * p3.getX() * (p1.getY() - p2.getY()) + p2.getX() * p2.getX() * (p3.getY() - p1.getY()) - + p1.getX() * p1.getX() * - (p2.getY() - p3.getY())); - return -B / (2 * A); - } - -} diff --git a/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java new file mode 100644 index 000000000..6781db554 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java @@ -0,0 +1,38 @@ +package org.team100.lib.trajectory; + +import java.util.List; + +import org.jfree.data.xy.VectorSeries; +import org.team100.lib.trajectory.path.spline.HolonomicSpline; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class SplineToVectorSeries { + + /** Length of the vector indicating heading */ + private final double m_scale; + + public SplineToVectorSeries(double scale) { + m_scale = scale; + } + + /** Maps x to x, y to y */ + public VectorSeries convert(String name, List splines) { + VectorSeries s = new VectorSeries(name); + for (HolonomicSpline spline : splines) { + for (double j = 0; j < 0.99; j += 0.1) { + Pose2d p = spline.getPose2d(j); + double x = p.getX(); + double y = p.getY(); + Rotation2d heading = p.getRotation(); + double dx = m_scale * heading.getCos(); + double dy = m_scale * heading.getSin(); + s.add(x, y, dx, dy); + } + + } + return s; + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java new file mode 100644 index 000000000..924ca078e --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -0,0 +1,153 @@ +package org.team100.lib.trajectory; + +import java.awt.Dimension; +import java.awt.Frame; +import java.util.List; +import java.util.function.ToDoubleFunction; + +import javax.swing.JDialog; +import javax.swing.WindowConstants; + +import org.jfree.chart.ChartPanel; +import org.jfree.chart.JFreeChart; +import org.jfree.chart.axis.NumberAxis; +import org.jfree.chart.axis.NumberTickUnit; +import org.jfree.chart.plot.XYPlot; +import org.jfree.chart.renderer.xy.VectorRenderer; +import org.jfree.data.Range; +import org.jfree.data.xy.VectorSeries; +import org.jfree.data.xy.VectorSeriesCollection; +import org.team100.lib.trajectory.path.spline.HolonomicSpline; +import org.team100.lib.trajectory.timing.TimedPose; + +import edu.wpi.first.math.geometry.Pose2d; + +public class TrajectoryPlotter { + public static final boolean SHOW = false; + + private final TrajectoryToVectorSeries converter; + private final SplineToVectorSeries splineConverter; + + public TrajectoryPlotter(double arrowLength) { + converter = new TrajectoryToVectorSeries(arrowLength); + splineConverter = new SplineToVectorSeries(arrowLength); + } + + public void plot(String name, Trajectory100 t) { + VectorSeries converted = convert(name, t); + Range xrange = xRange(t); + Range yrange = yRange(t); + if (SHOW) + actuallyPlot(name, xrange, yrange, 1, converted); + } + + public VectorSeries convert(String name, Trajectory100 t) { + return converter.convert(name, t); + } + + public void plot(String name, List s) { + VectorSeries converted = convert(name, s); + Range xrange = xRange(s); + Range yrange = yRange(s); + if (SHOW) + actuallyPlot(name, xrange, yrange, 1, converted); + } + + public VectorSeries convert(String name, List s) { + return splineConverter.convert(name, s); + } + + public void actuallyPlot( + String name, + Range xrange, + Range yrange, + double tick, + VectorSeries... converted) { + VectorSeriesCollection dataSet = new VectorSeriesCollection(); + for (VectorSeries c : converted) { + dataSet.addSeries(c); + } + XYPlot plot = new XYPlot( + dataSet, + new NumberAxis("X"), + new NumberAxis("Y"), + new VectorRenderer()); + NumberAxis domain = (NumberAxis) plot.getDomainAxis(); + NumberAxis range = (NumberAxis) plot.getRangeAxis(); + domain.setRangeWithMargins(xrange); + range.setRangeWithMargins(yrange); + domain.setTickUnit(new NumberTickUnit(tick)); + range.setTickUnit(new NumberTickUnit(tick)); + + ChartPanel panel = new ChartPanel(new JFreeChart(plot)); + // "true" means "modal" so wait for close. + JDialog frame = new JDialog((Frame) null, "plot", true); + frame.setContentPane(panel); + + frame.setPreferredSize(new Dimension(500, 500)); + frame.pack(); + frame.setVisible(true); + frame.setDefaultCloseOperation(WindowConstants.DISPOSE_ON_CLOSE); + } + + public Range xRange(Trajectory100 t) { + return range(t, (p) -> p.state().getPose().pose().getX()); + } + + public Range yRange(Trajectory100 t) { + return range(t, (p) -> p.state().getPose().pose().getY()); + } + + public Range xRange(List s) { + return range(s, (p) -> p.getX()); + } + + public Range yRange(List s) { + return range(s, (p) -> p.getY()); + } + + /** Adds margin */ + Range range(Trajectory100 t, ToDoubleFunction f) { + double max = Double.NEGATIVE_INFINITY; + double min = Double.POSITIVE_INFINITY; + for (TimedPose p : t.getPoints()) { + double x = f.applyAsDouble(p); + if (x + 0.1 > max) + max = x + 0.1; + if (x - 0.1 < min) + min = x - 0.1; + } + return new Range(min, max); + } + + /** Adds margin */ + Range range(List splines, ToDoubleFunction f) { + double max = Double.NEGATIVE_INFINITY; + double min = Double.POSITIVE_INFINITY; + + for (HolonomicSpline spline : splines) { + for (double j = 0; j < 0.99; j += 0.1) { + Pose2d p = spline.getPose2d(j); + double x = f.applyAsDouble(p); + if (x + 0.1 > max) + max = x + 0.1; + if (x - 0.1 < min) + min = x - 0.1; + } + } + return new Range(min, max); + } + + public static void plot( + List splines, + double arrows, + double ticks) { + TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); + VectorSeries series = plotter.convert("before", splines); + Range xrange = plotter.xRange(splines); + Range yrange = plotter.yRange(splines); + + if (SHOW) + plotter.actuallyPlot("compare", xrange, yrange, ticks, series); + } +} diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java similarity index 92% rename from comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java rename to lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index cd08f6c1b..20e083536 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -1,4 +1,4 @@ -package org.team100.frc2025.CalgamesArm; +package org.team100.lib.trajectory; import java.util.List; @@ -8,8 +8,6 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.ConstantConstraint; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -41,7 +39,7 @@ void testSimple() throws InterruptedException { new Pose2d(0, 0, new Rotation2d()), new Pose2d(10, 1, new Rotation2d())); if (SHOW) - new TrajectoryPlotter(0.1).plot(t, "simple"); + new TrajectoryPlotter(0.1).plot("simple", t ); } /** @@ -72,7 +70,7 @@ void testCurved() throws InterruptedException { DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) - new TrajectoryPlotter(0.1).plot(t, "simple"); + new TrajectoryPlotter(0.1).plot("curved", t); } /** @@ -104,6 +102,6 @@ void testMultipleWaypoints() throws InterruptedException { DirectionSE2.TO_Y)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) - new TrajectoryPlotter(0.1).plot(t, "simple"); + new TrajectoryPlotter(0.1).plot("multiple", t); } } diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java similarity index 72% rename from comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java rename to lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java index 798201fbe..8debe6270 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java @@ -1,15 +1,12 @@ -package org.team100.frc2025.CalgamesArm; +package org.team100.lib.trajectory; import org.jfree.data.xy.VectorSeries; import org.team100.lib.geometry.Pose2dWithDirection; -import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.timing.TimedPose; import edu.wpi.first.math.geometry.Rotation2d; public class TrajectoryToVectorSeries { - /** Time between samples */ - private static final double DT = 0.1; /** Length of the vector indicating heading */ private final double m_scale; @@ -19,10 +16,11 @@ public TrajectoryToVectorSeries(double scale) { } /** Maps x to x, y to y */ - public VectorSeries convert(Trajectory100 t) { - VectorSeries s = new VectorSeries("trajectory"); + public VectorSeries convert(String name, Trajectory100 t) { + VectorSeries s = new VectorSeries(name); double duration = t.duration(); - for (double time = 0; time < duration; time += DT) { + double dt = duration/20; + for (double time = 0; time < duration; time += dt) { TimedPose p = t.sample(time); Pose2dWithDirection pp = p.state().getPose(); double x = pp.translation().getX(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 9d6fb49fc..b5313fc08 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -11,17 +11,8 @@ import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.TestLoggerFactory; -import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; -import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.path.spline.HolonomicSpline; -import org.team100.lib.trajectory.timing.ScheduleGenerator; -import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; -import org.team100.lib.trajectory.timing.TimingConstraint; -import org.team100.lib.trajectory.timing.TimingConstraintFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -31,52 +22,6 @@ public class PathFactoryTest implements Timeless { private static final boolean DEBUG = false; private static final double DELTA = 0.01; - private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); - - @Test - void testForced() throws TimingException { - List waypoints = List.of( - new Pose2d(0, 0, new Rotation2d()), - new Pose2d(1, 0, new Rotation2d()), - new Pose2d(1, 1, new Rotation2d())); - Path100 path = PathFactory.withoutControlPoints(waypoints, 0.01, 0.01, 0.1); - // sample the path so we can see it - for (double t = 0; t <= path.getMaxDistance(); t += 0.1) { - if (DEBUG) - System.out.printf("%.1f %5.2f %5.2f\n", - t, - path.sample(t).getPose().translation().getX(), - path.sample(t).getPose().translation().getY()); - } - // schedule it so we can see it - // no constraints - TimingConstraintFactory f = new TimingConstraintFactory(SwerveKinodynamicsFactory.forTest(logger)); - List constraints = f.forTest(logger); - ScheduleGenerator scheduler = new ScheduleGenerator(constraints); - Trajectory100 trajectory = scheduler.timeParameterizeTrajectory( - path, - 0.0127, - 0.05, - 0.05); - - for (double t = 0; t < trajectory.duration(); t += 0.1) { - if (DEBUG) - System.out.printf("%.1f %5.2f %5.2f\n", - t, - trajectory.sample(t).state().getPose().translation().getX(), - trajectory.sample(t).state().getPose().translation().getY()); - } - - assertEquals(13, path.length()); - Pose2dWithMotion p = path.getPoint(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); - assertEquals(0, p.getHeadingRateRad_M(), DELTA); - p = path.getPoint(1); - assertEquals(0.3, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); - assertEquals(0, p.getHeadingRateRad_M(), DELTA); - } @Test void testBackingUp() { diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index b1b3bb055..3b2c38b4c 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -1,6 +1,7 @@ package org.team100.lib.trajectory.path.spline; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.ArrayList; @@ -17,12 +18,14 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; +import org.team100.lib.trajectory.TrajectoryPlotter; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.CapsizeAccelerationConstraint; import org.team100.lib.trajectory.timing.ScheduleGenerator; import org.team100.lib.trajectory.timing.TimingConstraint; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -38,7 +41,6 @@ void testCourse() { Translation2d t = new Translation2d(1, 0).rotateBy(course); assertEquals(0.707, t.getX(), DELTA); assertEquals(0.707, t.getY(), DELTA); - } @Test @@ -54,6 +56,10 @@ void testLinear() { new Translation2d(1, 0), new Rotation2d()), DirectionSE2.TO_X)); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); + Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); @@ -81,6 +87,10 @@ void testLinear2() { new Translation2d(2, 0), new Rotation2d()), DirectionSE2.TO_X)); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); + Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); @@ -109,15 +119,8 @@ void testRotation() { new Rotation2d(1)), DirectionSE2.TO_X)); - if (DEBUG) { - System.out.println("d, x, y, heading, course"); - for (double tt = 0; tt <= 1.0; tt += 0.01) { - var ss = s.getPose2dWithMotion(tt); - Pose2d pose = ss.getPose().pose(); - System.out.printf("%6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", - tt, pose.getX(), pose.getY(), pose.getRotation().getRadians(), ss.getCourse().getRadians()); - } - } + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); // now that the magic numbers apply to the rotational scaling, the heading rate // is constant. @@ -229,53 +232,56 @@ void testCircle() { splines.add(s2); splines.add(s3); - // what do the unoptimized splines look like? - // for (int i = 0; i < 4; ++i) { - // HolonomicSpline s = splines.get(i); - // for (double j = 0; j < 0.99; j += 0.1) { - // Pose2d p = s.getPose2d(j); - // System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - // i + j, p.getX(), p.getY(), p.getRotation().getRadians()); - // } - // } + // before optimization + + // spline joints are C1 smooth (i.e. same slope) + assertTrue(verifyC1(splines)); + // spline joints are C2 smooth (i.e. same curvature) + assertTrue(verifyC2(splines)); - // optimization does not help, the individual splines are already "optimal." + checkCircle(splines, 0.020, 0.026); - SplineUtil.forceC1(splines); + // optimize and compare + TrajectoryPlotter.plot(splines, 0.1, 1); - SplineUtil.optimizeSpline(splines); + // after optimization // spline joints are C1 smooth (i.e. same slope) - assertTrue(SplineUtil.verifyC1(splines)); + assertTrue(verifyC1(splines)); // spline joints are C2 smooth (i.e. same curvature) - assertTrue(SplineUtil.verifyC2(splines)); - // spline joints are C3 smooth (i.e. same rate of change of curvature) - // assertTrue(SplineUtil.verifyC3(splines)); + assertTrue(verifyC2(splines)); + + // optimization very slightly reduces the range error but increases the azimuth + // error. + checkCircle(splines, 0.018, 0.041); + } - for (int i = 0; i < 4; ++i) { - HolonomicSpline s = splines.get(i); + private void checkCircle(List splines, double rangeError, double azimuthError) { + double actualRangeError = 0; + double actualAzimuthError = 0; + for (HolonomicSpline s : splines) { for (double j = 0; j < 0.99; j += 0.1) { Pose2d p = s.getPose2d(j); - // the heading should point to the origin all the time, more or less. + // the position should be on the circle + double range = p.getTranslation().getNorm(); + actualRangeError = Math.max(actualRangeError, Math.abs(1.0 - range)); + // the heading should point to the origin all the time. Rotation2d angleFromOrigin = p.getTranslation().unaryMinus().getAngle(); Rotation2d error = angleFromOrigin.minus(p.getRotation()); // there's about 2 degrees of error here because the spline is not quite a // circle. // 3/10/25 i made generation coarser so it's less accurate. - assertEquals(0, error.getRadians(), 0.05); - if (DEBUG) - System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - i + j, p.getX(), p.getY(), p.getRotation().getRadians()); + actualAzimuthError = Math.max(actualAzimuthError, Math.abs(error.getRadians())); } } + assertEquals(0, actualRangeError, rangeError); + assertEquals(0, actualAzimuthError, azimuthError); + } @Test - void testMismatchedThetaDerivatives() { - // if the theta derivative and each endpoint is the average delta, - // what happens when adjacent segments have different deltas? - // the optimizer needs to make them the same. + void testLine() { double magicNumber = 1.0; // turn a bit to the left HolonomicSpline s0 = new HolonomicSpline( @@ -307,36 +313,64 @@ void testMismatchedThetaDerivatives() { splines.add(s0); splines.add(s1); - // for (int i = 0; i < 2; ++i) { - // HolonomicSpline s = splines.get(i); - // for (double j = 0; j < 0.99; j += 0.1) { - // Pose2d p = s.getPose2d(j); - // if (DEBUG) - // System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - // i + j, p.getX(), p.getY(), p.getRotation().getRadians()); - // } - // } + TrajectoryPlotter.plot(splines, 0.1, 1); - SplineUtil.forceC1(splines); + // spline joints are not C1 smooth + assertFalse(verifyC1(splines)); + // spline joints are C2 smooth (i.e. same curvature) + assertTrue(verifyC2(splines)); + } - SplineUtil.optimizeSpline(splines); + /** + * A kinda-realistic test path: + * + * * start facing towards the driver + * * back up + * * rotate towards +y, also drive towards +y + * + * Does optimization really help here? + */ + @Test + void testPath0() { + double magicNumber = 1.0; + // turn a bit to the left + Pose2dWithDirection p0 = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d(-1, 0)), + new DirectionSE2(1, 0, 0)); + Pose2dWithDirection p1 = new Pose2dWithDirection( + new Pose2d( + new Translation2d(0.7, 0.3), + new Rotation2d(-1, 1)), + new DirectionSE2(1, 1, -1)); + Pose2dWithDirection p2 = new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d(0, 1)), + new DirectionSE2(0, 1, 0)); + + HolonomicSpline s01 = new HolonomicSpline( + p0, p1, magicNumber, magicNumber); + HolonomicSpline s12 = new HolonomicSpline( + p1, p2, magicNumber, magicNumber); + List splines = new ArrayList<>(); + splines.add(s01); + splines.add(s12); - // spline joints are C1 smooth (i.e. same slope) - assertTrue(SplineUtil.verifyC1(splines)); + // before + // spline joints are not C1 smooth + assertTrue(verifyC1(splines)); // spline joints are C2 smooth (i.e. same curvature) - assertTrue(SplineUtil.verifyC2(splines)); - // spline joints are C3 smooth (i.e. same rate of change of curvature) - // assertTrue(SplineUtil.verifyC3(splines)); + assertTrue(verifyC2(splines)); - for (int i = 0; i < 2; ++i) { - HolonomicSpline s = splines.get(i); - for (double j = 0; j < 0.99; j += 0.1) { - Pose2d p = s.getPose2d(j); - if (DEBUG) - System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - i + j, p.getX(), p.getY(), p.getRotation().getRadians()); - } - } + TrajectoryPlotter.plot(splines, 0.1, 1); + + // after + // spline joints are not C1 smooth + assertTrue(verifyC1(splines)); + // spline joints are C2 smooth (i.e. same curvature) + assertTrue(verifyC2(splines)); } @Test @@ -376,19 +410,7 @@ void testMismatchedXYDerivatives() { splines.add(s0); splines.add(s1); - // for (int i = 0; i < 2; ++i) { - // HolonomicSpline s = splines.get(i); - // for (double j = 0; j < 0.99; j += 0.1) { - // Pose2d p = s.getPose2d(j); - // if (DEBUG) - // System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - // i + j, p.getX(), p.getY(), p.getRotation().getRadians()); - // } - // } - - SplineUtil.forceC1(splines); - - SplineUtil.optimizeSpline(splines); + TrajectoryPlotter.plot(splines, 0.1, 1); for (HolonomicSpline s : splines) { if (DEBUG) @@ -396,21 +418,9 @@ void testMismatchedXYDerivatives() { } // spline joints are C1 smooth (i.e. same slope) - assertTrue(SplineUtil.verifyC1(splines)); + assertFalse(verifyC1(splines)); // spline joints are C2 smooth (i.e. same curvature) - assertTrue(SplineUtil.verifyC2(splines)); - // spline joints are C3 smooth (i.e. same rate of change of curvature) - // assertTrue(SplineUtil.verifyC3(splines)); - - for (int i = 0; i < 2; ++i) { - HolonomicSpline s = splines.get(i); - for (double j = 0; j < 0.99; j += 0.1) { - Pose2d p = s.getPose2d(j); - if (DEBUG) - System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - i + j, p.getX(), p.getY(), p.getRotation().getRadians()); - } - } + assertTrue(verifyC2(splines)); Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05)); if (DEBUG) @@ -419,6 +429,10 @@ void testMismatchedXYDerivatives() { ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); Trajectory100 trajectory = scheduleGenerator.timeParameterizeTrajectory(path, 0.05, 0, 0); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("plot", trajectory); + if (DEBUG) System.out.printf("trajectory %s\n", trajectory); @@ -447,6 +461,10 @@ void testEntryVelocity() { } List splines = List.of(s0); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("splines", splines); + List motion = PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05); if (DEBUG) { for (Pose2dWithMotion p : motion) { @@ -474,7 +492,73 @@ void testEntryVelocity() { // a = v^2/r so v = sqrt(ar) = 2.858 Trajectory100 trajectory = scheduleGenerator.timeParameterizeTrajectory(path, 0.05, 2.858, 2.858); + + plotter.plot("plot", trajectory); + if (DEBUG) System.out.printf("trajectory %s\n", trajectory); } + + /** + * True if adjacent spline endpoints have (nearly) identical derivative terms. + */ + static boolean verifyC1(List splines) { + if (splines.size() < 2) + return true; + for (int i = 0; i < splines.size() - 1; ++i) { + HolonomicSpline s0 = splines.get(i); + HolonomicSpline s1 = splines.get(i + 1); + if (!MathUtil.isNear(s0.dx(1), s1.dx(0), 1e-6)) { + if (DEBUG) { + System.out.printf("bad x C1 %f %f\n", s0.dx(1), s1.dx(0)); + } + return false; + } + if (!MathUtil.isNear(s0.dy(1), s1.dy(0), 1e-6)) { + if (DEBUG) { + System.out.printf("bad y C1 %f %f\n", s0.dy(1), s1.dy(0)); + } + return false; + } + if (!MathUtil.isNear(s0.dtheta(1), s1.dtheta(0), 1e-6)) { + if (DEBUG) { + System.out.printf("bad theta C1 %f %f\n", s0.dtheta(1), s1.dtheta(0)); + } + return false; + } + } + return true; + } + + /** + * True if adjacent spline endpoints have (nearly) identical second-derivative + * terms. + */ + static boolean verifyC2(List splines) { + if (splines.size() < 2) + return true; + for (int i = 0; i < splines.size() - 1; ++i) { + HolonomicSpline s0 = splines.get(i); + HolonomicSpline s1 = splines.get(i + 1); + if (!MathUtil.isNear(s0.ddx(1), s1.ddx(0), 1e-6)) { + if (DEBUG) { + System.out.printf("bad x C2 %f %f\n", s0.ddx(1), s1.ddx(0)); + } + return false; + } + if (!MathUtil.isNear(s0.ddy(1), s1.ddy(0), 1e-6)) { + if (DEBUG) { + System.out.printf("bad y C2 %f %f\n", s0.ddy(1), s1.ddy(0)); + } + return false; + } + if (!MathUtil.isNear(s0.ddtheta(1), s1.ddtheta(0), 1e-6)) { + if (DEBUG) { + System.out.printf("bad theta C2 %f %f\n", s0.ddtheta(1), s1.ddtheta(0)); + } + return false; + } + } + return true; + } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java index 283d19585..944d3b7db 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java @@ -1,24 +1,21 @@ package org.team100.lib.trajectory.path.spline; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - import java.util.ArrayList; import java.util.List; import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.trajectory.TrajectoryPlotter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -class QuinticHermiteOptimizerTest { - private static double kEpsilon = 1e-12; +public class QuinticHermiteOptimizerTest { @Test - void test() { + void test0() { Pose2dWithDirection a = new Pose2dWithDirection( new Pose2d( new Translation2d(0, 100), @@ -39,7 +36,12 @@ void test() { splines.add(new HolonomicSpline(a, b)); splines.add(new HolonomicSpline(b, c)); - assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); + TrajectoryPlotter.plot(splines, 5, 10); + + } + + @Test + void test1() { Pose2dWithDirection d = new Pose2dWithDirection( new Pose2d( @@ -62,13 +64,17 @@ void test() { new Rotation2d()), DirectionSE2.MINUS_X); - List splines1 = new ArrayList<>(); - splines1.add(new HolonomicSpline(d, e)); - splines1.add(new HolonomicSpline(e, f)); - splines1.add(new HolonomicSpline(f, g)); + List splines = new ArrayList<>(); + splines.add(new HolonomicSpline(d, e)); + splines.add(new HolonomicSpline(e, f)); + splines.add(new HolonomicSpline(f, g)); - assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); + TrajectoryPlotter.plot(splines, 5, 10); + } + + @Test + void test2() { Pose2dWithDirection h = new Pose2dWithDirection( new Pose2d( new Translation2d(0, 0), @@ -101,13 +107,12 @@ void test() { splines2.add(new HolonomicSpline(j, k)); splines2.add(new HolonomicSpline(k, l)); - assertTrue(SplineUtil.optimizeSpline(splines2) < 0.05); - assertEquals(0.0, splines2.get(0).getCurvature(1.0), kEpsilon); - assertEquals(0.0, splines2.get(2).getCurvature(1.0), kEpsilon); + TrajectoryPlotter.plot(splines2, 5, 10); + } @Test - void testHolonomic() { + void test3() { Pose2dWithDirection a = new Pose2dWithDirection( new Pose2d( new Translation2d(0, 100), @@ -128,7 +133,12 @@ void testHolonomic() { splines.add(new HolonomicSpline(a, b)); splines.add(new HolonomicSpline(b, c)); - assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); + TrajectoryPlotter.plot(splines, 5, 10); + + } + + @Test + void test4() { Pose2dWithDirection d = new Pose2dWithDirection( new Pose2d( @@ -151,12 +161,16 @@ void testHolonomic() { new Rotation2d()), DirectionSE2.MINUS_X); - List splines1 = new ArrayList<>(); - splines1.add(new HolonomicSpline(d, e)); - splines1.add(new HolonomicSpline(e, f)); - splines1.add(new HolonomicSpline(f, g)); + List splines = new ArrayList<>(); + splines.add(new HolonomicSpline(d, e)); + splines.add(new HolonomicSpline(e, f)); + splines.add(new HolonomicSpline(f, g)); + + TrajectoryPlotter.plot(splines, 5, 10); + } - assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); + @Test + void test5() { Pose2dWithDirection h = new Pose2dWithDirection( new Pose2d( @@ -184,16 +198,13 @@ void testHolonomic() { new Rotation2d(Math.PI / 2)), DirectionSE2.MINUS_Y); - List splines2 = new ArrayList<>(); - splines2.add(new HolonomicSpline(h, i)); - splines2.add(new HolonomicSpline(i, j)); - splines2.add(new HolonomicSpline(j, k)); - splines2.add(new HolonomicSpline(k, l)); + List splines = new ArrayList<>(); + splines.add(new HolonomicSpline(h, i)); + splines.add(new HolonomicSpline(i, j)); + splines.add(new HolonomicSpline(j, k)); + splines.add(new HolonomicSpline(k, l)); - assertTrue(SplineUtil.optimizeSpline(splines2) < 0.05); - assertEquals(0.0, splines2.get(0).getCurvature(1.0), kEpsilon); - assertEquals(0.0, splines2.get(2).getCurvature(1.0), kEpsilon); + TrajectoryPlotter.plot(splines, 5, 10); } - } From c165b0a1382a5049c043a8e83791ee8b2b922034 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 15 Dec 2025 21:51:49 -0800 Subject: [PATCH 06/42] 2d spline uses SE(2) direction for endpoints instead of guessing the theta endpoints --- .../team100/lib/geometry/DirectionSE2.java | 6 + .../team100/lib/geometry/GeometryUtil.java | 59 +++++- .../team100/lib/trajectory/path/Path100.java | 42 ++-- .../lib/trajectory/path/PathFactory.java | 51 ++--- .../path/spline/HolonomicSpline.java | 180 ++++++------------ .../trajectory/timing/ScheduleGenerator.java | 3 + .../team100/lib/targeting/TargetsTest.java | 6 +- .../lib/trajectory/PathToVectorSeries.java | 45 +++++ .../lib/trajectory/Trajectory100Test.java | 43 ----- .../lib/trajectory/TrajectoryPlannerTest.java | 5 +- .../lib/trajectory/TrajectoryPlotter.java | 56 ++++++ .../lib/trajectory/path/Path100Test.java | 29 ++- .../lib/trajectory/path/PathFactoryTest.java | 38 +++- .../path/spline/HolonomicSplineTest.java | 152 +++++++++++---- .../trajectory/path/spline/SplineR1Test.java | 2 +- .../timing/ScheduleGeneratorTest.java | 42 ++-- .../team100/lib/util/GeometryUtilTest.java | 20 +- 17 files changed, 472 insertions(+), 307 deletions(-) create mode 100644 lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index 565c53bbd..7d6b32ec4 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -48,4 +48,10 @@ public boolean equals(Object obj) { && Math.abs(other.y - y) < 1E-9 && Math.abs(other.theta - theta) < 1E-9; } + + @Override + public String toString() { + return String.format("%5.3f %5.3f %5.3f", x, y, theta); + } + } diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 85bb0d1aa..60aac3982 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -144,12 +144,14 @@ public static boolean isParallel(Rotation2d a, Rotation2d b) { * path length for nonzero omega. */ public static double norm(Twist2d a) { - // Common case of dy == 0 - if (a.dy == 0.0) - return Math.abs(a.dx); return Math.hypot(a.dx, a.dy); } + /* All components of the twist. */ + public static double normL2(Twist2d a) { + return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); + } + public static double norm(Twist3d t) { Vector v = VecBuilder.fill(t.dx, t.dy, t.dz, t.rx, t.ry, t.rz); return v.norm(); @@ -204,15 +206,20 @@ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { return new Pose2d(lerpT, lerpR); } - // TODO: fix interpolation - public static Pose2dWithDirection interpolate(Pose2dWithDirection a, Pose2dWithDirection b, double x) { + public static DirectionSE2 interpolate(DirectionSE2 a, DirectionSE2 b, double x) { + return new DirectionSE2( + MathUtil.interpolate(a.x, b.x, x), + MathUtil.interpolate(a.y, b.y, x), + MathUtil.interpolate(a.theta, b.theta, x)); + } + + public static Pose2dWithDirection interpolate( + Pose2dWithDirection a, + Pose2dWithDirection b, + double x) { return new Pose2dWithDirection( interpolate(a.pose(), b.pose(), x), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(interpolate2( - a.course().toRotation(), - b.course().toRotation(), x)), - 0)); + interpolate(a.course(), b.course(), x)); } /** @@ -254,6 +261,10 @@ public static Rotation3d interpolate(Rotation3d a, Rotation3d b, double x) { MathUtil.interpolate(a.getZ(), b.getZ(), x)); } + // TODO: move this whole section about distances to a separate class. + // + // https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + public static double distance(PoseWithCurvature a, PoseWithCurvature b) { // this is not used return norm(slog(transformBy(inverse(a.poseMeters), b.poseMeters))); @@ -267,6 +278,34 @@ public static double distanceM(Pose2d a, Pose2d b) { return norm(slog(transformBy(inverse(a), b))); } + /** + * Double-geodesic combines the angular distance with the translational + * distance, weighting 1 radian equal to 1 meter. + * + * This is not the geodesic distance, which is zero for spin-in-place. It's just + * the L2 norm for all three dimensions. + * + * TODO: adjustable weights + * + * @see https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + */ + public static double doubleGeodesicDistance(Pose2d a, Pose2d b) { + Translation2d tDiff = a.getTranslation().minus(b.getTranslation()); + double tSqDist = dot(tDiff, tDiff); + double aDiff = a.getRotation().minus(b.getRotation()).getRadians(); + if (DEBUG) + System.out.printf("double geodesic distance t %f a %f\n", tSqDist, aDiff * aDiff); + return Math.sqrt(aDiff * aDiff + tSqDist); + } + + public static double doubleGeodesicDistance(Pose2dWithMotion a, Pose2dWithMotion b) { + return doubleGeodesicDistance(a.getPose(), b.getPose()); + } + + public static double doubleGeodesicDistance(Pose2dWithDirection a, Pose2dWithDirection b) { + return doubleGeodesicDistance(a.pose(), b.pose()); + } + public static double distanceM(Translation2d a, Translation2d b) { return a.getDistance(b); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 00cdbaeb9..e9e2df4a2 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import java.util.List; +import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.timing.ScheduleGenerator; @@ -13,7 +14,7 @@ */ public class Path100 { private final List m_points; - /** in meters */ + /** double geodesic distance which is meters and radians */ private final double[] m_distances; public Path100(final List states) { @@ -26,8 +27,11 @@ public Path100(final List states) { m_points.add(states.get(0)); for (int i = 1; i < states.size(); ++i) { m_points.add(states.get(i)); - m_distances[i] = m_distances[i - 1] - + getPoint(i - 1).distanceM(getPoint(i)); + Pose2dWithMotion p0 = getPoint(i - 1); + Pose2dWithMotion p1 = getPoint(i); + // use the distance metric that includes rotation + double dist = GeometryUtil.doubleGeodesicDistance(p0, p1); + m_distances[i] = m_distances[i - 1] + dist; } } @@ -39,7 +43,7 @@ public int length() { return m_points.size(); } - public Pose2dWithMotion getPoint(final int index) { + public Pose2dWithMotion getPoint(int index) { if (m_points.isEmpty()) return null; return m_points.get(index); @@ -63,24 +67,24 @@ public double getMinDistance() { */ public Pose2dWithMotion sample(double distance) throws ScheduleGenerator.TimingException { if (distance >= getMaxDistance()) { - Pose2dWithMotion point = getPoint(length() - 1); - return point; + // off the end + return getPoint(length() - 1); } if (distance <= 0.0) { - Pose2dWithMotion point = getPoint(0); - return point; + // before the start + return getPoint(0); } - for (int i = 1; i < m_distances.length; ++i) { - final Pose2dWithMotion point = getPoint(i); - if (m_distances[i] >= distance) { - final Pose2dWithMotion prev_s = getPoint(i - 1); - if (Math.abs(m_distances[i] - m_distances[i - 1]) <= 1e-12) { - return point; - } else { - return prev_s.interpolate( - point, - (distance - m_distances[i - 1]) / (m_distances[i] - m_distances[i - 1])); - } + for (int i = 1; i < length(); ++i) { + // walk through the points to bracket the desired distance + Pose2dWithMotion p0 = getPoint(i - 1); + Pose2dWithMotion p1 = getPoint(i); + double d0 = m_distances[i - 1]; + double d1 = m_distances[i]; + double d = d1 - d0; + if (d1 >= distance) { + // Found the bracket. + double s = (distance - d0) / d; + return p0.interpolate(p1, s); } } throw new ScheduleGenerator.TimingException(); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index a76faea5c..860e17a4e 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -9,10 +9,11 @@ import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Twist2d; public class PathFactory { + private static final boolean DEBUG = false; public static Path100 pathFromWaypoints( List waypoints, @@ -39,9 +40,12 @@ public static Path100 pathFromWaypoints( double maxDTheta) { List splines = new ArrayList<>(waypoints.size() - 1); for (int i = 1; i < waypoints.size(); ++i) { - splines.add(new HolonomicSpline( + HolonomicSpline s = new HolonomicSpline( waypoints.get(i - 1), - waypoints.get(i))); + waypoints.get(i)); + if (DEBUG) + System.out.printf("%d %s\n", i, s); + splines.add(s); } return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); } @@ -87,6 +91,8 @@ public static List parameterizeSplines( rv.add(splines.get(0).getPose2dWithMotion(0.0)); for (int i = 0; i < splines.size(); i++) { HolonomicSpline s = splines.get(i); + if (DEBUG) + System.out.printf("%d %s\n", i, s); List samples = parameterizeSpline(s, maxDx, maxDy, maxDTheta, 0.0, 1.0); samples.remove(0); rv.addAll(samples); @@ -94,6 +100,9 @@ public static List parameterizeSplines( return rv; } + /** + * Recursively add points until they're close to the real curve. + */ private static void getSegmentArc( HolonomicSpline s, List rv, @@ -103,29 +112,27 @@ private static void getSegmentArc( double maxDy, double maxDTheta) { Pose2d p0 = s.getPose2d(t0); - Pose2d phalf = s.getPose2d(t0 + (t1 - t0) * .5); + double thalf = (t0 + t1) / 2; + Pose2d phalf = s.getPose2d(thalf); Pose2d p1 = s.getPose2d(t1); - Twist2d twist_full = Pose2d.kZero.log(GeometryUtil.transformBy(GeometryUtil.inverse(p0), p1)); - Pose2d phalf_predicted = GeometryUtil.transformBy(p0, - Pose2d.kZero.exp(GeometryUtil.scale(twist_full, 0.5))); - Pose2d error = GeometryUtil.transformBy(GeometryUtil.inverse(phalf), phalf_predicted); - if (GeometryUtil.norm(twist_full) < 1e-6) { - // the Rotation2d below will be garbage in this case so give up. - return; - } - Rotation2d course_predicted = (new Rotation2d(twist_full.dx, twist_full.dy)) - .rotateBy(phalf_predicted.getRotation()); + // twist from p0 to p1 + Twist2d twist_full = p0.log(p1); + // twist halfway from p0 to p1 + Twist2d twist_half = GeometryUtil.scale(twist_full, 0.5); + // point halfway from p0 to p1 + Pose2d phalf_predicted = p0.exp(twist_half); + // difference between twist and sample + Transform2d error = phalf_predicted.minus(phalf); - Rotation2d course_half = s.getCourse(t0 + (t1 - t0) * .5).orElse(course_predicted); - double course_error = course_predicted.unaryMinus().rotateBy(course_half).getRadians(); - if (Math.abs(error.getTranslation().getY()) > maxDy || - Math.abs(error.getTranslation().getX()) > maxDx || - Math.abs(error.getRotation().getRadians()) > maxDTheta || - Math.abs(course_error) > maxDTheta) { - getSegmentArc(s, rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta); - getSegmentArc(s, rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta); + if (Math.abs(error.getTranslation().getX()) > maxDx || + Math.abs(error.getTranslation().getY()) > maxDy || + Math.abs(error.getRotation().getRadians()) > maxDTheta) { + // add a point in between + getSegmentArc(s, rv, t0, thalf, maxDx, maxDy, maxDTheta); + getSegmentArc(s, rv, thalf, t1, maxDx, maxDy, maxDTheta); } else { + // midpoint is close enough, this looks good rv.add(s.getPose2dWithMotion(t1)); } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index a4a63f037..cda409a99 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -2,12 +2,10 @@ import java.util.Optional; -import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.util.Math100; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -25,7 +23,7 @@ * motion; that's not true here. * * This happily produces "splines" with sharp corners, if the segment - * derivatives don't match. To fix that, use SplineUtil.forceC1(). + * derivatives don't match. */ public class HolonomicSpline { private static final boolean DEBUG = false; @@ -45,9 +43,6 @@ public class HolonomicSpline { /** * The theta endpoint derivative is just the average theta rate, which is new, * it used to be zero. - * - * You'll probably want to call SplineUtil.forceC1() and - * SplineUtil.optimizeSpline() after creating these segments. */ public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1) { this(p0, p1, 1.2, 1.2); @@ -58,16 +53,18 @@ public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1) { * endpoints, i.e. how "strongly" the derivative affects the curve. High magic * number means low curvature at the endpoint. * - * The theta endpoint derivative is just the average theta rate, which is new, - * it used to be zero. - * - * Maybe the theta rate should be specified instead of taking the average. - * - * You'll probably want to call SplineUtil.forceC1() and - * SplineUtil.optimizeSpline() after creating these segments. + * Previously, the theta endpoint derivatives were the average rate, which + * yielded paths with a lot of rotation at the last second. Typically this isn't + * what you want: you're approaching some target, and rotation is disruptive to + * everything: vision, actuation, everything. + * + * Instead, we now use the "course" to specify the whole SE(2) direction, so if + * you want rotation at the end, you can say that, and if you want no rotation + * at the end, you can say that too. * - * Magic numbers also scale theta endpoints, so that the translation and - * rotation splines track together. + * All second derivatives are zero, and we don't try to change this anymore with + * optimization. Optimization just doesn't help very much, and it's a pain when + * it behaves strangely. * * @param p0 starting pose * @param p1 ending pose @@ -75,55 +72,46 @@ public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1) { * @param mN1 ending magic number */ public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1, double mN0, double mN1) { - double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); - double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); + // Distance metric includes both translation and rotation. This is not + // the geodesic distance, which is zero for spin-in-place. It's just the + // L2 norm for all three dimensions. + double distance = GeometryUtil.doubleGeodesicDistance(p0.pose(), p1.pose()); + if (DEBUG) + System.out.printf("distance %f\n", distance); + double scale0 = mN0 * distance; + double scale1 = mN1 * distance; + if (DEBUG) { System.out.printf("scale %f %f\n", scale0, scale1); } - // Translation2d course0 = new Translation2d(1,0).rotateBy(p0.course()); - // Translation2d course1 = new Translation2d(1,0).rotateBy(p1.course()); - + // Endpoints: double x0 = p0.translation().getX(); double x1 = p1.translation().getX(); - // first derivatives are just the course - // double dx0 = course0.getX() * scale0; - double dx0 = p0.course().x * scale0; - // double dx1 = course1.getX() * scale1; - double dx1 = p1.course().x * scale1; - // second derivatives are zero at the ends - double ddx0 = 0; - double ddx1 = 0; - double y0 = p0.translation().getY(); double y1 = p1.translation().getY(); - // first derivatives are just the course - // double dy0 = course0.getY() * scale0; + // To avoid 180 degrees, heading uses an offset + m_heading0 = p0.heading(); + double delta = p1.heading().minus(p0.heading()).getRadians(); + + // First derivatives are the course: + double dx0 = p0.course().x * scale0; + double dx1 = p1.course().x * scale1; double dy0 = p0.course().y * scale0; - // double dy1 = course1.getY() * scale1; double dy1 = p1.course().y * scale1; - // second derivatives are zero at the ends + double dtheta0 = p0.course().theta * scale0; + double dtheta1 = p1.course().theta * scale1; + + // Second derivatives are zero: + double ddx0 = 0; + double ddx1 = 0; double ddy0 = 0; double ddy1 = 0; + double ddtheta0 = 0; + double ddtheta1 = 0; m_x = SplineR1.get(x0, x1, dx0, dx1, ddx0, ddx1); m_y = SplineR1.get(y0, y1, dy0, dy1, ddy0, ddy1); - - m_heading0 = p0.heading(); - double delta = p1.heading().minus(p0.heading()).getRadians(); - - // previously dtheta at the endpoints was zero, which is bad: it meant the omega - // value varied all over the place, even for theta paths that should be - // completely smooth. - // a reasonable derivative for theta is just the average (i.e. the course from - // the preceding point to the following point) - // this will produce a "corner" in theta, which you may want to fix with - // SplineUtil.forceC1(). - double dtheta0 = delta * mN0; - double dtheta1 = delta * mN1; - // second derivatives are zero at the ends - double ddtheta0 = 0; - double ddtheta1 = 0; m_heading = SplineR1.get(0.0, delta, dtheta0, dtheta1, ddtheta0, ddtheta1); } @@ -132,43 +120,41 @@ public String toString() { return "HolonomicSpline [m_x=" + m_x + ", m_y=" + m_y + ", m_theta=" + m_heading + ", m_r0=" + m_heading0 + "]"; } - /** This is used by various optimization steps. */ - private HolonomicSpline( - SplineR1 x, - SplineR1 y, - SplineR1 theta, - Rotation2d r0) { - m_x = x; - m_y = y; - m_heading = theta; - m_heading0 = r0; - } - + /** + * @param p [0,1] + */ public Pose2dWithMotion getPose2dWithMotion(double p) { return new Pose2dWithMotion( new Pose2dWithDirection( new Pose2d(getPoint(p), getHeading(p)), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(getCourse(p).orElseThrow()), 0)), + getCourse(p)), getDHeadingDs(p), getCurvature(p), getDCurvatureDs(p)); } + /** So we can see it */ + public void printSamples() { + System.out.println("p, x, heading, heading rate"); + for (double p = 0; p < 1; p += 0.05) { + Pose2dWithMotion pwm = getPose2dWithMotion(p); + System.out.printf("%f, %f, %f, %f\n", + p, + pwm.getPose().pose().getX(), + pwm.getPose().heading().getRadians(), + pwm.getHeadingRateRad_M()); + } + } + /** - * Course is the direction of motion, regardless of the direction the robot is - * facing (heading). It's optional to account for the motionless case. - * - * Course is the same for holonomic and nonholonomic splines. + * Course is the direction of motion in SE(2), which means it includes both + * cartesian dimensions and also the rotation dimension. */ - public Optional getCourse(double t) { + public DirectionSE2 getCourse(double t) { double dx = dx(t); double dy = dy(t); - if (Math100.epsilonEquals(dx, 0.0) && Math100.epsilonEquals(dy, 0.0)) { - // rotation below would be garbage so give up - return Optional.empty(); - } - return Optional.of(new Rotation2d(dx, dy)); + double dtheta = dtheta(t); + return new DirectionSE2(dx, dy, dtheta); } public Pose2d getPose2d(double p) { @@ -185,54 +171,6 @@ protected double getDHeading(double t) { return m_heading.getVelocity(t); } - HolonomicSpline replaceFirstDerivatives( - double dx0, - double dx1, - double dy0, - double dy1, - double dtheta0, - double dtheta1) { - return new HolonomicSpline( - SplineR1.get( - m_x.getPosition(0), - m_x.getPosition(1), - dx0, - dx1, - m_x.getAcceleration(0), - m_x.getAcceleration(1)), - SplineR1.get( - m_y.getPosition(0), - m_y.getPosition(1), - dy0, - dy1, - m_y.getAcceleration(0), - m_y.getAcceleration(1)), - SplineR1.get( - m_heading.getPosition(0), - m_heading.getPosition(1), - dtheta0, - dtheta1, - m_heading.getAcceleration(0), - m_heading.getAcceleration(1)), - m_heading0); - } - - /** - * Return a new spline that is a copy of this one, but incrementing the second - * derivatives by the specified amounts. - */ - HolonomicSpline addToSecondDerivatives( - double ddx0_sub, - double ddx1_sub, - double ddy0_sub, - double ddy1_sub) { - return new HolonomicSpline( - m_x.addCoefs(SplineR1.get(0, 0, 0, 0, ddx0_sub, ddx1_sub)), - m_y.addCoefs(SplineR1.get(0, 0, 0, 0, ddy0_sub, ddy1_sub)), - m_heading, - m_heading0); - } - /** * Change in heading per distance traveled, i.e. spatial change in heading. * dtheta/ds (radians/meter). diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 7f440dd9b..830847b06 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -12,6 +12,7 @@ * schedule. */ public class ScheduleGenerator { + private static final boolean DEBUG = false; private static final double EPSILON = 1e-6; /** this is the default, in order to make the constraints set the actual */ private static final double HIGH_ACCEL = 1000; @@ -39,6 +40,8 @@ public Trajectory100 timeParameterizeTrajectory( List samples = new ArrayList<>(num_states); for (int i = 0; i < num_states; ++i) { Pose2dWithMotion state = path.sample(Math.min(i * step, maxDistance)); + if (DEBUG) + System.out.printf("%d %s\n", i, state); samples.add(state); } return timeParameterizeTrajectory(samples, start_vel, end_vel); diff --git a/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java index ab3f09d59..96940a0c8 100644 --- a/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java +++ b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java @@ -82,7 +82,7 @@ void testTranslations() throws InterruptedException { ModelR3 p = new ModelR3(); Targets reader = new Targets(logger, logger, (x) -> p); - Thread.sleep(50); + Thread.sleep(200); SimulatedTargetWriter writer = new SimulatedTargetWriter( logger, List.of(Camera.TEST4), @@ -90,13 +90,13 @@ void testTranslations() throws InterruptedException { new Translation2d[] { new Translation2d(1, 0) }); // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); stepTime(); writer.update(); // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); stepTime(); reader.update(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java new file mode 100644 index 000000000..e5a515a5f --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java @@ -0,0 +1,45 @@ +package org.team100.lib.trajectory; + +import org.jfree.data.xy.VectorSeries; +import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.trajectory.path.Path100; +import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class PathToVectorSeries { + private static final boolean DEBUG = false; + /** Length of the vector indicating heading */ + private final double m_scale; + + public PathToVectorSeries(double scale) { + m_scale = scale; + } + + /** Maps x to x, y to y */ + public VectorSeries convert(String name, Path100 path) { + VectorSeries s = new VectorSeries(name); + try { + double l = path.getMaxDistance(); + double dl = l / 20; + for (double d = 0; d < l; d += dl) { + if (DEBUG) + System.out.printf("%f\n", d); + Pose2dWithMotion pwm; + pwm = path.sample(d); + Pose2d p = pwm.getPose().pose(); + double x = p.getX(); + double y = p.getY(); + Rotation2d heading = p.getRotation(); + double dx = m_scale * heading.getCos(); + double dy = m_scale * heading.getSin(); + s.add(x, y, dx, dy); + } + } catch (TimingException e) { + e.printStackTrace(); + } + return s; + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 260122b0d..9b060f2e1 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -143,49 +143,6 @@ void testSampleThoroughly() { assertEquals(1.000, trajectory.sample(1.5).state().getPose().translation().getX(), DELTA); } - @Test - void testSampleThoroughlyWithRotation() { - List waypoints = List.of( - new Pose2dWithDirection( - new Pose2d( - new Translation2d(), - Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( - new Pose2d( - new Translation2d(1, 0), - Rotation2d.kCCW_Pi_2), - DirectionSE2.TO_X)); - - SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); - List constraints = new TimingConstraintFactory(limits).fast(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - - Trajectory100 trajectory = planner.restToRest(waypoints); - - // these numbers are sensitive to the "faceting" of the trajectory into poses. - assertEquals(1.561, trajectory.duration(), DELTA); - assertEquals(0.000, trajectory.sample(0).state().getPose().translation().getX(), DELTA); - assertEquals(0.010, trajectory.sample(0.1).state().getPose().translation().getX(), DELTA); - assertEquals(0.04, trajectory.sample(0.2).state().getPose().translation().getX(), DELTA); - assertEquals(0.09, trajectory.sample(0.3).state().getPose().translation().getX(), DELTA); - assertEquals(0.16, trajectory.sample(0.4).state().getPose().translation().getX(), DELTA); - assertEquals(0.247, trajectory.sample(0.5).state().getPose().translation().getX(), DELTA); - assertEquals(0.337, trajectory.sample(0.6).state().getPose().translation().getX(), DELTA); - assertEquals(0.427, trajectory.sample(0.7).state().getPose().translation().getX(), DELTA); - assertEquals(0.517, trajectory.sample(0.8).state().getPose().translation().getX(), DELTA); - assertEquals(0.607, trajectory.sample(0.9).state().getPose().translation().getX(), DELTA); - assertEquals(0.697, trajectory.sample(1).state().getPose().translation().getX(), DELTA); - assertEquals(0.787, trajectory.sample(1.1).state().getPose().translation().getX(), DELTA); - assertEquals(0.869, trajectory.sample(1.2).state().getPose().translation().getX(), DELTA); - assertEquals(0.931, trajectory.sample(1.3).state().getPose().translation().getX(), DELTA); - assertEquals(0.973, trajectory.sample(1.4).state().getPose().translation().getX(), DELTA); - assertEquals(0.996, trajectory.sample(1.5).state().getPose().translation().getX(), DELTA); - assertEquals(1.000, trajectory.sample(1.6).state().getPose().translation().getX(), DELTA); - assertEquals(1.000, trajectory.sample(1.7).state().getPose().translation().getX(), DELTA); - - } - /** Does the index help? No. */ // There's no need to run this all the time // @Test diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 4f1e30077..4459bb4e3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -7,9 +7,9 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -175,7 +175,8 @@ void testBackingUp2() { ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(-1, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); - assertEquals(1.176, traj.duration(), DELTA); + TrajectoryPlotter.plot(traj, 0.1, 1); + assertEquals(1.549, traj.duration(), DELTA); } @Test diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java index 924ca078e..d0bbbce19 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -17,6 +17,7 @@ import org.jfree.data.Range; import org.jfree.data.xy.VectorSeries; import org.jfree.data.xy.VectorSeriesCollection; +import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.timing.TimedPose; @@ -27,10 +28,12 @@ public class TrajectoryPlotter { private final TrajectoryToVectorSeries converter; private final SplineToVectorSeries splineConverter; + private final PathToVectorSeries pathConverter; public TrajectoryPlotter(double arrowLength) { converter = new TrajectoryToVectorSeries(arrowLength); splineConverter = new SplineToVectorSeries(arrowLength); + pathConverter = new PathToVectorSeries(arrowLength); } public void plot(String name, Trajectory100 t) { @@ -45,6 +48,10 @@ public VectorSeries convert(String name, Trajectory100 t) { return converter.convert(name, t); } + public VectorSeries convert(String name, Path100 path) { + return pathConverter.convert(name, path); + } + public void plot(String name, List s) { VectorSeries converted = convert(name, s); Range xrange = xRange(s); @@ -106,6 +113,14 @@ public Range yRange(List s) { return range(s, (p) -> p.getY()); } + public Range xRange(Path100 path) { + return range(path, (p) -> p.getX()); + } + + public Range yRange(Path100 path) { + return range(path, (p) -> p.getY()); + } + /** Adds margin */ Range range(Trajectory100 t, ToDoubleFunction f) { double max = Double.NEGATIVE_INFINITY; @@ -138,6 +153,34 @@ Range range(List splines, ToDoubleFunction f) { return new Range(min, max); } + /** Adds margin */ + Range range(Path100 t, ToDoubleFunction f) { + double max = Double.NEGATIVE_INFINITY; + double min = Double.POSITIVE_INFINITY; + for (int i = 0; i < t.length(); ++i) { + Pose2d p = t.getPoint(i).getPose().pose(); + double x = f.applyAsDouble(p); + if (x + 0.1 > max) + max = x + 0.1; + if (x - 0.1 < min) + min = x - 0.1; + } + return new Range(min, max); + } + + public static void plot( + Trajectory100 t, + double arrows, + double ticks) { + TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); + VectorSeries series = plotter.convert("trajectory", t); + Range xrange = plotter.xRange(t); + Range yrange = plotter.yRange(t); + + if (SHOW) + plotter.actuallyPlot("compare", xrange, yrange, ticks, series); + } + public static void plot( List splines, double arrows, @@ -150,4 +193,17 @@ public static void plot( if (SHOW) plotter.actuallyPlot("compare", xrange, yrange, ticks, series); } + + public static void plot( + Path100 path, + double arrows, + double ticks) { + TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); + VectorSeries series = plotter.convert("path", path); + Range xrange = plotter.xRange(path); + Range yrange = plotter.yRange(path); + + if (SHOW) + plotter.actuallyPlot("compare", xrange, yrange, ticks, series); + } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index b78bdb681..4da97ee97 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -6,7 +6,6 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import java.util.Optional; import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; @@ -84,8 +83,8 @@ public Rotation2d getHeading(double t) { } @Override - public Optional getCourse(double t) { - return Optional.of(Rotation2d.kZero); + public DirectionSE2 getCourse(double t) { + return new DirectionSE2(0, 0, 0); } @Override @@ -181,8 +180,8 @@ void test() throws TimingException { } assertEquals(0.0, path.getMinDistance(), DELTA); - // paths no longer use constant-twist arcs so this is just the lengths - assertEquals(84, path.getMaxDistance(), DELTA); + // constant-twist arcs + assertEquals(84.108, path.getMaxDistance(), DELTA); // initial sample is exactly at the start Pose2dWithMotion sample0 = path.sample(0.0); @@ -196,40 +195,40 @@ void test() throws TimingException { assertEquals(0, sample0.getPose().heading().getDegrees()); Pose2dWithMotion sample12 = path.sample(12.0); - assertEquals(12, sample12.getPose().translation().getX(), DELTA); + assertEquals(11.997, sample12.getPose().translation().getX(), DELTA); assertEquals(0, sample12.getPose().translation().getY(), DELTA); // course should be +x assertEquals(0, sample12.getCourse().getDegrees(), DELTA); - assertEquals(15, sample12.getPose().heading().getDegrees(), DELTA); + assertEquals(14.996, sample12.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample5 = path.sample(48); assertEquals(36, sample5.getPose().translation().getX(), DELTA); - assertEquals(12, sample5.getPose().translation().getY(), DELTA); - assertEquals(45, sample5.getCourse().getDegrees(), DELTA); + assertEquals(11.983, sample5.getPose().translation().getY(), DELTA); + assertEquals(45.082, sample5.getCourse().getDegrees(), DELTA); assertEquals(60, sample5.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample6 = path.sample(60); assertEquals(36, sample6.getPose().translation().getX(), DELTA); - assertEquals(24, sample6.getPose().translation().getY(), DELTA); - assertEquals(0, sample6.getCourse().getDegrees(), DELTA); + assertEquals(23.983, sample6.getPose().translation().getY(), DELTA); + assertEquals(0.041, sample6.getCourse().getDegrees(), DELTA); assertEquals(60, sample6.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample72 = path.sample(72.0); - assertEquals(48, sample72.getPose().translation().getX(), DELTA); + assertEquals(47.937, sample72.getPose().translation().getX(), DELTA); assertEquals(24, sample72.getPose().translation().getY(), DELTA); // course should be +x assertEquals(0, sample72.getCourse().getDegrees(), DELTA); - assertEquals(120, sample72.getPose().heading().getDegrees(), DELTA); + assertEquals(119.687, sample72.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample8 = path.sample(84); - assertEquals(60, sample8.getPose().translation().getX(), DELTA); + assertEquals(59.892, sample8.getPose().translation().getX(), DELTA); assertEquals(24, sample8.getPose().translation().getY(), DELTA); assertEquals(0, sample8.getCourse().getDegrees(), DELTA); - assertEquals(180, sample8.getPose().heading().getDegrees(), DELTA); + assertEquals(179.460, sample8.getPose().heading().getDegrees(), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index b5313fc08..a46da425e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -12,6 +12,7 @@ import org.team100.lib.geometry.Pose2dWithDirection; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.testing.Timeless; +import org.team100.lib.trajectory.TrajectoryPlotter; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; @@ -102,6 +103,25 @@ void testLinear() { assertEquals(0, p.getHeadingRateRad_M(), DELTA); } + /** Turning in place works now. */ + @Test + void testSpin() { + List waypoints = List.of( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), + new DirectionSE2(0, 0, 1)), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kCCW_90deg), + new DirectionSE2(0, 0, 1))); + Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); + TrajectoryPlotter.plot(path, 0.1, 1); + } + + /** Hard corners work now. */ @Test void testActualCorner() { List waypoints = List.of( @@ -125,35 +145,37 @@ void testActualCorner() { new Translation2d(1, 1), new Rotation2d()), DirectionSE2.TO_Y)); - assertThrows(IllegalArgumentException.class, - () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); + Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); + TrajectoryPlotter.plot(path, 0.1, 1); } @Test void testComposite() { + // note none of these directions include rotation; it's all in between. List waypoints = List.of( new Pose2dWithDirection( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X), + new DirectionSE2(1, 0, 0)), new Pose2dWithDirection( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X), + new DirectionSE2(1, 0, 0)), new Pose2dWithDirection( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X), + new DirectionSE2(1, 0, 0)), new Pose2dWithDirection( new Pose2d( new Translation2d(2, 0), new Rotation2d(1)), - DirectionSE2.TO_X)); - assertThrows(IllegalArgumentException.class, - () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); + new DirectionSE2(1, 0, 0))); + Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); + TrajectoryPlotter.plot(foo, 0.1, 1); + assertEquals(4, foo.length(), 0.001); } @Test diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 3b2c38b4c..e97c05ef4 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -106,7 +106,10 @@ void testLinear2() { } @Test - void testRotation() { + void testRotationSoft() { + // move ahead 1m while rotation 1 rad to the left + // this has no rotation at the ends. + // the rotation rate is zero at the ends and much higher in the middle. HolonomicSpline s = new HolonomicSpline( new Pose2dWithDirection( new Pose2d( @@ -122,20 +125,76 @@ void testRotation() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); + if (DEBUG) + s.printSamples(); + // now that the magic numbers apply to the rotational scaling, the heading rate // is constant. Translation2d t = s.getPoint(0); assertEquals(0, t.getX(), DELTA); t = s.getPoint(1); assertEquals(1, t.getX(), DELTA); + + Pose2dWithMotion p = s.getPose2dWithMotion(0); + assertEquals(0, p.getPose().translation().getX(), DELTA); + assertEquals(0, p.getPose().heading().getRadians(), DELTA); + // initial rotation rate is zero + assertEquals(0, p.getHeadingRateRad_M(), DELTA); + + p = s.getPose2dWithMotion(0.5); + assertEquals(0.5, p.getPose().translation().getX(), DELTA); + assertEquals(0.5, p.getPose().heading().getRadians(), DELTA); + // high rotation rate in the middle + assertEquals(4.807, p.getHeadingRateRad_M(), DELTA); + + p = s.getPose2dWithMotion(1); + assertEquals(1, p.getPose().translation().getX(), DELTA); + assertEquals(1, p.getPose().heading().getRadians(), DELTA); + // rotation rate is zero at the end + assertEquals(0, p.getHeadingRateRad_M(), DELTA); + + } + + @Test + void testRotationFast() { + // move ahead 1m while rotation 1 rad to the left + // this has lots of rotation at the ends + // the "spatial" rotation rate is constant, i.e. + // rotation and translation speed are proportional. + HolonomicSpline s = new HolonomicSpline( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 1)), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 1))); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); + + s.printSamples(); + + // now that the magic numbers apply to the rotational scaling, the heading rate + // is constant. + Translation2d t = s.getPoint(0); + assertEquals(0, t.getX(), DELTA); + t = s.getPoint(1); + assertEquals(1, t.getX(), DELTA); + Pose2dWithMotion p = s.getPose2dWithMotion(0); assertEquals(0, p.getPose().translation().getX(), DELTA); assertEquals(0, p.getPose().heading().getRadians(), DELTA); assertEquals(1, p.getHeadingRateRad_M(), DELTA); + p = s.getPose2dWithMotion(0.5); assertEquals(0.5, p.getPose().translation().getX(), DELTA); assertEquals(0.5, p.getPose().heading().getRadians(), DELTA); assertEquals(1, p.getHeadingRateRad_M(), DELTA); + p = s.getPose2dWithMotion(1); assertEquals(1, p.getPose().translation().getX(), DELTA); assertEquals(1, p.getPose().heading().getRadians(), DELTA); @@ -157,15 +216,37 @@ void testRotation2() { new Translation2d(1, 0), new Rotation2d(-2.5)), DirectionSE2.TO_X)); - if (DEBUG) { - System.out.println("d, x, y, heading, course"); - for (double tt = 0; tt <= 1.0; tt += 0.01) { - var ss = s.getPose2dWithMotion(tt); - Pose2d pose = ss.getPose().pose(); - System.out.printf("%6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", - tt, pose.getX(), pose.getY(), pose.getRotation().getRadians(), ss.getCourse().getRadians()); - } - } + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); + } + + /** Turning in place splines work. */ + @Test + void spin() { + double magicNumber = 0.9; + HolonomicSpline s0 = new HolonomicSpline( + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), + new DirectionSE2(0, 0, 1)), + new Pose2dWithDirection( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kCCW_90deg), + new DirectionSE2(0, 0, 1)), + magicNumber, magicNumber); + + List splines = new ArrayList<>(); + splines.add(s0); + + // before + assertTrue(verifyC1(splines)); + // spline joints are C2 smooth (i.e. same curvature) + assertTrue(verifyC2(splines)); + + // optimize and compare + TrajectoryPlotter.plot(splines, 0.1, 1); } @Test @@ -173,58 +254,57 @@ void testCircle() { // four splines that make a circle should have nice even curvature and velocity // throughout. The circle is centered at zero, the heading always points there. // - // the spline code currently does not behave correctly, it wants the theta - // speed to be zero at each of the spline endpoints below. + // This now uses the specification of SE(2) "course" which includes rotation. // - // magic number of 1 makes something about 1.5% from a circle, close enough. - double magicNumber = 1.0; + // I fiddled with the magic number to make a pretty good circle. + double magicNumber = 0.9; HolonomicSpline s0 = new HolonomicSpline( new Pose2dWithDirection( new Pose2d( new Translation2d(1, 0), Rotation2d.k180deg), - DirectionSE2.TO_Y), + new DirectionSE2(0, 1, 1)), new Pose2dWithDirection( new Pose2d( new Translation2d(0, 1), Rotation2d.kCW_90deg), - DirectionSE2.MINUS_X), + new DirectionSE2(-1, 0, 1)), magicNumber, magicNumber); HolonomicSpline s1 = new HolonomicSpline( new Pose2dWithDirection( new Pose2d( new Translation2d(0, 1), Rotation2d.kCW_90deg), - DirectionSE2.MINUS_X), + new DirectionSE2(-1, 0, 1)), new Pose2dWithDirection( new Pose2d( new Translation2d(-1, 0), Rotation2d.kZero), - DirectionSE2.MINUS_Y), + new DirectionSE2(0, -1, 1)), magicNumber, magicNumber); HolonomicSpline s2 = new HolonomicSpline( new Pose2dWithDirection( new Pose2d( new Translation2d(-1, 0), Rotation2d.kZero), - DirectionSE2.MINUS_Y), + new DirectionSE2(0, -1, 1)), new Pose2dWithDirection( new Pose2d( new Translation2d(0, -1), Rotation2d.kCCW_90deg), - DirectionSE2.TO_X), + new DirectionSE2(1, 0, 1)), magicNumber, magicNumber); HolonomicSpline s3 = new HolonomicSpline( new Pose2dWithDirection( new Pose2d( new Translation2d(0, -1), Rotation2d.kCCW_90deg), - DirectionSE2.TO_X), + new DirectionSE2(1, 0, 1)), new Pose2dWithDirection( new Pose2d( new Translation2d(1, 0), Rotation2d.k180deg), - DirectionSE2.TO_Y), + new DirectionSE2(0, 1, 1)), magicNumber, magicNumber); List splines = new ArrayList<>(); splines.add(s0); @@ -232,28 +312,16 @@ void testCircle() { splines.add(s2); splines.add(s3); - // before optimization - - // spline joints are C1 smooth (i.e. same slope) + // before assertTrue(verifyC1(splines)); // spline joints are C2 smooth (i.e. same curvature) assertTrue(verifyC2(splines)); - checkCircle(splines, 0.020, 0.026); + checkCircle(splines, 0.011, 0.005); // optimize and compare TrajectoryPlotter.plot(splines, 0.1, 1); - // after optimization - - // spline joints are C1 smooth (i.e. same slope) - assertTrue(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) - assertTrue(verifyC2(splines)); - - // optimization very slightly reduces the range error but increases the azimuth - // error. - checkCircle(splines, 0.018, 0.041); } private void checkCircle(List splines, double rangeError, double azimuthError) { @@ -332,7 +400,7 @@ void testLine() { */ @Test void testPath0() { - double magicNumber = 1.0; + double magicNumber = 0.7; // turn a bit to the left Pose2dWithDirection p0 = new Pose2dWithDirection( new Pose2d( @@ -341,7 +409,7 @@ void testPath0() { new DirectionSE2(1, 0, 0)); Pose2dWithDirection p1 = new Pose2dWithDirection( new Pose2d( - new Translation2d(0.7, 0.3), + new Translation2d(0.707, 0.293), new Rotation2d(-1, 1)), new DirectionSE2(1, 1, -1)); Pose2dWithDirection p2 = new Pose2dWithDirection( @@ -350,8 +418,12 @@ void testPath0() { new Rotation2d(0, 1)), new DirectionSE2(0, 1, 0)); + if (DEBUG) + System.out.println("s01"); HolonomicSpline s01 = new HolonomicSpline( p0, p1, magicNumber, magicNumber); + if (DEBUG) + System.out.println("s12"); HolonomicSpline s12 = new HolonomicSpline( p1, p2, magicNumber, magicNumber); List splines = new ArrayList<>(); @@ -359,17 +431,13 @@ void testPath0() { splines.add(s12); // before - // spline joints are not C1 smooth assertTrue(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) assertTrue(verifyC2(splines)); TrajectoryPlotter.plot(splines, 0.1, 1); // after - // spline joints are not C1 smooth assertTrue(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) assertTrue(verifyC2(splines)); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java index 27716aa87..21ebd2c40 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java @@ -13,7 +13,7 @@ * */ public class SplineR1Test { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; /** * Making the end derivatives the average, with zero second derivative, makes diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index b93a580aa..a3cd1acb8 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -21,6 +21,7 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.trajectory.Trajectory100; +import org.team100.lib.trajectory.TrajectoryPlotter; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.TimingConstraint.MinMaxAcceleration; @@ -108,26 +109,33 @@ public void checkTrajectory( */ @Test void testJustTurningInPlace() { - Path100 path = new Path100(Arrays.asList( - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), 1, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(Math.PI)), 0), 1, 0, 0))); - // Triangle profile. - assertThrows(IllegalArgumentException.class, - () -> buildAndCheckTrajectory( - path, 1.0, new ArrayList(), 0.0, 0.0, 20.0, 5.0)); + Path100 path = new Path100(Arrays.asList( + new Pose2dWithMotion( + new Pose2dWithDirection( + new Pose2d(0, 0, new Rotation2d(0)), + new DirectionSE2(0, 0, 1)), + 1, 0, 0), + new Pose2dWithMotion( + new Pose2dWithDirection( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + new DirectionSE2(0, 0, 1)), + 1, 0, 0))); + if (DEBUG) + System.out.printf("PATH: %s\n", path); + TrajectoryPlotter.plot(path, 0.1, 1); + + List constraints = new ArrayList(); + ScheduleGenerator u = new ScheduleGenerator(constraints); + Trajectory100 timed_traj = u.timeParameterizeTrajectory( + path, + 1.0, + 0.0, + 0.0); + final Trajectory100 traj = timed_traj; - // Trapezoidal profile. - assertThrows(IllegalArgumentException.class, - () -> buildAndCheckTrajectory( - path, 1.0, new ArrayList(), 0.0, 0.0, 10.0, 5.0)); + assertTrue(traj.isEmpty()); - // Trapezoidal profile with start and end velocities. - assertThrows(IllegalArgumentException.class, - () -> buildAndCheckTrajectory( - path, 1.0, new ArrayList(), 5.0, 2.0, 10.0, 5.0)); } /** diff --git a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java index 939bb8527..5560ce9d8 100644 --- a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java @@ -87,12 +87,13 @@ void testSlog() { } @Test - void testDistance2() { - assertEquals(1, GeometryUtil.distanceM(new Translation2d(1, 0), new Translation2d(0, 0)), DELTA); + void testDistance2d() { + assertEquals(1, GeometryUtil.distanceM( + new Translation2d(1, 0), new Translation2d(0, 0)), DELTA); } @Test - void testDistance3() { + void testDistance3d() { assertEquals(1, GeometryUtil.distanceM( new Translation3d(1, 0, 0), new Translation3d(0, 0, 0)), DELTA); } @@ -135,7 +136,18 @@ void testDistance() { new Pose2d(1, 0, Rotation2d.kCCW_Pi_2), new Pose2d(0, 1, Rotation2d.kZero)), DELTA); - + // pure rotation yields zero distance, which isn't really what we want. + assertEquals(0, + GeometryUtil.distanceM( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(0, 0, Rotation2d.kCCW_90deg)), + DELTA); + // use double geodesic distance to fix that. + assertEquals(1.571, + GeometryUtil.doubleGeodesicDistance( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(0, 0, Rotation2d.kCCW_90deg)), + DELTA); } @Test From d085158bc64adfcc9cd71c2ec3954f8f8ba7d539 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 16 Dec 2025 09:25:46 -0800 Subject: [PATCH 07/42] add magic number to waypoint --- .../frc2025/CalgamesArm/CalgamesMech.java | 66 +++---- .../frc2025/CalgamesArm/MechTrajectories.java | 6 +- .../frc2025/Swerve/Auto/GoToCoralStation.java | 12 +- .../team100/frc2025/robot/LolipopAuto.java | 8 +- .../org/team100/frc2025/robot/Prewarmer.java | 14 +- .../CalgamesArm/TrajectoryJointTest.java | 10 +- .../team100/lib/geometry/AccelerationSE2.java | 5 - .../team100/lib/geometry/DirectionSE2.java | 30 +++- .../team100/lib/geometry/GeometryUtil.java | 13 +- .../org/team100/lib/geometry/JerkSE2.java | 20 --- .../lib/geometry/Pose2dWithDirection.java | 44 ----- .../lib/geometry/Pose2dWithMotion.java | 39 +++-- .../org/team100/lib/geometry/WaypointSE2.java | 91 ++++++++++ .../team100/lib/logging/LoggerFactory.java | 4 +- ...veToPoseWithTrajectoryAndExitVelocity.java | 12 +- .../r3/commands/GoToPosePosition.java | 10 +- .../tank/commands/ToPoseWithTrajectory.java | 6 +- .../lib/trajectory/TrajectoryPlanner.java | 76 +++----- .../lib/trajectory/path/PathFactory.java | 29 +--- .../path/spline/HolonomicSpline.java | 22 +-- .../trajectory/timing/JointConstraint.java | 4 +- .../lib/trajectory/timing/TimedPose.java | 16 +- .../TrajectoryVisualization.java | 4 +- .../r3/FullStateControllerR3Test.java | 76 ++++---- .../r3/ReferenceControllerR3Test.java | 16 +- .../DriveToPoseWithTrajectoryTest.java | 6 +- .../DriveWithTrajectoryFunctionTest.java | 8 +- .../subsystems/swerve/SwerveControlTest.java | 18 +- .../subsystems/swerve/SwerveModelTest.java | 18 +- .../lib/trajectory/Trajectory100Test.java | 46 ++--- .../lib/trajectory/TrajectoryPlannerTest.java | 32 ++-- .../lib/trajectory/TrajectoryTest.java | 28 +-- .../trajectory/TrajectoryToVectorSeries.java | 4 +- .../lib/trajectory/path/Path100Test.java | 46 ++--- .../lib/trajectory/path/PathFactoryTest.java | 109 ++++++------ .../path/spline/HolonomicSplineTest.java | 162 ++++++++---------- .../spline/QuinticHermiteOptimizerTest.java | 98 +++++------ ...CentripetalAccelerationConstraintTest.java | 22 +-- .../timing/ConstantConstraintTest.java | 10 +- .../timing/DiamondConstraintTest.java | 26 +-- .../timing/JointConstraintTest.java | 22 +-- .../timing/ScheduleGeneratorTest.java | 36 ++-- .../SwerveDriveDynamicsConstraintTest.java | 22 +-- .../lib/trajectory/timing/TimedStateTest.java | 10 +- .../timing/TorqueConstraintTest.java | 26 +-- .../timing/TrajectoryVelocityProfileTest.java | 10 +- .../VelocityLimitRegionConstraintTest.java | 10 +- .../timing/YawRateConstraintTest.java | 16 +- 48 files changed, 709 insertions(+), 709 deletions(-) delete mode 100644 lib/src/main/java/org/team100/lib/geometry/JerkSE2.java delete mode 100644 lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java index f54e734ae..9ba192f23 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java @@ -14,7 +14,7 @@ import org.team100.lib.config.PIDConstants; import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.Pose2dWithDirection; +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; @@ -490,87 +490,87 @@ public Command processorWithProfile() { public MoveAndHold homeToL1() { return m_transit.endless("homeToL1", - Pose2dWithDirection.make(m_home, -1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(L2, 1.3), - Pose2dWithDirection.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", - Pose2dWithDirection.make(m_home, 1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(L2, -1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(m_home, 0.8), - Pose2dWithDirection.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", - Pose2dWithDirection.make(L3, -1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(m_home, 0.1), - Pose2dWithDirection.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", - Pose2dWithDirection.make(m_home, 0.1), - Pose2dWithDirection.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", - Pose2dWithDirection.make(L4, -1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(L4_BACK, 1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(m_home, 1.5), - Pose2dWithDirection.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", - Pose2dWithDirection.make(m_home, 0), - Pose2dWithDirection.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", - Pose2dWithDirection.make(ALGAE_L2, -1.0), - Pose2dWithDirection.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", - Pose2dWithDirection.make(ALGAE_L3, -1.0), - Pose2dWithDirection.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 level) { */ public MoveAndHold homeToBarge() { return m_transit.endless("homeToBarge", - Pose2dWithDirection.make(m_home, 0), - Pose2dWithDirection.make(BARGE, -1)); + WaypointSE2.irrotational(m_home, 0, 1.2), + WaypointSE2.irrotational(BARGE, -1, 1.2)); } public MoveAndHold bargeToHome() { return m_transit.endless("bargeToHome", - Pose2dWithDirection.make(BARGE, 2.5), - Pose2dWithDirection.make(m_home, Math.PI)); + WaypointSE2.irrotational(BARGE, 2.5, 1.2), + WaypointSE2.irrotational(m_home, Math.PI, 1.2)); } diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java index 0de6d0537..95b4d620f 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -4,7 +4,7 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.geometry.Pose2dWithDirection; +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; @@ -62,7 +62,7 @@ public MechTrajectories( } /** A command that goes from the start to the end and then finishes. */ - public Command terminal(String name, Pose2dWithDirection start, Pose2dWithDirection 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( @@ -73,7 +73,7 @@ public Command terminal(String name, Pose2dWithDirection start, Pose2dWithDirect } /** A command that goes from the start to the end and then waits forever. */ - public MoveAndHold endless(String name, Pose2dWithDirection start, Pose2dWithDirection 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( diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index 6e3ca15e3..99db5f03b 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -8,7 +8,7 @@ import org.team100.lib.field.FieldConstants.CoralStation; import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +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; @@ -53,17 +53,17 @@ public Trajectory100 apply(Pose2d currentPose) { Rotation2d newInitialSpline = FieldConstants.calculateDeltaSpline( courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust); - List waypoints = new ArrayList<>(); + List waypoints = new ArrayList<>(); waypoints.add( - new Pose2dWithDirection( + new WaypointSE2( currentPose, DirectionSE2.fromDirections( - DirectionR2.fromRotation(newInitialSpline), 0))); + DirectionR2.fromRotation(newInitialSpline), 0), 1)); waypoints.add( - new Pose2dWithDirection( + new WaypointSE2( goal, DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0))); + DirectionR2.fromRotation(courseToGoal), 0), 1)); return m_planner.restToRest(waypoints); } diff --git a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java index ea8903eea..df838b5d8 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java +++ b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java @@ -12,7 +12,7 @@ import org.team100.lib.controller.r3.FullStateControllerR3; import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.ReefPoint; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.r3.ProfileR3; import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; @@ -52,9 +52,9 @@ public Command get() { DriveWithTrajectoryFunction toReefTrajectory = new DriveWithTrajectoryFunction( m_log, m_machinery.m_drive, m_autoController, m_machinery.m_trajectoryViz, (p) -> m_planner.restToRest(List.of( - Pose2dWithDirection.make(m_machinery.m_drive.getPose(), Math.PI), - Pose2dWithDirection.make( - new Pose2d(3, 5, new Rotation2d(0)), -2)))); + WaypointSE2.irrotational(m_machinery.m_drive.getPose(), Math.PI, 1.2), + WaypointSE2.irrotational( + new Pose2d(3, 5, new Rotation2d(0)), -2, 1.2)))); DriveToPoseWithProfile toReefA = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, diff --git a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java index 8e3a2f606..1cc43ea65 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java @@ -5,7 +5,7 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; @@ -29,13 +29,15 @@ public static void init(Machinery machinery) { double startS = Takt.actual(); // Exercise the trajectory planner. - List waypoints = new ArrayList<>(); - waypoints.add(new Pose2dWithDirection( + List waypoints = new ArrayList<>(); + waypoints.add(new WaypointSE2( new Pose2d(new Translation2d(), Rotation2d.kZero), - DirectionSE2.TO_X)); - waypoints.add(new Pose2dWithDirection( + DirectionSE2.TO_X, + 1)); + waypoints.add(new WaypointSE2( new Pose2d(new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, + 1)); TrajectoryPlanner planner = new TrajectoryPlanner( new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger)); planner.restToRest(waypoints); diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java index 342e4d0bf..623860b1c 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.AccelerationSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -50,10 +50,10 @@ void homeToL4() { TrajectoryPlanner m_planner = new TrajectoryPlanner(c); Trajectory100 t = m_planner.restToRest(List.of( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(0)), 0), - Pose2dWithDirection.make( - new Pose2d(1.9, 0.5, new Rotation2d(2.5)), 2))); + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), + WaypointSE2.irrotational( + new Pose2d(1.9, 0.5, new Rotation2d(2.5)), 2, 1.2))); ElevatorArmWristKinematics k = new ElevatorArmWristKinematics( 0.5, 0.3); diff --git a/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java b/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java index 33053e034..24055233d 100644 --- a/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java @@ -28,11 +28,6 @@ public AccelerationSE2 minus(AccelerationSE2 other) { return new AccelerationSE2(x - other.x, y - other.y, theta - other.theta); } - public JerkSE2 jerk(AccelerationSE2 previous, double dt) { - AccelerationSE2 v = minus(previous).div(dt); - return new JerkSE2(v.x(), v.y(), v.theta()); - } - public AccelerationSE2 times(double scalar) { return new AccelerationSE2(x * scalar, y * scalar, theta * scalar); } diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index 7d6b32ec4..1bb77730b 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -1,5 +1,7 @@ package org.team100.lib.geometry; +import org.team100.lib.util.Math100; + import edu.wpi.first.math.geometry.Rotation2d; /** @@ -11,6 +13,8 @@ * It is exactly a unit-length Twist2d. */ public class DirectionSE2 { + private static final boolean DEBUG = false; + public final double x; public final double y; public final double theta; @@ -43,10 +47,28 @@ public static final DirectionSE2 fromDirections(DirectionR2 course, double theta @Override public boolean equals(Object obj) { - return obj instanceof DirectionSE2 other - && Math.abs(other.x - x) < 1E-9 - && Math.abs(other.y - y) < 1E-9 - && Math.abs(other.theta - theta) < 1E-9; + if (!(obj instanceof DirectionSE2)) { + if (DEBUG) + System.out.println("wrong type"); + return false; + } + DirectionSE2 other = (DirectionSE2) obj; + if (!Math100.epsilonEquals(other.x, x)) { + if (DEBUG) + System.out.println("wrong x"); + return false; + } + if (!Math100.epsilonEquals(other.y, y)) { + if (DEBUG) + System.out.println("wrong y"); + return false; + } + if (!Math100.epsilonEquals(other.theta, theta)) { + if (DEBUG) + System.out.println("wrong theta"); + return false; + } + return true; } @Override diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 60aac3982..995619c50 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -213,13 +213,14 @@ public static DirectionSE2 interpolate(DirectionSE2 a, DirectionSE2 b, double x) MathUtil.interpolate(a.theta, b.theta, x)); } - public static Pose2dWithDirection interpolate( - Pose2dWithDirection a, - Pose2dWithDirection b, + public static WaypointSE2 interpolate( + WaypointSE2 a, + WaypointSE2 b, double x) { - return new Pose2dWithDirection( + return new WaypointSE2( interpolate(a.pose(), b.pose(), x), - interpolate(a.course(), b.course(), x)); + interpolate(a.course(), b.course(), x), + MathUtil.interpolate(a.scale(), b.scale(), x)); } /** @@ -302,7 +303,7 @@ public static double doubleGeodesicDistance(Pose2dWithMotion a, Pose2dWithMotion return doubleGeodesicDistance(a.getPose(), b.getPose()); } - public static double doubleGeodesicDistance(Pose2dWithDirection a, Pose2dWithDirection b) { + public static double doubleGeodesicDistance(WaypointSE2 a, WaypointSE2 b) { return doubleGeodesicDistance(a.pose(), b.pose()); } diff --git a/lib/src/main/java/org/team100/lib/geometry/JerkSE2.java b/lib/src/main/java/org/team100/lib/geometry/JerkSE2.java deleted file mode 100644 index efaee487e..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/JerkSE2.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.team100.lib.geometry; - -/** - * Third time-derivative of Pose2d. - * - * Uses R3 tangent, not SE(2) manifold. - */ -public record JerkSE2(double x, double y, double theta) { - public double norm() { - return Math.hypot(x, y); - } - - public JerkSE2 times(double scalar) { - return new JerkSE2(x * scalar, y * scalar, theta * scalar); - } - - public AccelerationSE2 integrate(double dtSec) { - return new AccelerationSE2(x * dtSec, y * dtSec, theta * dtSec); - } -} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java deleted file mode 100644 index f632256c5..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithDirection.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.team100.lib.geometry; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * For constructing splines, paths, and trajectories. - * - * This is similar to ModelR3, except it doesn't have the notion of velocity, - * only direction. - * - * @param pose location and orientation - * @param course direction of travel (includes rotation) - */ -public record Pose2dWithDirection( - Pose2d pose, - DirectionSE2 course) { - - public Translation2d translation() { - return pose.getTranslation(); - } - - public Rotation2d heading() { - return pose.getRotation(); - } - - /** Course without rotation */ - public static Pose2dWithDirection make(Pose2d p, double course) { - return new Pose2dWithDirection( - p, - new DirectionSE2(Math.cos(course), Math.sin(course), 0)); - } - - /** - * For tank drive, heading and course are the same. This is like the WPI - * trajectory. - */ - public static Pose2dWithDirection tank(Pose2d p) { - Rotation2d r = p.getRotation(); - return new Pose2dWithDirection( - p, new DirectionSE2(r.getCos(), r.getSin(), 0)); - } -} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 8a5e29e86..275db4fb6 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -6,15 +6,16 @@ import edu.wpi.first.math.geometry.Rotation2d; /** - * Pose2dWithDirection with: + * WaypointSE2 with: * * * the spatial rate of change in heading * * the spatial rate of change in course * * the spatial rate of change in course curvature */ public class Pose2dWithMotion { + private static final boolean DEBUG = false; /** Pose and course. */ - private final Pose2dWithDirection m_pose; + private final WaypointSE2 m_pose; /** Change in heading per meter of motion, rad/m. */ private final double m_headingRate; /** Change in course per change in distance, rad/m. */ @@ -23,14 +24,13 @@ public class Pose2dWithMotion { private final double m_dCurvatureDsRad_M2; /** - * @param pose location and heading of the robot - * @param course motion direction, radians + * @param pose location and heading and direction of travel * @param headingRate change in heading, per meter traveled * @param curvatureRad_M change in course per meter traveled. * @param dCurvatureDsRad_M2 acceleration in course per meter traveled squared. */ public Pose2dWithMotion( - Pose2dWithDirection pose, + WaypointSE2 pose, double headingRate, double curvatureRad_M, double dCurvatureDsRad_M2) { @@ -40,7 +40,7 @@ public Pose2dWithMotion( m_dCurvatureDsRad_M2 = dCurvatureDsRad_M2; } - public Pose2dWithDirection getPose() { + public WaypointSE2 getPose() { return m_pose; } @@ -84,14 +84,33 @@ public double distanceM(final Pose2dWithMotion other) { public boolean equals(final Object other) { if (!(other instanceof Pose2dWithMotion)) { + if (DEBUG) + System.out.println("wrong type"); return false; } Pose2dWithMotion p2dwc = (Pose2dWithMotion) other; - return m_pose.equals(p2dwc.m_pose) && - Math100.epsilonEquals(m_headingRate, p2dwc.m_headingRate) && - Math100.epsilonEquals(m_curvatureRad_M, p2dwc.m_curvatureRad_M) && - Math100.epsilonEquals(m_dCurvatureDsRad_M2, p2dwc.m_dCurvatureDsRad_M2); + if (!m_pose.equals(p2dwc.m_pose)) { + if (DEBUG) + System.out.println("wrong waypoint"); + return false; + } + if (!Math100.epsilonEquals(m_headingRate, p2dwc.m_headingRate)) { + if (DEBUG) + System.out.println("wrong heading rate"); + return false; + } + if (!Math100.epsilonEquals(m_curvatureRad_M, p2dwc.m_curvatureRad_M)) { + if (DEBUG) + System.out.println("wrong curvature"); + return false; + } + if (!Math100.epsilonEquals(m_dCurvatureDsRad_M2, p2dwc.m_dCurvatureDsRad_M2)) { + if (DEBUG) + System.out.println("wrong dcurvature"); + return false; + } + return true; } public String toString() { diff --git a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java new file mode 100644 index 000000000..f3aebe095 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java @@ -0,0 +1,91 @@ +package org.team100.lib.geometry; + +import org.team100.lib.util.Math100; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * For constructing splines. + * + * The scale factor is somewhat like "velocity" but in the spline constructor it + * is scaled to the total length of the spline, so it affects the shape in a + * scale-invariant way. A common scale is 1.2, but 1.0 is often useful. I've + * used 0.9 to make pretty good circles from four splines. Elsewhere this factor + * is sometimes called the "magic number". + * + * Our waypoints have no notion of acceleration, which means the joints in the + * resulting trajectories are (very briefly) laterally "unloaded". I don't think + * this makes any measurable difference in real-world movement. + * + * It would be possible to have different scale factors for each side of the + * waypoint (so one side could be straighter and the other side could be + * curvier), but I don't think it would be worth the complexity. + * + * @param pose location and orientation + * @param course direction of travel (in SE(2), course includes rotation) + * @param scale influence of the course, relative to the spline parameter [0,1] + */ +public record WaypointSE2(Pose2d pose, DirectionSE2 course, double scale) { + private static final boolean DEBUG = false; + + public Translation2d translation() { + return pose.getTranslation(); + } + + public Rotation2d heading() { + return pose.getRotation(); + } + + /** Course without rotation, with unit scale. */ + public static WaypointSE2 irrotational(Pose2d p, double course, double scale) { + return new WaypointSE2(p, new DirectionSE2(Math.cos(course), Math.sin(course), 0), scale); + } + + /** + * For tank drive, heading and course are the same, with unit scale. + * This is like the WPI non-holonomic trajectory. + */ + public static WaypointSE2 tank(Pose2d p) { + Rotation2d r = p.getRotation(); + return new WaypointSE2(p, new DirectionSE2(r.getCos(), r.getSin(), 0), 1.0); + } + + @Override + public String toString() { + return String.format("%s %s %5.3f", pose, course, scale); + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof WaypointSE2)) { + if (DEBUG) + System.out.println("wrong type"); + return false; + } + + WaypointSE2 other = (WaypointSE2) obj; + + if (!pose.equals(other.pose)) { + if (DEBUG) + System.out.println("wrong pose"); + return false; + } + + if (!course.equals(other.course)) { + if (DEBUG) + System.out.println("wrong course"); + return false; + } + + if (!Math100.epsilonEquals(scale, other.scale)) { + if (DEBUG) + System.out.printf("wrong scale %s %s\n", scale, other.scale); + return false; + } + + return true; + } + +} diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java index e5229a6bf..e053465a8 100644 --- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java +++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java @@ -10,7 +10,7 @@ import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.GlobalVelocityR2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.localization.Blip24; @@ -515,7 +515,7 @@ public class Pose2dWithMotionLogger { public void log(Supplier vals) { if (!allow(m_level)) return; - Pose2dWithDirection val = vals.get().getPose(); + WaypointSE2 val = vals.get().getPose(); m_pose2dLogger.log(val::pose); m_rotation2dLogger.log(() -> val.course().toRotation()); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java index 8a2c9d47e..abd145993 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java @@ -7,7 +7,7 @@ import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -57,18 +57,20 @@ public void initialize() { Pose2d pose = m_drive.getState().pose(); Translation2d toGoal = m_goal.getTranslation().minus(pose.getTranslation()); VelocitySE2 startVelocity = m_drive.getState().velocity(); - Pose2dWithDirection startWaypoint = new Pose2dWithDirection( + WaypointSE2 startWaypoint = new WaypointSE2( pose, DirectionSE2.fromDirections( DirectionR2.fromRotation( startVelocity.angle().orElse(toGoal.getAngle())), - 0)); - Pose2dWithDirection endWaypoint = new Pose2dWithDirection( + 0), + 1); + WaypointSE2 endWaypoint = new WaypointSE2( m_goal, DirectionSE2.fromDirections( DirectionR2.fromRotation( m_endVelocity.angle().orElse(toGoal.getAngle())), - 0)); + 0), + 1); Trajectory100 trajectory = m_planner.generateTrajectory( List.of(startWaypoint, endWaypoint), startVelocity.norm(), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java index 42eeb1338..9aa68aa33 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java @@ -5,7 +5,7 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; import org.team100.lib.subsystems.r3.PositionSubsystemR3; @@ -22,7 +22,7 @@ public class GoToPosePosition extends MoveAndHold { private final LoggerFactory m_log; private final PositionSubsystemR3 m_subsystem; - private final Pose2dWithDirection m_goal; + private final WaypointSE2 m_goal; private final Rotation2d m_course; private final TrajectoryPlanner m_trajectoryPlanner; @@ -32,7 +32,7 @@ public GoToPosePosition( LoggerFactory parent, PositionSubsystemR3 subsystem, Rotation2d course, - Pose2dWithDirection goal, + WaypointSE2 goal, TrajectoryPlanner trajectoryPlanner) { m_log = parent.type(this); m_subsystem = subsystem; @@ -44,10 +44,10 @@ public GoToPosePosition( @Override public void initialize() { - Pose2dWithDirection m_currentPose = new Pose2dWithDirection( + WaypointSE2 m_currentPose = new WaypointSE2( m_subsystem.getState().pose(), DirectionSE2.fromDirections( - DirectionR2.fromRotation(m_course), 0)); + DirectionR2.fromRotation(m_course), 0), 1); Trajectory100 m_trajectory = m_trajectoryPlanner.restToRest( List.of(m_currentPose, m_goal)); m_referenceController = new PositionReferenceControllerR3( diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java index 4e72ca4e8..bd7bc86e8 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java @@ -4,7 +4,7 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.tank.TankDrive; import org.team100.lib.trajectory.Trajectory100; @@ -49,8 +49,8 @@ public void initialize() { m_startTimeS = Takt.get(); // may return null m_trajectory = m_planner.restToRest(List.of( - Pose2dWithDirection.tank(m_drive.getPose()), - Pose2dWithDirection.tank(m_goal))); + WaypointSE2.tank(m_drive.getPose()), + WaypointSE2.tank(m_goal))); m_viz.setViz(m_trajectory); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 017cac682..16c066564 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -5,7 +5,7 @@ import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.path.Path100; @@ -103,14 +103,10 @@ public Trajectory100 line(Pose2d initial) { initial.plus(new Transform2d(1, 0, Rotation2d.kZero))); } - public Trajectory100 restToRest(List waypoints) { + public Trajectory100 restToRest(List waypoints) { return generateTrajectory(waypoints, 0.0, 0.0); } - public Trajectory100 restToRest(List waypoints, List mN) { - return generateTrajectory(waypoints, 0.0, 0.0, mN); - } - public Trajectory100 movingToRest(ModelR3 startState, Pose2d end) { return movingToMoving(startState, new ModelR3(end)); } @@ -134,22 +130,22 @@ public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { // use the start velocity to adjust the first magic number. // divide by the distance because the spline multiplies by it double e1 = 2.0 * startVelocity.norm() / full.getNorm(); - List magicNumbers = List.of(e1, 1.2); try { return generateTrajectory( List.of( - new Pose2dWithDirection( + new WaypointSE2( startState.pose(), DirectionSE2.fromDirections( - DirectionR2.fromRotation(startingAngle), 0)), - new Pose2dWithDirection( + DirectionR2.fromRotation(startingAngle), 0), + e1), + new WaypointSE2( endState.pose(), DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0))), + DirectionR2.fromRotation(courseToGoal), 0), + 1.2)), startVelocity.norm(), - endVelocity.norm(), - magicNumbers); + endVelocity.norm()); } catch (TrajectoryGenerationException e) { System.out.println("WARNING: Trajectory Generation Exception"); return new Trajectory100(); @@ -174,22 +170,22 @@ public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, // use the start velocity to adjust the first magic number. // divide by the distance because the spline multiplies by it double e1 = 2.0 * startVelocity.norm() / full.getNorm(); - List magicNumbers = List.of(e1, 1.2); try { return generateTrajectory( List.of( - new Pose2dWithDirection( + new WaypointSE2( startState.pose(), DirectionSE2.fromDirections( - DirectionR2.fromRotation(startCourse), 0)), - new Pose2dWithDirection( + DirectionR2.fromRotation(startCourse), 0), + e1), + new WaypointSE2( endState.pose(), DirectionSE2.fromDirections( - DirectionR2.fromRotation(endCourse), 0))), + DirectionR2.fromRotation(endCourse), 0), + 1.2)), splineEntranceVelocity, - splineExitVelocity, - magicNumbers); + splineExitVelocity); } catch (TrajectoryGenerationException e) { System.out.println("WARNING: Trajectory Generation Exception"); return new Trajectory100(); @@ -214,14 +210,16 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { try { return restToRest( List.of( - new Pose2dWithDirection( + new WaypointSE2( start, DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0)), - new Pose2dWithDirection( + DirectionR2.fromRotation(courseToGoal), 0), + 1), + new WaypointSE2( end, DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0)))); + DirectionR2.fromRotation(courseToGoal), 0), + 1))); } catch (TrajectoryGenerationException e) { return null; } @@ -231,7 +229,7 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { * The shape of the spline accommodates the start and end velocities. */ public Trajectory100 generateTrajectory( - List waypoints, + List waypoints, double start_vel, double end_vel) { try { @@ -256,32 +254,4 @@ public Trajectory100 generateTrajectory( return new Trajectory100(); } } - - public Trajectory100 generateTrajectory( - List waypoints, - double start_vel, - double end_vel, - List mN) { - try { - // Create a path from splines. - Path100 path = PathFactory.pathFromWaypoints( - waypoints, - m_splineTolerance, - m_splineTolerance, - m_splineRotationTolerance, - mN); - // Generate the timed trajectory. - return m_scheduleGenerator.timeParameterizeTrajectory( - path, - m_trajectoryStep, - start_vel, - end_vel); - } catch (IllegalArgumentException e) { - // catches various kinds of malformed input, returns a no-op. - // this should never actually happen. - System.out.println("WARNING: Bad trajectory input!!"); - e.printStackTrace(); - return new Trajectory100(); - } - } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 860e17a4e..9cda0f56d 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -4,7 +4,7 @@ import java.util.List; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.path.spline.HolonomicSpline; @@ -16,36 +16,13 @@ public class PathFactory { private static final boolean DEBUG = false; public static Path100 pathFromWaypoints( - List waypoints, - double maxDx, - double maxDy, - double maxDTheta, - List magicNumbers) { - List splines = new ArrayList<>(waypoints.size() - 1); - for (int i = 1; i < waypoints.size(); ++i) { - splines.add( - new HolonomicSpline( - waypoints.get(i - 1), - waypoints.get(i), - magicNumbers.get(i - 1), - magicNumbers.get(i))); - } - return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); - } - - public static Path100 pathFromWaypoints( - List waypoints, + List waypoints, double maxDx, double maxDy, double maxDTheta) { List splines = new ArrayList<>(waypoints.size() - 1); for (int i = 1; i < waypoints.size(); ++i) { - HolonomicSpline s = new HolonomicSpline( - waypoints.get(i - 1), - waypoints.get(i)); - if (DEBUG) - System.out.printf("%d %s\n", i, s); - splines.add(s); + splines.add(new HolonomicSpline(waypoints.get(i - 1), waypoints.get(i))); } return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index cda409a99..fe0365cec 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -4,7 +4,7 @@ import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import edu.wpi.first.math.geometry.Pose2d; @@ -40,14 +40,6 @@ public class HolonomicSpline { */ private final Rotation2d m_heading0; - /** - * The theta endpoint derivative is just the average theta rate, which is new, - * it used to be zero. - */ - public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1) { - this(p0, p1, 1.2, 1.2); - } - /** * Specify the magic number you want: this scales the derivatives at the * endpoints, i.e. how "strongly" the derivative affects the curve. High magic @@ -68,18 +60,16 @@ public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1) { * * @param p0 starting pose * @param p1 ending pose - * @param mN0 starting magic number - * @param mN1 ending magic number */ - public HolonomicSpline(Pose2dWithDirection p0, Pose2dWithDirection p1, double mN0, double mN1) { + public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { // Distance metric includes both translation and rotation. This is not // the geodesic distance, which is zero for spin-in-place. It's just the // L2 norm for all three dimensions. double distance = GeometryUtil.doubleGeodesicDistance(p0.pose(), p1.pose()); if (DEBUG) System.out.printf("distance %f\n", distance); - double scale0 = mN0 * distance; - double scale1 = mN1 * distance; + double scale0 = p0.scale() * distance; + double scale1 = p1.scale() * distance; if (DEBUG) { System.out.printf("scale %f %f\n", scale0, scale1); @@ -125,9 +115,9 @@ public String toString() { */ public Pose2dWithMotion getPose2dWithMotion(double p) { return new Pose2dWithMotion( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d(getPoint(p), getHeading(p)), - getCourse(p)), + getCourse(p), 1), getDHeadingDs(p), getCurvature(p), getDCurvatureDs(p)); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java index 17cf57d8b..23d102615 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java @@ -2,7 +2,7 @@ import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.state.ControlR3; import org.team100.lib.state.ModelR3; @@ -46,7 +46,7 @@ public JointConstraint( @Override public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - Pose2dWithDirection pose = state.getPose(); + WaypointSE2 pose = state.getPose(); // Velocity if translation speed were 1.0 m/s. VelocitySE2 v = new VelocitySE2( state.getCourse().getCos(), diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java index 6102dcede..572a2c7cd 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java @@ -1,6 +1,7 @@ package org.team100.lib.trajectory.timing; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.util.Math100; import edu.wpi.first.math.MathUtil; @@ -11,6 +12,7 @@ * The timing fields are set by the ScheduleGenerator. */ public class TimedPose { + private static final boolean DEBUG = false; private final Pose2dWithMotion m_state; /** Time we achieve this state. */ private final double m_timeS; @@ -91,13 +93,21 @@ public double distance(TimedPose other) { @Override public boolean equals(final Object other) { if (!(other instanceof TimedPose)) { + if (DEBUG) + System.out.println("wrong type"); return false; } TimedPose ts = (TimedPose) other; - boolean stateEqual = m_state.equals(ts.m_state); - if (!stateEqual) { + if (!m_state.equals(ts.m_state)) { + if (DEBUG) + System.out.println("wrong state"); return false; } - return Math.abs(m_timeS - ts.m_timeS) <= 1e-12; + if (!Math100.epsilonEquals(m_timeS, ts.m_timeS)) { + if (DEBUG) + System.out.println("wrong time"); + return false; + } + return true; } } diff --git a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java index 81c6b030c..9eb45bf51 100644 --- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java +++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java @@ -2,7 +2,7 @@ import java.util.List; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; @@ -32,7 +32,7 @@ private static double[] fromTrajectory100(Trajectory100 m_trajectory) { double[] arr = new double[m_trajectory.length() * 3]; int ndx = 0; for (TimedPose p : m_trajectory.getPoints()) { - Pose2dWithDirection pose = p.state().getPose(); + WaypointSE2 pose = p.state().getPose(); arr[ndx + 0] = pose.translation().getX(); arr[ndx + 1] = pose.translation().getY(); arr[ndx + 2] = pose.heading().getDegrees(); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java index 8a7eb165e..0ce9a430a 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java @@ -6,7 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DeltaSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; @@ -180,8 +180,8 @@ void testErrZero() { ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); @@ -197,8 +197,8 @@ void testErrXAhead() { ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); @@ -213,8 +213,8 @@ void testErrXBehind() { ModelR3 measurement = new ModelR3(new Pose2d(0, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(1, err.getX(), 0.001); @@ -230,8 +230,8 @@ void testErrXAheadWithRotation() { ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d(1))); ModelR3 currentReference = ModelR3 .fromTimedPose(new TimedPose(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(1)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(1)), 0, 1.2), 0, 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); @@ -249,8 +249,8 @@ void testErrorAhead() { // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, // no curvature 0); // no change in curvature @@ -276,8 +276,8 @@ void testErrorSideways() { // motion is in a straight line, down the x axis // setpoint is +x, facing down y Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), 0), + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), 0, 1.2), 0, 0, // no curvature 0); // no change in curvature @@ -304,7 +304,7 @@ void testVelocityErrorZero() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make(new Pose2d(0, 0, new Rotation2d(0)), 0), 0, + WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, // no curvature 0); // no change in curvature double t = 0; @@ -332,8 +332,8 @@ void testVelocityErrorAhead() { // motion is in a straight line, down the x axis // at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, // no curvature 0); // no change in curvature @@ -358,8 +358,8 @@ void testFeedForwardAhead() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, // no curvature 0);// no change in curvature @@ -384,8 +384,8 @@ void testFeedForwardSideways() { // motion is in a straight line, down the x axis // setpoint is the same Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2), 0, 0, // no curvature 0); // no change in curvature @@ -410,8 +410,8 @@ void testFeedForwardTurning() { // motion is tangential to the x axis but turning left // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, 1, // driving and turning 0); // no change in curvature @@ -435,8 +435,8 @@ void testFeedbackAhead() { // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); // setpoint is also at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -472,8 +472,8 @@ void testFeedbackAheadPlusY() { // measurement is plus-Y, facing ahead Pose2d currentState = new Pose2d(0, 1, Rotation2d.kZero); // setpoint is at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -509,8 +509,8 @@ void testFeedbackAheadPlusTheta() { // measurement is plus-theta Pose2d currentState = new Pose2d(0, 0, new Rotation2d(1.0)); // setpoint is also at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -549,8 +549,8 @@ void testFeedbackSideways() { // measurement is at the origin, facing down the y axis Pose2d currentState = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint is the same - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -585,8 +585,8 @@ void testFeedbackSidewaysPlusY() { // measurement is plus-y, facing down the y axis Pose2d currentState = new Pose2d(0, 1, Rotation2d.kCCW_Pi_2); // setpoint is parallel at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -621,8 +621,8 @@ void testFullFeedbackAhead() { // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); // setpoint is also at the origin - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -660,8 +660,8 @@ void testFullFeedbackSideways() { // measurement is at the origin, facing +y Pose2d currentPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint postion is the same - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; @@ -704,8 +704,8 @@ void testFullFeedbackSidewaysWithRotation() { new VelocitySE2(0.5, 0, 0)); // setpoint postion is ahead in x and y and theta - Pose2dWithDirection setpointPose = Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0); + WaypointSE2 setpointPose = WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2); // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; diff --git a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java index 4e44f8eba..d6a93121a 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java @@ -11,7 +11,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -130,22 +130,22 @@ void testTrajectoryDone() { @Test void testFieldRelativeTrajectory() { - List waypoints = new ArrayList<>(); - waypoints.add(new Pose2dWithDirection( + List waypoints = new ArrayList<>(); + waypoints.add(new WaypointSE2( new Pose2d( new Translation2d(), Rotation2d.fromDegrees(180)), - DirectionSE2.TO_X)); - waypoints.add(new Pose2dWithDirection( + DirectionSE2.TO_X, 1)); + waypoints.add(new WaypointSE2( new Pose2d( new Translation2d(100, 4), Rotation2d.fromDegrees(180)), - DirectionSE2.TO_X)); - waypoints.add(new Pose2dWithDirection( + DirectionSE2.TO_X, 1)); + waypoints.add(new WaypointSE2( new Pose2d( new Translation2d(196, 13), Rotation2d.fromDegrees(0)), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); double start_vel = 0.0; double end_vel = 0.0; diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java index 1a80e80c5..2a03a2a9d 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java @@ -8,7 +8,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.controller.r3.ControllerFactoryR3; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.logging.LoggerFactory; @@ -57,8 +57,8 @@ void testSimple() throws IOException { List.of( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 0))), controller, diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java index 55a40f135..210712908 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java @@ -5,7 +5,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.controller.r3.ControllerFactoryR3; import org.team100.lib.controller.r3.FullStateControllerR3; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -47,9 +47,9 @@ public class DriveWithTrajectoryFunctionTest implements Timeless { Trajectory100 makeTrajectory(Pose2d startingPose) { return planner.restToRest( List.of( - Pose2dWithDirection.make(startingPose, 0), - Pose2dWithDirection.make( - new Pose2d(1, 2, new Rotation2d(Math.PI / 2)), Math.PI / 2))); + WaypointSE2.irrotational(startingPose, 0, 1.2), + WaypointSE2.irrotational( + new Pose2d(1, 2, new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2))); } @Test diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index aa7df059f..0abcee354 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ControlR3; @@ -30,8 +30,8 @@ void testTimedPose() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); @@ -47,8 +47,8 @@ void testTimedPose2() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); @@ -64,8 +64,8 @@ void testTimedPose3() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); @@ -82,8 +82,8 @@ void testTimedPose4() { ControlR3 s = ControlR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index 0a87cfea4..16be48ce7 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -4,7 +4,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; @@ -31,8 +31,8 @@ void testTimedPose() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); @@ -48,8 +48,8 @@ void testTimedPose2() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); @@ -65,8 +65,8 @@ void testTimedPose3() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); @@ -83,8 +83,8 @@ void testTimedPose4() { ModelR3 s = ModelR3.fromTimedPose( new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 9b060f2e1..9e7305c23 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -7,7 +7,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -38,15 +38,15 @@ void testPreviewAndAdvance() { Translation2d goalTranslation = end.getTranslation(); Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( start, DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0)), - new Pose2dWithDirection( + DirectionR2.fromRotation(angleToGoal), 0), 1), + new WaypointSE2( end, DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0))); + DirectionR2.fromRotation(angleToGoal), 0), 1)); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -76,15 +76,15 @@ void testSample() { Translation2d goalTranslation = end.getTranslation(); Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( start, DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0)), - new Pose2dWithDirection( + DirectionR2.fromRotation(angleToGoal), 0), 1), + new WaypointSE2( end, DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0))); + DirectionR2.fromRotation(angleToGoal), 0), 1)); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -106,17 +106,17 @@ void testSample() { @Test void testSampleThoroughly() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); @@ -147,22 +147,22 @@ void testSampleThoroughly() { // There's no need to run this all the time // @Test void testSamplePerformance() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(10, 0), Rotation2d.kCCW_Pi_2), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(10, 10), Rotation2d.kPi), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 4459bb4e3..89dc1127f 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -8,7 +8,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -36,17 +36,17 @@ class TrajectoryPlannerTest implements Timeless { @Test void testLinear() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1.2)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); @@ -58,17 +58,17 @@ void testLinear() { @Test void testBackingUp() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.MINUS_X), - new Pose2dWithDirection( + DirectionSE2.MINUS_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1.2)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); // these are the same as StraightLineTrajectoryTest. @@ -95,17 +95,17 @@ void testBackingUp() { */ @Test void testPerformance() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1.2)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); long startTimeNs = System.nanoTime(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 20e083536..01af3aad6 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -45,7 +45,7 @@ void testSimple() throws InterruptedException { /** * Yields a curve. * - * A Pose2dWithDirection allows separate specification of heading (which way the + * A WaypointSE2 allows separate specification of heading (which way the * front of the robot is facing) and course (which way the robot is moving). * * In this case, is facing +x, and moving +x, and it ends up moving +y but @@ -57,17 +57,17 @@ void testCurved() throws InterruptedException { new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(9, 9), new Rotation2d(-Math.PI / 2)), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) new TrajectoryPlotter(0.1).plot("curved", t); @@ -84,22 +84,22 @@ void testMultipleWaypoints() throws InterruptedException { new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); TrajectoryPlanner p = new TrajectoryPlanner(c); - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(5, 5), new Rotation2d(-2)), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(9, 9), new Rotation2d(-Math.PI / 2)), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) new TrajectoryPlotter(0.1).plot("multiple", t); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java index 8debe6270..ee9837fb5 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java @@ -1,7 +1,7 @@ package org.team100.lib.trajectory; import org.jfree.data.xy.VectorSeries; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.timing.TimedPose; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,7 +22,7 @@ public VectorSeries convert(String name, Trajectory100 t) { double dt = duration/20; for (double time = 0; time < duration; time += dt) { TimedPose p = t.sample(time); - Pose2dWithDirection pp = p.state().getPose(); + WaypointSE2 pp = p.state().getPose(); double x = pp.translation().getX(); double y = pp.translation().getY(); Rotation2d heading = pp.heading(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 4da97ee97..567f58ab6 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -10,7 +10,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; @@ -31,20 +31,20 @@ class Path100Test { private static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(24, 0, new Rotation2d(Math.toRadians(30))), 0), + WaypointSE2.irrotational( + new Pose2d(24, 0, new Rotation2d(Math.toRadians(30))), 0, 1.2), 0, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(36, 12, new Rotation2d(Math.toRadians(60))), 0), + WaypointSE2.irrotational( + new Pose2d(36, 12, new Rotation2d(Math.toRadians(60))), 0, 1.2), 0, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(60, 12, new Rotation2d(Math.toRadians(90))), 0), + WaypointSE2.irrotational( + new Pose2d(60, 12, new Rotation2d(Math.toRadians(90))), 0, 1.2), 0, 0, 0)); @Test @@ -61,16 +61,16 @@ void testEmpty() { void testSimple() { // spline is in the x direction, no curvature. HolonomicSpline spline = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X)) { + DirectionSE2.TO_X, 1)) { @Override public Translation2d getPoint(double t) { @@ -146,24 +146,24 @@ void testStateAccessors() { void test() throws TimingException { List waypoints = Arrays.asList( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0.0, 0.0, new Rotation2d(Math.toRadians(0))), 0), + WaypointSE2.irrotational( + new Pose2d(0.0, 0.0, new Rotation2d(Math.toRadians(0))), 0, 1.2), 0.1, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(24.0, 0.0, new Rotation2d(Math.toRadians(30))), 0), + WaypointSE2.irrotational( + new Pose2d(24.0, 0.0, new Rotation2d(Math.toRadians(30))), 0, 1.2), 0.1, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(36.0, 0.0, new Rotation2d(Math.toRadians(60))), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(36.0, 0.0, new Rotation2d(Math.toRadians(60))), Math.PI / 2, 1.2), 1e6, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(36.0, 24.0, new Rotation2d(Math.toRadians(60))), 0), + WaypointSE2.irrotational( + new Pose2d(36.0, 24.0, new Rotation2d(Math.toRadians(60))), 0, 1.2), 0.1, 0, 0), new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(60.0, 24.0, new Rotation2d(Math.toRadians(180))), 0), + WaypointSE2.irrotational( + new Pose2d(60.0, 24.0, new Rotation2d(Math.toRadians(180))), 0, 1.2), 0.1, 0, 0)); // Create the reference trajectory (straight line motion between waypoints). diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index a46da425e..e45769230 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -9,7 +9,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.TrajectoryPlotter; @@ -26,17 +26,17 @@ public class PathFactoryTest implements Timeless { @Test void testBackingUp() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.MINUS_X), - new Pose2dWithDirection( + DirectionSE2.MINUS_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.0127, @@ -48,22 +48,22 @@ void testBackingUp() { /** Preserves the tangent at the corner and so makes a little "S" */ @Test void testCorner() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); assertEquals(9, path.length()); @@ -79,17 +79,17 @@ void testCorner() { @Test void testLinear() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.01, 0.01, 0.1); assertEquals(2, path.length()); @@ -106,17 +106,17 @@ void testLinear() { /** Turning in place works now. */ @Test void testSpin() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - new DirectionSE2(0, 0, 1)), - new Pose2dWithDirection( + new DirectionSE2(0, 0, 1), 1), + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kCCW_90deg), - new DirectionSE2(0, 0, 1))); + new DirectionSE2(0, 0, 1), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(path, 0.1, 1); } @@ -124,27 +124,27 @@ void testSpin() { /** Hard corners work now. */ @Test void testActualCorner() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_Y), - new Pose2dWithDirection( + DirectionSE2.TO_Y, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(path, 0.1, 1); } @@ -152,27 +152,27 @@ void testActualCorner() { @Test void testComposite() { // note none of these directions include rotation; it's all in between. - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - new DirectionSE2(1, 0, 0)), - new Pose2dWithDirection( + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - new DirectionSE2(1, 0, 0)), - new Pose2dWithDirection( + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - new DirectionSE2(1, 0, 0)), - new Pose2dWithDirection( + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( new Pose2d( new Translation2d(2, 0), new Rotation2d(1)), - new DirectionSE2(1, 0, 0))); + new DirectionSE2(1, 0, 0), 1)); Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(foo, 0.1, 1); assertEquals(4, foo.length(), 0.001); @@ -180,16 +180,16 @@ void testComposite() { @Test void test() { - Pose2dWithDirection p1 = new Pose2dWithDirection( + WaypointSE2 p1 = new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.TO_X); - Pose2dWithDirection p2 = new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2); + WaypointSE2 p2 = new WaypointSE2( new Pose2d( new Translation2d(15, 10), Rotation2d.kZero), - new DirectionSE2(1, 5, 0)); + new DirectionSE2(1, 5, 0), 1.2); HolonomicSpline s = new HolonomicSpline(p1, p2); List samples = PathFactory.parameterizeSpline(s, 0.05, 0.05, 0.1, 0.0, 1.0); @@ -215,17 +215,16 @@ void test() { @Test void testDx() { HolonomicSpline s0 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, -1), Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_Y), - 1.0, 1.0); + DirectionSE2.TO_Y, 1)); List splines = List.of(s0); List motion = PathFactory.parameterizeSplines(splines, 0.001, 0.001, 0.001); for (Pose2dWithMotion p : motion) { @@ -241,17 +240,17 @@ void testDx() { */ @Test void testPerformance() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1.2)); long startTimeNs = System.nanoTime(); Path100 t = new Path100(new ArrayList<>()); final int iterations = 100; diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index e97c05ef4..32b86e0dd 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -9,7 +9,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -46,16 +46,16 @@ void testCourse() { @Test void testLinear() { HolonomicSpline s = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -77,16 +77,16 @@ void testLinear() { @Test void testLinear2() { HolonomicSpline s = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(2, 0), new Rotation2d()), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -111,16 +111,16 @@ void testRotationSoft() { // this has no rotation at the ends. // the rotation rate is zero at the ends and much higher in the middle. HolonomicSpline s = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1.2)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -162,16 +162,16 @@ void testRotationFast() { // the "spatial" rotation rate is constant, i.e. // rotation and translation speed are proportional. HolonomicSpline s = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - new DirectionSE2(1, 0, 1)), - new Pose2dWithDirection( + new DirectionSE2(1, 0, 1), 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - new DirectionSE2(1, 0, 1))); + new DirectionSE2(1, 0, 1), 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -206,16 +206,16 @@ void testRotationFast() { void testRotation2() { // Make sure the rotation goes over +/- pi HolonomicSpline s = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d(2.5)), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(-2.5)), - DirectionSE2.TO_X)); + DirectionSE2.TO_X, 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); } @@ -223,19 +223,18 @@ void testRotation2() { /** Turning in place splines work. */ @Test void spin() { - double magicNumber = 0.9; + double scale = 0.9; HolonomicSpline s0 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - new DirectionSE2(0, 0, 1)), - new Pose2dWithDirection( + new DirectionSE2(0, 0, 1), scale), + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kCCW_90deg), - new DirectionSE2(0, 0, 1)), - magicNumber, magicNumber); + new DirectionSE2(0, 0, 1), scale)); List splines = new ArrayList<>(); splines.add(s0); @@ -256,56 +255,52 @@ void testCircle() { // // This now uses the specification of SE(2) "course" which includes rotation. // - // I fiddled with the magic number to make a pretty good circle. - double magicNumber = 0.9; + // I fiddled with the scale to make a pretty good circle. + double scale = 0.9; HolonomicSpline s0 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.k180deg), - new DirectionSE2(0, 1, 1)), - new Pose2dWithDirection( + new DirectionSE2(0, 1, 1), scale), + new WaypointSE2( new Pose2d( new Translation2d(0, 1), Rotation2d.kCW_90deg), - new DirectionSE2(-1, 0, 1)), - magicNumber, magicNumber); + new DirectionSE2(-1, 0, 1), scale)); HolonomicSpline s1 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, 1), Rotation2d.kCW_90deg), - new DirectionSE2(-1, 0, 1)), - new Pose2dWithDirection( + new DirectionSE2(-1, 0, 1), scale), + new WaypointSE2( new Pose2d( new Translation2d(-1, 0), Rotation2d.kZero), - new DirectionSE2(0, -1, 1)), - magicNumber, magicNumber); + new DirectionSE2(0, -1, 1), scale)); HolonomicSpline s2 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(-1, 0), Rotation2d.kZero), - new DirectionSE2(0, -1, 1)), - new Pose2dWithDirection( + new DirectionSE2(0, -1, 1), scale), + new WaypointSE2( new Pose2d( new Translation2d(0, -1), Rotation2d.kCCW_90deg), - new DirectionSE2(1, 0, 1)), - magicNumber, magicNumber); + new DirectionSE2(1, 0, 1), scale)); HolonomicSpline s3 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, -1), Rotation2d.kCCW_90deg), - new DirectionSE2(1, 0, 1)), - new Pose2dWithDirection( + new DirectionSE2(1, 0, 1), scale), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.k180deg), - new DirectionSE2(0, 1, 1)), - magicNumber, magicNumber); + new DirectionSE2(0, 1, 1), scale)); List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); @@ -350,33 +345,30 @@ private void checkCircle(List splines, double rangeError, doubl @Test void testLine() { - double magicNumber = 1.0; // turn a bit to the left HolonomicSpline s0 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X), - magicNumber, magicNumber); + DirectionSE2.TO_X, 1)); // turn much more to the left HolonomicSpline s1 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(2, 0), Rotation2d.k180deg), - DirectionSE2.TO_X), - magicNumber, magicNumber); + DirectionSE2.TO_X, 1)); List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); @@ -400,32 +392,29 @@ void testLine() { */ @Test void testPath0() { - double magicNumber = 0.7; - // turn a bit to the left - Pose2dWithDirection p0 = new Pose2dWithDirection( + double scale = 0.7; + WaypointSE2 p0 = new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d(-1, 0)), - new DirectionSE2(1, 0, 0)); - Pose2dWithDirection p1 = new Pose2dWithDirection( + new DirectionSE2(1, 0, 0), scale); + WaypointSE2 p1 = new WaypointSE2( new Pose2d( new Translation2d(0.707, 0.293), new Rotation2d(-1, 1)), - new DirectionSE2(1, 1, -1)); - Pose2dWithDirection p2 = new Pose2dWithDirection( + new DirectionSE2(1, 1, -1), scale); + WaypointSE2 p2 = new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d(0, 1)), - new DirectionSE2(0, 1, 0)); + new DirectionSE2(0, 1, 0), scale); if (DEBUG) System.out.println("s01"); - HolonomicSpline s01 = new HolonomicSpline( - p0, p1, magicNumber, magicNumber); + HolonomicSpline s01 = new HolonomicSpline(p0, p1); if (DEBUG) System.out.println("s12"); - HolonomicSpline s12 = new HolonomicSpline( - p1, p2, magicNumber, magicNumber); + HolonomicSpline s12 = new HolonomicSpline(p1, p2); List splines = new ArrayList<>(); splines.add(s01); splines.add(s12); @@ -445,35 +434,33 @@ void testPath0() { void testMismatchedXYDerivatives() { // corners seem to be allowed, but yield un-traversable trajectories // with infinite accelerations at the corners, so they should be prohibited. - double magicNumber = 1.0; + // this goes straight ahead to (1,0) // derivatives point straight ahead HolonomicSpline s0 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X), - magicNumber, magicNumber); + DirectionSE2.TO_X, 1)); // this is a sharp turn to the left // derivatives point to the left HolonomicSpline s1 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_Y), - new Pose2dWithDirection( + DirectionSE2.TO_Y, 1), + new WaypointSE2( new Pose2d( new Translation2d(1, 1), Rotation2d.kZero), - DirectionSE2.TO_Y), - magicNumber, magicNumber); + DirectionSE2.TO_Y, 1)); List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); @@ -511,17 +498,16 @@ void testEntryVelocity() { // radius is 1 m. HolonomicSpline s0 = new HolonomicSpline( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d( new Translation2d(0, -1), Rotation2d.kZero), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_Y), - 1.2, 1.2); + DirectionSE2.TO_Y, 1.2)); if (DEBUG) { for (double t = 0; t < 1; t += 0.03) { System.out.printf("%5.3f %5.3f\n", s0.x(t), s0.y(t)); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java index 944d3b7db..65b37b175 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java @@ -5,7 +5,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.TrajectoryPlotter; import edu.wpi.first.math.geometry.Pose2d; @@ -16,21 +16,21 @@ public class QuinticHermiteOptimizerTest { @Test void test0() { - Pose2dWithDirection a = new Pose2dWithDirection( + WaypointSE2 a = new WaypointSE2( new Pose2d( new Translation2d(0, 100), new Rotation2d()), - DirectionSE2.MINUS_Y); - Pose2dWithDirection b = new Pose2dWithDirection( + DirectionSE2.MINUS_Y, 1); + WaypointSE2 b = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d()), - DirectionSE2.TO_X); - Pose2dWithDirection c = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 c = new WaypointSE2( new Pose2d( new Translation2d(100, 100), new Rotation2d()), - DirectionSE2.TO_Y); + DirectionSE2.TO_Y, 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); @@ -43,26 +43,26 @@ void test0() { @Test void test1() { - Pose2dWithDirection d = new Pose2dWithDirection( + WaypointSE2 d = new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_Y); - Pose2dWithDirection e = new Pose2dWithDirection( + DirectionSE2.TO_Y, 1); + WaypointSE2 e = new WaypointSE2( new Pose2d( new Translation2d(0, 50), new Rotation2d()), - DirectionSE2.TO_X); - Pose2dWithDirection f = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 f = new WaypointSE2( new Pose2d( new Translation2d(100, 50), new Rotation2d()), - DirectionSE2.MINUS_Y); - Pose2dWithDirection g = new Pose2dWithDirection( + DirectionSE2.MINUS_Y, 1); + WaypointSE2 g = new WaypointSE2( new Pose2d( new Translation2d(100, 0), new Rotation2d()), - DirectionSE2.MINUS_X); + DirectionSE2.MINUS_X, 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(d, e)); @@ -75,31 +75,31 @@ void test1() { @Test void test2() { - Pose2dWithDirection h = new Pose2dWithDirection( + WaypointSE2 h = new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X); - Pose2dWithDirection i = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 i = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d()), - DirectionSE2.TO_X); - Pose2dWithDirection j = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 j = new WaypointSE2( new Pose2d( new Translation2d(100, 50), new Rotation2d()), - new DirectionSE2(1, 1, 0)); - Pose2dWithDirection k = new Pose2dWithDirection( + new DirectionSE2(1, 1, 0), 1); + WaypointSE2 k = new WaypointSE2( new Pose2d( new Translation2d(150, 0), new Rotation2d()), - DirectionSE2.MINUS_Y); - Pose2dWithDirection l = new Pose2dWithDirection( + DirectionSE2.MINUS_Y, 1); + WaypointSE2 l = new WaypointSE2( new Pose2d( new Translation2d(150, -50), new Rotation2d()), - DirectionSE2.MINUS_Y); + DirectionSE2.MINUS_Y, 1); List splines2 = new ArrayList<>(); splines2.add(new HolonomicSpline(h, i)); @@ -113,21 +113,21 @@ void test2() { @Test void test3() { - Pose2dWithDirection a = new Pose2dWithDirection( + WaypointSE2 a = new WaypointSE2( new Pose2d( new Translation2d(0, 100), new Rotation2d()), - DirectionSE2.MINUS_Y); - Pose2dWithDirection b = new Pose2dWithDirection( + DirectionSE2.MINUS_Y, 1); + WaypointSE2 b = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X); - Pose2dWithDirection c = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 c = new WaypointSE2( new Pose2d( new Translation2d(100, 100), new Rotation2d(Math.PI)), - DirectionSE2.TO_Y); + DirectionSE2.TO_Y, 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); @@ -140,26 +140,26 @@ void test3() { @Test void test4() { - Pose2dWithDirection d = new Pose2dWithDirection( + WaypointSE2 d = new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_Y); - Pose2dWithDirection e = new Pose2dWithDirection( + DirectionSE2.TO_Y, 1); + WaypointSE2 e = new WaypointSE2( new Pose2d( new Translation2d(0, 50), new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X); - Pose2dWithDirection f = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 f = new WaypointSE2( new Pose2d( new Translation2d(100, 50), new Rotation2d(Math.PI)), - DirectionSE2.MINUS_Y); - Pose2dWithDirection g = new Pose2dWithDirection( + DirectionSE2.MINUS_Y, 1); + WaypointSE2 g = new WaypointSE2( new Pose2d( new Translation2d(100, 0), new Rotation2d()), - DirectionSE2.MINUS_X); + DirectionSE2.MINUS_X, 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(d, e)); @@ -172,31 +172,31 @@ void test4() { @Test void test5() { - Pose2dWithDirection h = new Pose2dWithDirection( + WaypointSE2 h = new WaypointSE2( new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X); - Pose2dWithDirection i = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 i = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X); - Pose2dWithDirection j = new Pose2dWithDirection( + DirectionSE2.TO_X, 1); + WaypointSE2 j = new WaypointSE2( new Pose2d( new Translation2d(100, 50), new Rotation2d(Math.PI)), - new DirectionSE2(1, 1, 0)); - Pose2dWithDirection k = new Pose2dWithDirection( + new DirectionSE2(1, 1, 0), 1); + WaypointSE2 k = new WaypointSE2( new Pose2d( new Translation2d(150, 0), new Rotation2d()), - DirectionSE2.MINUS_Y); - Pose2dWithDirection l = new Pose2dWithDirection( + DirectionSE2.MINUS_Y, 1); + WaypointSE2 l = new WaypointSE2( new Pose2d( new Translation2d(150, -50), new Rotation2d(Math.PI / 2)), - DirectionSE2.MINUS_Y); + DirectionSE2.MINUS_Y, 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(h, i)); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java index 0745fc81e..dd3cef148 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -29,8 +29,8 @@ void testSimple() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1, 0); // motionless, so 100% of the capsize accel is available assertEquals(-8.166, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -48,8 +48,8 @@ void testSimpleMoving() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1, 0); // moving, only some of the capsize accel is available assertEquals(-5.257, c.getMinMaxAcceleration(p, 2.5).getMinAccel(), DELTA); @@ -67,8 +67,8 @@ void testSimpleOverspeed() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1, 0); // above the velocity limit assertEquals(-1, c.getMinMaxAcceleration(p, 3).getMinAccel(), DELTA); @@ -85,8 +85,8 @@ void testSimple2() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); @@ -102,8 +102,8 @@ void testStraightLine() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java index f2a608d6a..001f968ef 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -21,8 +21,8 @@ public class ConstantConstraintTest implements Timeless { void testVelocity() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); } @@ -31,8 +31,8 @@ void testVelocity() { void testAccel() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); assertEquals(-3, c.getMinMaxAcceleration(state, 1).getMinAccel(), DELTA); assertEquals(3, c.getMinMaxAcceleration(state, 1).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java index 55fa6d57e..30d937199 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -22,20 +22,20 @@ void testSquare() { // here the two speeds are the same DiamondConstraint c = new DiamondConstraint(logger, 1, 1, 4); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); // moving purely in x, get the x number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); // moving purely in y, get the y number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2), 0, 0, 0); // moving diagonally, get less. assertEquals(0.707, c.getMaxVelocity(state).getValue(), DELTA); @@ -45,20 +45,20 @@ void testSquare() { void testVelocity() { DiamondConstraint c = new DiamondConstraint(logger, 2, 3, 4); Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); // moving purely in x, get the x number assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); // moving purely in y, get the y number assertEquals(3, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2), 0, 0, 0); // moving diagonally, get less. assertEquals(1.697, c.getMaxVelocity(state).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java index e3649a627..f0546f8a3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; @@ -25,8 +25,8 @@ void test1() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // motion +x, limiter is elevator. Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(3, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(3, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -42,8 +42,8 @@ void test2() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // motion +y, shoulder and wrist have same constraint Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -59,8 +59,8 @@ void test3() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist, motion +x, bend doesn't matter. Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), 0), + WaypointSE2.irrotational( + new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), 0, 1.2), 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -76,8 +76,8 @@ void test4() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist, motion +y, shoulder is the limiter Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2), 0, 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -93,8 +93,8 @@ void test5() { JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); // bent wrist and shoulder, arm at 45, motion +y, Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(2, 1 + Math.sqrt(2), new Rotation2d(Math.PI / 2)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(2, 1 + Math.sqrt(2), new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2), 0, 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index a3cd1acb8..c2c77d580 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -13,7 +13,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -37,14 +37,14 @@ public class ScheduleGeneratorTest { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); public static final List WAYPOINTS = Arrays.asList( - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(24.0, 0.0, new Rotation2d(0)), 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(36, 12, new Rotation2d(0)), 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(60, 12, new Rotation2d(0)), 0), 0, 0, 0)); + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(24.0, 0.0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(36, 12, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(60, 12, new Rotation2d(0)), 0, 1.2), 0, 0, 0)); public static final List HEADINGS = List.of( GeometryUtil.fromDegrees(0), @@ -112,14 +112,14 @@ void testJustTurningInPlace() { Path100 path = new Path100(Arrays.asList( new Pose2dWithMotion( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d(0, 0, new Rotation2d(0)), - new DirectionSE2(0, 0, 1)), + new DirectionSE2(0, 0, 1), 1), 1, 0, 0), new Pose2dWithMotion( - new Pose2dWithDirection( + new WaypointSE2( new Pose2d(0, 0, new Rotation2d(Math.PI)), - new DirectionSE2(0, 0, 1)), + new DirectionSE2(0, 0, 1), 1), 1, 0, 0))); if (DEBUG) System.out.printf("PATH: %s\n", path); @@ -280,17 +280,17 @@ void testV1() { */ @Test void testPerformance() { - List waypoints = List.of( - new Pose2dWithDirection( + List waypoints = List.of( + new WaypointSE2( new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X), - new Pose2dWithDirection( + DirectionSE2.TO_X, 1.2), + new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y)); + DirectionSE2.TO_Y, 1.2)); long startTimeNs = System.nanoTime(); final int iterations = 100; final double SPLINE_SAMPLE_TOLERANCE_M = 0.05; diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java index 3b070a599..7d5281643 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -26,23 +26,23 @@ void testVelocity() { // motionless double m = c.getMaxVelocity(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0)).getValue(); assertEquals(5, m, DELTA); // moving in +x, no curvature, no rotation m = c.getMaxVelocity(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0)).getValue(); // max allowed velocity is full speed assertEquals(5, m, DELTA); // moving in +x, 5 rad/meter m = c.getMaxVelocity(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 5, 0, 0)).getValue(); // at 5 rad/m with 0.5m sides the fastest you can go is 1.55 m/s. assertEquals(1.925, m, DELTA); @@ -53,8 +53,8 @@ void testVelocity() { // traveling 1 m/s, there are 4 m/s available for the fastest wheel // which means 11.314 rad/s, and also 11.314 rad/m since we're going 1 m/s. Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 11.313708, 0, 0); m = c.getMaxVelocity( state) @@ -72,8 +72,8 @@ void testAccel() { SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1); // this is constant MinMaxAcceleration m = c.getMinMaxAcceleration(new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0); assertEquals(-20, m.getMinAccel(), DELTA); assertEquals(10, m.getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index fa77e73b5..b7e79df36 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import edu.wpi.first.math.geometry.Pose2d; @@ -17,16 +17,16 @@ void test() { // At (0,0,0), t=0, v=0, acceleration=1 TimedPose start_state = new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 0.0, 0.0, 1.0); // At (.5,0,0), t=1, v=1, acceleration=0 TimedPose end_state = new TimedPose( new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0.5, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0.5, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), 1.0, 1.0, 0.0); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java index c23f959db..4b88bde47 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import edu.wpi.first.math.geometry.Pose2d; @@ -17,8 +17,8 @@ void testRadial() { TorqueConstraint jc = new TorqueConstraint(6); // moving +x at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0); // no tangential motion => no limit assertEquals(Double.NEGATIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -30,8 +30,8 @@ void testTangential() { TorqueConstraint jc = new TorqueConstraint(6); // at (1,0,0), moving (0,1,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); // tangential motion at 1 m assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -43,8 +43,8 @@ void testInclined() { TorqueConstraint jc = new TorqueConstraint(6); // moving +x+y at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 4), + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 4, 1.2), 0, 0, 0); // motion at 45 deg => higher limit assertEquals(-1.414, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -56,8 +56,8 @@ void testFar() { TorqueConstraint jc = new TorqueConstraint(6); // moving +y at (2,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(2, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(2, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); // more r => lower limit assertEquals(-0.5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -69,8 +69,8 @@ void testFar2() { TorqueConstraint jc = new TorqueConstraint(6); // moving +y at (3,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); // more r => lower limit assertEquals(-0.333, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); @@ -82,8 +82,8 @@ void testRealistic() { TorqueConstraint jc = new TorqueConstraint(30); // moving +y at (1,0,0) Pose2dWithMotion state = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2), + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0, 0); // should match the constant constraint at around 1 m assertEquals(-5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 0d5b545f2..343b5e0f5 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -6,7 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -32,10 +32,10 @@ public class TrajectoryVelocityProfileTest implements Timeless { // A five-meter straight line. public static final List WAYPOINTS = Arrays.asList( - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), 0, 0, 0), - new Pose2dWithMotion(Pose2dWithDirection.make( - new Pose2d(5, 0, new Rotation2d(0)), 0), 0, 0, 0)); + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0)); // No rotation. public static final List HEADINGS = List.of( diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java index a66a0cb85..f4dfa649a 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import edu.wpi.first.math.geometry.Pose2d; @@ -19,8 +19,8 @@ void testOutside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(-1, -1, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(-1, -1, new Rotation2d(0)), 0, 1.2), 0, // spatial, so rad/m 0, 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -34,8 +34,8 @@ void testInside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0.5, 0.5, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0.5, 0.5, new Rotation2d(0)), 0, 1.2), 0, // spatial, so rad/m 0, 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java index efc3d3200..2237e151b 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.Pose2dWithDirection; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -28,7 +28,7 @@ void testNormal() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make(new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m 0, 0); assertEquals(-8.485, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -42,8 +42,8 @@ void testVelocity2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest2(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m 0, 0); assertEquals(5.656, c.getMaxVelocity(p).getValue(), DELTA); @@ -57,8 +57,8 @@ void testAccel() { YAW_RATE_SCALE); // driving and spinning Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, 0, 0); // there is an accel limit. @@ -75,8 +75,8 @@ void testAccel2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forRealisticTest(logger), scale); Pose2dWithMotion p = new Pose2dWithMotion( - Pose2dWithDirection.make( - new Pose2d(0, 0, new Rotation2d(0)), 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m 0, 0); // this number is still quite high even with a low scale. From 18baedccf8a0852d38481fac9e33195970a9d122 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 16 Dec 2025 09:46:13 -0800 Subject: [PATCH 08/42] cleaning --- .../frc2025/Swerve/Auto/GoToCoralStation.java | 12 +---- .../org/team100/frc2025/robot/Prewarmer.java | 4 +- .../team100/lib/geometry/DirectionSE2.java | 28 +++++++----- .../org/team100/lib/geometry/WaypointSE2.java | 2 +- ...veToPoseWithTrajectoryAndExitVelocity.java | 10 +---- .../r3/commands/GoToPosePosition.java | 3 +- .../lib/trajectory/TrajectoryPlanner.java | 29 ++++-------- .../r3/ReferenceControllerR3Test.java | 6 +-- .../lib/trajectory/Trajectory100Test.java | 30 ++++--------- .../lib/trajectory/TrajectoryPlannerTest.java | 12 ++--- .../lib/trajectory/TrajectoryTest.java | 10 ++--- .../lib/trajectory/path/Path100Test.java | 4 +- .../lib/trajectory/path/PathFactoryTest.java | 32 +++++++------- .../path/spline/HolonomicSplineTest.java | 36 +++++++-------- .../spline/QuinticHermiteOptimizerTest.java | 44 +++++++++---------- .../timing/ScheduleGeneratorTest.java | 4 +- 16 files changed, 117 insertions(+), 149 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index 99db5f03b..95cd44136 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -54,16 +54,8 @@ public Trajectory100 apply(Pose2d currentPose) { courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust); List waypoints = new ArrayList<>(); - waypoints.add( - new WaypointSE2( - currentPose, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(newInitialSpline), 0), 1)); - waypoints.add( - new WaypointSE2( - goal, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0), 1)); + waypoints.add(new WaypointSE2(currentPose, DirectionSE2.irrotational(newInitialSpline), 1)); + waypoints.add(new WaypointSE2(goal, DirectionSE2.irrotational(courseToGoal), 1)); return m_planner.restToRest(waypoints); } diff --git a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java index 1cc43ea65..96209e104 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java @@ -32,11 +32,11 @@ public static void init(Machinery machinery) { List waypoints = new ArrayList<>(); waypoints.add(new WaypointSE2( new Pose2d(new Translation2d(), Rotation2d.kZero), - DirectionSE2.TO_X, + new DirectionSE2(1, 0, 0), 1)); waypoints.add(new WaypointSE2( new Pose2d(new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X, + new DirectionSE2(1, 0, 0), 1)); TrajectoryPlanner planner = new TrajectoryPlanner( new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger)); diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index 1bb77730b..1010a7da6 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -31,18 +31,24 @@ public Rotation2d toRotation() { return new Rotation2d(x, y); } - public static final DirectionSE2 TO_X = new DirectionSE2(1, 0, 0); - public static final DirectionSE2 MINUS_X = new DirectionSE2(-1, 0, 0); - public static final DirectionSE2 TO_Y = new DirectionSE2(0, 1, 0); - public static final DirectionSE2 MINUS_Y = new DirectionSE2(0, -1, 0); - public static final DirectionSE2 SPIN = new DirectionSE2(0, 0, 1); + /** In the direction of the specified angle in radians, without rotation */ + public static DirectionSE2 irrotational(double rad) { + return fromDirections(rad, 0); + } + + /** In the direction of the specified angle, without rotation */ + public static DirectionSE2 irrotational(Rotation2d angle) { + return fromDirections(angle, 0); + } + + /** In the direction of the specified angle in radians, while rotating */ + public static DirectionSE2 fromDirections(double rad, double theta) { + return fromDirections(new Rotation2d(rad), theta); + } - /** - * @param course cartesian course - * @param theta direction of rotation - */ - public static final DirectionSE2 fromDirections(DirectionR2 course, double theta) { - return new DirectionSE2(course.x, course.y, theta); + /** In the direction of the specified angle, while rotating */ + public static DirectionSE2 fromDirections(Rotation2d angle, double theta) { + return new DirectionSE2(angle.getCos(), angle.getSin(), theta); } @Override diff --git a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java index f3aebe095..ec71c854b 100644 --- a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java @@ -40,7 +40,7 @@ public Rotation2d heading() { /** Course without rotation, with unit scale. */ public static WaypointSE2 irrotational(Pose2d p, double course, double scale) { - return new WaypointSE2(p, new DirectionSE2(Math.cos(course), Math.sin(course), 0), scale); + return new WaypointSE2(p, DirectionSE2.irrotational(course), scale); } /** diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java index abd145993..bc0a34f95 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java @@ -59,17 +59,11 @@ public void initialize() { VelocitySE2 startVelocity = m_drive.getState().velocity(); WaypointSE2 startWaypoint = new WaypointSE2( pose, - DirectionSE2.fromDirections( - DirectionR2.fromRotation( - startVelocity.angle().orElse(toGoal.getAngle())), - 0), + DirectionSE2.irrotational(startVelocity.angle().orElse(toGoal.getAngle())), 1); WaypointSE2 endWaypoint = new WaypointSE2( m_goal, - DirectionSE2.fromDirections( - DirectionR2.fromRotation( - m_endVelocity.angle().orElse(toGoal.getAngle())), - 0), + DirectionSE2.irrotational(m_endVelocity.angle().orElse(toGoal.getAngle())), 1); Trajectory100 trajectory = m_planner.generateTrajectory( List.of(startWaypoint, endWaypoint), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java index 9aa68aa33..15a75137a 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java @@ -46,8 +46,7 @@ public GoToPosePosition( public void initialize() { WaypointSE2 m_currentPose = new WaypointSE2( m_subsystem.getState().pose(), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(m_course), 0), 1); + DirectionSE2.irrotational(m_course), 1); Trajectory100 m_trajectory = m_trajectoryPlanner.restToRest( List.of(m_currentPose, m_goal)); m_referenceController = new PositionReferenceControllerR3( diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 16c066564..45c44acda 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -136,13 +136,11 @@ public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { List.of( new WaypointSE2( startState.pose(), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(startingAngle), 0), + DirectionSE2.irrotational(startingAngle), e1), new WaypointSE2( endState.pose(), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0), + DirectionSE2.irrotational(courseToGoal), 1.2)), startVelocity.norm(), endVelocity.norm()); @@ -176,13 +174,11 @@ public Trajectory100 movingToMoving(ModelR3 startState, Rotation2d startCourse, List.of( new WaypointSE2( startState.pose(), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(startCourse), 0), + DirectionSE2.irrotational(startCourse), e1), new WaypointSE2( endState.pose(), - DirectionSE2.fromDirections( - DirectionR2.fromRotation(endCourse), 0), + DirectionSE2.irrotational(endCourse), 1.2)), splineEntranceVelocity, splineExitVelocity); @@ -208,18 +204,11 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { Rotation2d courseToGoal = endTranslation.minus(startTranslation).getAngle(); try { - return restToRest( - List.of( - new WaypointSE2( - start, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0), - 1), - new WaypointSE2( - end, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(courseToGoal), 0), - 1))); + // direction towards goal without rotating + DirectionSE2 direction = DirectionSE2.irrotational(courseToGoal); + return restToRest(List.of( + new WaypointSE2(start, direction, 1), + new WaypointSE2(end, direction, 1))); } catch (TrajectoryGenerationException e) { return null; } diff --git a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java index d6a93121a..d90c66436 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java @@ -135,17 +135,17 @@ void testFieldRelativeTrajectory() { new Pose2d( new Translation2d(), Rotation2d.fromDegrees(180)), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); waypoints.add(new WaypointSE2( new Pose2d( new Translation2d(100, 4), Rotation2d.fromDegrees(180)), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); waypoints.add(new WaypointSE2( new Pose2d( new Translation2d(196, 13), Rotation2d.fromDegrees(0)), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); double start_vel = 0.0; double end_vel = 0.0; diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 9e7305c23..d0ebbbb95 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -39,14 +39,8 @@ void testPreviewAndAdvance() { Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); List waypoints = List.of( - new WaypointSE2( - start, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0), 1), - new WaypointSE2( - end, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0), 1)); + new WaypointSE2(start, DirectionSE2.irrotational(angleToGoal), 1), + new WaypointSE2(end, DirectionSE2.irrotational(angleToGoal), 1)); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -77,14 +71,8 @@ void testSample() { Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); List waypoints = List.of( - new WaypointSE2( - start, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0), 1), - new WaypointSE2( - end, - DirectionSE2.fromDirections( - DirectionR2.fromRotation(angleToGoal), 0), 1)); + new WaypointSE2(start, DirectionSE2.irrotational(angleToGoal), 1), + new WaypointSE2(end, DirectionSE2.irrotational(angleToGoal), 1)); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); @@ -111,12 +99,12 @@ void testSampleThoroughly() { new Pose2d( new Translation2d(), Rotation2d.kZero), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); @@ -152,17 +140,17 @@ void testSamplePerformance() { new Pose2d( new Translation2d(), Rotation2d.kZero), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(10, 0), Rotation2d.kCCW_Pi_2), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(10, 10), Rotation2d.kPi), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 89dc1127f..ededa3223 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -41,12 +41,12 @@ void testLinear() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1.2), + new DirectionSE2(1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X, 1.2)); + new DirectionSE2(1, 0, 0), 1.2)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); @@ -63,12 +63,12 @@ void testBackingUp() { new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.MINUS_X, 1.2), + new DirectionSE2(-1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1.2)); + new DirectionSE2(1, 0, 0), 1.2)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); // these are the same as StraightLineTrajectoryTest. @@ -100,12 +100,12 @@ void testPerformance() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1.2), + new DirectionSE2(1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y, 1.2)); + new DirectionSE2(0, 1, 0), 1.2)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); long startTimeNs = System.nanoTime(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 01af3aad6..f160cdeb3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -62,12 +62,12 @@ void testCurved() throws InterruptedException { new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(9, 9), new Rotation2d(-Math.PI / 2)), - DirectionSE2.TO_Y, 1)); + new DirectionSE2(0, 1, 0), 1)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) new TrajectoryPlotter(0.1).plot("curved", t); @@ -89,17 +89,17 @@ void testMultipleWaypoints() throws InterruptedException { new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(5, 5), new Rotation2d(-2)), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(9, 9), new Rotation2d(-Math.PI / 2)), - DirectionSE2.TO_Y, 1)); + new DirectionSE2(0, 1, 0), 1)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) new TrajectoryPlotter(0.1).plot("multiple", t); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 567f58ab6..204539ab3 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -65,12 +65,12 @@ void testSimple() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X, 1)) { + new DirectionSE2(1, 0, 0), 1)) { @Override public Translation2d getPoint(double t) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index e45769230..f69707ce7 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -31,12 +31,12 @@ void testBackingUp() { new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.MINUS_X, 1), + new DirectionSE2(-1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.0127, @@ -53,17 +53,17 @@ void testCorner() { new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y, 1)); + new DirectionSE2(0, 1, 0), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); assertEquals(9, path.length()); @@ -84,12 +84,12 @@ void testLinear() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.01, 0.01, 0.1); assertEquals(2, path.length()); @@ -129,22 +129,22 @@ void testActualCorner() { new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_Y, 1), + new DirectionSE2(0, 1, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y, 1)); + new DirectionSE2(0, 1, 0), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(path, 0.1, 1); } @@ -184,7 +184,7 @@ void test() { new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1.2); + new DirectionSE2(1, 0, 0), 1.2); WaypointSE2 p2 = new WaypointSE2( new Pose2d( new Translation2d(15, 10), @@ -219,12 +219,12 @@ void testDx() { new Pose2d( new Translation2d(0, -1), Rotation2d.kZero), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_Y, 1)); + new DirectionSE2(0, 1, 0), 1)); List splines = List.of(s0); List motion = PathFactory.parameterizeSplines(splines, 0.001, 0.001, 0.001); for (Pose2dWithMotion p : motion) { @@ -245,12 +245,12 @@ void testPerformance() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1.2), + new DirectionSE2(1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y, 1.2)); + new DirectionSE2(0, 1, 0), 1.2)); long startTimeNs = System.nanoTime(); Path100 t = new Path100(new ArrayList<>()); final int iterations = 100; diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 32b86e0dd..974c9ee67 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -50,12 +50,12 @@ void testLinear() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d()), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -81,12 +81,12 @@ void testLinear2() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(2, 0), new Rotation2d()), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -115,12 +115,12 @@ void testRotationSoft() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1.2), + new DirectionSE2(1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X, 1.2)); + new DirectionSE2(1, 0, 0), 1.2)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); @@ -210,12 +210,12 @@ void testRotation2() { new Pose2d( new Translation2d(), new Rotation2d(2.5)), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(-2.5)), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); } @@ -351,24 +351,24 @@ void testLine() { new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); // turn much more to the left HolonomicSpline s1 = new HolonomicSpline( new WaypointSE2( new Pose2d( new Translation2d(1, 0), new Rotation2d(1)), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(2, 0), Rotation2d.k180deg), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); @@ -442,12 +442,12 @@ void testMismatchedXYDerivatives() { new Pose2d( new Translation2d(0, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1), + new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_X, 1)); + new DirectionSE2(1, 0, 0), 1)); // this is a sharp turn to the left // derivatives point to the left HolonomicSpline s1 = new HolonomicSpline( @@ -455,12 +455,12 @@ void testMismatchedXYDerivatives() { new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_Y, 1), + new DirectionSE2(0, 1, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 1), Rotation2d.kZero), - DirectionSE2.TO_Y, 1)); + new DirectionSE2(0, 1, 0), 1)); List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); @@ -502,12 +502,12 @@ void testEntryVelocity() { new Pose2d( new Translation2d(0, -1), Rotation2d.kZero), - DirectionSE2.TO_X, 1.2), + new DirectionSE2(1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 0), Rotation2d.kZero), - DirectionSE2.TO_Y, 1.2)); + new DirectionSE2(0, 1, 0), 1.2)); if (DEBUG) { for (double t = 0; t < 1; t += 0.03) { System.out.printf("%5.3f %5.3f\n", s0.x(t), s0.y(t)); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java index 65b37b175..4e3b77a3b 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java @@ -20,17 +20,17 @@ void test0() { new Pose2d( new Translation2d(0, 100), new Rotation2d()), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); WaypointSE2 b = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d()), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 c = new WaypointSE2( new Pose2d( new Translation2d(100, 100), new Rotation2d()), - DirectionSE2.TO_Y, 1); + new DirectionSE2(0, 1, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); @@ -47,22 +47,22 @@ void test1() { new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_Y, 1); + new DirectionSE2(0, 1, 0), 1); WaypointSE2 e = new WaypointSE2( new Pose2d( new Translation2d(0, 50), new Rotation2d()), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 f = new WaypointSE2( new Pose2d( new Translation2d(100, 50), new Rotation2d()), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); WaypointSE2 g = new WaypointSE2( new Pose2d( new Translation2d(100, 0), new Rotation2d()), - DirectionSE2.MINUS_X, 1); + new DirectionSE2(-1, 0, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(d, e)); @@ -79,12 +79,12 @@ void test2() { new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 i = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d()), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 j = new WaypointSE2( new Pose2d( new Translation2d(100, 50), @@ -94,12 +94,12 @@ void test2() { new Pose2d( new Translation2d(150, 0), new Rotation2d()), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); WaypointSE2 l = new WaypointSE2( new Pose2d( new Translation2d(150, -50), new Rotation2d()), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); List splines2 = new ArrayList<>(); splines2.add(new HolonomicSpline(h, i)); @@ -117,17 +117,17 @@ void test3() { new Pose2d( new Translation2d(0, 100), new Rotation2d()), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); WaypointSE2 b = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 c = new WaypointSE2( new Pose2d( new Translation2d(100, 100), new Rotation2d(Math.PI)), - DirectionSE2.TO_Y, 1); + new DirectionSE2(0, 1, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); @@ -144,22 +144,22 @@ void test4() { new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_Y, 1); + new DirectionSE2(0, 1, 0), 1); WaypointSE2 e = new WaypointSE2( new Pose2d( new Translation2d(0, 50), new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 f = new WaypointSE2( new Pose2d( new Translation2d(100, 50), new Rotation2d(Math.PI)), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); WaypointSE2 g = new WaypointSE2( new Pose2d( new Translation2d(100, 0), new Rotation2d()), - DirectionSE2.MINUS_X, 1); + new DirectionSE2(-1, 0, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(d, e)); @@ -176,12 +176,12 @@ void test5() { new Pose2d( new Translation2d(0, 0), new Rotation2d()), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 i = new WaypointSE2( new Pose2d( new Translation2d(50, 0), new Rotation2d(Math.PI / 2)), - DirectionSE2.TO_X, 1); + new DirectionSE2(1, 0, 0), 1); WaypointSE2 j = new WaypointSE2( new Pose2d( new Translation2d(100, 50), @@ -191,12 +191,12 @@ void test5() { new Pose2d( new Translation2d(150, 0), new Rotation2d()), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); WaypointSE2 l = new WaypointSE2( new Pose2d( new Translation2d(150, -50), new Rotation2d(Math.PI / 2)), - DirectionSE2.MINUS_Y, 1); + new DirectionSE2(0, -1, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(h, i)); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index c2c77d580..b7a405776 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -285,12 +285,12 @@ void testPerformance() { new Pose2d( new Translation2d(), new Rotation2d()), - DirectionSE2.TO_X, 1.2), + new DirectionSE2(1, 0, 0), 1.2), new WaypointSE2( new Pose2d( new Translation2d(1, 1), new Rotation2d()), - DirectionSE2.TO_Y, 1.2)); + new DirectionSE2(0, 1, 0), 1.2)); long startTimeNs = System.nanoTime(); final int iterations = 100; final double SPLINE_SAMPLE_TOLERANCE_M = 0.05; From d85848caf44006a06971eaff6b30f55daf131208 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 16 Dec 2025 12:28:38 -0800 Subject: [PATCH 09/42] remove third derivative of position in splines it was only ever used by optimization --- .../lib/geometry/Pose2dWithMotion.java | 26 +----- .../path/spline/HolonomicSpline.java | 92 +------------------ .../r3/FullStateControllerR3Test.java | 76 ++++----------- .../DriveToPoseWithTrajectoryTest.java | 2 +- .../subsystems/swerve/SwerveControlTest.java | 8 +- .../subsystems/swerve/SwerveModelTest.java | 8 +- .../lib/trajectory/path/Path100Test.java | 23 ++--- ...CentripetalAccelerationConstraintTest.java | 10 +- .../timing/ConstantConstraintTest.java | 4 +- .../timing/DiamondConstraintTest.java | 12 +-- .../timing/JointConstraintTest.java | 10 +- .../timing/ScheduleGeneratorTest.java | 12 +-- .../SwerveDriveDynamicsConstraintTest.java | 10 +- .../lib/trajectory/timing/TimedStateTest.java | 4 +- .../timing/TorqueConstraintTest.java | 12 +-- .../timing/TrajectoryVelocityProfileTest.java | 4 +- .../VelocityLimitRegionConstraintTest.java | 4 +- .../timing/YawRateConstraintTest.java | 8 +- 18 files changed, 88 insertions(+), 237 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 275db4fb6..6d474ffb5 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -20,24 +20,20 @@ public class Pose2dWithMotion { private final double m_headingRate; /** Change in course per change in distance, rad/m. */ private final double m_curvatureRad_M; - /** Change in curvature per meter, rad/m^2 */ - private final double m_dCurvatureDsRad_M2; + /** * @param pose location and heading and direction of travel * @param headingRate change in heading, per meter traveled * @param curvatureRad_M change in course per meter traveled. - * @param dCurvatureDsRad_M2 acceleration in course per meter traveled squared. */ public Pose2dWithMotion( WaypointSE2 pose, double headingRate, - double curvatureRad_M, - double dCurvatureDsRad_M2) { + double curvatureRad_M) { m_pose = pose; m_headingRate = headingRate; m_curvatureRad_M = curvatureRad_M; - m_dCurvatureDsRad_M2 = dCurvatureDsRad_M2; } public WaypointSE2 getPose() { @@ -63,18 +59,12 @@ public double getCurvature() { return m_curvatureRad_M; } - /** Radians per meter squared */ - public double getDCurvatureDs() { - return m_dCurvatureDsRad_M2; - } - /** This no longer uses a constant-twist arc, it's a straight line. */ public Pose2dWithMotion interpolate(final Pose2dWithMotion other, double x) { return new Pose2dWithMotion( GeometryUtil.interpolate(m_pose, other.m_pose, x), MathUtil.interpolate(m_headingRate, other.m_headingRate, x), - Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x), - Math100.interpolate(m_dCurvatureDsRad_M2, other.m_dCurvatureDsRad_M2, x)); + Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x)); } /** This no longer uses a constant-twist arc, it's a straight line. */ @@ -105,24 +95,18 @@ public boolean equals(final Object other) { System.out.println("wrong curvature"); return false; } - if (!Math100.epsilonEquals(m_dCurvatureDsRad_M2, p2dwc.m_dCurvatureDsRad_M2)) { - if (DEBUG) - System.out.println("wrong dcurvature"); - return false; - } return true; } public String toString() { return String.format( - "x %5.3f, y %5.3f, theta %5.3f, course %s, dtheta %5.3f, curvature %5.3f, dcurvature_ds %5.3f", + "x %5.3f, y %5.3f, theta %5.3f, course %s, dtheta %5.3f, curvature %5.3f", m_pose.translation().getX(), m_pose.translation().getY(), m_pose.heading().getRadians(), m_pose.course(), m_headingRate, - m_curvatureRad_M, - m_dCurvatureDsRad_M2); + m_curvatureRad_M); } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index fe0365cec..0a906ceab 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -119,8 +119,7 @@ public Pose2dWithMotion getPose2dWithMotion(double p) { new Pose2d(getPoint(p), getHeading(p)), getCourse(p), 1), getDHeadingDs(p), - getCurvature(p), - getDCurvatureDs(p)); + getCurvature(p)); } /** So we can see it */ @@ -169,17 +168,6 @@ private double getDHeadingDs(double p) { return getDHeading(p) / getVelocity(p); } - /** - * DCurvatureDs is the change in curvature per distance traveled, i.e. the - * "spatial change in curvature" - * - * dk/dp / ds/dp = dk/ds - * rad/mp / m/p = rad/m^2 - */ - private double getDCurvatureDs(double p) { - return getDCurvature(p) / getVelocity(p); - } - /** Returns pose in the nonholonomic sense, where the rotation is the course */ Optional getStartPose() { double dx = dx(0); @@ -252,18 +240,6 @@ protected Translation2d getPoint(double t) { return m_heading.getAcceleration(t); } - double dddx(double t) { - return m_x.getJerk(t); - } - - double dddy(double t) { - return m_y.getJerk(t); - } - - double dddtheta(double t) { - return m_heading.getJerk(t); - } - /** * Velocity is the change in position per parameter, p: ds/dp (meters per p). * Since p is not time, it is not "velocity" in the usual sense. @@ -285,70 +261,4 @@ protected double getCurvature(double t) { double ddy = ddy(t); return (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.sqrt((dx * dx + dy * dy))); } - - /** - * DCurvature is the change in curvature per change in p. - * dk/dp (rad/m per p) - * If you want change in curvature per meter, use getDCurvatureDs. - */ - protected double getDCurvature(double t) { - double dx = dx(t); - double dy = dy(t); - double dx2dy2 = (dx * dx + dy * dy); - double ddx = ddx(t); - double ddy = ddy(t); - double dddx = dddx(t); - double dddy = dddy(t); - double num = (dx * dddy - dddx * dy) * dx2dy2 - - 3 * (dx * ddy - ddx * dy) * (dx * ddx + dy * ddy); - return num / (dx2dy2 * dx2dy2 * Math.sqrt(dx2dy2)); - } - - double dCurvature2(double t) { - double dx = dx(t); - double dy = dy(t); - double dx2dy2 = (dx * dx + dy * dy); - if (dx2dy2 == 0) - throw new IllegalArgumentException(); - double ddx = ddx(t); - double ddy = ddy(t); - double dddx = dddx(t); - double dddy = dddy(t); - double num = (dx * dddy - dddx * dy) * dx2dy2 - - 3 * (dx * ddy - ddx * dy) * (dx * ddx + dy * ddy); - return num * num / (dx2dy2 * dx2dy2 * dx2dy2 * dx2dy2 * dx2dy2); - } - - /** integrate curvature over the length of the spline. */ - double maxCurvature() { - double dt = 1.0 / SAMPLES; - double maxC = 0; - for (double t = 0; t < 1.0; t += dt) { - maxC = Math.max(maxC, getCurvature(t)); - } - return maxC; - } - - /** integrate curvature over the length of the spline. */ - double sumCurvature() { - double dt = 1.0 / SAMPLES; - double sum = 0; - for (double t = 0; t < 1.0; t += dt) { - sum += (dt * getCurvature(t)); - } - return sum; - } - - /** - * @return integral of dCurvature^2 over the length of the spline - */ - double sumDCurvature2() { - double dt = 1.0 / SAMPLES; - double sum = 0; - for (double t = 0; t < 1.0; t += dt) { - sum += (dt * dCurvature2(t)); - } - return sum; - } - } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java index 0ce9a430a..e0a3ed094 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java @@ -182,7 +182,7 @@ void testErrZero() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(0, err.getX(), 0.001); @@ -199,7 +199,7 @@ void testErrXAhead() { .fromTimedPose(new TimedPose(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), 0, 0, 0)); + 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -215,7 +215,7 @@ void testErrXBehind() { .fromTimedPose(new TimedPose(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), 0, 0, 0)); + 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -232,7 +232,7 @@ void testErrXAheadWithRotation() { .fromTimedPose(new TimedPose(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(1)), 0, 1.2), - 0, 0, 0), 0, 0, 0)); + 0, 0), 0, 0, 0)); DeltaSE2 err = controller.positionError(measurement, currentReference); assertEquals(-1, err.getX(), 0.001); assertEquals(0, err.getY(), 0.001); @@ -251,9 +251,7 @@ void testErrorAhead() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, - 0, // no curvature - 0); // no change in curvature + 0, 0); double t = 0; // moving double velocity = 1; @@ -278,9 +276,7 @@ void testErrorSideways() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), 0, 1.2), - 0, - 0, // no curvature - 0); // no change in curvature + 0, 0); double t = 0; // moving double velocity = 1; @@ -304,9 +300,7 @@ void testVelocityErrorZero() { // motion is in a straight line, down the x axis // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, - 0, // no curvature - 0); // no change in curvature + WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0); double t = 0; // moving double velocity = 1; @@ -334,9 +328,7 @@ void testVelocityErrorAhead() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, - 0, // no curvature - 0); // no change in curvature + 0, 0); double t = 0; // moving double velocity = 1; @@ -360,9 +352,7 @@ void testFeedForwardAhead() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, - 0, // no curvature - 0);// no change in curvature + 0, 0); double t = 0; // moving double velocity = 1; @@ -386,9 +376,7 @@ void testFeedForwardSideways() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2), - 0, - 0, // no curvature - 0); // no change in curvature + 0, 0); double t = 0; // moving double velocity = 1; @@ -412,9 +400,7 @@ void testFeedForwardTurning() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 1, - 1, // driving and turning - 0); // no change in curvature + 1, 1); double t = 0; // moving double velocity = 1; @@ -440,13 +426,10 @@ void testFeedbackAhead() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -477,13 +460,10 @@ void testFeedbackAheadPlusY() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -514,13 +494,10 @@ void testFeedbackAheadPlusTheta() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -554,13 +531,10 @@ void testFeedbackSideways() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -590,13 +564,10 @@ void testFeedbackSidewaysPlusY() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -626,13 +597,10 @@ void testFullFeedbackAhead() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -665,13 +633,10 @@ void testFullFeedbackSideways() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; @@ -709,13 +674,10 @@ void testFullFeedbackSidewaysWithRotation() { // motion is in a straight line, down the x axis // no curvature double curvatureRad_M = 0; - // no change in curvature - double dCurvatureDsRad_M2 = 0; Pose2dWithMotion state = new Pose2dWithMotion( setpointPose, 0, - curvatureRad_M, - dCurvatureDsRad_M2); + curvatureRad_M); double t = 0; // moving double velocity = 1; diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java index 2a03a2a9d..324f35ae3 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java @@ -59,7 +59,7 @@ void testSimple() throws IOException { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 0, 0))), controller, viz); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index 0abcee354..c6d7988f9 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -32,7 +32,7 @@ void testTimedPose() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -49,7 +49,7 @@ void testTimedPose2() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -66,7 +66,7 @@ void testTimedPose3() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -84,7 +84,7 @@ void testTimedPose4() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 1, 0), + 0, 1), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index 16be48ce7..710ac0508 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -33,7 +33,7 @@ void testTimedPose() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -50,7 +50,7 @@ void testTimedPose2() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -67,7 +67,7 @@ void testTimedPose3() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -85,7 +85,7 @@ void testTimedPose4() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 1, 0), + 0, 1), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 204539ab3..a9f33943b 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -33,19 +33,19 @@ class Path100Test { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(24, 0, new Rotation2d(Math.toRadians(30))), 0, 1.2), - 0, 0, 0), + 0, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(36, 12, new Rotation2d(Math.toRadians(60))), 0, 1.2), - 0, 0, 0), + 0, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(60, 12, new Rotation2d(Math.toRadians(90))), 0, 1.2), - 0, 0, 0)); + 0, 0)); @Test void testEmpty() { @@ -97,11 +97,6 @@ public double getCurvature(double t) { return 0; } - @Override - public double getDCurvature(double t) { - return 0; - } - @Override public double getVelocity(double t) { return 1; @@ -148,23 +143,23 @@ void test() throws TimingException { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0.0, 0.0, new Rotation2d(Math.toRadians(0))), 0, 1.2), - 0.1, 0, 0), + 0.1, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(24.0, 0.0, new Rotation2d(Math.toRadians(30))), 0, 1.2), - 0.1, 0, 0), + 0.1, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(36.0, 0.0, new Rotation2d(Math.toRadians(60))), Math.PI / 2, 1.2), - 1e6, 0, 0), + 1e6, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(36.0, 24.0, new Rotation2d(Math.toRadians(60))), 0, 1.2), - 0.1, 0, 0), + 0.1, 0), new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(60.0, 24.0, new Rotation2d(Math.toRadians(180))), 0, 1.2), - 0.1, 0, 0)); + 0.1, 0)); // Create the reference trajectory (straight line motion between waypoints). Path100 path = new Path100(waypoints); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java index dd3cef148..478ed0b06 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java @@ -31,7 +31,7 @@ void testSimple() { Pose2dWithMotion p = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 1, 0); + 0, 1); // motionless, so 100% of the capsize accel is available assertEquals(-8.166, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(8.166, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); @@ -50,7 +50,7 @@ void testSimpleMoving() { Pose2dWithMotion p = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 1, 0); + 0, 1); // moving, only some of the capsize accel is available assertEquals(-5.257, c.getMinMaxAcceleration(p, 2.5).getMinAccel(), DELTA); assertEquals(5.257, c.getMinMaxAcceleration(p, 2.5).getMaxAccel(), DELTA); @@ -69,7 +69,7 @@ void testSimpleOverspeed() { Pose2dWithMotion p = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 1, 0); + 0, 1); // above the velocity limit assertEquals(-1, c.getMinMaxAcceleration(p, 3).getMinAccel(), DELTA); assertEquals(0, c.getMinMaxAcceleration(p, 3).getMaxAccel(), DELTA); @@ -87,7 +87,7 @@ void testSimple2() { Pose2dWithMotion p = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 1, 0); + 0, 1); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(2.021, c.getMaxVelocity(p).getValue(), DELTA); @@ -104,7 +104,7 @@ void testStraightLine() { Pose2dWithMotion p = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java index 001f968ef..4a2d0bedd 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java @@ -23,7 +23,7 @@ void testVelocity() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); } @@ -33,7 +33,7 @@ void testAccel() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); assertEquals(-3, c.getMinMaxAcceleration(state, 1).getMinAccel(), DELTA); assertEquals(3, c.getMinMaxAcceleration(state, 1).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java index 30d937199..2900c344f 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java @@ -24,19 +24,19 @@ void testSquare() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); // moving purely in x, get the x number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); // moving purely in y, get the y number assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2), - 0, 0, 0); + 0, 0); // moving diagonally, get less. assertEquals(0.707, c.getMaxVelocity(state).getValue(), DELTA); } @@ -47,19 +47,19 @@ void testVelocity() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); // moving purely in x, get the x number assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); // moving purely in y, get the y number assertEquals(3, c.getMaxVelocity(state).getValue(), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2), - 0, 0, 0); + 0, 0); // moving diagonally, get less. assertEquals(1.697, c.getMaxVelocity(state).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java index f0546f8a3..7009be877 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java @@ -27,7 +27,7 @@ void test1() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(3, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -44,7 +44,7 @@ void test2() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -61,7 +61,7 @@ void test3() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), 0, 1.2), - 0, 0, 0); + 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -78,7 +78,7 @@ void test4() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -95,7 +95,7 @@ void test5() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(2, 1 + Math.sqrt(2), new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index b7a405776..061f3ce89 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -38,13 +38,13 @@ public class ScheduleGeneratorTest { public static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(24.0, 0.0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2d(24.0, 0.0, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(36, 12, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2d(36, 12, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(60, 12, new Rotation2d(0)), 0, 1.2), 0, 0, 0)); + new Pose2d(60, 12, new Rotation2d(0)), 0, 1.2), 0, 0)); public static final List HEADINGS = List.of( GeometryUtil.fromDegrees(0), @@ -115,12 +115,12 @@ void testJustTurningInPlace() { new WaypointSE2( new Pose2d(0, 0, new Rotation2d(0)), new DirectionSE2(0, 0, 1), 1), - 1, 0, 0), + 1, 0), new Pose2dWithMotion( new WaypointSE2( new Pose2d(0, 0, new Rotation2d(Math.PI)), new DirectionSE2(0, 0, 1), 1), - 1, 0, 0))); + 1, 0))); if (DEBUG) System.out.printf("PATH: %s\n", path); TrajectoryPlotter.plot(path, 0.1, 1); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java index 7d5281643..076222a85 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java @@ -28,14 +28,14 @@ void testVelocity() { double m = c.getMaxVelocity(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0)).getValue(); + 0, 0)).getValue(); assertEquals(5, m, DELTA); // moving in +x, no curvature, no rotation m = c.getMaxVelocity(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0)).getValue(); + 0, 0)).getValue(); // max allowed velocity is full speed assertEquals(5, m, DELTA); @@ -43,7 +43,7 @@ void testVelocity() { m = c.getMaxVelocity(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 5, 0, 0)).getValue(); + 5, 0)).getValue(); // at 5 rad/m with 0.5m sides the fastest you can go is 1.55 m/s. assertEquals(1.925, m, DELTA); @@ -55,7 +55,7 @@ void testVelocity() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 11.313708, 0, 0); + 11.313708, 0); m = c.getMaxVelocity( state) .getValue(); @@ -74,7 +74,7 @@ void testAccel() { MinMaxAcceleration m = c.getMinMaxAcceleration(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), 0); + 0, 0), 0); assertEquals(-20, m.getMinAccel(), DELTA); assertEquals(10, m.getMaxAccel(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index b7e79df36..a6be8f201 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -19,7 +19,7 @@ void test() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 0.0, 0.0, 1.0); // At (.5,0,0), t=1, v=1, acceleration=0 @@ -27,7 +27,7 @@ void test() { new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0.5, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0), + 0, 0), 1.0, 1.0, 0.0); TimedPose i0 = start_state.interpolate2(end_state, 0.0); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java index 4b88bde47..37b31b6c6 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java @@ -19,7 +19,7 @@ void testRadial() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), - 0, 0, 0); + 0, 0); // no tangential motion => no limit assertEquals(Double.NEGATIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -32,7 +32,7 @@ void testTangential() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); // tangential motion at 1 m assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -45,7 +45,7 @@ void testInclined() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 4, 1.2), - 0, 0, 0); + 0, 0); // motion at 45 deg => higher limit assertEquals(-1.414, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(1.414, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -58,7 +58,7 @@ void testFar() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(2, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); // more r => lower limit assertEquals(-0.5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(0.5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -71,7 +71,7 @@ void testFar2() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); // more r => lower limit assertEquals(-0.333, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(0.333, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); @@ -84,7 +84,7 @@ void testRealistic() { Pose2dWithMotion state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0, 0); + 0, 0); // should match the constant constraint at around 1 m assertEquals(-5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); assertEquals(5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 343b5e0f5..2780e9932 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -33,9 +33,9 @@ public class TrajectoryVelocityProfileTest implements Timeless { // A five-meter straight line. public static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0), + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0, 0)); + new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0)); // No rotation. public static final List HEADINGS = List.of( diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java index f4dfa649a..c4e37f9fb 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java @@ -22,7 +22,7 @@ void testOutside() { WaypointSE2.irrotational( new Pose2d(-1, -1, new Rotation2d(0)), 0, 1.2), 0, // spatial, so rad/m - 0, 0); + 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); @@ -37,7 +37,7 @@ void testInside() { WaypointSE2.irrotational( new Pose2d(0.5, 0.5, new Rotation2d(0)), 0, 1.2), 0, // spatial, so rad/m - 0, 0); + 0); assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(Double.POSITIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(1, c.getMaxVelocity(p).getValue(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java index 2237e151b..65e29e445 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java @@ -30,7 +30,7 @@ void testNormal() { Pose2dWithMotion p = new Pose2dWithMotion( WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m - 0, 0); + 0); assertEquals(-8.485, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); assertEquals(8.485, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); assertEquals(2.828, c.getMaxVelocity(p).getValue(), DELTA); @@ -45,7 +45,7 @@ void testVelocity2() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m - 0, 0); + 0); assertEquals(5.656, c.getMaxVelocity(p).getValue(), DELTA); } @@ -60,7 +60,7 @@ void testAccel() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, - 0, 0); + 0); // there is an accel limit. assertEquals(-8.485, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); @@ -78,7 +78,7 @@ void testAccel2() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m - 0, 0); + 0); // this number is still quite high even with a low scale. assertEquals(-16.971, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); From 15a3c76e2d78b94398895ffaa3295c3e18221d05 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 16 Dec 2025 13:21:20 -0800 Subject: [PATCH 10/42] cleaning --- .../team100/lib/geometry/DirectionSE2.java | 9 ++- .../lib/geometry/Pose2dWithMotion.java | 16 ++-- .../org/team100/lib/geometry/WaypointSE2.java | 3 + .../java/org/team100/lib/state/ModelR3.java | 15 ++-- .../path/spline/HolonomicSpline.java | 80 +++++++------------ .../trajectory/timing/JointConstraint.java | 5 +- .../timing/SwerveDriveDynamicsConstraint.java | 2 +- .../trajectory/timing/TorqueConstraint.java | 8 +- .../lib/trajectory/path/Path100Test.java | 15 ++-- .../lib/trajectory/path/PathFactoryTest.java | 7 +- 10 files changed, 73 insertions(+), 87 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index 1010a7da6..96e88980c 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -5,12 +5,15 @@ import edu.wpi.first.math.geometry.Rotation2d; /** - * A direction (i.e. unit-length vector) in the SE2 manifold, - * rigid transformations in two dimensions (which have three dimensions). + * A direction (i.e. unit-length vector) in the SE(2) manifold, describing the + * evolution of Pose2d over some parameterization. + * + * SE(2) is the space of rigid transformations in two dimensions (thus SE(2) is + * three-dimensional, x, y, and theta). * * This is useful for representing spline controls for Pose2d. * - * It is exactly a unit-length Twist2d. + * This is exactly a unit-length Twist2d. */ public class DirectionSE2 { private static final boolean DEBUG = false; diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 6d474ffb5..38e7abfbc 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -3,10 +3,10 @@ import org.team100.lib.util.Math100; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; /** - * WaypointSE2 with: + * WaypointSE2 and curvature in SE(2). Curvature is a unit vector describing how + * direction changes with the spline parameter. * * * the spatial rate of change in heading * * the spatial rate of change in course @@ -21,11 +21,10 @@ public class Pose2dWithMotion { /** Change in course per change in distance, rad/m. */ private final double m_curvatureRad_M; - /** - * @param pose location and heading and direction of travel - * @param headingRate change in heading, per meter traveled - * @param curvatureRad_M change in course per meter traveled. + * @param pose location and heading and direction of travel + * @param headingRate change in heading, per meter traveled + * @param curvatureRad_M change in course per meter traveled. */ public Pose2dWithMotion( WaypointSE2 pose, @@ -40,11 +39,6 @@ public WaypointSE2 getPose() { return m_pose; } - // TODO: change to DirectionSE2 - public Rotation2d getCourse() { - return m_pose.course().toRotation(); - } - /** * Heading rate is radians per meter. * diff --git a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java index ec71c854b..dddb54f27 100644 --- a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java @@ -7,6 +7,9 @@ import edu.wpi.first.math.geometry.Translation2d; /** + * Pose and direction in SE(2). Direction is a unit vector describing how pose + * changes with the spline parameter. + * * For constructing splines. * * The scale factor is somewhat like "velocity" but in the spline constructor it diff --git a/lib/src/main/java/org/team100/lib/state/ModelR3.java b/lib/src/main/java/org/team100/lib/state/ModelR3.java index e113d66bc..f48c96bcf 100644 --- a/lib/src/main/java/org/team100/lib/state/ModelR3.java +++ b/lib/src/main/java/org/team100/lib/state/ModelR3.java @@ -1,7 +1,9 @@ package org.team100.lib.state; import org.team100.lib.geometry.GlobalVelocityR2; +import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.trajectory.timing.TimedPose; @@ -128,14 +130,17 @@ public Model100 theta() { * Transform timed pose into swerve state. */ public static ModelR3 fromTimedPose(TimedPose timedPose) { - double xx = timedPose.state().getPose().translation().getX(); - double yx = timedPose.state().getPose().translation().getY(); - double thetax = timedPose.state().getPose().heading().getRadians(); - Rotation2d course = timedPose.state().getCourse(); + Pose2dWithMotion state = timedPose.state(); + WaypointSE2 pose = state.getPose(); + Translation2d translation = pose.translation(); + double xx = translation.getX(); + double yx = translation.getY(); + double thetax = pose.heading().getRadians(); + Rotation2d course = state.getPose().course().toRotation(); double velocityM_s = timedPose.velocityM_S(); double xv = course.getCos() * velocityM_s; double yv = course.getSin() * velocityM_s; - double thetav = timedPose.state().getHeadingRateRad_M() * velocityM_s; + double thetav = state.getHeadingRateRad_M() * velocityM_s; return new ModelR3( new Model100(xx, xv), new Model100(yx, yv), diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 0a906ceab..8ec29e288 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -1,11 +1,9 @@ package org.team100.lib.trajectory.path.spline; -import java.util.Optional; - import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,9 +25,6 @@ */ public class HolonomicSpline { private static final boolean DEBUG = false; - // curvature measurement performance scales with sample count so make it kinda - // low. most splines go between 0.5 and 5 meters so this is steps of 2 to 20 cm. - private static final int SAMPLES = 25; private final SplineR1 m_x; private final SplineR1 m_y; @@ -58,8 +53,8 @@ public class HolonomicSpline { * optimization. Optimization just doesn't help very much, and it's a pain when * it behaves strangely. * - * @param p0 starting pose - * @param p1 ending pose + * @param p0 starting pose + * @param p1 ending pose */ public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { // Distance metric includes both translation and rotation. This is not @@ -136,13 +131,14 @@ public void printSamples() { } /** - * Course is the direction of motion in SE(2), which means it includes both - * cartesian dimensions and also the rotation dimension. + * Direction of motion in SE(2). Includes both cartesian dimensions and also the + * rotation dimension. This is exactly a unit-length twist in the motion + * direction. */ - public DirectionSE2 getCourse(double t) { - double dx = dx(t); - double dy = dy(t); - double dtheta = dtheta(t); + public DirectionSE2 getCourse(double p) { + double dx = dx(p); + double dy = dy(p); + double dtheta = dtheta(p); return new DirectionSE2(dx, dy, dtheta); } @@ -168,32 +164,6 @@ private double getDHeadingDs(double p) { return getDHeading(p) / getVelocity(p); } - /** Returns pose in the nonholonomic sense, where the rotation is the course */ - Optional getStartPose() { - double dx = dx(0); - double dy = dy(0); - if (Math.abs(dx) < 1e-6 && Math.abs(dy) < 1e-6) { - // rotation below would be garbage, so give up. - return Optional.empty(); - } - return Optional.of(new Pose2d( - getPoint(0), - new Rotation2d(dx, dy))); - } - - /** Returns pose in the nonholonomic sense, where the rotation is the course */ - Optional getEndPose() { - double dx = dx(1); - double dy = dy(1); - if (Math.abs(dx) < 1e-6 && Math.abs(dy) < 1e-6) { - // rotation below would be garbage, so give up. - return Optional.empty(); - } - return Optional.of(new Pose2d( - getPoint(1), - new Rotation2d(dx, dy))); - } - /** * Cartesian coordinate in meters. * @@ -216,28 +186,34 @@ protected Translation2d getPoint(double t) { return getHeading(t).getRadians(); } - double dx(double t) { - return m_x.getVelocity(t); + /** dx/dp */ + double dx(double p) { + return m_x.getVelocity(p); } - double dy(double t) { - return m_y.getVelocity(t); + /** dy/dp */ + double dy(double p) { + return m_y.getVelocity(p); } - double dtheta(double t) { - return m_heading.getVelocity(t); + /** dtheta/dp */ + double dtheta(double p) { + return m_heading.getVelocity(p); } - double ddx(double t) { - return m_x.getAcceleration(t); + /** d^2x/dp^2 */ + double ddx(double p) { + return m_x.getAcceleration(p); } - double ddy(double t) { - return m_y.getAcceleration(t); + /** d^2y/dp^2 */ + double ddy(double p) { + return m_y.getAcceleration(p); } - double ddtheta(double t) { - return m_heading.getAcceleration(t); + /** d^2theta/dp^2 */ + double ddtheta(double p) { + return m_heading.getAcceleration(p); } /** diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java index 23d102615..f08922ff5 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java @@ -48,9 +48,10 @@ public JointConstraint( public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { WaypointSE2 pose = state.getPose(); // Velocity if translation speed were 1.0 m/s. + Rotation2d course = state.getPose().course().toRotation(); VelocitySE2 v = new VelocitySE2( - state.getCourse().getCos(), - state.getCourse().getSin(), + course.getCos(), + course.getSin(), state.getHeadingRateRad_M()); ModelR3 m = new ModelR3(pose.pose(), v); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java index 257963f76..034347098 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -44,7 +44,7 @@ public SwerveDriveDynamicsConstraint( public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { // First check instantaneous velocity and compute a limit based on drive // velocity. - Rotation2d course = state.getCourse(); + Rotation2d course = state.getPose().course().toRotation(); Rotation2d course_local = course.minus(state.getPose().heading()); double vx = course_local.getCos(); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java index a75bcdc31..087eaf591 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java @@ -1,6 +1,7 @@ package org.team100.lib.trajectory.timing; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -53,15 +54,16 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { @Override public MinMaxAcceleration getMinMaxAcceleration( Pose2dWithMotion state, double velocityM_S) { - Rotation2d course = state.getCourse(); + WaypointSE2 pose = state.getPose(); + Rotation2d course = pose.course().toRotation(); // acceleration unit vector Translation2d u = new Translation2d(1.0, course); - Translation2d r = state.getPose().translation(); + Translation2d r = pose.translation(); double cross = r.getX() * u.getY() - r.getY() * u.getX(); double a = Math.abs(m_maxTorque / (M * cross)); if (DEBUG) { System.out.printf("Torque Constraint a: %6.3f p: %s r: %6.3f course: %6.3f\n", - a, state.getPose(), r.getNorm(), course.getRadians()); + a, pose, r.getNorm(), course.getRadians()); } return new MinMaxAcceleration(-a, a); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index a9f33943b..d2dc3d219 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -170,7 +170,8 @@ void test() throws TimingException { Pose2dWithMotion s = path.sample(d); Pose2d p = s.getPose().pose(); System.out.printf("%d, %6.3f, %6.3f, %6.3f, %6.3f\n", - d, p.getX(), p.getY(), p.getRotation().getRadians(), s.getCourse().getRadians()); + d, p.getX(), p.getY(), p.getRotation().getRadians(), + s.getPose().course().toRotation().getRadians()); } } @@ -184,7 +185,7 @@ void test() throws TimingException { assertEquals(0, sample0.getPose().translation().getY(), DELTA); // course is +x - assertEquals(0, sample0.getCourse().getDegrees()); + assertEquals(0, sample0.getPose().course().toRotation().getDegrees()); // heading is 0 assertEquals(0, sample0.getPose().heading().getDegrees()); @@ -194,20 +195,20 @@ void test() throws TimingException { assertEquals(0, sample12.getPose().translation().getY(), DELTA); // course should be +x - assertEquals(0, sample12.getCourse().getDegrees(), DELTA); + assertEquals(0, sample12.getPose().course().toRotation().getDegrees(), DELTA); assertEquals(14.996, sample12.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample5 = path.sample(48); assertEquals(36, sample5.getPose().translation().getX(), DELTA); assertEquals(11.983, sample5.getPose().translation().getY(), DELTA); - assertEquals(45.082, sample5.getCourse().getDegrees(), DELTA); + assertEquals(45.082, sample5.getPose().course().toRotation().getDegrees(), DELTA); assertEquals(60, sample5.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample6 = path.sample(60); assertEquals(36, sample6.getPose().translation().getX(), DELTA); assertEquals(23.983, sample6.getPose().translation().getY(), DELTA); - assertEquals(0.041, sample6.getCourse().getDegrees(), DELTA); + assertEquals(0.041, sample6.getPose().course().toRotation().getDegrees(), DELTA); assertEquals(60, sample6.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample72 = path.sample(72.0); @@ -215,14 +216,14 @@ void test() throws TimingException { assertEquals(24, sample72.getPose().translation().getY(), DELTA); // course should be +x - assertEquals(0, sample72.getCourse().getDegrees(), DELTA); + assertEquals(0, sample72.getPose().course().toRotation().getDegrees(), DELTA); assertEquals(119.687, sample72.getPose().heading().getDegrees(), DELTA); Pose2dWithMotion sample8 = path.sample(84); assertEquals(59.892, sample8.getPose().translation().getX(), DELTA); assertEquals(24, sample8.getPose().translation().getY(), DELTA); - assertEquals(0, sample8.getCourse().getDegrees(), DELTA); + assertEquals(0, sample8.getPose().course().toRotation().getDegrees(), DELTA); assertEquals(179.460, sample8.getPose().heading().getDegrees(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index f69707ce7..7f8b7c34d 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -206,9 +206,10 @@ void test() { cur_pose = sample; } - assertEquals(15.0, cur_pose.getPose().translation().getX(), 0.001); - assertEquals(10.0, cur_pose.getPose().translation().getY(), 0.001); - assertEquals(78.690, cur_pose.getCourse().getDegrees(), 0.001); + WaypointSE2 pose = cur_pose.getPose(); + assertEquals(15.0, pose.translation().getX(), 0.001); + assertEquals(10.0, pose.translation().getY(), 0.001); + assertEquals(78.690, pose.course().toRotation().getDegrees(), 0.001); assertEquals(20.416, arclength, 0.001); } From b5627eb297d23b77ea380b51c69745998c1cf6e6 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 16 Dec 2025 13:27:05 -0800 Subject: [PATCH 11/42] remove jointconstraint since it was never finished --- .../frc2025/CalgamesArm/MechTrajectories.java | 23 +--- .../trajectory/timing/JointConstraint.java | 124 ------------------ .../timing/JointConstraintTest.java | 104 --------------- 3 files changed, 5 insertions(+), 246 deletions(-) delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java delete mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java index 95b4d620f..24c6aab4b 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -8,12 +8,9 @@ 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 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; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java deleted file mode 100644 index f08922ff5..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java +++ /dev/null @@ -1,124 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import org.team100.lib.geometry.AccelerationSE2; -import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.WaypointSE2; -import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.prr.AnalyticalJacobian; -import org.team100.lib.subsystems.prr.EAWConfig; -import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; -import org.team100.lib.subsystems.prr.JointAccelerations; -import org.team100.lib.subsystems.prr.JointVelocities; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * For cartesian trajectories executed by non-cartesian mechanisms: each joint - * has its own velocity and acceleration constraints, which are converted to the - * cartesian TimingConstraint via the Jacobian. - * - * Acceleration constraints here are constant. - * - * TODO: make acceleration constraint depend on position, to account for - * gravity, because it's really a motor torque constraint, not an acceleration - * constraint per se. - * TODO: make this constraint actually work, I think it's broken. - */ -public class JointConstraint implements TimingConstraint { - private final ElevatorArmWristKinematics m_k; - private final AnalyticalJacobian m_j; - private final JointVelocities m_maxJv; - private final JointAccelerations m_maxJa; - - public JointConstraint( - ElevatorArmWristKinematics k, - AnalyticalJacobian j, - JointVelocities maxJv, - JointAccelerations maxJa) { - m_k = k; - m_j = j; - m_maxJv = maxJv; - m_maxJa = maxJa; - } - - @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - WaypointSE2 pose = state.getPose(); - // Velocity if translation speed were 1.0 m/s. - Rotation2d course = state.getPose().course().toRotation(); - VelocitySE2 v = new VelocitySE2( - course.getCos(), - course.getSin(), - state.getHeadingRateRad_M()); - ModelR3 m = new ModelR3(pose.pose(), v); - - // corresponding vector in joint space - JointVelocities qdot = m_j.inverse(m); - - // as a fraction of maxima - double elevatorScale = Math.abs(qdot.elevator() / m_maxJv.elevator()); - double shoulderScale = Math.abs(qdot.shoulder() / m_maxJv.shoulder()); - double wristScale = Math.abs(qdot.wrist() / m_maxJv.wrist()); - - double maxScale = Math.max(elevatorScale, Math.max(shoulderScale, wristScale)); - - // scale qdot to the nearest maximum - JointVelocities maxQdotInMotionDirection = qdot.times(1 / maxScale); - - EAWConfig q = m_k.inverse(pose.pose()); - - VelocitySE2 maxV = m_j.forward(q, maxQdotInMotionDirection); - double norm = maxV.norm(); - if (Double.isNaN(norm)) - return new NonNegativeDouble(0); - return new NonNegativeDouble(norm); - } - - @Override - public MinMaxAcceleration getMinMaxAcceleration( - Pose2dWithMotion state, double velocityM_S) { - Pose2d pose = state.getPose().pose(); - Rotation2d course2 = state.getPose().course().toRotation(); - - double c = course2.getCos(); - double s = course2.getSin(); - double r = state.getHeadingRateRad_M(); - double vx = velocityM_S * s; - double vy = velocityM_S * c; - double omega = velocityM_S * r; - - // actual cartesian velocity - VelocitySE2 v = new VelocitySE2(vx, vy, omega); - - EAWConfig q = m_k.inverse(pose); - // actual qdot - JointVelocities qdot = m_j.inverse(new ModelR3(pose, v)); - - // find accel in motion - AccelerationSE2 unitA = new AccelerationSE2(c, s, r); - ControlR3 sc = new ControlR3(pose, v, unitA); - // corresponding a vector in joint space - JointAccelerations qddot = m_j.inverseA(sc); - - // as a fraction of maxima - double elevatorScale = Math.abs(qddot.elevator() / m_maxJa.elevator()); - double shoulderScale = Math.abs(qddot.shoulder() / m_maxJa.shoulder()); - double wristScale = Math.abs(qddot.wrist() / m_maxJa.wrist()); - - double maxScale = Math.max(elevatorScale, Math.max(shoulderScale, wristScale)); - - // scale qddot to the nearest maximum - JointAccelerations maxQddotInMotionDirection = qddot.times(1 / maxScale); - - AccelerationSE2 fa = m_j.forwardA(q, qdot, maxQddotInMotionDirection); - - double norm = fa.norm(); - if (Double.isNaN(norm)) - return new MinMaxAcceleration(0, 0); - return new MinMaxAcceleration(-1.0 * norm, 1.0 * norm); - } - -} diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java deleted file mode 100644 index 7009be877..000000000 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java +++ /dev/null @@ -1,104 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.WaypointSE2; -import org.team100.lib.geometry.Pose2dWithMotion; -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 edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public class JointConstraintTest { - private static final double DELTA = 0.001; - - @Test - void test1() { - ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); - AnalyticalJacobian j = new AnalyticalJacobian(k); - JointVelocities maxJv = new JointVelocities(1, 1, 1); - JointAccelerations maxJa = new JointAccelerations(1, 1, 1); - JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); - // motion +x, limiter is elevator. - Pose2dWithMotion state = new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(3, 0, new Rotation2d(0)), 0, 1.2), - 0, 0); - assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); - assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); - } - - @Test - void test2() { - ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); - AnalyticalJacobian j = new AnalyticalJacobian(k); - JointVelocities maxJv = new JointVelocities(1, 1, 1); - JointAccelerations maxJa = new JointAccelerations(1, 1, 1); - JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); - // motion +y, shoulder and wrist have same constraint - Pose2dWithMotion state = new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2), - 0, 0); - assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); - assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); - } - - @Test - void test3() { - ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); - AnalyticalJacobian j = new AnalyticalJacobian(k); - JointVelocities maxJv = new JointVelocities(1, 1, 1); - JointAccelerations maxJa = new JointAccelerations(1, 1, 1); - JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); - // bent wrist, motion +x, bend doesn't matter. - Pose2dWithMotion state = new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), 0, 1.2), - 0, 0); - assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); - assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); - } - - @Test - void test4() { - ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); - AnalyticalJacobian j = new AnalyticalJacobian(k); - JointVelocities maxJv = new JointVelocities(1, 1, 1); - JointAccelerations maxJa = new JointAccelerations(1, 1, 1); - JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); - // bent wrist, motion +y, shoulder is the limiter - Pose2dWithMotion state = new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(2, 1, new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2), - 0, 0); - assertEquals(2, jc.getMaxVelocity(state).getValue(), DELTA); - assertEquals(-2, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(2, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); - } - - @Test - void test5() { - ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); - AnalyticalJacobian j = new AnalyticalJacobian(k); - JointVelocities maxJv = new JointVelocities(1, 1, 1); - JointAccelerations maxJa = new JointAccelerations(1, 1, 1); - JointConstraint jc = new JointConstraint(k, j, maxJv, maxJa); - // bent wrist and shoulder, arm at 45, motion +y, - Pose2dWithMotion state = new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(2, 1 + Math.sqrt(2), new Rotation2d(Math.PI / 2)), Math.PI / 2, 1.2), - 0, 0); - assertEquals(1, jc.getMaxVelocity(state).getValue(), DELTA); - assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); - } - -} From 47ecd76ec2effc4006d79439f0ae9e291dc69337 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 16 Dec 2025 19:47:52 -0800 Subject: [PATCH 12/42] more trajectory WIP, not working --- .../team100/lib/geometry/GeometryUtil.java | 3 + .../lib/geometry/Pose2dWithMotion.java | 18 ++- .../org/team100/lib/geometry/WaypointSE2.java | 9 -- .../java/org/team100/lib/state/ControlR3.java | 6 +- .../java/org/team100/lib/state/ModelR3.java | 4 +- .../java/org/team100/lib/trajectory/README.md | 56 ++++++- .../team100/lib/trajectory/path/Path100.java | 4 - .../lib/trajectory/path/PathFactory.java | 4 +- .../path/spline/HolonomicSpline.java | 75 +++------ .../trajectory/timing/ConstrainedState.java | 5 + .../trajectory/timing/DiamondConstraint.java | 2 +- .../trajectory/timing/ScheduleGenerator.java | 3 + .../timing/SwerveDriveDynamicsConstraint.java | 2 +- .../lib/trajectory/timing/TimedPose.java | 8 +- .../trajectory/timing/TorqueConstraint.java | 2 +- .../timing/VelocityLimitRegionConstraint.java | 2 +- .../TrajectoryVisualization.java | 6 +- .../org/team100/lib/geometry/TestSE2Math.java | 52 +++++- .../lib/trajectory/Trajectory100Test.java | 148 +++++++++++------- .../lib/trajectory/TrajectoryPlannerTest.java | 6 +- .../trajectory/TrajectoryToVectorSeries.java | 6 +- .../lib/trajectory/path/Path100Test.java | 101 +++--------- .../lib/trajectory/path/PathFactoryTest.java | 24 +-- .../path/spline/HolonomicSplineTest.java | 67 ++++---- .../timing/ScheduleGeneratorTest.java | 20 ++- .../lib/trajectory/timing/TimedStateTest.java | 2 +- 26 files changed, 347 insertions(+), 288 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 995619c50..825ebb38b 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -288,7 +288,10 @@ public static double distanceM(Pose2d a, Pose2d b) { * * TODO: adjustable weights * + * Note the Chirikjian paper below suggests using mass and inertia for weighting + * * @see https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + * @see https://rpk.lcsr.jhu.edu/wp-content/uploads/2017/08/Partial-Bi-Invariance-of-SE3-Metrics1.pdf */ public static double doubleGeodesicDistance(Pose2d a, Pose2d b) { Translation2d tDiff = a.getTranslation().minus(b.getTranslation()); diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 38e7abfbc..c5eeddc39 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -54,19 +54,21 @@ public double getCurvature() { } /** This no longer uses a constant-twist arc, it's a straight line. */ - public Pose2dWithMotion interpolate(final Pose2dWithMotion other, double x) { + public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) { return new Pose2dWithMotion( GeometryUtil.interpolate(m_pose, other.m_pose, x), MathUtil.interpolate(m_headingRate, other.m_headingRate, x), Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x)); } - /** This no longer uses a constant-twist arc, it's a straight line. */ - public double distanceM(final Pose2dWithMotion other) { - return m_pose.translation().getDistance(other.m_pose.translation()); + /** This now uses double-geodesic distance, i.e. L2 norm including rotation. */ + public double distanceM(Pose2dWithMotion other) { + return GeometryUtil.doubleGeodesicDistance(this, other); + // return + // m_pose.pose().getTranslation().getDistance(other.m_pose.pose().getTranslation()); } - public boolean equals(final Object other) { + public boolean equals(Object other) { if (!(other instanceof Pose2dWithMotion)) { if (DEBUG) System.out.println("wrong type"); @@ -95,9 +97,9 @@ public boolean equals(final Object other) { public String toString() { return String.format( "x %5.3f, y %5.3f, theta %5.3f, course %s, dtheta %5.3f, curvature %5.3f", - m_pose.translation().getX(), - m_pose.translation().getY(), - m_pose.heading().getRadians(), + m_pose.pose().getTranslation().getX(), + m_pose.pose().getTranslation().getY(), + m_pose.pose().getRotation().getRadians(), m_pose.course(), m_headingRate, m_curvatureRad_M); diff --git a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java index dddb54f27..1e3c2a81f 100644 --- a/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; /** * Pose and direction in SE(2). Direction is a unit vector describing how pose @@ -33,14 +32,6 @@ public record WaypointSE2(Pose2d pose, DirectionSE2 course, double scale) { private static final boolean DEBUG = false; - public Translation2d translation() { - return pose.getTranslation(); - } - - public Rotation2d heading() { - return pose.getRotation(); - } - /** Course without rotation, with unit scale. */ public static WaypointSE2 irrotational(Pose2d p, double course, double scale) { return new WaypointSE2(p, DirectionSE2.irrotational(course), scale); diff --git a/lib/src/main/java/org/team100/lib/state/ControlR3.java b/lib/src/main/java/org/team100/lib/state/ControlR3.java index 5e9acb245..155afdbd7 100644 --- a/lib/src/main/java/org/team100/lib/state/ControlR3.java +++ b/lib/src/main/java/org/team100/lib/state/ControlR3.java @@ -125,9 +125,9 @@ public Control100 theta() { * Correctly accounts for centripetal acceleration. */ public static ControlR3 fromTimedPose(TimedPose timedPose) { - double xx = timedPose.state().getPose().translation().getX(); - double yx = timedPose.state().getPose().translation().getY(); - double thetax = timedPose.state().getPose().heading().getRadians(); + double xx = timedPose.state().getPose().pose().getTranslation().getX(); + double yx = timedPose.state().getPose().pose().getTranslation().getY(); + double thetax = timedPose.state().getPose().pose().getRotation().getRadians(); double velocityM_s = timedPose.velocityM_S(); Rotation2d course = timedPose.state().getPose().course().toRotation(); diff --git a/lib/src/main/java/org/team100/lib/state/ModelR3.java b/lib/src/main/java/org/team100/lib/state/ModelR3.java index f48c96bcf..edfabfb07 100644 --- a/lib/src/main/java/org/team100/lib/state/ModelR3.java +++ b/lib/src/main/java/org/team100/lib/state/ModelR3.java @@ -132,10 +132,10 @@ public Model100 theta() { public static ModelR3 fromTimedPose(TimedPose timedPose) { Pose2dWithMotion state = timedPose.state(); WaypointSE2 pose = state.getPose(); - Translation2d translation = pose.translation(); + Translation2d translation = pose.pose().getTranslation(); double xx = translation.getX(); double yx = translation.getY(); - double thetax = pose.heading().getRadians(); + double thetax = pose.pose().getRotation().getRadians(); Rotation2d course = state.getPose().course().toRotation(); double velocityM_s = timedPose.velocityM_S(); double xv = course.getCos() * velocityM_s; diff --git a/lib/src/main/java/org/team100/lib/trajectory/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md index db8bb6bf6..c4adb5989 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/README.md @@ -19,4 +19,58 @@ The process of constructing a trajectory has three stages: To use a trajectory, you `sample()` it, with time (in seconds) as the parameter. The resulting `TimedPose` is interpolated between from the list above. -If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains, it will work well enough to set the course and heading to be the same at each waypoint. \ No newline at end of file +If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains, it will work well enough to set the course and heading to be the same at each waypoint. + +## Math + +The process above is confusing. Here's what it should do instead. + +1. make a spline based on the endpoints +2. construct a list of *spline parameter values* that satisfy the secant constraint +3. construct a discrete mapping from those spline parameters to time +4. at runtime, interpolate + +there could be some caching of the spline values to save some multiplications. + +the key is to not keep so many copies of the pose data around; there's no reason for it +since we don't do optimization anymore. + +The essential math here is as follows. + +The spline position is $q(s)$ for the parameter $s$ + +The velocity with respect to $s$ is $q'(s) = dq/ds$ + +The accleration with respect to $s$ is $q''(s) = d^2q/ds^2$ + +So we're constructing a function for $s(t)$ and its derivatives. + +so using the chain rule: $v(t) = \dot{q}(t) = \dfrac{dq}{ds} \dfrac{dq}{dt} = q'(s)\dot{s}$ + +and using the product rule: $a(t) = \ddot{q}(t) = q''(s)\dot{s}^2 + q'(s)\ddot{s}$ + +## Constructing $s(t)$ + +The way it works now, the last step in `timeParameterizeTrajectory` is to integrate through the +constrained states. + +* Each state has a "total distance so far" number; this is used to find the length of the previous segment. +* each state has a velocity number (real velocity wrt time). +* the acceleration during the previous segment is calculated based on the difference in velocity and the length. this acceleration is attached to the *previous* state, i.e. the state velocity is instantaneous but the acceleration applies to the entire following segment. +* The duration of the previous segment is calculated based on the difference in velocities and the derived acceleration above +* segment duration is cumulated +* each state is annotated with the cumulative time, the endpoint velocity, and zero for the acceleration (it is filled in on the next loop). + +## Sampling + +at runtime, the `TimedPose` list is sampled by time, which means finding floor and ceiling +states and interpolating. The interpolant is the fraction of time between states, which +is used to derive the fraction of distance between states (i.e. along the secant line), +which is used as the interpolant between poses. Note the acceleration is not interpolated +since it is constant within a segment. + +Is caching states and interpolating faster than computing the spline on the fly? + +No! see `Trajectory100Test.testSamplePerformance` which shows that it's about the same, +around 200 ns per sample. The budget is 20000000 ns, so the sampling time is +one millionth of the budget. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index e9e2df4a2..29aa5336d 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -56,10 +56,6 @@ public double getMaxDistance() { return m_distances[m_distances.length - 1]; } - public double getMinDistance() { - return 0.0; - } - /** * Walks through all the points to find the bracketing points. * diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 9cda0f56d..1bbaa6a86 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -83,8 +83,8 @@ public static List parameterizeSplines( private static void getSegmentArc( HolonomicSpline s, List rv, - double t0, - double t1, + double t0, // [0,1] not time + double t1, // [0,1] not time double maxDx, double maxDy, double maxDTheta) { diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 8ec29e288..4c943f967 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -71,13 +71,13 @@ public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { } // Endpoints: - double x0 = p0.translation().getX(); - double x1 = p1.translation().getX(); - double y0 = p0.translation().getY(); - double y1 = p1.translation().getY(); + double x0 = p0.pose().getTranslation().getX(); + double x1 = p1.pose().getTranslation().getX(); + double y0 = p0.pose().getTranslation().getY(); + double y1 = p1.pose().getTranslation().getY(); // To avoid 180 degrees, heading uses an offset - m_heading0 = p0.heading(); - double delta = p1.heading().minus(p0.heading()).getRadians(); + m_heading0 = p0.pose().getRotation(); + double delta = p1.pose().getRotation().minus(p0.pose().getRotation()).getRadians(); // First derivatives are the course: double dx0 = p0.course().x * scale0; @@ -106,36 +106,23 @@ public String toString() { } /** + * TODO: eliminate the waypoint here, for sure eliminate the scale. + * * @param p [0,1] */ public Pose2dWithMotion getPose2dWithMotion(double p) { return new Pose2dWithMotion( - new WaypointSE2( - new Pose2d(getPoint(p), getHeading(p)), - getCourse(p), 1), + new WaypointSE2(getPose2d(p), getCourse(p), 1), // <<< TODO: remove the "1" getDHeadingDs(p), getCurvature(p)); } - /** So we can see it */ - public void printSamples() { - System.out.println("p, x, heading, heading rate"); - for (double p = 0; p < 1; p += 0.05) { - Pose2dWithMotion pwm = getPose2dWithMotion(p); - System.out.printf("%f, %f, %f, %f\n", - p, - pwm.getPose().pose().getX(), - pwm.getPose().heading().getRadians(), - pwm.getHeadingRateRad_M()); - } - } - /** * Direction of motion in SE(2). Includes both cartesian dimensions and also the * rotation dimension. This is exactly a unit-length twist in the motion * direction. */ - public DirectionSE2 getCourse(double p) { + private DirectionSE2 getCourse(double p) { double dx = dx(p); double dy = dy(p); double dtheta = dtheta(p); @@ -143,16 +130,12 @@ public DirectionSE2 getCourse(double p) { } public Pose2d getPose2d(double p) { - return new Pose2d(getPoint(p), getHeading(p)); + return new Pose2d(new Translation2d(x(p), y(p)), getHeading(p)); } //////////////////////////////////////////////////////////////////////// - protected Rotation2d getHeading(double t) { - return m_heading0.rotateBy(Rotation2d.fromRadians(m_heading.getPosition(t))); - } - - protected double getDHeading(double t) { + private double getDHeading(double t) { return m_heading.getVelocity(t); } @@ -164,26 +147,20 @@ private double getDHeadingDs(double p) { return getDHeading(p) / getVelocity(p); } - /** - * Cartesian coordinate in meters. - * - * @param t ranges from 0 to 1 - * @return the point on the spline for that t value - */ - protected Translation2d getPoint(double t) { - return new Translation2d(x(t), y(t)); - } - - double x(double t) { - return m_x.getPosition(t); + /** x at p */ + double x(double p) { + return m_x.getPosition(p); } - double y(double t) { - return m_y.getPosition(t); + /** y at p */ + double y(double p) { + return m_y.getPosition(p); } - double theta(double t) { - return getHeading(t).getRadians(); + /** heading at p */ + private Rotation2d getHeading(double t) { + double headingFromZero = m_heading.getPosition(t); + return m_heading0.rotateBy(Rotation2d.fromRadians(headingFromZero)); } /** dx/dp */ @@ -196,7 +173,7 @@ protected Translation2d getPoint(double t) { return m_y.getVelocity(p); } - /** dtheta/dp */ + /** dheading/dp */ double dtheta(double p) { return m_heading.getVelocity(p); } @@ -211,7 +188,7 @@ protected Translation2d getPoint(double t) { return m_y.getAcceleration(p); } - /** d^2theta/dp^2 */ + /** d^2heading/dp^2 */ double ddtheta(double p) { return m_heading.getAcceleration(p); } @@ -220,7 +197,7 @@ protected Translation2d getPoint(double t) { * Velocity is the change in position per parameter, p: ds/dp (meters per p). * Since p is not time, it is not "velocity" in the usual sense. */ - protected double getVelocity(double t) { + private double getVelocity(double t) { return Math.hypot(dx(t), dy(t)); } @@ -230,7 +207,7 @@ protected double getVelocity(double t) { * Note the denominator is distance in this case, not the parameter, p. * but the argument to this function *is* the parameter, p. :-) */ - protected double getCurvature(double t) { + private double getCurvature(double t) { double dx = dx(t); double dy = dy(t); double ddx = ddx(t); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index ba53cc3c0..b6d8102e6 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -57,10 +57,15 @@ public Pose2dWithMotion getState() { return m_state; } + /** this should be L2 distance */ public double getDistanceM() { return m_distanceM; } + /** + * if we're using L2 norm then this isn't meters and isn't always even cartesian + * at all + */ public double getVelocityM_S() { return m_velocityM_S; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java index a13a258f8..5177c8d84 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java @@ -35,7 +35,7 @@ public DiamondConstraint(LoggerFactory parent, double maxVX, double maxVY, doubl @Override public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { Rotation2d course = state.getPose().course().toRotation(); - Rotation2d heading = state.getPose().heading(); + Rotation2d heading = state.getPose().pose().getRotation(); Rotation2d strafe = course.minus(heading); // a rhombus is a superellipse with exponent 1 // https://en.wikipedia.org/wiki/Superellipse diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 830847b06..90adc07b6 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -98,6 +98,8 @@ private List forwardPass(List samples, doubl private void forwardWork(ConstrainedState s0, ConstrainedState s1) { // constant-twist path length between states + // actually this is now the double-geodesic metric (L2 for all 3 dimensions) + // which means it is not just meters. double dsM = s1.getState().distanceM(s0.getState()); // We may need to iterate to find the maximum end velocity and common @@ -211,6 +213,7 @@ private void backwardsWork(ConstrainedState s0, ConstrainedState s1) { private static Trajectory100 integrate(List states) throws TimingException { List poses = new ArrayList<>(states.size()); double time = 0.0; // time along path + // this should be L2 distance double distance = 0.0; // distance along path double v0 = 0.0; for (int i = 0; i < states.size(); ++i) { diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java index 034347098..814f227e7 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -46,7 +46,7 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { // velocity. Rotation2d course = state.getPose().course().toRotation(); - Rotation2d course_local = course.minus(state.getPose().heading()); + Rotation2d course_local = course.minus(state.getPose().pose().getRotation()); double vx = course_local.getCos(); double vy = course_local.getSin(); // rad/m diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java index 572a2c7cd..9bb359d86 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java @@ -16,7 +16,13 @@ public class TimedPose { private final Pose2dWithMotion m_state; /** Time we achieve this state. */ private final double m_timeS; - /** ds/dt */ + + /** + * ds/dt + * + * if we're using L2 norm then this isn't meters and isn't always even cartesian + * at all + */ private final double m_velocityM_S; /** d^2s/dt^2 */ private double m_accelM_S_S; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java index 087eaf591..655c67250 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java @@ -58,7 +58,7 @@ public MinMaxAcceleration getMinMaxAcceleration( Rotation2d course = pose.course().toRotation(); // acceleration unit vector Translation2d u = new Translation2d(1.0, course); - Translation2d r = pose.translation(); + Translation2d r = pose.pose().getTranslation(); double cross = r.getX() * u.getY() - r.getY() * u.getX(); double a = Math.abs(m_maxTorque / (M * cross)); if (DEBUG) { diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java index e47aafb73..04d72bee5 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java @@ -25,7 +25,7 @@ public VelocityLimitRegionConstraint( @Override public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - final Translation2d translation = state.getPose().translation(); + final Translation2d translation = state.getPose().pose().getTranslation(); if (translation.getX() <= m_max.getX() && translation.getX() >= m_min.getX() && translation.getY() <= m_max.getY() && translation.getY() >= m_min.getY()) { return new NonNegativeDouble(m_limit); diff --git a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java index 9eb45bf51..4edc72556 100644 --- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java +++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java @@ -33,9 +33,9 @@ private static double[] fromTrajectory100(Trajectory100 m_trajectory) { int ndx = 0; for (TimedPose p : m_trajectory.getPoints()) { WaypointSE2 pose = p.state().getPose(); - arr[ndx + 0] = pose.translation().getX(); - arr[ndx + 1] = pose.translation().getY(); - arr[ndx + 2] = pose.heading().getDegrees(); + arr[ndx + 0] = pose.pose().getTranslation().getX(); + arr[ndx + 1] = pose.pose().getTranslation().getY(); + arr[ndx + 2] = pose.pose().getRotation().getDegrees(); ndx += 3; } return arr; diff --git a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java index 2bc60e880..4ecf7ef40 100644 --- a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java +++ b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java @@ -52,9 +52,9 @@ void testRotation2d() { // this test is silly // assertTrue(1 / kTestEpsilon < rot2.getTan()); // this tests the angle-wrapping thing that wpi doesn't do - //assertEquals(90, rot2.getDegrees(), kTestEpsilon); + // assertEquals(90, rot2.getDegrees(), kTestEpsilon); assertEquals(-270, rot2.getDegrees(), EPSILON); - assertEquals(-3*Math.PI / 2, rot2.getRadians(), EPSILON); + assertEquals(-3 * Math.PI / 2, rot2.getRadians(), EPSILON); rot1 = Rotation2d.fromDegrees(1); rot2 = rot1.unaryMinus(); @@ -251,24 +251,30 @@ void testPose2d() { } @Test - void testTwist() { + void testTwist0() { // Exponentiation (integrate twist to obtain a Pose2d) Twist2d twist = new Twist2d(1.0, 0.0, 0.0); Pose2d pose = Pose2d.kZero.exp(twist); assertEquals(1.0, pose.getTranslation().getX(), EPSILON); assertEquals(0.0, pose.getTranslation().getY(), EPSILON); assertEquals(0.0, pose.getRotation().getDegrees(), EPSILON); + } + @Test + void testTwist1() { // Scaled. - twist = new Twist2d(1.0, 0.0, 0.0); - pose = Pose2d.kZero.exp(GeometryUtil.scale(twist, 2.5)); + Twist2d twist = new Twist2d(1.0, 0.0, 0.0); + Pose2d pose = Pose2d.kZero.exp(GeometryUtil.scale(twist, 2.5)); assertEquals(2.5, pose.getTranslation().getX(), EPSILON); assertEquals(0.0, pose.getTranslation().getY(), EPSILON); assertEquals(0.0, pose.getRotation().getDegrees(), EPSILON); + } + @Test + void testTwist2() { // Logarithm (find the twist to apply to obtain a given Pose2d) - pose = new Pose2d(new Translation2d(2.0, 2.0), Rotation2d.fromRadians(Math.PI / 2)); - twist = Pose2d.kZero.log(pose); + Pose2d pose = new Pose2d(new Translation2d(2.0, 2.0), Rotation2d.fromRadians(Math.PI / 2)); + Twist2d twist = Pose2d.kZero.log(pose); assertEquals(Math.PI, twist.dx, EPSILON); assertEquals(0.0, twist.dy, EPSILON); assertEquals(Math.PI / 2, twist.dtheta, EPSILON); @@ -279,4 +285,36 @@ void testTwist() { assertEquals(new_pose.getTranslation().getY(), pose.getTranslation().getY(), EPSILON); assertEquals(new_pose.getRotation().getDegrees(), pose.getRotation().getDegrees(), EPSILON); } + + @Test + void testTwist3() { + // a pure rotation + Pose2d start = new Pose2d(0, 0, new Rotation2d(0)); + Pose2d end = new Pose2d(0, 0, new Rotation2d(1)); + Twist2d between = start.log(end); + assertEquals(0, between.dx, EPSILON); + assertEquals(0.0, between.dy, EPSILON); + assertEquals(1, between.dtheta, EPSILON); + // in this case, the distances are the same. + assertEquals(1, GeometryUtil.normL2(between), EPSILON); + assertEquals(1, GeometryUtil.doubleGeodesicDistance(start, end), EPSILON); + } + + @Test + void testTwist4() { + // rotation AND translation + Pose2d start = new Pose2d(0, 0, new Rotation2d(0)); + Pose2d end = new Pose2d(1, 0, new Rotation2d(1)); + Twist2d between = start.log(end); + assertEquals(0.915, between.dx, 0.001); + assertEquals(-0.5, between.dy, EPSILON); + assertEquals(1, between.dtheta, EPSILON); + // in this case, the distances are NOT the same. + assertEquals(1.445, GeometryUtil.normL2(between), 0.001); + assertEquals(1.414, GeometryUtil.doubleGeodesicDistance(start, end), 0.001); + // this just seems wrong + assertEquals(1.043, GeometryUtil.distanceM(start, end), 0.001); + // this is just the cartesian part + assertEquals(1, start.getTranslation().getDistance(end.getTranslation()), 0.001); + } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index d0ebbbb95..24aa53da0 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -5,7 +5,6 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; @@ -14,6 +13,11 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; +import org.team100.lib.trajectory.path.Path100; +import org.team100.lib.trajectory.path.PathFactory; +import org.team100.lib.trajectory.path.spline.HolonomicSpline; +import org.team100.lib.trajectory.timing.ScheduleGenerator; +import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; import org.team100.lib.trajectory.timing.TimedPose; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; @@ -25,7 +29,7 @@ class Trajectory100Test implements Timeless { private static final double DELTA = 0.001; - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @Test @@ -48,16 +52,16 @@ void testPreviewAndAdvance() { Trajectory100 trajectory = planner.restToRest(waypoints); TimedPose sample = trajectory.sample(0); - assertEquals(0, sample.state().getPose().translation().getX(), DELTA); + assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(1); - assertEquals(1, sample.state().getPose().translation().getX(), DELTA); + assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(2); - assertEquals(1, sample.state().getPose().translation().getX(), DELTA); + assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(3); - assertEquals(1, sample.state().getPose().translation().getX(), DELTA); + assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA); } @Test @@ -81,11 +85,11 @@ void testSample() { assertEquals(1.417, trajectory.duration(), DELTA); TimedPose sample = trajectory.sample(0); - assertEquals(0, sample.state().getPose().translation().getX(), DELTA); + assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(1); - assertEquals(0.825, sample.state().getPose().translation().getX(), DELTA); + assertEquals(0.825, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(2); - assertEquals(1, sample.state().getPose().translation().getX(), DELTA); + assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA); } /** @@ -113,65 +117,101 @@ void testSampleThoroughly() { Trajectory100 trajectory = planner.restToRest(waypoints); assertEquals(1.417, trajectory.duration(), DELTA); - assertEquals(0.000, trajectory.sample(0).state().getPose().translation().getX(), DELTA); - assertEquals(0.010, trajectory.sample(0.1).state().getPose().translation().getX(), DELTA); - assertEquals(0.040, trajectory.sample(0.2).state().getPose().translation().getX(), DELTA); - assertEquals(0.090, trajectory.sample(0.3).state().getPose().translation().getX(), DELTA); - assertEquals(0.160, trajectory.sample(0.4).state().getPose().translation().getX(), DELTA); - assertEquals(0.250, trajectory.sample(0.5).state().getPose().translation().getX(), DELTA); - assertEquals(0.360, trajectory.sample(0.6).state().getPose().translation().getX(), DELTA); - assertEquals(0.487, trajectory.sample(0.7).state().getPose().translation().getX(), DELTA); - assertEquals(0.618, trajectory.sample(0.8).state().getPose().translation().getX(), DELTA); - assertEquals(0.732, trajectory.sample(0.9).state().getPose().translation().getX(), DELTA); - assertEquals(0.825, trajectory.sample(1).state().getPose().translation().getX(), DELTA); - assertEquals(0.899, trajectory.sample(1.1).state().getPose().translation().getX(), DELTA); - assertEquals(0.953, trajectory.sample(1.2).state().getPose().translation().getX(), DELTA); - assertEquals(0.987, trajectory.sample(1.3).state().getPose().translation().getX(), DELTA); - assertEquals(1.000, trajectory.sample(1.4).state().getPose().translation().getX(), DELTA); - assertEquals(1.000, trajectory.sample(1.5).state().getPose().translation().getX(), DELTA); + assertEquals(0.000, trajectory.sample(0.0).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.010, trajectory.sample(0.1).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.040, trajectory.sample(0.2).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.090, trajectory.sample(0.3).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.160, trajectory.sample(0.4).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.250, trajectory.sample(0.5).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.360, trajectory.sample(0.6).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.487, trajectory.sample(0.7).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.618, trajectory.sample(0.8).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.732, trajectory.sample(0.9).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.825, trajectory.sample(1.0).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.899, trajectory.sample(1.1).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.953, trajectory.sample(1.2).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.987, trajectory.sample(1.3).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(1.000, trajectory.sample(1.4).state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(1.000, trajectory.sample(1.5).state().getPose().pose().getTranslation().getX(), DELTA); } - /** Does the index help? No. */ - // There's no need to run this all the time - // @Test - void testSamplePerformance() { - List waypoints = List.of( - new WaypointSE2( - new Pose2d( - new Translation2d(), - Rotation2d.kZero), - new DirectionSE2(1, 0, 0), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(10, 0), - Rotation2d.kCCW_Pi_2), - new DirectionSE2(1, 0, 0), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(10, 10), - Rotation2d.kPi), - new DirectionSE2(1, 0, 0), 1)); + /** + * Does the index help? No. + * + * Does interpolation help, relative to just sampling the spline directly? No. + * + * There's no need to run this all the time + */ + @Test + void testSamplePerformance() throws TimingException { + WaypointSE2 p0 = new WaypointSE2(new Pose2d(new Translation2d(), Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 p1 = new WaypointSE2(new Pose2d(new Translation2d(10, 0), Rotation2d.kCCW_Pi_2), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 p2 = new WaypointSE2(new Pose2d(new Translation2d(10, 10), Rotation2d.kPi), + new DirectionSE2(1, 0, 0), 1); + List waypoints = List.of(p0, p1, p2); + + int reps = 100000; + int times = 10; - SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); - List constraints = new TimingConstraintFactory(limits).fast(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + // SAMPLE SPLINE DIRECTLY (200 ns) - Trajectory100 trajectory = planner.restToRest(waypoints); + HolonomicSpline spline = new HolonomicSpline(p0, p1); - assertEquals(1851, trajectory.length()); - int reps = 500000; - int times = 10; long start = System.nanoTime(); for (int rep = 0; rep < reps; ++rep) { for (int t = 0; t < times; ++t) { - trajectory.sample(0.1 * t); + spline.getPose2dWithMotion(0.1 * t); } } long end = System.nanoTime(); long duration = end - start; if (DEBUG) - System.out.printf("duration (ns) total (ms) %.0f per sample (ns) %.2f\n", + System.out.printf("SPLINE duration (ns) total (ms) %.0f per sample (ns) %.2f\n", + 0.000001 * duration, (double) duration / (reps * times)); + + // INTERPOLATE SPLINE POINTS (170 ns) + + Path100 path = PathFactory.pathFromWaypoints( + waypoints, 0.02, 0.2, 0.1); + assertEquals(23.049, path.getMaxDistance(), 0.001); + + start = System.nanoTime(); + for (int rep = 0; rep < reps; ++rep) { + for (int t = 0; t < times; ++t) { + path.sample(0.1 * t); + } + } + end = System.nanoTime(); + duration = end - start; + if (DEBUG) + System.out.printf("PATH duration (ns) total (ms) %.0f per sample (ns) %.2f\n", 0.000001 * duration, (double) duration / (reps * times)); + + ///////////// + // INTERPOLATE TRAJECTORY POINTS (335 ns) + + SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); + List constraints = new TimingConstraintFactory(limits).fast(logger); + ScheduleGenerator generator = new ScheduleGenerator(constraints); + + Trajectory100 trajectory = generator.timeParameterizeTrajectory(path, 0.1, 0, 0); + + assertEquals(232, trajectory.length()); + + start = System.nanoTime(); + for (int rep = 0; rep < reps; ++rep) { + for (int t = 0; t < times; ++t) { + trajectory.sample(0.1 * t); + } + } + end = System.nanoTime(); + duration = end - start; + if (DEBUG) + System.out.printf("TRAJECTORY duration (ns) total (ms) %.0f per sample (ns) %.2f\n", + 0.000001 * duration, (double) duration / (reps * times)); + } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index ededa3223..c18fc5d02 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -52,7 +52,7 @@ void testLinear() { Trajectory100 t = planner.restToRest(waypoints); assertEquals(12, t.length()); TimedPose p = t.getPoint(6); - assertEquals(0.6, p.state().getPose().translation().getX(), DELTA); + assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -83,7 +83,7 @@ void testBackingUp() { Trajectory100 t = planner.generateTrajectory( waypoints, start_vel, end_vel); TimedPose p = t.getPoint(6); - assertEquals(0.272, p.state().getPose().translation().getX(), DELTA); + assertEquals(0.272, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -124,7 +124,7 @@ void testPerformance() { } assertEquals(18, t.length()); TimedPose p = t.getPoint(6); - assertEquals(0.585, p.state().getPose().translation().getX(), DELTA); + assertEquals(0.585, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java index ee9837fb5..dadde8e51 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java @@ -23,9 +23,9 @@ public VectorSeries convert(String name, Trajectory100 t) { for (double time = 0; time < duration; time += dt) { TimedPose p = t.sample(time); WaypointSE2 pp = p.state().getPose(); - double x = pp.translation().getX(); - double y = pp.translation().getY(); - Rotation2d heading = pp.heading(); + double x = pp.pose().getTranslation().getX(); + double y = pp.pose().getTranslation().getY(); + Rotation2d heading = pp.pose().getRotation(); double dx = m_scale * heading.getCos(); double dy = m_scale * heading.getSin(); s.add(x, y, dx, dy); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index d2dc3d219..11adaab9f 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -10,8 +10,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; @@ -57,60 +57,6 @@ void testEmpty() { assertEquals(0, path.length(), 0.001); } - @Test - void testSimple() { - // spline is in the x direction, no curvature. - HolonomicSpline spline = new HolonomicSpline( - new WaypointSE2( - new Pose2d( - new Translation2d(), - new Rotation2d()), - new DirectionSE2(1, 0, 0), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - new Rotation2d()), - new DirectionSE2(1, 0, 0), 1)) { - - @Override - public Translation2d getPoint(double t) { - return new Translation2d(t, 0); - } - - @Override - public Rotation2d getHeading(double t) { - return Rotation2d.kZero; - } - - @Override - public DirectionSE2 getCourse(double t) { - return new DirectionSE2(0, 0, 0); - } - - @Override - public double getDHeading(double t) { - return 0; - } - - @Override - public double getCurvature(double t) { - return 0; - } - - @Override - public double getVelocity(double t) { - return 1; - } - }; - List splines = new ArrayList<>(); - splines.add(spline); - double maxDx = 0.1; - double maxDy = 0.1; - double maxDTheta = 0.1; - Path100 path = new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); - assertEquals(2, path.length(), 0.001); - } - @Test void testConstruction() { Path100 traj = new Path100(WAYPOINTS); @@ -127,10 +73,10 @@ void testStateAccessors() { assertEquals(WAYPOINTS.get(2), traj.getPoint(2)); assertEquals(WAYPOINTS.get(3), traj.getPoint(3)); - assertEquals(HEADINGS.get(0), traj.getPoint(0).getPose().heading()); - assertEquals(HEADINGS.get(1), traj.getPoint(1).getPose().heading()); - assertEquals(HEADINGS.get(2), traj.getPoint(2).getPose().heading()); - assertEquals(HEADINGS.get(3), traj.getPoint(3).getPose().heading()); + assertEquals(HEADINGS.get(0), traj.getPoint(0).getPose().pose().getRotation()); + assertEquals(HEADINGS.get(1), traj.getPoint(1).getPose().pose().getRotation()); + assertEquals(HEADINGS.get(2), traj.getPoint(2).getPose().pose().getRotation()); + assertEquals(HEADINGS.get(3), traj.getPoint(3).getPose().pose().getRotation()); } /** @@ -175,56 +121,55 @@ void test() throws TimingException { } } - assertEquals(0.0, path.getMinDistance(), DELTA); // constant-twist arcs assertEquals(84.108, path.getMaxDistance(), DELTA); // initial sample is exactly at the start Pose2dWithMotion sample0 = path.sample(0.0); - assertEquals(0, sample0.getPose().translation().getX(), DELTA); - assertEquals(0, sample0.getPose().translation().getY(), DELTA); + assertEquals(0, sample0.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, sample0.getPose().pose().getTranslation().getY(), DELTA); // course is +x assertEquals(0, sample0.getPose().course().toRotation().getDegrees()); // heading is 0 - assertEquals(0, sample0.getPose().heading().getDegrees()); + assertEquals(0, sample0.getPose().pose().getRotation().getDegrees()); Pose2dWithMotion sample12 = path.sample(12.0); - assertEquals(11.997, sample12.getPose().translation().getX(), DELTA); - assertEquals(0, sample12.getPose().translation().getY(), DELTA); + assertEquals(11.997, sample12.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, sample12.getPose().pose().getTranslation().getY(), DELTA); // course should be +x assertEquals(0, sample12.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(14.996, sample12.getPose().heading().getDegrees(), DELTA); + assertEquals(14.996, sample12.getPose().pose().getRotation().getDegrees(), DELTA); Pose2dWithMotion sample5 = path.sample(48); - assertEquals(36, sample5.getPose().translation().getX(), DELTA); - assertEquals(11.983, sample5.getPose().translation().getY(), DELTA); + assertEquals(36, sample5.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(11.983, sample5.getPose().pose().getTranslation().getY(), DELTA); assertEquals(45.082, sample5.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(60, sample5.getPose().heading().getDegrees(), DELTA); + assertEquals(60, sample5.getPose().pose().getRotation().getDegrees(), DELTA); Pose2dWithMotion sample6 = path.sample(60); - assertEquals(36, sample6.getPose().translation().getX(), DELTA); - assertEquals(23.983, sample6.getPose().translation().getY(), DELTA); + assertEquals(36, sample6.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(23.983, sample6.getPose().pose().getTranslation().getY(), DELTA); assertEquals(0.041, sample6.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(60, sample6.getPose().heading().getDegrees(), DELTA); + assertEquals(60, sample6.getPose().pose().getRotation().getDegrees(), DELTA); Pose2dWithMotion sample72 = path.sample(72.0); - assertEquals(47.937, sample72.getPose().translation().getX(), DELTA); - assertEquals(24, sample72.getPose().translation().getY(), DELTA); + assertEquals(47.937, sample72.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(24, sample72.getPose().pose().getTranslation().getY(), DELTA); // course should be +x assertEquals(0, sample72.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(119.687, sample72.getPose().heading().getDegrees(), DELTA); + assertEquals(119.687, sample72.getPose().pose().getRotation().getDegrees(), DELTA); Pose2dWithMotion sample8 = path.sample(84); - assertEquals(59.892, sample8.getPose().translation().getX(), DELTA); - assertEquals(24, sample8.getPose().translation().getY(), DELTA); + assertEquals(59.892, sample8.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(24, sample8.getPose().pose().getTranslation().getY(), DELTA); assertEquals(0, sample8.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(179.460, sample8.getPose().heading().getDegrees(), DELTA); + assertEquals(179.460, sample8.getPose().pose().getRotation().getDegrees(), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 7f8b7c34d..d4985e524 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -68,12 +68,12 @@ void testCorner() { assertEquals(9, path.length()); Pose2dWithMotion p = path.getPoint(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = path.getPoint(1); - assertEquals(1, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } @@ -94,12 +94,12 @@ void testLinear() { waypoints, 0.01, 0.01, 0.1); assertEquals(2, path.length()); Pose2dWithMotion p = path.getPoint(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = path.getPoint(1); - assertEquals(1, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } @@ -207,8 +207,8 @@ void test() { } WaypointSE2 pose = cur_pose.getPose(); - assertEquals(15.0, pose.translation().getX(), 0.001); - assertEquals(10.0, pose.translation().getY(), 0.001); + assertEquals(15.0, pose.pose().getTranslation().getX(), 0.001); + assertEquals(10.0, pose.pose().getTranslation().getY(), 0.001); assertEquals(78.690, pose.course().toRotation().getDegrees(), 0.001); assertEquals(20.416, arclength, 0.001); } @@ -230,7 +230,7 @@ void testDx() { List motion = PathFactory.parameterizeSplines(splines, 0.001, 0.001, 0.001); for (Pose2dWithMotion p : motion) { if (DEBUG) - System.out.printf("%5.3f %5.3f\n", p.getPose().translation().getX(), p.getPose().translation().getY()); + System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), p.getPose().pose().getTranslation().getY()); } } @@ -272,7 +272,7 @@ void testPerformance() { } assertEquals(5, t.length()); Pose2dWithMotion p = t.getPoint(1); - assertEquals(0.417, p.getPose().translation().getX(), DELTA); + assertEquals(0.417, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 974c9ee67..ce56922ee 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -60,17 +60,17 @@ void testLinear() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); - Translation2d t = s.getPoint(0); + Translation2d t = s.getPose2d(0).getTranslation(); assertEquals(0, t.getX(), DELTA); - t = s.getPoint(1); + t = s.getPose2d(1).getTranslation(); assertEquals(1, t.getX(), DELTA); Pose2dWithMotion p = s.getPose2dWithMotion(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); - assertEquals(1, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } @@ -91,17 +91,17 @@ void testLinear2() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); - Translation2d t = s.getPoint(0); + Translation2d t = s.getPose2d(0).getTranslation(); assertEquals(0, t.getX(), DELTA); - t = s.getPoint(1); + t = s.getPose2d(1).getTranslation(); assertEquals(2, t.getX(), DELTA); Pose2dWithMotion p = s.getPose2dWithMotion(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); - assertEquals(2, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(2, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } @@ -125,31 +125,28 @@ void testRotationSoft() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); - if (DEBUG) - s.printSamples(); - // now that the magic numbers apply to the rotational scaling, the heading rate // is constant. - Translation2d t = s.getPoint(0); + Translation2d t = s.getPose2d(0).getTranslation(); assertEquals(0, t.getX(), DELTA); - t = s.getPoint(1); + t = s.getPose2d(1).getTranslation(); assertEquals(1, t.getX(), DELTA); Pose2dWithMotion p = s.getPose2dWithMotion(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); // initial rotation rate is zero assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(0.5); - assertEquals(0.5, p.getPose().translation().getX(), DELTA); - assertEquals(0.5, p.getPose().heading().getRadians(), DELTA); + assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); // high rotation rate in the middle assertEquals(4.807, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); - assertEquals(1, p.getPose().translation().getX(), DELTA); - assertEquals(1, p.getPose().heading().getRadians(), DELTA); + assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(1, p.getPose().pose().getRotation().getRadians(), DELTA); // rotation rate is zero at the end assertEquals(0, p.getHeadingRateRad_M(), DELTA); @@ -176,28 +173,26 @@ void testRotationFast() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("rotation", List.of(s)); - s.printSamples(); - // now that the magic numbers apply to the rotational scaling, the heading rate // is constant. - Translation2d t = s.getPoint(0); + Translation2d t = s.getPose2d(0).getTranslation(); assertEquals(0, t.getX(), DELTA); - t = s.getPoint(1); + t = s.getPose2d(1).getTranslation(); assertEquals(1, t.getX(), DELTA); Pose2dWithMotion p = s.getPose2dWithMotion(0); - assertEquals(0, p.getPose().translation().getX(), DELTA); - assertEquals(0, p.getPose().heading().getRadians(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(1, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(0.5); - assertEquals(0.5, p.getPose().translation().getX(), DELTA); - assertEquals(0.5, p.getPose().heading().getRadians(), DELTA); + assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(1, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); - assertEquals(1, p.getPose().translation().getX(), DELTA); - assertEquals(1, p.getPose().heading().getRadians(), DELTA); + assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(1, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(1, p.getHeadingRateRad_M(), DELTA); } @@ -522,15 +517,15 @@ void testEntryVelocity() { List motion = PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05); if (DEBUG) { for (Pose2dWithMotion p : motion) { - System.out.printf("%5.3f %5.3f\n", p.getPose().translation().getX(), p.getPose().translation().getY()); + System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), p.getPose().pose().getTranslation().getY()); } } Path100 path = new Path100(motion); if (DEBUG) { for (int i = 0; i < path.length(); ++i) { System.out.printf("%5.3f %5.3f\n", - path.getPoint(i).getPose().translation().getX(), - path.getPoint(i).getPose().translation().getY()); + path.getPoint(i).getPose().pose().getTranslation().getX(), + path.getPoint(i).getPose().pose().getTranslation().getY()); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index 061f3ce89..c595b4803 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -3,7 +3,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.ArrayList; @@ -13,8 +12,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -31,7 +30,7 @@ import edu.wpi.first.math.geometry.Translation2d; public class ScheduleGeneratorTest { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; public static final double EPSILON = 1e-12; private static final double DELTA = 0.01; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -121,18 +120,23 @@ void testJustTurningInPlace() { new Pose2d(0, 0, new Rotation2d(Math.PI)), new DirectionSE2(0, 0, 1), 1), 1, 0))); + + // our distance metric combines meters and radians with equal weight + // because this is pure rotation, the "distance" here is in radians. + assertEquals(Math.PI, path.getMaxDistance(), DELTA); if (DEBUG) - System.out.printf("PATH: %s\n", path); + System.out.printf("PATH:\n%s\n", path); TrajectoryPlotter.plot(path, 0.1, 1); List constraints = new ArrayList(); ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 timed_traj = u.timeParameterizeTrajectory( + Trajectory100 traj = u.timeParameterizeTrajectory( path, 1.0, 0.0, 0.0); - final Trajectory100 traj = timed_traj; + if (DEBUG) + System.out.printf("TRAJ:\n%s\n", traj); assertTrue(traj.isEmpty()); @@ -203,7 +207,7 @@ void testConditionalVelocityConstraint() { class ConditionalTimingConstraint implements TimingConstraint { @Override public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - if (state.getPose().translation().getX() >= 24.0) { + if (state.getPose().pose().getTranslation().getX() >= 24.0) { return new NonNegativeDouble(5.0); } else { return new NonNegativeDouble(Double.POSITIVE_INFINITY); @@ -319,7 +323,7 @@ void testPerformance() { } assertEquals(18, t.length()); TimedPose p = t.getPoint(6); - assertEquals(0.575, p.state().getPose().translation().getX(), DELTA); + assertEquals(0.575, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index a6be8f201..6b42dbb3a 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -40,6 +40,6 @@ void test() { assertEquals(0.5, intermediate_state.getTimeS(), EPSILON); assertEquals(start_state.acceleration(), intermediate_state.acceleration(), EPSILON); assertEquals(0.5, intermediate_state.velocityM_S(), EPSILON); - assertEquals(0.125, intermediate_state.state().getPose().translation().getX(), EPSILON); + assertEquals(0.125, intermediate_state.state().getPose().pose().getTranslation().getX(), EPSILON); } } From 09b0056fb626a71fab59cf31f1ecfd4856e4ff16 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 17 Dec 2025 10:34:01 -0800 Subject: [PATCH 13/42] splines and trajectories WIP --- .../lib/trajectory/path/PathFactory.java | 2 +- .../path/spline/HolonomicSpline.java | 106 ++++++----- .../lib/trajectory/ParameterizationTest.java | 120 ++++++++++++ .../lib/trajectory/PathToVectorSeries.java | 12 ++ .../lib/trajectory/SplineToVectorSeries.java | 66 ++++++- .../lib/trajectory/TrajectoryPlannerTest.java | 2 +- .../lib/trajectory/TrajectoryPlotter.java | 178 ++++++++---------- .../lib/trajectory/TrajectoryTest.java | 8 +- .../trajectory/TrajectoryToVectorSeries.java | 43 ++++- .../lib/trajectory/path/PathFactoryTest.java | 6 +- .../path/spline/HolonomicSplineTest.java | 18 +- .../spline/QuinticHermiteOptimizerTest.java | 12 +- .../timing/ScheduleGeneratorTest.java | 2 +- 13 files changed, 384 insertions(+), 191 deletions(-) create mode 100644 lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 1bbaa6a86..31a86b7df 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -24,7 +24,7 @@ public static Path100 pathFromWaypoints( for (int i = 1; i < waypoints.size(); ++i) { splines.add(new HolonomicSpline(waypoints.get(i - 1), waypoints.get(i))); } - return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); + return new Path100(parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); } /** diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 4c943f967..cace22346 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -53,6 +53,8 @@ public class HolonomicSpline { * optimization. Optimization just doesn't help very much, and it's a pain when * it behaves strangely. * + * To avoid confusion, the parameter should always be called "s". + * * @param p0 starting pose * @param p1 ending pose */ @@ -108,13 +110,13 @@ public String toString() { /** * TODO: eliminate the waypoint here, for sure eliminate the scale. * - * @param p [0,1] + * @param s [0,1] */ - public Pose2dWithMotion getPose2dWithMotion(double p) { + public Pose2dWithMotion getPose2dWithMotion(double s) { return new Pose2dWithMotion( - new WaypointSE2(getPose2d(p), getCourse(p), 1), // <<< TODO: remove the "1" - getDHeadingDs(p), - getCurvature(p)); + new WaypointSE2(getPose2d(s), getCourse(s), 1), // <<< TODO: remove the "1" + getDHeadingDs(s), + getCurvature(s)); } /** @@ -122,96 +124,96 @@ public Pose2dWithMotion getPose2dWithMotion(double p) { * rotation dimension. This is exactly a unit-length twist in the motion * direction. */ - private DirectionSE2 getCourse(double p) { - double dx = dx(p); - double dy = dy(p); - double dtheta = dtheta(p); + private DirectionSE2 getCourse(double s) { + double dx = dx(s); + double dy = dy(s); + double dtheta = dtheta(s); return new DirectionSE2(dx, dy, dtheta); } - public Pose2d getPose2d(double p) { - return new Pose2d(new Translation2d(x(p), y(p)), getHeading(p)); + public Pose2d getPose2d(double s) { + return new Pose2d(new Translation2d(x(s), y(s)), getHeading(s)); } //////////////////////////////////////////////////////////////////////// - private double getDHeading(double t) { - return m_heading.getVelocity(t); + private double getDHeading(double s) { + return m_heading.getVelocity(s); } /** * Change in heading per distance traveled, i.e. spatial change in heading. * dtheta/ds (radians/meter). */ - private double getDHeadingDs(double p) { - return getDHeading(p) / getVelocity(p); + private double getDHeadingDs(double s) { + return getDHeading(s) / getVelocity(s); } - /** x at p */ - double x(double p) { - return m_x.getPosition(p); + /** x at s */ + public double x(double s) { + return m_x.getPosition(s); } - /** y at p */ - double y(double p) { - return m_y.getPosition(p); + /** y at s */ + double y(double s) { + return m_y.getPosition(s); } - /** heading at p */ - private Rotation2d getHeading(double t) { - double headingFromZero = m_heading.getPosition(t); + /** heading at s */ + private Rotation2d getHeading(double s) { + double headingFromZero = m_heading.getPosition(s); return m_heading0.rotateBy(Rotation2d.fromRadians(headingFromZero)); } - /** dx/dp */ - double dx(double p) { - return m_x.getVelocity(p); + /** dx/ds */ + public double dx(double s) { + return m_x.getVelocity(s); } - /** dy/dp */ - double dy(double p) { - return m_y.getVelocity(p); + /** dy/ds */ + double dy(double s) { + return m_y.getVelocity(s); } - /** dheading/dp */ - double dtheta(double p) { - return m_heading.getVelocity(p); + /** dheading/ds */ + double dtheta(double s) { + return m_heading.getVelocity(s); } - /** d^2x/dp^2 */ - double ddx(double p) { - return m_x.getAcceleration(p); + /** d^2x/ds^2 */ + public double ddx(double s) { + return m_x.getAcceleration(s); } - /** d^2y/dp^2 */ - double ddy(double p) { - return m_y.getAcceleration(p); + /** d^2y/ds^2 */ + double ddy(double s) { + return m_y.getAcceleration(s); } - /** d^2heading/dp^2 */ - double ddtheta(double p) { - return m_heading.getAcceleration(p); + /** d^2heading/ds^2 */ + double ddtheta(double s) { + return m_heading.getAcceleration(s); } /** - * Velocity is the change in position per parameter, p: ds/dp (meters per p). - * Since p is not time, it is not "velocity" in the usual sense. + * Velocity is the change in position per parameter, p: dx/ds (meters per s). + * Since s is not time, it is not "velocity" in the usual sense. */ - private double getVelocity(double t) { - return Math.hypot(dx(t), dy(t)); + private double getVelocity(double s) { + return Math.hypot(dx(s), dy(s)); } /** * Curvature is the change in motion direction per distance traveled. * rad/m. * Note the denominator is distance in this case, not the parameter, p. - * but the argument to this function *is* the parameter, p. :-) + * but the argument to this function *is* the parameter, s. :-) */ - private double getCurvature(double t) { - double dx = dx(t); - double dy = dy(t); - double ddx = ddx(t); - double ddy = ddy(t); + private double getCurvature(double s) { + double dx = dx(s); + double dy = dy(s); + double ddx = ddx(s); + double ddy = ddy(s); return (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.sqrt((dx * dx + dy * dy))); } } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java new file mode 100644 index 000000000..8f62d3840 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java @@ -0,0 +1,120 @@ +package org.team100.lib.trajectory; + +import java.util.List; +import java.util.function.Supplier; + +import org.jfree.chart.renderer.xy.StandardXYItemRenderer; +import org.jfree.chart.renderer.xy.XYItemRenderer; +import org.jfree.data.xy.XYDataset; +import org.jfree.data.xy.XYSeries; +import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.trajectory.path.PathFactory; +import org.team100.lib.trajectory.path.spline.HolonomicSpline; +import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TimingConstraint; +import org.team100.lib.trajectory.timing.YawRateConstraint; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** To visualize the different ways to parameterize a spline. */ +public class ParameterizationTest { + private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); + + private static final Supplier renderer = () -> new StandardXYItemRenderer( + StandardXYItemRenderer.SHAPES); + + /** + * Shows x as a function of the spline parameter, s. + */ + @Test + void testSplineStraight() { + // a straight line in x, since the direction is also +x + // note the zero scale here to force zero velocity at the ends + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d(0)), + new DirectionSE2(1, 0, 0), 0), // <<< scale = ZERO + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(0)), + new DirectionSE2(1, 0, 0), 0)); // << scale = ZERO + XYSeries sx = SplineToVectorSeries.x("x", List.of(spline)); + XYSeries sxPrime = SplineToVectorSeries.xPrime("xprime", List.of(spline)); + XYSeries sxPrimePrime = SplineToVectorSeries.xPrimePrime("xprimeprime", List.of(spline)); + + XYDataset d1 = TrajectoryPlotter.collect(sx); + XYDataset d2 = TrajectoryPlotter.collect(sxPrime); + XYDataset d3 = TrajectoryPlotter.collect(sxPrimePrime); + + TrajectoryPlotter.actuallyPlot("spline", renderer, d1, d2, d3); + } + + /** + * Show x as a function of the pose list index. + * The pose list has no parameter, it's just a list + */ + @Test + void testPoses() { + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d(0)), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(0)), + new DirectionSE2(1, 0, 0), 1)); + + List poses = PathFactory.parameterizeSplines( + List.of(spline), 0.02, 0.2, 0.1); + + XYSeries sx = PathToVectorSeries.x("spline", poses); + XYDataset dataSet = TrajectoryPlotter.collect(sx); + TrajectoryPlotter.actuallyPlot("poses", renderer, dataSet); + } + + @Test + void testTrajectory() { + List c = List.of( + new ConstantConstraint(log, 2, 0.5), + new YawRateConstraint(log, 1, 1)); + TrajectoryPlanner p = new TrajectoryPlanner(c); + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d(0)), + new DirectionSE2(0, 1, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(0)), + new DirectionSE2(0, 1, 0), 1)); + Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("trajectory", trajectory); + + // XYSeries tx = TrajectoryToVectorSeries.x("x", trajectory); + // XYSeries txDot = TrajectoryToVectorSeries.xdot("xdot", trajectory); + // XYDataset d1 = TrajectoryPlotter.collect(tx); + // XYDataset d2 = TrajectoryPlotter.collect(txDot); + + // TrajectoryPlotter.actuallyPlot("trajectory", renderer, d1, d2); + + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java index e5a515a5f..b9d0391e0 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java @@ -1,6 +1,9 @@ package org.team100.lib.trajectory; +import java.util.List; + import org.jfree.data.xy.VectorSeries; +import org.jfree.data.xy.XYSeries; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; @@ -42,4 +45,13 @@ public VectorSeries convert(String name, Path100 path) { return s; } + public static XYSeries x(String name, List poses) { + XYSeries series = new XYSeries(name); + for (int i = 0; i < poses.size(); ++i) { + Pose2dWithMotion pose = poses.get(i); + series.add(i, pose.getPose().pose().getX()); + } + return series; + } + } diff --git a/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java index 6781db554..1f1c0c4bd 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java @@ -3,6 +3,7 @@ import java.util.List; import org.jfree.data.xy.VectorSeries; +import org.jfree.data.xy.XYSeries; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; @@ -10,6 +11,7 @@ public class SplineToVectorSeries { + private static final double DS = 0.05; /** Length of the vector indicating heading */ private final double m_scale; @@ -17,22 +19,74 @@ public SplineToVectorSeries(double scale) { m_scale = scale; } - /** Maps x to x, y to y */ + /** + * Show little arrows. + * + * @return (x, y, dx, dy) + */ public VectorSeries convert(String name, List splines) { - VectorSeries s = new VectorSeries(name); + VectorSeries series = new VectorSeries(name); for (HolonomicSpline spline : splines) { - for (double j = 0; j < 0.99; j += 0.1) { - Pose2d p = spline.getPose2d(j); + for (double s = 0; s <= 1.001; s += DS) { + Pose2d p = spline.getPose2d(s); double x = p.getX(); double y = p.getY(); Rotation2d heading = p.getRotation(); double dx = m_scale * heading.getCos(); double dy = m_scale * heading.getSin(); - s.add(x, y, dx, dy); + series.add(x, y, dx, dy); } } - return s; + return series; + } + + /** + * X as a function of s. + * + * @return (s, x) + */ + public static XYSeries x(String name, List splines) { + XYSeries series = new XYSeries(name); + for (HolonomicSpline spline : splines) { + for (double s = 0; s <= 1.001; s += DS) { + double x = spline.x(s); + series.add(s, x); + } + } + return series; + } + + /** + * X prime: dx/ds, as a function of s. + * + * @return (s, x') + */ + public static XYSeries xPrime(String name, List splines) { + XYSeries series = new XYSeries(name); + for (HolonomicSpline spline : splines) { + for (double s = 0; s <= 1.001; s += DS) { + double x = spline.dx(s); + series.add(s, x); + } + } + return series; + } + + /** + * X prime prime: d^2x/ds^2, as a function of s. + * + * @return (s, x'') + */ + public static XYSeries xPrimePrime(String name, List splines) { + XYSeries series = new XYSeries(name); + for (HolonomicSpline spline : splines) { + for (double s = 0; s <= 1.001; s += DS) { + double x = spline.ddx(s); + series.add(s, x); + } + } + return series; } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index c18fc5d02..b58e2d4b1 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -175,7 +175,7 @@ void testBackingUp2() { ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(-1, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); - TrajectoryPlotter.plot(traj, 0.1, 1); + TrajectoryPlotter.plot(traj, 0.1); assertEquals(1.549, traj.duration(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java index d0bbbce19..367c13f2e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -3,20 +3,25 @@ import java.awt.Dimension; import java.awt.Frame; import java.util.List; +import java.util.function.Supplier; import java.util.function.ToDoubleFunction; +import javax.swing.BoxLayout; import javax.swing.JDialog; import javax.swing.WindowConstants; import org.jfree.chart.ChartPanel; import org.jfree.chart.JFreeChart; import org.jfree.chart.axis.NumberAxis; -import org.jfree.chart.axis.NumberTickUnit; import org.jfree.chart.plot.XYPlot; import org.jfree.chart.renderer.xy.VectorRenderer; +import org.jfree.chart.renderer.xy.XYItemRenderer; import org.jfree.data.Range; import org.jfree.data.xy.VectorSeries; import org.jfree.data.xy.VectorSeriesCollection; +import org.jfree.data.xy.XYDataset; +import org.jfree.data.xy.XYSeries; +import org.jfree.data.xy.XYSeriesCollection; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.timing.TimedPose; @@ -24,7 +29,8 @@ import edu.wpi.first.math.geometry.Pose2d; public class TrajectoryPlotter { - public static final boolean SHOW = false; + public static final boolean SHOW = true; + private static final int SIZE = 500; private final TrajectoryToVectorSeries converter; private final SplineToVectorSeries splineConverter; @@ -38,10 +44,9 @@ public TrajectoryPlotter(double arrowLength) { public void plot(String name, Trajectory100 t) { VectorSeries converted = convert(name, t); - Range xrange = xRange(t); - Range yrange = yRange(t); + XYDataset dataSet = TrajectoryPlotter.collect(converted); if (SHOW) - actuallyPlot(name, xrange, yrange, 1, converted); + actuallyPlot(name, () -> new VectorRenderer(), dataSet); } public VectorSeries convert(String name, Trajectory100 t) { @@ -54,79 +59,36 @@ public VectorSeries convert(String name, Path100 path) { public void plot(String name, List s) { VectorSeries converted = convert(name, s); - Range xrange = xRange(s); - Range yrange = yRange(s); + XYDataset dataSet = TrajectoryPlotter.collect(converted); if (SHOW) - actuallyPlot(name, xrange, yrange, 1, converted); + actuallyPlot(name, () -> new VectorRenderer(), dataSet); } public VectorSeries convert(String name, List s) { return splineConverter.convert(name, s); } - public void actuallyPlot( - String name, - Range xrange, - Range yrange, - double tick, - VectorSeries... converted) { + public static XYDataset collect(VectorSeries... converted) { VectorSeriesCollection dataSet = new VectorSeriesCollection(); for (VectorSeries c : converted) { dataSet.addSeries(c); } - XYPlot plot = new XYPlot( - dataSet, - new NumberAxis("X"), - new NumberAxis("Y"), - new VectorRenderer()); - NumberAxis domain = (NumberAxis) plot.getDomainAxis(); - NumberAxis range = (NumberAxis) plot.getRangeAxis(); - domain.setRangeWithMargins(xrange); - range.setRangeWithMargins(yrange); - domain.setTickUnit(new NumberTickUnit(tick)); - range.setTickUnit(new NumberTickUnit(tick)); - - ChartPanel panel = new ChartPanel(new JFreeChart(plot)); - // "true" means "modal" so wait for close. - JDialog frame = new JDialog((Frame) null, "plot", true); - frame.setContentPane(panel); - - frame.setPreferredSize(new Dimension(500, 500)); - frame.pack(); - frame.setVisible(true); - frame.setDefaultCloseOperation(WindowConstants.DISPOSE_ON_CLOSE); - } - - public Range xRange(Trajectory100 t) { - return range(t, (p) -> p.state().getPose().pose().getX()); - } - - public Range yRange(Trajectory100 t) { - return range(t, (p) -> p.state().getPose().pose().getY()); + return dataSet; } - public Range xRange(List s) { - return range(s, (p) -> p.getX()); - } - - public Range yRange(List s) { - return range(s, (p) -> p.getY()); - } - - public Range xRange(Path100 path) { - return range(path, (p) -> p.getX()); - } - - public Range yRange(Path100 path) { - return range(path, (p) -> p.getY()); + public static XYDataset collect(XYSeries... series) { + XYSeriesCollection dataSet = new XYSeriesCollection(); + for (XYSeries c : series) { + dataSet.addSeries(c); + } + return dataSet; } - /** Adds margin */ - Range range(Trajectory100 t, ToDoubleFunction f) { + public static Range xRange(XYDataset dataset) { double max = Double.NEGATIVE_INFINITY; double min = Double.POSITIVE_INFINITY; - for (TimedPose p : t.getPoints()) { - double x = f.applyAsDouble(p); + for (int i = 0; i < dataset.getItemCount(0); ++i) { + double x = dataset.getXValue(0, i); if (x + 0.1 > max) max = x + 0.1; if (x - 0.1 < min) @@ -135,75 +97,85 @@ Range range(Trajectory100 t, ToDoubleFunction f) { return new Range(min, max); } - /** Adds margin */ - Range range(List splines, ToDoubleFunction f) { + public static Range yRange(XYDataset dataset) { double max = Double.NEGATIVE_INFINITY; double min = Double.POSITIVE_INFINITY; - - for (HolonomicSpline spline : splines) { - for (double j = 0; j < 0.99; j += 0.1) { - Pose2d p = spline.getPose2d(j); - double x = f.applyAsDouble(p); - if (x + 0.1 > max) - max = x + 0.1; - if (x - 0.1 < min) - min = x - 0.1; - } + for (int i = 0; i < dataset.getItemCount(0); ++i) { + double y = dataset.getYValue(0, i); + if (y + 0.1 > max) + max = y + 0.1; + if (y - 0.1 < min) + min = y - 0.1; } return new Range(min, max); } - /** Adds margin */ - Range range(Path100 t, ToDoubleFunction f) { - double max = Double.NEGATIVE_INFINITY; - double min = Double.POSITIVE_INFINITY; - for (int i = 0; i < t.length(); ++i) { - Pose2d p = t.getPoint(i).getPose().pose(); - double x = f.applyAsDouble(p); - if (x + 0.1 > max) - max = x + 0.1; - if (x - 0.1 < min) - min = x - 0.1; + /** + * renderer is a supplier because new XYPlot writes the dataset name into the renderer. + * + * mutability is bad. + */ + public static void actuallyPlot( + String name, + Supplier renderer, + XYDataset... dataSets) { + + // "true" means "modal" so wait for close. + JDialog frame = new JDialog((Frame) null, name, true); + frame.setLayout(new BoxLayout(frame.getContentPane(), BoxLayout.Y_AXIS)); + + for (XYDataset dataSet : dataSets) { + XYPlot plot = new XYPlot( + dataSet, + new NumberAxis("X"), + new NumberAxis("Y"), + renderer.get()); + NumberAxis domain = (NumberAxis) plot.getDomainAxis(); + NumberAxis range = (NumberAxis) plot.getRangeAxis(); + domain.setRangeWithMargins(xRange(dataSet)); + range.setRangeWithMargins(yRange(dataSet)); + + ChartPanel panel = new ChartPanel(new JFreeChart(plot)); + panel.setPreferredSize(new Dimension(SIZE, SIZE / dataSets.length)); + frame.add(panel); } - return new Range(min, max); + + frame.pack(); + frame.setVisible(true); + frame.setDefaultCloseOperation(WindowConstants.DISPOSE_ON_CLOSE); } + + + + public static void plot( Trajectory100 t, - double arrows, - double ticks) { + double arrows) { TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); VectorSeries series = plotter.convert("trajectory", t); - Range xrange = plotter.xRange(t); - Range yrange = plotter.yRange(t); - + XYDataset dataSet = TrajectoryPlotter.collect(series); if (SHOW) - plotter.actuallyPlot("compare", xrange, yrange, ticks, series); + actuallyPlot("compare", () -> new VectorRenderer(), dataSet); } public static void plot( List splines, - double arrows, - double ticks) { + double arrows) { TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); VectorSeries series = plotter.convert("before", splines); - Range xrange = plotter.xRange(splines); - Range yrange = plotter.yRange(splines); - + XYDataset dataSet = TrajectoryPlotter.collect(series); if (SHOW) - plotter.actuallyPlot("compare", xrange, yrange, ticks, series); + actuallyPlot("compare", () -> new VectorRenderer(), dataSet); } public static void plot( Path100 path, - double arrows, - double ticks) { + double arrows) { TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); VectorSeries series = plotter.convert("path", path); - Range xrange = plotter.xRange(path); - Range yrange = plotter.yRange(path); - + XYDataset dataSet = TrajectoryPlotter.collect(series); if (SHOW) - plotter.actuallyPlot("compare", xrange, yrange, ticks, series); + actuallyPlot("compare", () -> new VectorRenderer(), dataSet); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index f160cdeb3..acbd9d3b9 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -20,7 +20,7 @@ * These pop up GUI windows, so leave them commented out when you check in. */ public class TrajectoryTest { - private static final boolean SHOW = false; + private static final boolean SHOW = true; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); /** @@ -39,7 +39,7 @@ void testSimple() throws InterruptedException { new Pose2d(0, 0, new Rotation2d()), new Pose2d(10, 1, new Rotation2d())); if (SHOW) - new TrajectoryPlotter(0.1).plot("simple", t ); + new TrajectoryPlotter(0.5).plot("simple", t ); } /** @@ -70,7 +70,7 @@ void testCurved() throws InterruptedException { new DirectionSE2(0, 1, 0), 1)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) - new TrajectoryPlotter(0.1).plot("curved", t); + new TrajectoryPlotter(0.5).plot("curved", t); } /** @@ -102,6 +102,6 @@ void testMultipleWaypoints() throws InterruptedException { new DirectionSE2(0, 1, 0), 1)); Trajectory100 t = p.restToRest(waypoints); if (SHOW) - new TrajectoryPlotter(0.1).plot("multiple", t); + new TrajectoryPlotter(0.3).plot("multiple", t); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java index dadde8e51..debe90508 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java @@ -1,6 +1,7 @@ package org.team100.lib.trajectory; import org.jfree.data.xy.VectorSeries; +import org.jfree.data.xy.XYSeries; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.timing.TimedPose; @@ -8,6 +9,7 @@ public class TrajectoryToVectorSeries { + private static final int POINTS = 20; /** Length of the vector indicating heading */ private final double m_scale; @@ -19,7 +21,7 @@ public TrajectoryToVectorSeries(double scale) { public VectorSeries convert(String name, Trajectory100 t) { VectorSeries s = new VectorSeries(name); double duration = t.duration(); - double dt = duration/20; + double dt = duration / POINTS; for (double time = 0; time < duration; time += dt) { TimedPose p = t.sample(time); WaypointSE2 pp = p.state().getPose(); @@ -32,4 +34,43 @@ public VectorSeries convert(String name, Trajectory100 t) { } return s; } + + /** + * X as a function of t. + * + * @return (t, x) + */ + public static XYSeries x(String name, Trajectory100 trajectory) { + XYSeries series = new XYSeries(name); + double duration = trajectory.duration(); + double dt = duration / POINTS; + for (double t = 0; t <= duration + 0.0001; t += dt) { + TimedPose p = trajectory.sample(t); + WaypointSE2 pp = p.state().getPose(); + double x = pp.pose().getTranslation().getX(); + series.add(t, x); + } + return series; + } + + /** + * X dot: dx/dt, as a function of t. + * + * @return (t, \dot{x}) + */ + public static XYSeries xdot(String name, Trajectory100 trajectory) { + XYSeries series = new XYSeries(name); + double duration = trajectory.duration(); + double dt = duration / POINTS; + for (double t = 0; t <= duration + 0.0001; t += dt) { + TimedPose p = trajectory.sample(t); + Rotation2d course = p.state().getPose().course().toRotation(); + double velocityM_s = p.velocityM_S(); + System.out.println(velocityM_s); + System.out.println(course); + double xv = course.getCos() * velocityM_s; + series.add(t, xv); + } + return series; + } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index d4985e524..5ed148dc8 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -118,7 +118,7 @@ void testSpin() { Rotation2d.kCCW_90deg), new DirectionSE2(0, 0, 1), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - TrajectoryPlotter.plot(path, 0.1, 1); + TrajectoryPlotter.plot(path, 0.1); } /** Hard corners work now. */ @@ -146,7 +146,7 @@ void testActualCorner() { new Rotation2d()), new DirectionSE2(0, 1, 0), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - TrajectoryPlotter.plot(path, 0.1, 1); + TrajectoryPlotter.plot(path, 0.1); } @Test @@ -174,7 +174,7 @@ void testComposite() { new Rotation2d(1)), new DirectionSE2(1, 0, 0), 1)); Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - TrajectoryPlotter.plot(foo, 0.1, 1); + TrajectoryPlotter.plot(foo, 0.1); assertEquals(4, foo.length(), 0.001); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index ce56922ee..1639c330f 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -234,13 +234,10 @@ void spin() { List splines = new ArrayList<>(); splines.add(s0); - // before assertTrue(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) assertTrue(verifyC2(splines)); - // optimize and compare - TrajectoryPlotter.plot(splines, 0.1, 1); + TrajectoryPlotter.plot(splines, 0.1); } @Test @@ -310,7 +307,7 @@ void testCircle() { checkCircle(splines, 0.011, 0.005); // optimize and compare - TrajectoryPlotter.plot(splines, 0.1, 1); + TrajectoryPlotter.plot(splines, 0.1); } @@ -368,7 +365,7 @@ void testLine() { splines.add(s0); splines.add(s1); - TrajectoryPlotter.plot(splines, 0.1, 1); + TrajectoryPlotter.plot(splines, 0.1); // spline joints are not C1 smooth assertFalse(verifyC1(splines)); @@ -414,15 +411,10 @@ void testPath0() { splines.add(s01); splines.add(s12); - // before assertTrue(verifyC1(splines)); assertTrue(verifyC2(splines)); - TrajectoryPlotter.plot(splines, 0.1, 1); - - // after - assertTrue(verifyC1(splines)); - assertTrue(verifyC2(splines)); + TrajectoryPlotter.plot(splines, 0.1); } @Test @@ -460,7 +452,7 @@ void testMismatchedXYDerivatives() { splines.add(s0); splines.add(s1); - TrajectoryPlotter.plot(splines, 0.1, 1); + TrajectoryPlotter.plot(splines, 0.1); for (HolonomicSpline s : splines) { if (DEBUG) diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java index 4e3b77a3b..0d52b54aa 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/QuinticHermiteOptimizerTest.java @@ -36,7 +36,7 @@ void test0() { splines.add(new HolonomicSpline(a, b)); splines.add(new HolonomicSpline(b, c)); - TrajectoryPlotter.plot(splines, 5, 10); + TrajectoryPlotter.plot(splines, 5); } @@ -69,7 +69,7 @@ void test1() { splines.add(new HolonomicSpline(e, f)); splines.add(new HolonomicSpline(f, g)); - TrajectoryPlotter.plot(splines, 5, 10); + TrajectoryPlotter.plot(splines, 5); } @@ -107,7 +107,7 @@ void test2() { splines2.add(new HolonomicSpline(j, k)); splines2.add(new HolonomicSpline(k, l)); - TrajectoryPlotter.plot(splines2, 5, 10); + TrajectoryPlotter.plot(splines2, 5); } @@ -133,7 +133,7 @@ void test3() { splines.add(new HolonomicSpline(a, b)); splines.add(new HolonomicSpline(b, c)); - TrajectoryPlotter.plot(splines, 5, 10); + TrajectoryPlotter.plot(splines, 5); } @@ -166,7 +166,7 @@ void test4() { splines.add(new HolonomicSpline(e, f)); splines.add(new HolonomicSpline(f, g)); - TrajectoryPlotter.plot(splines, 5, 10); + TrajectoryPlotter.plot(splines, 5); } @Test @@ -204,7 +204,7 @@ void test5() { splines.add(new HolonomicSpline(j, k)); splines.add(new HolonomicSpline(k, l)); - TrajectoryPlotter.plot(splines, 5, 10); + TrajectoryPlotter.plot(splines, 5); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index c595b4803..f887104e9 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -126,7 +126,7 @@ void testJustTurningInPlace() { assertEquals(Math.PI, path.getMaxDistance(), DELTA); if (DEBUG) System.out.printf("PATH:\n%s\n", path); - TrajectoryPlotter.plot(path, 0.1, 1); + TrajectoryPlotter.plot(path, 0.1); List constraints = new ArrayList(); ScheduleGenerator u = new ScheduleGenerator(constraints); From 090b8a0a6aa298e9a0882815c118dd4ce34d384a Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 17 Dec 2025 20:59:29 -0800 Subject: [PATCH 14/42] more trajectory WIP --- .../team100/lib/geometry/DirectionSE2.java | 9 ++ .../lib/geometry/Pose2dWithMotion.java | 11 ++ .../tank/commands/FixedTrajectory.java | 2 +- .../tank/commands/ToPoseWithTrajectory.java | 2 +- .../java/org/team100/lib/trajectory/NEW.md | 101 +++++++++++++++ .../lib/trajectory/TrajectoryPlanner.java | 11 +- .../team100/lib/trajectory/path/Path100.java | 3 +- .../lib/trajectory/path/PathFactory.java | 35 +++--- .../path/spline/HolonomicSpline.java | 16 ++- .../trajectory/timing/ScheduleGenerator.java | 36 ++++-- .../lib/trajectory/timing/TimedPose.java | 6 + .../lib/trajectory/ParameterizationTest.java | 37 ++++++ .../lib/trajectory/TrajectoryTest.java | 97 +++++++++++++- .../path/spline/HolonomicSplineTest.java | 118 ++++++++++-------- 14 files changed, 396 insertions(+), 88 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/trajectory/NEW.md diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index 96e88980c..a3d57f090 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -3,6 +3,7 @@ import org.team100.lib.util.Math100; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; /** * A direction (i.e. unit-length vector) in the SE(2) manifold, describing the @@ -29,6 +30,14 @@ public DirectionSE2(double px, double py, double ptheta) { theta = ptheta / h; } + public Twist2d minus(DirectionSE2 other) { + return new Twist2d(x - other.x, y - other.y, theta - other.theta); + } + + public double normL2() { + return Math.sqrt(x * x + y * y + theta * theta); + } + /** Cartesian part of direction, as an old-fashioned Rotation2d */ public Rotation2d toRotation() { return new Rotation2d(x, y); diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index c5eeddc39..8c4e84c61 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -63,11 +63,22 @@ public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) { /** This now uses double-geodesic distance, i.e. L2 norm including rotation. */ public double distanceM(Pose2dWithMotion other) { + // + // this should match HolonomicSpline.getVelocity() for the + // dheading/dt thing to work. + // + // return GeometryUtil.doubleGeodesicDistance(this, other); + // + // // return // m_pose.pose().getTranslation().getDistance(other.m_pose.pose().getTranslation()); } + public double distanceCartesian(Pose2dWithMotion other) { + return m_pose.pose().getTranslation().getDistance(other.m_pose.pose().getTranslation()); + } + public boolean equals(Object other) { if (!(other instanceof Pose2dWithMotion)) { if (DEBUG) diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java index eab775a33..d999f24dc 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java @@ -65,7 +65,7 @@ public void execute() { Pose2d currentPose = m_drive.getPose(); Pose2d poseReference = current.state().getPose().pose(); double velocityReference = next.velocityM_S(); - double omegaReference = next.velocityM_S() * next.state().getCurvature(); + double omegaReference = next.velocityM_S() * next.state().getHeadingRateRad_M(); ChassisSpeeds speeds = m_controller.calculate( currentPose, poseReference, velocityReference, omegaReference); m_drive.setVelocity(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java index bd7bc86e8..2cf44c051 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java @@ -67,7 +67,7 @@ public void execute() { Pose2d currentPose = m_drive.getPose(); Pose2d poseReference = current.state().getPose().pose(); double velocityReference = next.velocityM_S(); - double omegaReference = next.velocityM_S() * next.state().getCurvature(); + double omegaReference = next.velocityM_S() * next.state().getHeadingRateRad_M(); ChassisSpeeds speeds = m_controller.calculate( currentPose, poseReference, velocityReference, omegaReference); m_drive.setVelocity(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); diff --git a/lib/src/main/java/org/team100/lib/trajectory/NEW.md b/lib/src/main/java/org/team100/lib/trajectory/NEW.md new file mode 100644 index 000000000..d0db7bb05 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/NEW.md @@ -0,0 +1,101 @@ +# New Trajectories + +## Background +Prior to 2026, Team 100 trajectories were implemented using code adapted from +254's 2022 repo. This solution was holonomic (unlike the WPI version), and included +optimization of inner knots to minimize change in curvature. The 254 code was written +in a very complex style, so the adaptation was a significant departure, but it still +contained many idiosyncrasies. + +It also supported many things that just didn't seem to matter very much. All our actual +trajectories for the past 2 seasons have had exactly one segment, so the optimizer +never even ran. Even in tests it doesn't seem to do anything that can't be achieved through +careful tuning, which we do anyway. And there was a class of things it couldn't do: +paths with corners, paths with only rotation, very short paths. Worst of all were +the remaining idiosyncrasies of implementation: many copies of similar poses and +related data, mutability. This was not something any student spent any time looking +closely at, it was just a black box. + +For 2026, we should have a trajectory generation system that the students can understand, +end-to-end. It should be much simpler, and without any of the 254 remnants. + +## Design + +As before, there will be several stages: + +* Make a spline +* Sample the spline to make a piecewise linear path close enough to the real spline +* Assign timing, velocity, and acceleration to each point in the path + +The representation will use SE(2), rather than the mix of R2 and SE(2) we used before. +This means that the distance metric will include rotation: for now we'll use the +L2 norm on all three components, with equal weighting. We might come back to the +weighting if it seems to matter. + +The SE(2) implementation will mean that rotation-only paths will work exactly the +same as linear or blended ones. + +Each step in the process will be completely separate, unlike the previous version, +where the spline parameter (and derivatives) were used as part of the later steps. +Thus the scheduler should be able to work on *any* sequence of poses. + +## Velocity + +All three components of velocity of each sample are used for feedforward +and for constraints. Because the acceleration within each segment is constant, +the velocities can be interpolated from the velocities at the endpoints, +or integrated from the starting end, given the acceleration at that point. + +The previous scheduler computed (scalar, pathwise) velocity from the +(constrained) acceleration along a segment. Heading velocity was +computed from the pathwise velocity using the direction of travel +("heading rate per meter"). This obviously won't work when the +pathwise velocity is zero, as it would be for turn-in-place segments. + +For these computations, yet another annotation of the path is used, +with `ConstrainedState`, which remembers (pathwise, scalar, mutable) +velocity and acceleration. + +Accordingly, the sampled datatype needs to have SE(2) velocity, whether +the integration or interpolation approach is used. + +## Acceleration + +All three components of acceleration of each sample are affected +by constraints, during scheduling. + +It would also be nice to use acceleration for feedforward, like we do +for simpler mechanisms. + +The acceleration within each segment is constant, +by definition, but it's not knowable by looking at the timing of the +endpoint poses alone. It *is* calculable given the velocities at the +endpoints. + +The previous scheduler computed (scalar, pathwise) acceleration, +and records it in `TimedPose.` + +Both pathwise and centripetal acceleration are available in the Control +copy of the TimedPose, where it seems to be used in the Jacobian +(for planar mechanisms) -- it woudl be good to use this for driving too. + +Accordingly, the sampled datatype needs to have the SE(2) acceleration too. + +## Path Generation + +We've gone back and forth on pathing on the manifold or pathing on the tangent space, +with just the planar Euclidean metric. Using the manifold, for samples far apart, +makes noticeable "scalloping" of the resulting path. Of course, using the tangent +space also produces "facets," but they're straight. + +For this design, paths will be in the manifold, which means scallops, so the +samples should be relatively close together. + +## Scheduling + +Given a sequence of poses, the scheduler connects them with constant-direction arcs +(twists) in SE(2) that are traversed with constant acceleration. + +Using constant-direction arcs means that all the information about the path is given +by the poses themselves: there's no reason to use the spline to produce anything +about heading rate or curvature. diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 45c44acda..e24d5e7e6 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -3,10 +3,9 @@ import java.util.List; import java.util.function.Function; -import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; @@ -29,6 +28,7 @@ * 4. assign timestamps to each step */ public class TrajectoryPlanner { + private static final boolean DEBUG = true; /* * Maximum distance of the secant lines to the continuous spline. The resulting * path will have little scallops if it involves rotation. In SE(2), a constant @@ -228,12 +228,17 @@ public Trajectory100 generateTrajectory( m_splineTolerance, m_splineTolerance, m_splineRotationTolerance); + if (DEBUG) + System.out.printf("PATH\n%s\n", path); // Generate the timed trajectory. - return m_scheduleGenerator.timeParameterizeTrajectory( + Trajectory100 result = m_scheduleGenerator.timeParameterizeTrajectory( path, m_trajectoryStep, start_vel, end_vel); + if (DEBUG) + System.out.printf("TRAJECTORY\n%s\n", result); + return result; } catch (IllegalArgumentException e) { // catches various kinds of malformed input, returns a no-op. // this should never actually happen. diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 29aa5336d..08adbbced 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -57,7 +57,8 @@ public double getMaxDistance() { } /** - * Walks through all the points to find the bracketing points. + * Walks through all the points to find the bracketing points, and then + * interpolates between them. * * @param distance in meters, always a non-negative number. */ diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 31a86b7df..b29215e9b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -4,8 +4,8 @@ import java.util.List; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; @@ -13,7 +13,7 @@ import edu.wpi.first.math.geometry.Twist2d; public class PathFactory { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; public static Path100 pathFromWaypoints( List waypoints, @@ -69,7 +69,7 @@ public static List parameterizeSplines( for (int i = 0; i < splines.size(); i++) { HolonomicSpline s = splines.get(i); if (DEBUG) - System.out.printf("%d %s\n", i, s); + System.out.printf("SPLINE:\n%d\n%s\n", i, s); List samples = parameterizeSpline(s, maxDx, maxDy, maxDTheta, 0.0, 1.0); samples.remove(0); rv.addAll(samples); @@ -78,20 +78,25 @@ public static List parameterizeSplines( } /** - * Recursively add points until they're close to the real curve. + * Recursive bisection to find a series of secant lines close to the real curve. + * + * Note if the path is s-shaped, then bisection can find the middle :-) */ private static void getSegmentArc( - HolonomicSpline s, + HolonomicSpline spline, List rv, double t0, // [0,1] not time double t1, // [0,1] not time double maxDx, double maxDy, double maxDTheta) { - Pose2d p0 = s.getPose2d(t0); + // points must be this close together + // TODO: make this a parameter. + final double maxNorm = 0.1; + Pose2d p0 = spline.getPose2d(t0); double thalf = (t0 + t1) / 2; - Pose2d phalf = s.getPose2d(thalf); - Pose2d p1 = s.getPose2d(t1); + Pose2d phalf = spline.getPose2d(thalf); + Pose2d p1 = spline.getPose2d(t1); // twist from p0 to p1 Twist2d twist_full = p0.log(p1); @@ -102,15 +107,17 @@ private static void getSegmentArc( // difference between twist and sample Transform2d error = phalf_predicted.minus(phalf); - if (Math.abs(error.getTranslation().getX()) > maxDx || - Math.abs(error.getTranslation().getY()) > maxDy || - Math.abs(error.getRotation().getRadians()) > maxDTheta) { + if (Math.abs(error.getTranslation().getX()) > maxDx + || Math.abs(error.getTranslation().getY()) > maxDy + || Math.abs(error.getRotation().getRadians()) > maxDTheta + || GeometryUtil.normL2(twist_half) > maxNorm) { // add a point in between - getSegmentArc(s, rv, t0, thalf, maxDx, maxDy, maxDTheta); - getSegmentArc(s, rv, thalf, t1, maxDx, maxDy, maxDTheta); + // note the extra condition to avoid points too far apart. + getSegmentArc(spline, rv, t0, thalf, maxDx, maxDy, maxDTheta); + getSegmentArc(spline, rv, thalf, t1, maxDx, maxDy, maxDTheta); } else { // midpoint is close enough, this looks good - rv.add(s.getPose2dWithMotion(t1)); + rv.add(spline.getPose2dWithMotion(t1)); } } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index cace22346..bec9c2bc9 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -104,7 +104,10 @@ public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { @Override public String toString() { - return "HolonomicSpline [m_x=" + m_x + ", m_y=" + m_y + ", m_theta=" + m_heading + ", m_r0=" + m_heading0 + "]"; + return "HolonomicSpline [m_x=" + m_x + + ", m_y=" + m_y + + ", m_theta=" + m_heading + + ", m_r0=" + m_heading0 + "]"; } /** @@ -200,7 +203,16 @@ public double ddx(double s) { * Since s is not time, it is not "velocity" in the usual sense. */ private double getVelocity(double s) { - return Math.hypot(dx(s), dy(s)); + // + // + double dx = dx(s); + double dy = dy(s); + double dtheta = dtheta(s); + // return Math.hypot(dx, dy); + // + // + // now yields SE(2) L2 norm, not just cartesian. + return Math.sqrt(dx*dx+dy*dy+dtheta*dtheta); } /** diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 90adc07b6..b8ed8a0a2 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -12,7 +12,7 @@ * schedule. */ public class ScheduleGenerator { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; private static final double EPSILON = 1e-6; /** this is the default, in order to make the constraints set the actual */ private static final double HIGH_ACCEL = 1000; @@ -33,17 +33,7 @@ public Trajectory100 timeParameterizeTrajectory( double start_vel, double end_vel) { try { - double maxDistance = path.getMaxDistance(); - if (maxDistance == 0) - throw new IllegalArgumentException(); - int num_states = (int) Math.ceil(maxDistance / step + 1); - List samples = new ArrayList<>(num_states); - for (int i = 0; i < num_states; ++i) { - Pose2dWithMotion state = path.sample(Math.min(i * step, maxDistance)); - if (DEBUG) - System.out.printf("%d %s\n", i, state); - samples.add(state); - } + List samples = resample(path, step); return timeParameterizeTrajectory(samples, start_vel, end_vel); } catch (TimingException e) { e.printStackTrace(); @@ -52,11 +42,31 @@ public Trajectory100 timeParameterizeTrajectory( } } + /** + * Samples the path evenly by distance. + */ + private List resample(Path100 path, double step) throws TimingException { + double maxDistance = path.getMaxDistance(); + if (maxDistance == 0) + throw new IllegalArgumentException(); + int num_states = (int) Math.ceil(maxDistance / step + 1); + List samples = new ArrayList<>(num_states); + for (int i = 0; i < num_states; ++i) { + // the dtheta and curvature come from here and are never changed. + // the values here are just interpolated from the original values. + Pose2dWithMotion state = path.sample(Math.min(i * step, maxDistance)); + if (DEBUG) + System.out.printf("RESAMPLE: %d %s\n", i, state); + samples.add(state); + } + return samples; + } + /** * input is some set of samples (could be evenly sampled or not), output is * these same samples with time. */ - private Trajectory100 timeParameterizeTrajectory( + public Trajectory100 timeParameterizeTrajectory( List samples, double start_vel, double end_vel) throws TimingException { diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java index 9bb359d86..21b0738fc 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java @@ -92,10 +92,16 @@ public TimedPose interpolate2(TimedPose other, double x) { m_accelM_S_S); } + /** L2 norm */ public double distance(TimedPose other) { return m_state.distanceM(other.m_state); } + public double distanceCartesian(TimedPose other) { + return m_state.distanceCartesian(other.m_state); + } + + @Override public boolean equals(final Object other) { if (!(other instanceof TimedPose)) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java index 8f62d3840..be13114b5 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java @@ -60,6 +60,40 @@ void testSplineStraight() { TrajectoryPlotter.actuallyPlot("spline", renderer, d1, d2, d3); } + /** + * Shows x as a function of the spline parameter, s. + */ + @Test + void testSplineCurved() { + // a straight line in x, since the direction is also +x + // note the zero scale here to force zero velocity at the ends + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d(0)), + new DirectionSE2(0, 1, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(0)), + new DirectionSE2(0, 1, 0), 1)); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("spline", List.of(spline)); + + // XYSeries sx = SplineToVectorSeries.x("x", List.of(spline)); + // XYSeries sxPrime = SplineToVectorSeries.xPrime("xprime", List.of(spline)); + // XYSeries sxPrimePrime = SplineToVectorSeries.xPrimePrime("xprimeprime", + // List.of(spline)); + + // XYDataset d1 = TrajectoryPlotter.collect(sx); + // XYDataset d2 = TrajectoryPlotter.collect(sxPrime); + // XYDataset d3 = TrajectoryPlotter.collect(sxPrimePrime); + + // TrajectoryPlotter.actuallyPlot("spline", renderer, d1, d2, d3); + } + /** * Show x as a function of the pose list index. * The pose list has no parameter, it's just a list @@ -105,6 +139,9 @@ void testTrajectory() { new DirectionSE2(0, 1, 0), 1)); Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); + // this is wrong somehow + System.out.printf("TRAJECTORY\n%s\n", trajectory); + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("trajectory", trajectory); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index acbd9d3b9..70aca6c6a 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -4,11 +4,13 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TimedPose; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -20,6 +22,7 @@ * These pop up GUI windows, so leave them commented out when you check in. */ public class TrajectoryTest { + private static final boolean DEBUG = true; private static final boolean SHOW = true; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -39,7 +42,99 @@ void testSimple() throws InterruptedException { new Pose2d(0, 0, new Rotation2d()), new Pose2d(10, 1, new Rotation2d())); if (SHOW) - new TrajectoryPlotter(0.5).plot("simple", t ); + new TrajectoryPlotter(0.5).plot("simple", t); + } + + @Test + void testCircle() { + // see HolonomicSplineTest.testCircle(); + // this is to see how to create the dtheta and curvature + // without the spline. + double scale = 0.9; + WaypointSE2 p0 = new WaypointSE2( + new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), + new DirectionSE2(0, 1, 1), scale); + WaypointSE2 p1 = new WaypointSE2( + new Pose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg), + new DirectionSE2(-1, 0, 1), scale); + WaypointSE2 p2 = new WaypointSE2( + new Pose2d(new Translation2d(-1, 0), Rotation2d.kZero), + new DirectionSE2(0, -1, 1), scale); + WaypointSE2 p3 = new WaypointSE2( + new Pose2d(new Translation2d(0, -1), Rotation2d.kCCW_90deg), + new DirectionSE2(1, 0, 1), scale); + + List waypoints = List.of(p0, p1, p2, p3, p0); + + List c = List.of( + new ConstantConstraint(log, 2, 0.5), + new YawRateConstraint(log, 1, 1)); + TrajectoryPlanner p = new TrajectoryPlanner(c); + Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); + + // + TrajectoryPlotter.plot(trajectory, 0.25); + + } + + @Test + void testDheading() { + double scale = 0.9; + WaypointSE2 w0 = new WaypointSE2( + new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), + new DirectionSE2(0, 1, 1), scale); + WaypointSE2 w1 = new WaypointSE2( + new Pose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg), + new DirectionSE2(-1, 0, 1), scale); + List waypoints = List.of(w0, w1); + + List c = List.of( + new ConstantConstraint(log, 2, 0.5), + new YawRateConstraint(log, 1, 1)); + TrajectoryPlanner p = new TrajectoryPlanner(c); + Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); + double duration = trajectory.duration(); + TimedPose p0 = trajectory.sample(0); + if (DEBUG) + System.out.println( + "t, intrinsic_heading_dt, heading_dt, intrinsic_ca, extrinsic_ca, extrinsic v, intrinsic v, dcourse, dcourse1"); + for (double t = 0.04; t < duration; t += 0.04) { + TimedPose p1 = trajectory.sample(t); + double distance = p1.distance(p0); + double distanceCartesian = p1.distanceCartesian(p0); + Rotation2d heading0 = p0.state().getPose().pose().getRotation(); + Rotation2d heading1 = p1.state().getPose().pose().getRotation(); + double dheading = heading1.minus(heading0).getRadians(); + // compute time derivative of heading two ways: + // this just compares the poses and uses the known time step + double dheadingDt = dheading / 0.04; + // this uses the intrinsic heading rate and the velocity + // rad/m * m/s = rad/s + double intrinsicDheadingDt = p0.state().getHeadingRateRad_M() * p0.velocityM_S(); + // curvature is used to compute centripetal acceleration + // ca = v^2*curvature + DirectionSE2 course0 = p0.state().getPose().course(); + DirectionSE2 course1 = p1.state().getPose().course(); + p1.state().getPose().pose().log(p0.state().getPose().pose() + double dcourse1 = GeometryUtil.norm(course1.minus(course0)); + // double dcourse1 = GeometryUtil.normL2(course1.minus(course0)); + double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians(); + double radius = dcourse / distance; + // derived v is pretty close + double v = distance / 0.04; + double extrinsicCa = v * v / radius; + double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.state().getCurvature(); + if (DEBUG) + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + t, intrinsicDheadingDt, dheadingDt, + intrinsicCa, extrinsicCa, + v, p0.velocityM_S(), + dcourse, dcourse1); + p0 = p1; + } + + // + TrajectoryPlotter.plot(trajectory, 0.25); } /** diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 1639c330f..81b1e42b7 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -9,8 +9,9 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -31,7 +32,7 @@ import edu.wpi.first.math.geometry.Translation2d; class HolonomicSplineTest implements Timeless { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -249,55 +250,23 @@ void testCircle() { // // I fiddled with the scale to make a pretty good circle. double scale = 0.9; - HolonomicSpline s0 = new HolonomicSpline( - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - Rotation2d.k180deg), - new DirectionSE2(0, 1, 1), scale), - new WaypointSE2( - new Pose2d( - new Translation2d(0, 1), - Rotation2d.kCW_90deg), - new DirectionSE2(-1, 0, 1), scale)); - HolonomicSpline s1 = new HolonomicSpline( - new WaypointSE2( - new Pose2d( - new Translation2d(0, 1), - Rotation2d.kCW_90deg), - new DirectionSE2(-1, 0, 1), scale), - new WaypointSE2( - new Pose2d( - new Translation2d(-1, 0), - Rotation2d.kZero), - new DirectionSE2(0, -1, 1), scale)); - HolonomicSpline s2 = new HolonomicSpline( - new WaypointSE2( - new Pose2d( - new Translation2d(-1, 0), - Rotation2d.kZero), - new DirectionSE2(0, -1, 1), scale), - new WaypointSE2( - new Pose2d( - new Translation2d(0, -1), - Rotation2d.kCCW_90deg), - new DirectionSE2(1, 0, 1), scale)); - HolonomicSpline s3 = new HolonomicSpline( - new WaypointSE2( - new Pose2d( - new Translation2d(0, -1), - Rotation2d.kCCW_90deg), - new DirectionSE2(1, 0, 1), scale), - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - Rotation2d.k180deg), - new DirectionSE2(0, 1, 1), scale)); - List splines = new ArrayList<>(); - splines.add(s0); - splines.add(s1); - splines.add(s2); - splines.add(s3); + WaypointSE2 p0 = new WaypointSE2( + new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), + new DirectionSE2(0, 1, 1), scale); + WaypointSE2 p1 = new WaypointSE2( + new Pose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg), + new DirectionSE2(-1, 0, 1), scale); + WaypointSE2 p2 = new WaypointSE2( + new Pose2d(new Translation2d(-1, 0), Rotation2d.kZero), + new DirectionSE2(0, -1, 1), scale); + WaypointSE2 p3 = new WaypointSE2( + new Pose2d(new Translation2d(0, -1), Rotation2d.kCCW_90deg), + new DirectionSE2(1, 0, 1), scale); + HolonomicSpline s0 = new HolonomicSpline(p0, p1); + HolonomicSpline s1 = new HolonomicSpline(p1, p2); + HolonomicSpline s2 = new HolonomicSpline(p2, p3); + HolonomicSpline s3 = new HolonomicSpline(p3, p0); + List splines = List.of(s0, s1, s2, s3); // before assertTrue(verifyC1(splines)); @@ -311,6 +280,50 @@ void testCircle() { } + @Test + void testDheading() { + // does the spline-derived dheading match the post-hoc one? + double scale = 0.9; + WaypointSE2 w0 = new WaypointSE2( + new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), + new DirectionSE2(0, 1, 1), scale); + WaypointSE2 w1 = new WaypointSE2( + new Pose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg), + new DirectionSE2(-1, 0, 1), scale); + HolonomicSpline spline = new HolonomicSpline(w0, w1); + Pose2dWithMotion p0 = spline.getPose2dWithMotion(0.0); + if (DEBUG) + System.out.println( + "s, p0_heading_rate, p0_curvature, distance, post_hoc_heading_rate, post_hoc_curvature, post_hoc_heading_rate2, post_hoc_curvature2"); + for (double s = 0.01; s <= 1.0; s += 0.01) { + Pose2dWithMotion p1 = spline.getPose2dWithMotion(s); + double l2distance = p1.distanceM(p0); + double cartesianDistance = p1.distanceCartesian(p0); + Rotation2d heading0 = p0.getPose().pose().getRotation(); + Rotation2d heading1 = p1.getPose().pose().getRotation(); + double dheading = heading1.minus(heading0).getRadians(); + DirectionSE2 course0 = p0.getPose().course(); + DirectionSE2 course1 = p1.getPose().course(); + double curve = GeometryUtil.normL2(course1.minus(course0)); + // this value is wrong because we use the l2 distance, + // so it's kind of counting the rotation twice. + double dheadingDx = dheading / l2distance; + double curveDx = curve / l2distance; + // this value matches the intrinsic one since it just uses + // cartesian distance in the denominator. + double dheadingDx2 = dheading / cartesianDistance; + double curveDx2 = curve / cartesianDistance; + + if (DEBUG) + System.out.printf( + "%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f \n", + s, p0.getHeadingRateRad_M(), p0.getCurvature(), + l2distance, dheadingDx, curveDx, dheadingDx2, curveDx2); + p0 = p1; + } + + } + private void checkCircle(List splines, double rangeError, double azimuthError) { double actualRangeError = 0; double actualAzimuthError = 0; @@ -509,7 +522,8 @@ void testEntryVelocity() { List motion = PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05); if (DEBUG) { for (Pose2dWithMotion p : motion) { - System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), p.getPose().pose().getTranslation().getY()); + System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), + p.getPose().pose().getTranslation().getY()); } } Path100 path = new Path100(motion); From 887ad0789fa4661866bd65fa50fd39d8d69b7ede Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 18 Dec 2025 10:10:17 -0800 Subject: [PATCH 15/42] trajectory WIP --- .../team100/frc2025/Swerve/Auto/BigLoop.java | 67 +++++++++++++++++ .../org/team100/frc2025/robot/Binder.java | 32 ++++++-- .../org/team100/lib/geometry/StateSE2.java | 8 ++ .../java/org/team100/lib/trajectory/NEW.md | 75 +++++++++++++------ .../trajectory/timing/ScheduleGenerator.java | 12 ++- .../lib/trajectory/Trajectory100Test.java | 2 +- .../lib/trajectory/TrajectoryPlannerTest.java | 2 +- .../lib/trajectory/TrajectoryPlotter.java | 11 ++- .../lib/trajectory/TrajectoryTest.java | 4 +- .../lib/trajectory/path/PathFactoryTest.java | 18 ++--- .../path/spline/HolonomicSplineTest.java | 8 +- .../timing/ScheduleGeneratorTest.java | 59 ++++++++++++++- 12 files changed, 242 insertions(+), 56 deletions(-) create mode 100644 comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java create mode 100644 lib/src/main/java/org/team100/lib/geometry/StateSE2.java diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java new file mode 100644 index 000000000..8652e75c4 --- /dev/null +++ b/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 { + 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 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); + } +} diff --git a/comp/src/main/java/org/team100/frc2025/robot/Binder.java b/comp/src/main/java/org/team100/frc2025/robot/Binder.java index bddda98f6..9fc29c6ad 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Binder.java @@ -10,6 +10,7 @@ import org.team100.frc2025.CommandGroups.ScoreSmart.ScoreCoralSmartLuke; import org.team100.frc2025.Swerve.ManualWithBargeAssist; import org.team100.frc2025.Swerve.ManualWithProfiledReefLock; +import org.team100.frc2025.Swerve.Auto.BigLoop; import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.controller.r1.PIDFeedback; import org.team100.lib.controller.r3.ControllerFactoryR3; @@ -22,6 +23,7 @@ import org.team100.lib.profile.r3.HolonomicProfileFactory; import org.team100.lib.profile.r3.ProfileR3; import org.team100.lib.subsystems.prr.commands.FollowJointProfiles; +import org.team100.lib.subsystems.r3.commands.DriveWithTrajectoryFunction; import org.team100.lib.subsystems.r3.commands.FloorPickSequence2; import org.team100.lib.subsystems.r3.commands.ManualPosition; import org.team100.lib.subsystems.swerve.commands.SetRotation; @@ -140,7 +142,7 @@ public void bind() { m_machinery.m_swerveKinodynamics, 0.5, 1.0); // Pick a game piece from the floor, based on camera input. - + // This is the old one, commented out while I work on the new one. // whileTrue(driver::x, // parallel( @@ -188,17 +190,31 @@ public void bind() { // Drive to a scoring location at the reef and score. whileTrue(driver::b, m_machinery.m_manipulator.centerEject()); - whileTrue(driver::a, - // TODO make this seperate/combined with scoring in general - ScoreCoralSmartLuke.get( - coralSequence, m_machinery.m_mech, m_machinery.m_manipulator, - holonomicController, profile, m_machinery.m_drive, - m_machinery.m_localizer::setHeedRadiusM, buttons::level, buttons::point)); - // ScoreCoralSmart.get( + // whileTrue(driver::a, + // // TODO make this seperate/combined with scoring in general + // ScoreCoralSmartLuke.get( + // coralSequence, m_machinery.m_mech, m_machinery.m_manipulator, + // holonomicController, profile, m_machinery.m_drive, + // m_machinery.m_localizer::setHeedRadiusM, buttons::level, buttons::point)); + // // ScoreCoralSmart.get( // coralSequence, m_machinery.m_mech, m_machinery.m_manipulator, // holonomicController, profile, m_machinery.m_drive, // m_machinery.m_localizer::setHeedRadiusM, buttons::level, buttons::point)); + /////////////////////////////////////////////////// + // + // for testing + // + BigLoop bigloop = new BigLoop(rootLogger, m_machinery.m_swerveKinodynamics); + DriveWithTrajectoryFunction navigator = new DriveWithTrajectoryFunction( + rootLogger, m_machinery.m_drive, holonomicController, m_machinery.m_trajectoryViz, + bigloop); + whileTrue(driver::a, + navigator.until(navigator::isDone)); + // + // + /////////////////////////////////////////////////// + //////////////////////////////////////////////////////////// // // ALGAE diff --git a/lib/src/main/java/org/team100/lib/geometry/StateSE2.java b/lib/src/main/java/org/team100/lib/geometry/StateSE2.java new file mode 100644 index 000000000..8bc5a1d01 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/StateSE2.java @@ -0,0 +1,8 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.geometry.Pose2d; + +/** For the scheduler. */ +public record StateSE2(Pose2d pose, VelocitySE2 vel) { + +} diff --git a/lib/src/main/java/org/team100/lib/trajectory/NEW.md b/lib/src/main/java/org/team100/lib/trajectory/NEW.md index d0db7bb05..cd35aeded 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/NEW.md +++ b/lib/src/main/java/org/team100/lib/trajectory/NEW.md @@ -11,10 +11,11 @@ It also supported many things that just didn't seem to matter very much. All ou trajectories for the past 2 seasons have had exactly one segment, so the optimizer never even ran. Even in tests it doesn't seem to do anything that can't be achieved through careful tuning, which we do anyway. And there was a class of things it couldn't do: -paths with corners, paths with only rotation, very short paths. Worst of all were -the remaining idiosyncrasies of implementation: many copies of similar poses and -related data, mutability. This was not something any student spent any time looking -closely at, it was just a black box. +paths with corners, paths with only rotation, very short paths. It computed path-wise +velocity and acceleration, and had separate mechanisms for heading rate and +course rate, probably a remnant of a previous "tank" drive version. +In any case, none of it was anything any student spent any time looking closely at, +it was just a black box. For 2026, we should have a trajectory generation system that the students can understand, end-to-end. It should be much simpler, and without any of the 254 remnants. @@ -23,8 +24,8 @@ end-to-end. It should be much simpler, and without any of the 254 remnants. As before, there will be several stages: -* Make a spline -* Sample the spline to make a piecewise linear path close enough to the real spline +* Given a series of waypoints, make a spline +* Sample the spline to make a piecewise linear path with short-enough segments, close enough to the real spline * Assign timing, velocity, and acceleration to each point in the path The representation will use SE(2), rather than the mix of R2 and SE(2) we used before. @@ -39,6 +40,33 @@ Each step in the process will be completely separate, unlike the previous versio where the spline parameter (and derivatives) were used as part of the later steps. Thus the scheduler should be able to work on *any* sequence of poses. +## Splines + +The spline generator is the only thing that uses the direction in the waypoints; +after that, there's just a list of poses. + +## Sampling + +Previously there were two stages of sampling: bisection to get secants +close to the spline, and then walking the secant lines to get segments close +together. This design wasn't great: + +* S-shaped curves could fail the bisection process -- the middle point is "close" +even though others are not. +* walking the secants is inferior to just bisecting more + +The new version just bisects with both criteria (midpoint closeness and segment shortness) + +## Interpolation + +Originally, interpolation between samples integrated geodesics in the SE(2) manifold, +yielding scallops. This behavior was partially removed in 2025, and the new version +removes it entirely. There's no reason for the path between samples to have +constant direction in SE(2). + +The path between samples *does* have constant *acceleration*, because that makes +everything easier. + ## Velocity All three components of velocity of each sample are used for feedforward @@ -68,34 +96,39 @@ It would also be nice to use acceleration for feedforward, like we do for simpler mechanisms. The acceleration within each segment is constant, -by definition, but it's not knowable by looking at the timing of the +by definition, but it can't be calculated by looking at the timing of the endpoint poses alone. It *is* calculable given the velocities at the endpoints. The previous scheduler computed (scalar, pathwise) acceleration, -and records it in `TimedPose.` +and recorded it in `TimedPose.` Both pathwise and centripetal acceleration are available in the Control copy of the TimedPose, where it seems to be used in the Jacobian -(for planar mechanisms) -- it woudl be good to use this for driving too. +(for planar mechanisms) -- it would be good to use this for driving too. Accordingly, the sampled datatype needs to have the SE(2) acceleration too. -## Path Generation +## Constraints + +The previous design had separate constraint implmentations for path-wise +and centripetal acceleration, a remnant of its "tank" drive roots. The +actual acceleration constraint is some sort of truncated circle: + +* radius of the circle is determined by capsize force or carpet grip. +* truncation of the circle is determined by velocity -- back-EMF limits motor torque. -We've gone back and forth on pathing on the manifold or pathing on the tangent space, -with just the planar Euclidean metric. Using the manifold, for samples far apart, -makes noticeable "scalloping" of the resulting path. Of course, using the tangent -space also produces "facets," but they're straight. +At each step in scheduling, the directions of the velocity and acceleration +vectors are known, so the constraint API should supply them. The notion +of "centripital" acceleration should disappear. -For this design, paths will be in the manifold, which means scallops, so the -samples should be relatively close together. +The previous design also separated the cartesian and rotational components +of the velocity constraint; these should be combined. ## Scheduling -Given a sequence of poses, the scheduler connects them with constant-direction arcs -(twists) in SE(2) that are traversed with constant acceleration. +Given a sequence of poses, the scheduler annotates each with its velocity, +and creates an arc of constant acceleration linking each pair. -Using constant-direction arcs means that all the information about the path is given -by the poses themselves: there's no reason to use the spline to produce anything -about heading rate or curvature. +The result is not a series of SE(2) twists; each of the three +components are considered indepenent (to avoid scallops). diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index b8ed8a0a2..3b06a4aac 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -308,7 +308,11 @@ static double v1(double v0, double a, double ds) { * @param v1 final velocity * @param ds distance */ - static double accel(double v0, double v1, double ds) { + public static double accel(double v0, double v1, double ds) { + if (Math.abs(ds) < 1e-6) { + // prevent division by zero + return 0; + } /* * a = dv/dt * v = ds/dt @@ -319,7 +323,11 @@ static double accel(double v0, double v1, double ds) { * a = (v0+v1)(v1-v0)/2ds * a = (v1^2 - v0^2)/2ds */ - return (v1 * v1 - v0 * v0) / (2.0 * ds); + double a = (v1 * v1 - v0 * v0) / (2.0 * ds); + double dv = v1 - v0; + // this can be negative, which indicates that v1 precedes v0 + double dt = a / dv; + return a; } public static class TimingException extends Exception { diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 24aa53da0..6f254e939 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -175,7 +175,7 @@ void testSamplePerformance() throws TimingException { Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.02, 0.2, 0.1); - assertEquals(23.049, path.getMaxDistance(), 0.001); + assertEquals(23.063, path.getMaxDistance(), 0.001); start = System.nanoTime(); for (int rep = 0; rep < reps; ++rep) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index b58e2d4b1..de5328c21 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -187,7 +187,7 @@ void test2d() { ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 1, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); - assertEquals(2.958, traj.duration(), DELTA); + assertEquals(2.869, traj.duration(), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java index 367c13f2e..7e4992e40 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -29,7 +29,7 @@ import edu.wpi.first.math.geometry.Pose2d; public class TrajectoryPlotter { - public static final boolean SHOW = true; + public static final boolean SHOW = false; private static final int SIZE = 500; private final TrajectoryToVectorSeries converter; @@ -111,7 +111,8 @@ public static Range yRange(XYDataset dataset) { } /** - * renderer is a supplier because new XYPlot writes the dataset name into the renderer. + * renderer is a supplier because new XYPlot writes the dataset name into the + * renderer. * * mutability is bad. */ @@ -119,6 +120,8 @@ public static void actuallyPlot( String name, Supplier renderer, XYDataset... dataSets) { + if (!SHOW) + return; // "true" means "modal" so wait for close. JDialog frame = new JDialog((Frame) null, name, true); @@ -145,10 +148,6 @@ public static void actuallyPlot( frame.setDefaultCloseOperation(WindowConstants.DISPOSE_ON_CLOSE); } - - - - public static void plot( Trajectory100 t, double arrows) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 70aca6c6a..a97aa6f2a 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -23,7 +23,7 @@ */ public class TrajectoryTest { private static final boolean DEBUG = true; - private static final boolean SHOW = true; + private static final boolean SHOW = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); /** @@ -115,7 +115,7 @@ void testDheading() { // ca = v^2*curvature DirectionSE2 course0 = p0.state().getPose().course(); DirectionSE2 course1 = p1.state().getPose().course(); - p1.state().getPose().pose().log(p0.state().getPose().pose() + p1.state().getPose().pose().log(p0.state().getPose().pose()); double dcourse1 = GeometryUtil.norm(course1.minus(course0)); // double dcourse1 = GeometryUtil.normL2(course1.minus(course0)); double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 5ed148dc8..eb625fdee 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -42,7 +42,7 @@ void testBackingUp() { 0.0127, 0.0127, Math.toRadians(1.0)); - assertEquals(10, path.length()); + assertEquals(14, path.length()); } /** Preserves the tangent at the corner and so makes a little "S" */ @@ -66,12 +66,12 @@ void testCorner() { new DirectionSE2(0, 1, 0), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - assertEquals(9, path.length()); + assertEquals(17, path.length()); Pose2dWithMotion p = path.getPoint(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); - p = path.getPoint(1); + p = path.getPoint(8); assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); @@ -92,12 +92,12 @@ void testLinear() { new DirectionSE2(1, 0, 0), 1)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.01, 0.01, 0.1); - assertEquals(2, path.length()); + assertEquals(9, path.length()); Pose2dWithMotion p = path.getPoint(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); - p = path.getPoint(1); + p = path.getPoint(8); assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); @@ -175,7 +175,7 @@ void testComposite() { new DirectionSE2(1, 0, 0), 1)); Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(foo, 0.1); - assertEquals(4, foo.length(), 0.001); + assertEquals(28, foo.length(), 0.001); } @Test @@ -210,7 +210,7 @@ void test() { assertEquals(15.0, pose.pose().getTranslation().getX(), 0.001); assertEquals(10.0, pose.pose().getTranslation().getY(), 0.001); assertEquals(78.690, pose.course().toRotation().getDegrees(), 0.001); - assertEquals(20.416, arclength, 0.001); + assertEquals(20.428, arclength, 0.001); } @Test @@ -270,8 +270,8 @@ void testPerformance() { System.out.printf("total duration ms: %5.3f\n", totalDurationMs); System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations); } - assertEquals(5, t.length()); - Pose2dWithMotion p = t.getPoint(1); + assertEquals(17, t.length()); + Pose2dWithMotion p = t.getPoint(4); assertEquals(0.417, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 81b1e42b7..621101888 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -143,7 +143,7 @@ void testRotationSoft() { assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); // high rotation rate in the middle - assertEquals(4.807, p.getHeadingRateRad_M(), DELTA); + assertEquals(0.979, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); @@ -184,17 +184,17 @@ void testRotationFast() { Pose2dWithMotion p = s.getPose2dWithMotion(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); - assertEquals(1, p.getHeadingRateRad_M(), DELTA); + assertEquals(0.707, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(0.5); assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); - assertEquals(1, p.getHeadingRateRad_M(), DELTA); + assertEquals(0.707, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(1, p.getPose().pose().getRotation().getRadians(), DELTA); - assertEquals(1, p.getHeadingRateRad_M(), DELTA); + assertEquals(0.707, p.getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index f887104e9..32070c46a 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -10,9 +10,12 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.StateSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -104,7 +107,7 @@ public void checkTrajectory( } /** - * Turning in place does not work. + * Turning in place works now. */ @Test void testJustTurningInPlace() { @@ -138,7 +141,7 @@ void testJustTurningInPlace() { if (DEBUG) System.out.printf("TRAJ:\n%s\n", traj); - assertTrue(traj.isEmpty()); + assertFalse(traj.isEmpty()); } @@ -328,4 +331,56 @@ void testPerformance() { } + /** + * Fundamental math. + * + * Components are treated as independent. + */ + @Test + void testBasic0() { + // Given two states, find the acceleration between them. + StateSE2 s0 = new StateSE2( + new Pose2d(0, 0, new Rotation2d(0)), + new VelocitySE2(0, 0, 0)); + StateSE2 s1 = new StateSE2( + new Pose2d(1, 0, new Rotation2d(0)), + new VelocitySE2(1, 0, 0)); + AccelerationSE2 a = new AccelerationSE2( + ScheduleGenerator.accel( + s0.vel().x(), s1.vel().x(), s1.pose().getX() - s0.pose().getX()), + ScheduleGenerator.accel( + s0.vel().y(), s1.vel().y(), s1.pose().getY() - s0.pose().getY()), + ScheduleGenerator.accel( + s0.vel().theta(), s1.vel().theta(), + s1.pose().getRotation().minus(s0.pose().getRotation()).getRadians())); + // 1 meter at 1 m/s, a=0.5 m/s, t= 2 + assertEquals(0.5, a.x(), DELTA); + assertEquals(0, a.y(), DELTA); + assertEquals(0, a.theta(), DELTA); + } + + @Test + void testBasic1() { + // This case makes no sense. + StateSE2 s0 = new StateSE2( + new Pose2d(0, 0, new Rotation2d(0)), + new VelocitySE2(0, 0, 0)); + StateSE2 s1 = new StateSE2( + new Pose2d(1, 0, new Rotation2d(0)), // <<< positive position + new VelocitySE2(-1, 0, 0)); // <<< negative velocity + AccelerationSE2 a = new AccelerationSE2( + ScheduleGenerator.accel( + s0.vel().x(), s1.vel().x(), s1.pose().getX() - s0.pose().getX()), + ScheduleGenerator.accel( + s0.vel().y(), s1.vel().y(), s1.pose().getY() - s0.pose().getY()), + ScheduleGenerator.accel( + s0.vel().theta(), s1.vel().theta(), + s1.pose().getRotation().minus(s0.pose().getRotation()).getRadians())); + // 1 meter at 1 m/s, a=0.5 m/s, t= 2 + // this acceleration is for negative time, i.e. from s1 to s0, decelerating. + assertEquals(0.5, a.x(), DELTA); + assertEquals(0, a.y(), DELTA); + assertEquals(0, a.theta(), DELTA); + } + } From 6815c298fbdca1b553d79d07f710b0a28eed86d5 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 18 Dec 2025 20:32:56 -0800 Subject: [PATCH 16/42] more trajectory WIP --- .../kinodynamics/limiter/SwerveUtil.java | 2 +- .../java/org/team100/lib/trajectory/README.md | 15 ++- .../lib/trajectory/TrajectoryPlanner.java | 2 +- .../lib/trajectory/path/PathFactory.java | 2 +- .../lib/trajectory/path/spline/SplineR1.java | 32 +++---- .../trajectory/timing/ConstantConstraint.java | 3 + .../trajectory/timing/ConstrainedState.java | 12 ++- .../lib/trajectory/timing/DirectSchedule.java | 94 ++++++++++++++++++ .../trajectory/timing/ScheduleGenerator.java | 40 +++++++- .../timing/SwerveDriveDynamicsConstraint.java | 6 ++ .../lib/trajectory/Trajectory100Test.java | 2 +- .../lib/trajectory/TrajectoryTest.java | 2 +- .../path/spline/HolonomicSplineTest.java | 2 +- .../trajectory/timing/DirectScheduleTest.java | 95 +++++++++++++++++++ .../timing/ScheduleGeneratorTest.java | 2 +- .../timing/TrajectoryVelocityProfileTest.java | 50 +++++----- 16 files changed, 311 insertions(+), 50 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java create mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java index 65208de7d..b12e033d1 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java @@ -7,7 +7,7 @@ import org.team100.lib.util.Math100; public class SwerveUtil { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; /** * At low speed, accel is limited by the current limiters. diff --git a/lib/src/main/java/org/team100/lib/trajectory/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md index c4adb5989..ab353d251 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/README.md @@ -43,11 +43,20 @@ The velocity with respect to $s$ is $q'(s) = dq/ds$ The accleration with respect to $s$ is $q''(s) = d^2q/ds^2$ -So we're constructing a function for $s(t)$ and its derivatives. +So we're constructing a function for $s(t)$ and its time derivatives, -so using the chain rule: $v(t) = \dot{q}(t) = \dfrac{dq}{ds} \dfrac{dq}{dt} = q'(s)\dot{s}$ +$\dot{s} = \dfrac{ds}{dt}$ -and using the product rule: $a(t) = \ddot{q}(t) = q''(s)\dot{s}^2 + q'(s)\ddot{s}$ +$\ddot{s} = \dfrac{d^2s}{dt^2}$ + +Position is just the composite: +$x(t) = q(t) = q(s(t))$ + +so using the chain rule: +$v(t) = \dot{q}(t) = \dfrac{dq}{ds} \dfrac{ds}{dt} = q'(s(t))\dot{s}$ + +and using the product rule: +$a(t) = \ddot{q}(t) = q''(s)\dot{s}^2 + q'(s)\ddot{s}$ ## Constructing $s(t)$ diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index e24d5e7e6..cdff9560d 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -28,7 +28,7 @@ * 4. assign timestamps to each step */ public class TrajectoryPlanner { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; /* * Maximum distance of the secant lines to the continuous spline. The resulting * path will have little scallops if it involves rotation. In SE(2), a constant diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index b29215e9b..145664e08 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -13,7 +13,7 @@ import edu.wpi.first.math.geometry.Twist2d; public class PathFactory { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; public static Path100 pathFromWaypoints( List waypoints, diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java index c28e45d1e..516409398 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java @@ -130,39 +130,39 @@ SplineR1 addCoefs(SplineR1 other) { } /** - * @param t ranges from 0 to 1 - * @return the point on the spline for that t value + * @param s ranges from 0 to 1 + * @return the point on the spline for that s value */ - public double getPosition(double t) { - return a * t * t * t * t * t + b * t * t * t * t + c * t * t * t + d * t * t + e * t + f; + public double getPosition(double s) { + return a * s * s * s * s * s + b * s * s * s * s + c * s * s * s + d * s * s + e * s + f; } /** - * @return rate of change of position with respect to parameter, i.e. ds/dt + * @return rate of change of position with respect to parameter, i.e. dq/ds */ - public double getVelocity(double t) { - return 5 * a * t * t * t * t + 4 * b * t * t * t + 3 * c * t * t + 2 * d * t + e; + public double getVelocity(double s) { + return 5 * a * s * s * s * s + 4 * b * s * s * s + 3 * c * s * s + 2 * d * s + e; } /** - * @return acceleration of position with respect to parameter, i.e. d^2s/dt^2 + * @return acceleration of position with respect to parameter, i.e. d^2q/ds^2 */ - public double getAcceleration(double t) { - return 20 * a * t * t * t + 12 * b * t * t + 6 * c * t + 2 * d; + public double getAcceleration(double s) { + return 20 * a * s * s * s + 12 * b * s * s + 6 * c * s + 2 * d; } /** - * @return jerk of position with respect to parameter, i.e. d^3s/dt^3. + * @return jerk of position with respect to parameter, i.e. d^3q/ds^3. */ - public double getJerk(double t) { - return 60 * a * t * t + 24 * b * t + 6 * c; + public double getJerk(double s) { + return 60 * a * s * s + 24 * b * s + 6 * c; } /** - * @return snap of position with respect to parameter, i.e. d^4s/dt^4. + * @return snap of position with respect to parameter, i.e. d^4q/ds^4. */ - public double getSnap(double t) { - return 120 * a * t + 24 * b; + public double getSnap(double s) { + return 120 * a * s + 24 * b; } @Override diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java index 94989ef4d..2bb34f33a 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java @@ -7,6 +7,7 @@ /** Trivial constraint for testing. */ public class ConstantConstraint implements TimingConstraint { + private static final boolean DEBUG = true; private final Mutable m_maxVelocity; private final Mutable m_maxAccel; @@ -27,6 +28,8 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { @Override public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocityM_S) { + if (DEBUG) + System.out.printf("CONSTANT CONSTRAINT %f\n", m_maxAccel.getAsDouble()); return new MinMaxAcceleration(-m_maxAccel.getAsDouble(), m_maxAccel.getAsDouble()); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index b6d8102e6..6e18334ce 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -7,6 +7,7 @@ import org.team100.lib.util.Math100; class ConstrainedState { + private static final boolean DEBUG = true; // using MAX_VALUE tickles some bugs private static final double MAX_V = 100; private static final double MAX_A = 100; @@ -34,7 +35,11 @@ public void clampVelocity(List constraints) { for (TimingConstraint constraint : constraints) { NonNegativeDouble constraintVel = constraint.getMaxVelocity(m_state); double value = constraintVel.getValue(); - setVelocityM_S(Math.min(getVelocityM_S(), value)); + value = Math.min(getVelocityM_S(), value); + if (DEBUG) + System.out.printf("VELOCITY CONSTRAINT %s %5.3f\n", + constraint.getClass().getSimpleName(), value); + setVelocityM_S(value); } } @@ -49,6 +54,11 @@ public void clampAccel(List constraints) { m_minAccelM_S2 = Math.max(m_minAccelM_S2, minAccel); double maxAccel = Math100.notNaN(min_max_accel.getMaxAccel()); m_maxAccelM_S2 = Math.min(m_maxAccelM_S2, maxAccel); + if (DEBUG) + System.out.printf("ACCEL CONSTRAINT %s %5.3f %5.3f\n", + constraint.getClass().getSimpleName(), + m_minAccelM_S2, + m_maxAccelM_S2); } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java new file mode 100644 index 000000000..36990b794 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java @@ -0,0 +1,94 @@ +package org.team100.lib.trajectory.timing; + +import org.team100.lib.trajectory.path.spline.SplineR1; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +/** + * Experiment with schedule map, a stand-off function for the spline parameter + * as a function of time. + * + * Uses https://en.wikipedia.org/wiki/Finite_difference + * + * This matches the notation in README.md + */ +public class DirectSchedule { + private static final double EPSILON = 1e-6; + + private final SplineR1 m_spline; + private final InterpolatingDoubleTreeMap m_sdot; + // integrates sdot + private final InterpolatingDoubleTreeMap m_s; + private double m_maxT; + + public DirectSchedule(SplineR1 spline) { + m_spline = spline; + m_sdot = new InterpolatingDoubleTreeMap(); + m_s = new InterpolatingDoubleTreeMap(); + } + + public void put(double t, double sdot) { + m_sdot.put(t, sdot); + m_maxT = Math.max(m_maxT, t); + // keep s in sync + integrate(); + } + + private void integrate() { + double s = 0; + m_s.clear(); + double dt = 0.001; + m_s.put(0.0, 0.0); + for (double t = dt; t <= m_maxT; t += dt) { + double sdot = sdot(t); + s += sdot * dt; + m_s.put(t, s); + } + } + + /** position for time t */ + public double x(double t) { + return q(s(t)); + } + + public double v(double t) { + return qprime(s(t)) * sdot(t); + } + + public double a(double t) { + return qprimeprime(s(t)) * sdot(t) * sdot(t) + qprime(s(t)) * sdotdot(t); + } + + /** position for parameter s */ + public double q(double s) { + s = MathUtil.clamp(s, 0, 1); + return m_spline.getPosition(s); + } + + /** dq/ds, derivative of position wrt parameter s */ + public double qprime(double s) { + s = MathUtil.clamp(s, 0, 1); + return m_spline.getVelocity(s); + } + + /** d^2q/ds^2, second derivative of postion wrt parameter s */ + public double qprimeprime(double s) { + s = MathUtil.clamp(s, 0, 1); + return m_spline.getAcceleration(s); + } + + /** spline parameter, s, for time t */ + public double s(double t) { + return m_s.get(t); + } + + public double sdot(double t) { + return m_sdot.get(t); + } + + public double sdotdot(double t) { + return (sdot(t + EPSILON) - sdot(t)) / EPSILON; + } + +} diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 3b06a4aac..b8ea25be9 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -70,6 +70,8 @@ public Trajectory100 timeParameterizeTrajectory( List samples, double start_vel, double end_vel) throws TimingException { + if (samples.size() < 3) + throw new IllegalArgumentException("must have at least three samples"); List constrainedStates = forwardPass(samples, start_vel); Pose2dWithMotion lastState = samples.get(samples.size() - 1); backwardsPass(lastState, end_vel, constrainedStates); @@ -95,14 +97,23 @@ private List forwardPass(List samples, doubl // work forward through the samples List constrainedStates = new ArrayList<>(samples.size()); - for (Pose2dWithMotion sample : samples) { + for (int i = 1; i < samples.size(); ++i) { + Pose2dWithMotion sample = samples.get(i); double dsM = sample.distanceM(predecessor.getState()); + if (DEBUG) + System.out.printf("i%d dsM %f\n", i, dsM); ConstrainedState constrainedState = new ConstrainedState( sample, dsM + predecessor.getDistanceM()); constrainedStates.add(constrainedState); forwardWork(predecessor, constrainedState); predecessor = constrainedState; } + if (DEBUG) { + System.out.println("after forward pass:"); + for (int i = 0; i < constrainedStates.size(); ++i) { + System.out.printf("%d, %s\n", i, constrainedStates.get(i)); + } + } return constrainedStates; } @@ -117,6 +128,8 @@ private void forwardWork(ConstrainedState s0, ConstrainedState s1) { while (true) { // first try the previous state accel to get the new state velocity double v1 = v1(s0.getVelocityM_S(), s0.getMaxAccel(), dsM); + if (DEBUG) + System.out.printf("initial v1 %5.3f\n", v1); s1.setVelocityM_S(v1); // also use max accels for the new state accels @@ -126,8 +139,17 @@ private void forwardWork(ConstrainedState s0, ConstrainedState s1) { // reduce velocity according to constraints s1.clampVelocity(m_constraints); + if (DEBUG) + System.out.printf("accel before %5.3f\n", s1.getMaxAccel()); // reduce accel according to constraints + // in the failure case, this is clamped to zero, + // because the velocity is very high. + // and that's because the preceding accel is the near-infinite value + // which means we exceed the max in one time step + // so the key is to not do that, i think. s1.clampAccel(m_constraints); + if (DEBUG) + System.out.printf("accel after %5.3f\n", s1.getMaxAccel()); // motionless if (Math.abs(dsM) < EPSILON) { @@ -135,9 +157,15 @@ private void forwardWork(ConstrainedState s0, ConstrainedState s1) { } double accel = accel(s0.getVelocityM_S(), s1.getVelocityM_S(), dsM); + // in the failure case, max accel is zero so this always fails. if (accel > s1.getMaxAccel() + EPSILON) { // implied accel is too high because v1 is too high, perhaps because // a0 was too high, try again with the (lower) constrained value + // + // if the constrained value is zero, this doesn't work at all. + if (DEBUG) + System.out.printf("accel too high %5.3f, %5.3f, %5.3f\n", + accel, s0.getMaxAccel(), s1.getMaxAccel()); s0.setMaxAccel(s1.getMaxAccel()); continue; } @@ -169,6 +197,12 @@ private void backwardsPass( backwardsWork(constrainedState, successor); successor = constrainedState; } + if (DEBUG) { + System.out.println("after backwards pass:"); + for (int i = 0; i < constrainedStates.size(); ++i) { + System.out.printf("%d, %s\n", i, constrainedStates.get(i)); + } + } } /** s0 is earlier, s1 is "successor", we're walking backwards. */ @@ -226,10 +260,14 @@ private static Trajectory100 integrate(List states) throws Tim // this should be L2 distance double distance = 0.0; // distance along path double v0 = 0.0; + if (DEBUG) + System.out.println("i, v0, v1, ds"); for (int i = 0; i < states.size(); ++i) { ConstrainedState state = states.get(i); final double ds = state.getDistanceM() - distance; final double v1 = state.getVelocityM_S(); + if (DEBUG) + System.out.printf("%d, %5.3f, %5.3f, %5.3f\n", i, v0, v1, ds); double dt = 0.0; if (i > 0) { double prevAccel = accel(v0, v1, ds); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java index 814f227e7..14ebe3641 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -20,6 +20,7 @@ * omega limit calculation is correct. */ public class SwerveDriveDynamicsConstraint implements TimingConstraint { + private static final boolean DEBUG = true; private final SwerveKinodynamics m_limits; private final Mutable vScale; private final Mutable aScale; @@ -84,6 +85,11 @@ public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double v // min accel is stronger than max accel double minAccel = -1.0 * maxA(); double maxAccel = SwerveUtil.minAccel(m_limits, 1, 1, velocity); + // i think this only works because it's not *exactly* zero at full speed. + if (Math.abs(maxAccel) < 1e-3) + maxAccel = 0.0; + if (DEBUG) + System.out.printf("SWERVE CONSTRAINT %f\n", maxAccel); return new MinMaxAcceleration( minAccel, maxAccel); diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 6f254e939..274abb846 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -29,7 +29,7 @@ class Trajectory100Test implements Timeless { private static final double DELTA = 0.001; - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @Test diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index a97aa6f2a..7c482eb25 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -22,7 +22,7 @@ * These pop up GUI windows, so leave them commented out when you check in. */ public class TrajectoryTest { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final boolean SHOW = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 621101888..ebf62b4cc 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -32,7 +32,7 @@ import edu.wpi.first.math.geometry.Translation2d; class HolonomicSplineTest implements Timeless { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java new file mode 100644 index 000000000..c7cdf0dd6 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -0,0 +1,95 @@ +package org.team100.lib.trajectory.timing; + +import org.junit.jupiter.api.Test; +import org.team100.lib.trajectory.path.spline.SplineR1; + +public class DirectScheduleTest { + + @Test + void testSimple0() { + // constant velocity + // we want v=0 at the end, so the map here goes to zero at the end. + SplineR1 spline = SplineR1.get(0, 1, 1, 1, 0, 0); + DirectSchedule schedule = new DirectSchedule(spline); + // by hand, for amax = 2, vmax = 0.5 + schedule.put(-100.0, 1.0); + schedule.put(0.0, 0.0); + schedule.put(0.1, 0.2); + schedule.put(0.2, 0.4); + schedule.put(0.3, 0.5); + schedule.put(0.4, 0.5); + schedule.put(0.5, 0.5); + schedule.put(0.6, 0.5); + schedule.put(0.7, 0.5); + schedule.put(0.8, 0.5); + schedule.put(0.9, 0.5); + schedule.put(1.0, 0.5); + schedule.put(1.1, 0.5); + schedule.put(1.2, 0.5); + schedule.put(1.3, 0.5); + schedule.put(1.4, 0.5); + schedule.put(1.5, 0.5); + schedule.put(1.6, 0.5); + schedule.put(1.7, 0.5); + schedule.put(1.8, 0.5); + schedule.put(1.9, 0.5); + schedule.put(2.0, 0.5); + schedule.put(2.1, 0.4); + schedule.put(2.2, 0.2); + schedule.put(2.3, 0.0); + schedule.put(2.4, 0.0); + schedule.put(100.0, 0.0); + System.out.println("t, x, v, a"); + for (double t = 0; t <= 3.001; t += 0.1) { + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", + t, schedule.x(t), schedule.v(t), schedule.a(t)); + } + } + + @Test + void testSimple1() { + // dx = 0 at the ends + // we also want 0 at the ends, so the map is constant there + SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); + DirectSchedule schedule = new DirectSchedule(spline); + // by hand, for amax = 2, vmax = 0.5 + schedule.put(-100.0, 1.0); + schedule.put(0.0, 0.75); + schedule.put(0.1, 0.75); + schedule.put(0.2, 0.6); + schedule.put(0.3, 0.58); + schedule.put(0.4, 0.46); + schedule.put(0.5, 0.3); + schedule.put(0.6, 0.3); + schedule.put(0.7, 0.3); + schedule.put(0.8, 0.3); + schedule.put(0.9, 0.3); + schedule.put(1.0, 0.3); + schedule.put(1.1, 0.3); + schedule.put(1.2, 0.3); + schedule.put(1.3, 0.3); + schedule.put(1.4, 0.3); + schedule.put(1.5, 0.3); + schedule.put(1.6, 0.3); + schedule.put(1.7, 0.4); + schedule.put(1.8, 0.4); + schedule.put(1.9, 0.4); + schedule.put(2.0, 0.5); + schedule.put(2.1, 0.65); + schedule.put(2.2, 0.75); + schedule.put(2.3, 0.75); + schedule.put(2.4, 0.75); + schedule.put(2.5, 0.75); + schedule.put(2.6, 0.75); + schedule.put(2.7, 0.75); + schedule.put(2.8, 0.75); + schedule.put(100.0, 0.0); + System.out.println("t, x, v, a, qprime"); + for (double t = 0; t <= 3.001; t += 0.1) { + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + t, schedule.x(t), schedule.v(t), schedule.a(t), + schedule.qprime(schedule.s(t))); + } + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index 32070c46a..bc456ce59 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -33,7 +33,7 @@ import edu.wpi.first.math.geometry.Translation2d; public class ScheduleGeneratorTest { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; public static final double EPSILON = 1e-12; private static final double DELTA = 0.01; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 2780e9932..90e422dbe 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -6,8 +6,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -16,6 +16,7 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.path.Path100; +import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,13 +28,15 @@ * https://docs.google.com/spreadsheets/d/16UUCCz-qcPz_YZMnsJnVkVO1KGp5zHCOVo7EoJct2nA/edit?gid=0#gid=0 */ public class TrajectoryVelocityProfileTest implements Timeless { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); // A five-meter straight line. public static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion(WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0), + new Pose2dWithMotion(WaypointSE2.irrotational( + new Pose2d(2.5, 0, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0)); @@ -45,58 +48,61 @@ public class TrajectoryVelocityProfileTest implements Timeless { void print(Trajectory100 traj) { if (!DEBUG) return; + System.out.println("t, v"); for (double t = 0; t < traj.duration(); t += 0.02) { - System.out.printf("%12.6f %12.6f\n", t, traj.sample(t).velocityM_S()); + System.out.printf("%6.3f, %6.3f\n", t, traj.sample(t).velocityM_S()); } } - /** This uses the default max accel which is ridiculously high. */ + /** + * This uses the default max accel which is ridiculously high. + */ @Test - void testNoConstraint() { - Path100 path = new Path100(WAYPOINTS); + void testNoConstraint() throws TimingException { List constraints = new ArrayList(); ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory( - path, 0.1, 0, 0); + Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); print(traj); } - /** This produces a trapezoid. */ + /** + * This produces a trapezoid. + */ @Test - void testConstantConstraint() { - Path100 path = new Path100(WAYPOINTS); + void testConstantConstraint() throws TimingException { // somewhat realistic numbers SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTrajectoryTimingTest(logger); List constraints = List.of(new ConstantConstraint(logger, 1, 1, limits)); ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory( - path, 0.1, 0, 0); + Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); print(traj); } - /** This produces the desired current-limited exponential shape. */ + /** + * This produces the desired current-limited exponential shape. + */ @Test - void testSwerveConstraint() { - Path100 path = new Path100(WAYPOINTS); + void testSwerveConstraint() throws TimingException { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTrajectoryTimingTest(logger); List constraints = List.of(new SwerveDriveDynamicsConstraint(logger, limits, 1, 1)); ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory( - path, 0.1, 0, 0); + Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); print(traj); } - /** */ @Test - void testAuto() { - Path100 path = new Path100(WAYPOINTS); + void testAuto() throws TimingException { + // i think this is broken because one of the constraints + // is producing zero as the acceleration limit + // even though the velocity is also zero SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTrajectoryTimingTest(logger); TimingConstraintFactory timing = new TimingConstraintFactory(limits); List constraints = timing.testAuto(logger); ScheduleGenerator u = new ScheduleGenerator(constraints); Trajectory100 traj = u.timeParameterizeTrajectory( - path, 0.1, 0, 0); + new Path100(WAYPOINTS), 0.5, 0, 0); + // Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); print(traj); } } From 087ce4827b0f50ab2b153107fd85b44f1826011b Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 19 Dec 2025 10:06:01 -0800 Subject: [PATCH 17/42] multiple shooting experiment --- .../trajectory/timing/MultipleShooting.java | 85 +++++++++++++++++++ .../java/org/team100/lib/util/Math100.java | 4 +- .../timing/MultipleShootingTest.java | 34 ++++++++ 3 files changed, 122 insertions(+), 1 deletion(-) create mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java create mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java b/lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java new file mode 100644 index 000000000..eb4b528db --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java @@ -0,0 +1,85 @@ +package org.team100.lib.trajectory.timing; + +import java.util.List; + +import org.team100.lib.util.Math100; + +/** + * Experiment with a new implementation of the multiple shooting method. + * + * There are constraints on V and A, as well as continuity of X and V. + * + * @see https://en.wikipedia.org/wiki/Direct_multiple_shooting_method + * @see https://en.wikipedia.org/wiki/Trajectory_optimization + */ +public class MultipleShooting { + private final double m_maxV; + private final double m_maxA; + + public MultipleShooting(double maxV, double maxA) { + m_maxV = maxV; + m_maxA = maxA; + } + + public void solve(double[] x, double[] v, Double[] a) { + int n = x.length; + // estimate x_i from x_(i-1) + double[] x_est = new double[n]; + double[] dt_est = new double[n - 1]; + for (int i = 1; i < n; ++i) { + // forward integral + double dx = x[i] - x[i - 1]; + double v0 = v[i - 1]; + // choose max accel in direction of x1 if there's no a estimate + if (a[i - 1] == null) { + a[i - 1] = Math.signum(dx) * m_maxA; + } + double a0 = a[i - 1]; + // maybe instead of solving for t (because it can fail), just guess? + double A = 0.5 * a0; + double B = v0; + double C = -1.0 * dx; + List soln = Math100.solveQuadratic(A, B, C); + double dt = choose(soln); + dt_est[i - 1] = dt; + x_est[i] = x[i - 1] + v0 * dt + 0.5 * a0 * dt * dt; + } + // this contains v discontinuities + simulate(x, v, a, dt_est); + + } + + private void simulate(double[] x, double[] v, Double[] a, double[] dt_est) { + double DT = 0.01; + double t0 = 0; + System.out.println("t, a_t, v_t, x_t"); + for (int i = 0; i < x.length - 1; ++i) { + double x0 = x[i]; + double v0 = v[i]; + double a0 = a[i]; + for (double t = 0; t < dt_est[i]; t += DT) { + double v_t = v0 + a0 * t; + double x_t = x0 + v0 * t + 0.5 * a0 * t * t; + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", t0 + t, a0, v_t, x_t); + } + t0 += dt_est[i]; + } + } + + // choose smallest non-negative solution + private double choose(List soln) { + double x0 = Double.POSITIVE_INFINITY; + for (double x : soln) { + if (x >= 0 && x < x0) + x0 = x; + } + if (Double.isFinite(x0)) + return x0; + + // no solution + return 0; + + // throw new IllegalArgumentException(); + } + +} diff --git a/lib/src/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index fecc12e75..dad4eb7bb 100644 --- a/lib/src/main/java/org/team100/lib/util/Math100.java +++ b/lib/src/main/java/org/team100/lib/util/Math100.java @@ -10,6 +10,7 @@ * Various math utilities. */ public class Math100 { + private static final boolean DEBUG = false; private static final double EPSILON = 1e-6; /** @@ -17,7 +18,8 @@ public class Math100 { */ public static List solveQuadratic(double a, double b, double c) { double disc = b * b - 4 * a * c; - + if (DEBUG) + System.out.printf("a %f b %f c %f disc %f\n", a, b, c, disc); if (epsilonEquals(disc, 0.0)) { return List.of(-b / (2 * a)); } else if (disc > 0.0) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java new file mode 100644 index 000000000..1035fbd27 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java @@ -0,0 +1,34 @@ +package org.team100.lib.trajectory.timing; + +import org.junit.jupiter.api.Test; + +public class MultipleShootingTest { + @Test + void test0() { + // x is fixed + double[] x = new double[] { 1, 2, 2 }; + // v endpoints are fixed + double[] v = new double[] { 0, 0, 0 }; + // a is between, has no fixed values + Double[] a = new Double[] { null, null }; + double maxV = 1; + double maxA = 1; + MultipleShooting shooter = new MultipleShooting(maxV, maxA); + shooter.solve(x, v, a); + } + + @Test + void test1() { + // x is fixed + double[] x = new double[] { 1, 2, 3 }; + // v endpoints are fixed + double[] v = new double[] { 0, 0, 0 }; + // a is between, has no fixed values + Double[] a = new Double[] { null, null }; + double maxV = 1; + double maxA = 1; + MultipleShooting shooter = new MultipleShooting(maxV, maxA); + shooter.solve(x, v, a); + } + +} From 47126570231924d3490b60cdede4deb3083d7ab4 Mon Sep 17 00:00:00 2001 From: truher Date: Fri, 19 Dec 2025 23:12:58 -0800 Subject: [PATCH 18/42] fiddling with integrals --- .../lib/trajectory/timing/DirectSchedule.java | 4 +- .../trajectory/timing/DirectScheduleTest.java | 46 +++++++++++++++++++ 2 files changed, 49 insertions(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java index 36990b794..6ba489577 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java @@ -47,15 +47,17 @@ private void integrate() { } } - /** position for time t */ + /** q(t) position for time t */ public double x(double t) { return q(s(t)); } + /** qdot(t) */ public double v(double t) { return qprime(s(t)) * sdot(t); } + /** qdotdot(t) */ public double a(double t) { return qprimeprime(s(t)) * sdot(t) * sdot(t) + qprime(s(t)) * sdotdot(t); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index c7cdf0dd6..4771cda32 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -3,7 +3,10 @@ import org.junit.jupiter.api.Test; import org.team100.lib.trajectory.path.spline.SplineR1; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + public class DirectScheduleTest { + private static final double EPSILON = 1e-6; @Test void testSimple0() { @@ -92,4 +95,47 @@ void testSimple1() { } } + @Test + void testSimple2() { + SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); + InterpolatingDoubleTreeMap m = new InterpolatingDoubleTreeMap(); + for (double s = 0; s <= 1; s += 0.05) { + double q = spline.getPosition(s); + m.put(q, s); + } + System.out.println("q, s"); + for (double q = 0; q <= 1.001; q += 0.01) { + double s = m.get(q); + double qprime = spline.getVelocity(s); + double qprimeprime = spline.getAcceleration(s); + System.out.printf("%5.3f, %5.3f, %5.3f\n", q, s); + } + } + + @Test + void testSimple3() { + SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); + + double DS = 0.01; + double qq = 0; + System.out.println("s, q, qq, qError, qprime, qprimeprime"); + for (double s = 0; s <= 1.001; s += DS) { + double q = spline.getPosition(s); + double qprime = spline.getVelocity(s); + // https://en.wikipedia.org/wiki/Numerical_integration + if (s > 0) { + // trapezoid integration is good to about 50 ppm + // qq += (spline.getVelocity(s - DS) + spline.getVelocity(s)) * DS/2; + // simpsons rule is good to 2 ppb + qq += (spline.getVelocity(s - DS) + + 4 * spline.getVelocity(s - DS / 2) + + spline.getVelocity(s)) * DS / 6; + } + double qError = q - qq; + double qprimeprime = spline.getAcceleration(s); + System.out.printf("%10.8f, %10.8f, %10.8f, %12.10f, %10.8f, %10.8f\n", + s, q, qq, qError, qprime, qprimeprime); + } + } + } From 7e808eee07794ceb8381999edf408c07c11f285b Mon Sep 17 00:00:00 2001 From: truher Date: Sat, 20 Dec 2025 12:21:14 -0800 Subject: [PATCH 19/42] fiddling with direct method --- .../trajectory/timing/DirectScheduleTest.java | 152 ++++++++++++++++++ 1 file changed, 152 insertions(+) diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index 4771cda32..597c39fbc 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -138,4 +138,156 @@ void testSimple3() { } } + /** qdot constraint varies; for now it's a function of s */ + double qdotmax(double s) { + if (s > 0.4 && s < 0.6) + return 0.5; + return 1.0; + } + + double qdotdotmax(double s) { + return 5.0; + } + + @Test + void testSimple4() { + int n = 100; + SplineR1 q = SplineR1.get(0, 1, 0, 0, 0, 0); + + // these are the spline sample points; they can be + // in arbitrary locations. s is immutable. + double[] s = new double[n + 1]; + for (int i = 0; i < n + 1; ++i) { + s[i] = (double) i / n; + } + // start with initial estimate of sdot + double[] sdot = new double[n + 1]; + for (int i = 0; i < n + 1; ++i) { + sdot[i] = 1.0; + } + + // velocity constraint + for (int i = 0; i < n + 1; ++i) { + // first derivative of q wrt parameter s + double qprimei = q.getVelocity(s[i]); + // second derivative of q wrt parameter s + double qprimeprimei = q.getAcceleration(s[i]); + // first derivative of q wrt time, using chain rule + // qdot = dq/ds * ds/dt + double qdoti = qprimei * sdot[i]; + // also sdot(t) = 1/tprime(s) + // adjust sdot so that qdot is under the constraint + double qdotmaxi = qdotmax(s[i]); + if (Math.abs(qdoti) > qdotmaxi) { + sdot[i] = qdotmaxi / qprimei; + } + } + // accel constraint + for (int i = 1; i < n + 1; ++i) { + // previous + int j = i - 1; + // first derivative of q wrt parameter s + double qprimei = q.getVelocity(s[i]); + // second derivative of q wrt parameter s + double qprimeprimei = q.getAcceleration(s[i]); + + double ds = s[i] - s[j]; + + double sdotdoti = 0; + // trailing difference to get tprimeprime + double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; + // differentiate sdot to get sdotdot. + // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 + // sdotdot = -tprimeprime * sdot^3 + sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + + // chain rule + // d2q/dt2 = d2q/ds2 * (ds/dt)^2 + dq/ds * d2s/dt2 + // qdotdot = qprimeprime * sdot^2 + qprime * sdotdot + double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; + // adjust sdot so that qdotdot is under the constraint + double qdotdotmaxi = qdotdotmax(s[i]); + if (Math.abs(qdotdoti) > qdotdotmaxi) { + sdotdoti = (qdotdotmaxi - qprimeprimei * sdot[i] * sdot[i]) / qprimei; + // integrate to find sdot + // sdot = ds/dt + // dt = ds/sdot + double sdotnew = sdot[j] + sdotdoti * ds / sdot[i]; + if (Math.abs(sdotnew) < Math.abs(sdot[i])) + sdot[i] = sdotnew; + } + } + + // accel constraint backwards + for (int i = n-1; i > 0; --i) { + // previous + int j = i + 1; + // first derivative of q wrt parameter s + double qprimei = q.getVelocity(s[i]); + // second derivative of q wrt parameter s + double qprimeprimei = q.getAcceleration(s[i]); + + double ds = s[i] - s[j]; + + double sdotdoti = 0; + // trailing difference to get tprimeprime + double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; + // differentiate sdot to get sdotdot. + // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 + // sdotdot = -tprimeprime * sdot^3 + sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + + // chain rule + // d2q/dt2 = d2q/ds2 * (ds/dt)^2 + dq/ds * d2s/dt2 + // qdotdot = qprimeprime * sdot^2 + qprime * sdotdot + double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; + // adjust sdot so that qdotdot is under the constraint + double qdotdotmaxi = qdotdotmax(s[i]); + if (Math.abs(qdotdoti) > qdotdotmaxi) { + sdotdoti = (qdotdotmaxi - qprimeprimei * sdot[i] * sdot[i]) / qprimei; + // integrate to find sdot + // sdot = ds/dt + // dt = ds/sdot + double sdotnew = sdot[j] + sdotdoti * ds / sdot[i]; + if (Math.abs(sdotnew) < Math.abs(sdot[i])) + sdot[i] = sdotnew; + } + } + + // integrate and dump the result + System.out.println("t, s, sdot, sdotdot, q, qdot, qdotdot"); + double t = 0; + for (int i = 0; i < n + 1; ++i) { + + double qi = q.getPosition(s[i]); + double qprimei = q.getVelocity(s[i]); + double qprimeprimei = q.getAcceleration(s[i]); + double qdoti = qprimei * sdot[i]; + double sdotdoti = 0; + double qdotdoti = 0; + if (i > 0) { + // Integrate. + // We have sdot(t) but we want to integrate over s to find t + // The derivative of the inverse is the reciprocal + // https://en.wikipedia.org/wiki/Inverse_function_rule + // sdot(t) = 1/tprime(s) + double ds = s[i] - s[i - 1]; + // tprime = dt/ds + double tprimei = 1 / sdot[i]; + double dt1 = tprimei * ds; + + // trailing difference to get tprimeprime + double tprimeprimei = (1 / sdot[i] - 1 / sdot[i - 1]) / ds; + // differentiate sdot to get sdotdot. + // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 + sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; + + t += dt1; + } + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + t, s[i], sdot[i], sdotdoti, qi, qdoti, qdotdoti); + } + } + } From adccb31df1019b151811c73b9bb6e2f9356d3e9f Mon Sep 17 00:00:00 2001 From: truher Date: Sun, 21 Dec 2025 13:59:10 -0800 Subject: [PATCH 20/42] wip --- .../trajectory/timing/DirectScheduleTest.java | 50 ++++++++++--------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index 597c39fbc..9d107f328 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -182,6 +182,7 @@ void testSimple4() { sdot[i] = qdotmaxi / qprimei; } } + // accel constraint for (int i = 1; i < n + 1; ++i) { // previous @@ -193,13 +194,12 @@ void testSimple4() { double ds = s[i] - s[j]; - double sdotdoti = 0; // trailing difference to get tprimeprime double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; // differentiate sdot to get sdotdot. // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 // sdotdot = -tprimeprime * sdot^3 - sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + double sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; // chain rule // d2q/dt2 = d2q/ds2 * (ds/dt)^2 + dq/ds * d2s/dt2 @@ -207,35 +207,38 @@ void testSimple4() { double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; // adjust sdot so that qdotdot is under the constraint double qdotdotmaxi = qdotdotmax(s[i]); - if (Math.abs(qdotdoti) > qdotdotmaxi) { - sdotdoti = (qdotdotmaxi - qprimeprimei * sdot[i] * sdot[i]) / qprimei; - // integrate to find sdot - // sdot = ds/dt - // dt = ds/sdot - double sdotnew = sdot[j] + sdotdoti * ds / sdot[i]; - if (Math.abs(sdotnew) < Math.abs(sdot[i])) + if (Math.abs(qdotdoti) > Math.abs(qdotdotmaxi)) { + double sdotnew = Math.sqrt((2 * qdotdotmaxi * ds + qprimei * sdot[j] * sdot[j]) + / (2 * qprimeprimei * ds + qprimei)); + if (sdotnew < sdot[i]) { sdot[i] = sdotnew; + tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; + sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + } } } // accel constraint backwards - for (int i = n-1; i > 0; --i) { + for (int i = n - 1; i > 0; --i) { // previous int j = i + 1; // first derivative of q wrt parameter s - double qprimei = q.getVelocity(s[i]); + // note negative sign + double qprimei = - q.getVelocity(s[i]); // second derivative of q wrt parameter s - double qprimeprimei = q.getAcceleration(s[i]); + // note negative sign + double qprimeprimei = - q.getAcceleration(s[i]); + // this is a negative number double ds = s[i] - s[j]; - double sdotdoti = 0; // trailing difference to get tprimeprime + // this is a positive number double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; // differentiate sdot to get sdotdot. // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 // sdotdot = -tprimeprime * sdot^3 - sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + double sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; // chain rule // d2q/dt2 = d2q/ds2 * (ds/dt)^2 + dq/ds * d2s/dt2 @@ -243,14 +246,14 @@ void testSimple4() { double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; // adjust sdot so that qdotdot is under the constraint double qdotdotmaxi = qdotdotmax(s[i]); - if (Math.abs(qdotdoti) > qdotdotmaxi) { - sdotdoti = (qdotdotmaxi - qprimeprimei * sdot[i] * sdot[i]) / qprimei; - // integrate to find sdot - // sdot = ds/dt - // dt = ds/sdot - double sdotnew = sdot[j] + sdotdoti * ds / sdot[i]; - if (Math.abs(sdotnew) < Math.abs(sdot[i])) + if (Math.abs(qdotdoti) > Math.abs(qdotdotmaxi)) { + double sdotnew = Math.sqrt((2 * qdotdotmaxi * ds + qprimei * sdot[j] * sdot[j]) + / (2 * qprimeprimei * ds + qprimei)); + if (sdotnew < sdot[i]) { sdot[i] = sdotnew; + tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; + sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; + } } } @@ -266,18 +269,19 @@ void testSimple4() { double sdotdoti = 0; double qdotdoti = 0; if (i > 0) { + int j = i - 1; // Integrate. // We have sdot(t) but we want to integrate over s to find t // The derivative of the inverse is the reciprocal // https://en.wikipedia.org/wiki/Inverse_function_rule // sdot(t) = 1/tprime(s) - double ds = s[i] - s[i - 1]; + double ds = s[i] - s[j]; // tprime = dt/ds double tprimei = 1 / sdot[i]; double dt1 = tprimei * ds; // trailing difference to get tprimeprime - double tprimeprimei = (1 / sdot[i] - 1 / sdot[i - 1]) / ds; + double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; // differentiate sdot to get sdotdot. // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; From 9c88336bd8ecbb646fd42a127348f78878cbd260 Mon Sep 17 00:00:00 2001 From: truher Date: Mon, 22 Dec 2025 11:17:32 -0800 Subject: [PATCH 21/42] dt solve works --- .../trajectory/timing/DirectScheduleTest.java | 145 +++++++++++++++++- 1 file changed, 141 insertions(+), 4 deletions(-) diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index 9d107f328..ffff6c85a 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -1,7 +1,11 @@ package org.team100.lib.trajectory.timing; +import java.util.List; + import org.junit.jupiter.api.Test; +import org.team100.lib.optimization.GoldenSectionSearch; import org.team100.lib.trajectory.path.spline.SplineR1; +import org.team100.lib.util.Math100; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; @@ -138,14 +142,25 @@ void testSimple3() { } } - /** qdot constraint varies; for now it's a function of s */ + /** + * qdot constraint varies; for now it's a function of s + * always positive. + */ double qdotmax(double s) { - if (s > 0.4 && s < 0.6) + if (s > 0.45 && s < 0.55) return 0.5; return 1.0; } + /** + * a positive number + * TODO: both bounds (which are not the same) + */ double qdotdotmax(double s) { + if (s < 0.1) // soft start + return s * 50; + if (s > 0.9) // soft stop + return (1 - s) * 50; return 5.0; } @@ -224,10 +239,10 @@ void testSimple4() { int j = i + 1; // first derivative of q wrt parameter s // note negative sign - double qprimei = - q.getVelocity(s[i]); + double qprimei = -q.getVelocity(s[i]); // second derivative of q wrt parameter s // note negative sign - double qprimeprimei = - q.getAcceleration(s[i]); + double qprimeprimei = -q.getAcceleration(s[i]); // this is a negative number double ds = s[i] - s[j]; @@ -294,4 +309,126 @@ void testSimple4() { } } + /** + * schedules dt directly, doesn't try to us spline velocity etc. + * + * makes no attempt to limit jerk; jerk limiting would require an iterative + * solver, which would be nice to avoid, and it's not that important + * in practice. + */ + @Test + void testSimple5() { + int n = 100; + SplineR1 q = SplineR1.get(0, 1, 0, 0, 0, 0); + // these are the spline sample points; they can be + // in arbitrary locations. s is immutable. + double[] s = new double[n + 1]; + for (int i = 0; i < n + 1; ++i) { + s[i] = (double) i / n; + } + + // initial dt + // new dt is never less than this so choose a small number + double[] dt = new double[n + 1]; + for (int i = 0; i < n; ++i) { + dt[i] = 0.10 / n; + } + + // velocity constraint + for (int i = 1; i < n + 1; ++i) { + // dx is never negative + double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + // v is never negative + double v = dx / dt[i - 1]; + double vmax = qdotmax(s[i]); + double newdt = dx / vmax; + // only slower + dt[i - 1] = Math.max(dt[i - 1], newdt); + } + + // accel constraint forward, using backward finite differences + for (int i = 2; i < n + 1; ++i) { + double v0 = (q.getPosition(s[i - 1]) - q.getPosition(s[i - 2])) / dt[i - 2]; + double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + double v = dx / dt[i - 1]; + double a = (v - v0) / dt[i - 1]; + double amax = qdotdotmax(s[i]); + if (Math.abs(a) > amax) { + amax = Math.signum(a) * amax; + double A = amax; + double B = v0; + double C = -dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + dt[i - 1] = Math.max(dt[i - 1], newdt); + } + } + + // accel constraint backward, using forward finite differences + for (int i = n - 2; i >= 0; --i) { + double v0 = (q.getPosition(s[i + 2]) - q.getPosition(s[i + 1])) / dt[i + 1]; + double dx = q.getPosition(s[i + 1]) - q.getPosition(s[i]); + double v = dx / dt[i]; + double a = (v - v0) / dt[i]; + double amax = qdotdotmax(s[i]); + if (Math.abs(a) > amax) { + amax = Math.signum(a) * amax; + double A = amax; + double B = v0; + double C = -dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + dt[i] = Math.max(dt[i], newdt); + } + } + + // integrate and dump the result + System.out.println("s, dt, t, x, v, a"); + double t = 0; + for (int i = 0; i < n + 1; ++i) { + double x = q.getPosition(s[i]); + double v = 0; + double a = 0; + if (i > 0) { + t += dt[i - 1]; + } + // compute v using backward finite difference + if (i > 0) { + double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + v = dx / dt[i - 1]; + } + // compute a using backward finite difference + if (i > 1) { + double dx0 = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + double v0 = dx0 / dt[i - 1]; + double dx1 = q.getPosition(s[i - 1]) - q.getPosition(s[i - 2]); + double v1 = dx1 / dt[i - 2]; + a = (v0 - v1) / dt[i - 1]; + } + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + s[i], dt[i], t, x, v, a); + } + } + + /** + * choose smallest non-negative solution + * dt is never negative ... and i think also should never be zero ... + */ + private double choose(List soln) { + double x0 = Double.POSITIVE_INFINITY; + for (double x : soln) { + if (x >= 0 && x < x0) + x0 = x; + } + if (Double.isFinite(x0)) + return x0; + + // System.out.println("no solution"); + return 0; + + // throw new IllegalArgumentException(); + } + } From c6743cd3137312dca9c0ccb3c9341ec061c642ce Mon Sep 17 00:00:00 2001 From: truher Date: Mon, 22 Dec 2025 11:41:57 -0800 Subject: [PATCH 22/42] asymetric accel/decel constraint works --- .../trajectory/timing/DirectScheduleTest.java | 82 ++++++++++++++----- 1 file changed, 60 insertions(+), 22 deletions(-) diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index ffff6c85a..65c6d1b01 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -154,7 +154,6 @@ void testSimple3() { /** * a positive number - * TODO: both bounds (which are not the same) */ double qdotdotmax(double s) { if (s < 0.1) // soft start @@ -164,6 +163,17 @@ void testSimple3() { return 5.0; } + /** + * Since v is never negative, qdotdotmin always means deceleration. + * + * @returns a negative number + */ + double qdotdotmin(double s) { + if (s > 0.9) // soft stop + return (1 - s) * -50; + return -10.0; + } + @Test void testSimple4() { int n = 100; @@ -352,16 +362,30 @@ void testSimple5() { double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); double v = dx / dt[i - 1]; double a = (v - v0) / dt[i - 1]; - double amax = qdotdotmax(s[i]); - if (Math.abs(a) > amax) { - amax = Math.signum(a) * amax; - double A = amax; - double B = v0; - double C = -dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); - // only slower - dt[i - 1] = Math.max(dt[i - 1], newdt); + if (a > 0) { + double amax = qdotdotmax(s[i]); + if (a > amax) { + // System.out.printf("forward pos s %f a %f\n", s[i], a); + double A = amax; + double B = v0; + double C = -dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + dt[i - 1] = Math.max(dt[i - 1], newdt); + } + } else if (a < 0) { + double amin = qdotdotmin(s[i]); + if (a < amin) { + // System.out.printf("forward neg s %f a %f\n", s[i], a); + double A = amin; + double B = v0; + double C = -dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + dt[i - 1] = Math.max(dt[i - 1], newdt); + } } } @@ -370,17 +394,31 @@ void testSimple5() { double v0 = (q.getPosition(s[i + 2]) - q.getPosition(s[i + 1])) / dt[i + 1]; double dx = q.getPosition(s[i + 1]) - q.getPosition(s[i]); double v = dx / dt[i]; - double a = (v - v0) / dt[i]; - double amax = qdotdotmax(s[i]); - if (Math.abs(a) > amax) { - amax = Math.signum(a) * amax; - double A = amax; - double B = v0; - double C = -dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); - // only slower - dt[i] = Math.max(dt[i], newdt); + double a = (v0 - v) / dt[i]; + if (a > 0) { + double amax = qdotdotmax(s[i]); + if (a > amax) { + // System.out.printf("backward pos s %f v %f v0 %f a %f\n", s[i], v, v0, a); + double A = amax; + double B = -v0; + double C = dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + dt[i] = Math.max(dt[i], newdt); + } + } else if (a < 0) { + double amin = qdotdotmin(s[i]); + if (a < amin) { + // System.out.printf("backward neg s %f v %f v0 %f a %f\n", s[i], v, v0, a); + double A = amin; + double B = -v0; + double C = dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + dt[i] = Math.max(dt[i], newdt); + } } } From 5d8730c6a619f5bd9922ff3f12cbc8bf2aff4014 Mon Sep 17 00:00:00 2001 From: truher Date: Mon, 22 Dec 2025 13:56:42 -0800 Subject: [PATCH 23/42] velocity dependent accel constraint --- .../trajectory/timing/DiamondConstraint.java | 1 + .../timing/SwerveDriveDynamicsConstraint.java | 4 +- .../trajectory/timing/DirectScheduleTest.java | 151 +++++++++++------- 3 files changed, 93 insertions(+), 63 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java index 5177c8d84..cb3a68f3a 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java @@ -47,6 +47,7 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { @Override public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocityM_S) { + // TODO: this should also have a diamond shape return new MinMaxAcceleration(-m_maxAccel.getAsDouble(), m_maxAccel.getAsDouble()); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java index 14ebe3641..1511e286b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -46,8 +46,8 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { // First check instantaneous velocity and compute a limit based on drive // velocity. Rotation2d course = state.getPose().course().toRotation(); - - Rotation2d course_local = course.minus(state.getPose().pose().getRotation()); + Rotation2d heading = state.getPose().pose().getRotation(); + Rotation2d course_local = course.minus(heading); double vx = course_local.getCos(); double vy = course_local.getSin(); // rad/m diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index 65c6d1b01..8a1a6affe 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -143,7 +143,11 @@ void testSimple3() { } /** - * qdot constraint varies; for now it's a function of s + * qdot constraint varies; for now it's a function of s. + * + * In reality, depends on curvature and course and heading and heading rate, + * none of which vary during scheduling. + * * always positive. */ double qdotmax(double s) { @@ -154,24 +158,28 @@ void testSimple3() { /** * a positive number + * + * In reality, depends on pose and curvature and heading rate, and also pathwise + * velocity. of these only pathwise velocity changes during the scheduling. */ - double qdotdotmax(double s) { + double qdotdotmax(double s, double v) { if (s < 0.1) // soft start - return s * 50; - if (s > 0.9) // soft stop - return (1 - s) * 50; - return 5.0; + return (s * 50) - v; + return 5.0 - v; } /** * Since v is never negative, qdotdotmin always means deceleration. * + * In reality, depends on pose and curvature and heading rate, and also pathwise + * velocity. of these only pathwise velocity changes during the scheduling. + * * @returns a negative number */ - double qdotdotmin(double s) { + double qdotdotmin(double s, double v) { if (s > 0.9) // soft stop - return (1 - s) * -50; - return -10.0; + return (1 - s) * -50 + v; + return -10.0 + v; } @Test @@ -231,7 +239,7 @@ void testSimple4() { // qdotdot = qprimeprime * sdot^2 + qprime * sdotdot double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; // adjust sdot so that qdotdot is under the constraint - double qdotdotmaxi = qdotdotmax(s[i]); + double qdotdotmaxi = qdotdotmax(s[i], 0); if (Math.abs(qdotdoti) > Math.abs(qdotdotmaxi)) { double sdotnew = Math.sqrt((2 * qdotdotmaxi * ds + qprimei * sdot[j] * sdot[j]) / (2 * qprimeprimei * ds + qprimei)); @@ -270,7 +278,7 @@ void testSimple4() { // qdotdot = qprimeprime * sdot^2 + qprime * sdotdot double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; // adjust sdot so that qdotdot is under the constraint - double qdotdotmaxi = qdotdotmax(s[i]); + double qdotdotmaxi = qdotdotmax(s[i], 0); if (Math.abs(qdotdoti) > Math.abs(qdotdotmaxi)) { double sdotnew = Math.sqrt((2 * qdotdotmaxi * ds + qprimei * sdot[j] * sdot[j]) / (2 * qprimeprimei * ds + qprimei)); @@ -360,32 +368,44 @@ void testSimple5() { for (int i = 2; i < n + 1; ++i) { double v0 = (q.getPosition(s[i - 1]) - q.getPosition(s[i - 2])) / dt[i - 2]; double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); - double v = dx / dt[i - 1]; - double a = (v - v0) / dt[i - 1]; - if (a > 0) { - double amax = qdotdotmax(s[i]); - if (a > amax) { - // System.out.printf("forward pos s %f a %f\n", s[i], a); - double A = amax; - double B = v0; - double C = -dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); - // only slower - dt[i - 1] = Math.max(dt[i - 1], newdt); - } - } else if (a < 0) { - double amin = qdotdotmin(s[i]); - if (a < amin) { - // System.out.printf("forward neg s %f a %f\n", s[i], a); - double A = amin; - double B = v0; - double C = -dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); - // only slower - dt[i - 1] = Math.max(dt[i - 1], newdt); + while (true) { + double v = dx / dt[i - 1]; + // System.out.printf("i %d v %f\n", i, v); + double a = (v - v0) / dt[i - 1]; + if (a > 0) { + double amax = qdotdotmax(s[i], v); + if (a > amax) { + // System.out.printf("forward pos s %f a %f\n", s[i], a); + double A = amax; + double B = v0; + double C = -dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + if (newdt > dt[i - 1]) { + dt[i - 1] = newdt; + // System.out.printf("newdt %f\n", newdt); + continue; + } + } + } else if (a < 0) { + double amin = qdotdotmin(s[i], v); + if (a < amin) { + // System.out.printf("forward neg s %f a %f\n", s[i], a); + double A = amin; + double B = v0; + double C = -dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + if (newdt > dt[i - 1]) { + dt[i - 1] = newdt; + // System.out.printf("newdt %f\n", newdt); + continue; + } + } } + break; } } @@ -393,32 +413,41 @@ void testSimple5() { for (int i = n - 2; i >= 0; --i) { double v0 = (q.getPosition(s[i + 2]) - q.getPosition(s[i + 1])) / dt[i + 1]; double dx = q.getPosition(s[i + 1]) - q.getPosition(s[i]); - double v = dx / dt[i]; - double a = (v0 - v) / dt[i]; - if (a > 0) { - double amax = qdotdotmax(s[i]); - if (a > amax) { - // System.out.printf("backward pos s %f v %f v0 %f a %f\n", s[i], v, v0, a); - double A = amax; - double B = -v0; - double C = dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); - // only slower - dt[i] = Math.max(dt[i], newdt); - } - } else if (a < 0) { - double amin = qdotdotmin(s[i]); - if (a < amin) { - // System.out.printf("backward neg s %f v %f v0 %f a %f\n", s[i], v, v0, a); - double A = amin; - double B = -v0; - double C = dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); - // only slower - dt[i] = Math.max(dt[i], newdt); + while (true) { + double v = dx / dt[i]; + double a = (v0 - v) / dt[i]; + if (a > 0) { + double amax = qdotdotmax(s[i], v); + if (a > amax) { + // System.out.printf("backward pos s %f v %f v0 %f a %f\n", s[i], v, v0, a); + double A = amax; + double B = -v0; + double C = dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + if (newdt > dt[i]) { + dt[i] = newdt; + continue; + } + } + } else if (a < 0) { + double amin = qdotdotmin(s[i], v); + if (a < amin) { + // System.out.printf("backward neg s %f v %f v0 %f a %f\n", s[i], v, v0, a); + double A = amin; + double B = -v0; + double C = dx; + List soln = Math100.solveQuadratic(A, B, C); + double newdt = choose(soln); + // only slower + if (newdt > dt[i]) { + dt[i] = newdt; + continue; + } + } } + break; } } From 05f9311f32ac7b9c7f3c02556a1db815e24b1363 Mon Sep 17 00:00:00 2001 From: truher Date: Mon, 22 Dec 2025 19:58:35 -0800 Subject: [PATCH 24/42] more trajectory WIP --- .../lib/trajectory/timing/Scheduler100.java | 166 ++++++++++++++++++ .../trajectory/timing/DirectScheduleTest.java | 103 +++++------ .../lib/trajectory/timing/SchedulerTest.java | 86 +++++++++ 3 files changed, 297 insertions(+), 58 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java create mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java b/lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java new file mode 100644 index 000000000..e819a9d1c --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java @@ -0,0 +1,166 @@ +package org.team100.lib.trajectory.timing; + +import java.util.List; + +import org.team100.lib.util.Math100; + +/** + * Assign timestamps waypoints. + * + * This is the same approach as the old ScheduleGenerator: assign durations to + * the arcs between waypoints, using constraints on pathwise velocity and + * accel/decel, walk over the waypoints once forward, and + * once backward, then once more to add up the durations. + */ +public class Scheduler100 { + + public static interface Constraint { + /** + * Can depend on curvature and course and heading and heading rate, + * none of which vary during scheduling. + * + * @return a positive number. + */ + double vmax(double q); + + /** + * Since v is alwasy positive, amin means decel. + * + * Can depend on pose and curvature and heading rate, and also pathwise + * velocity. of these only pathwise velocity changes during the scheduling. + * + * @returns a negative number + */ + double amin(double q, double v); + + /** + * Since v is always positive, amax means accel. + * + * Can depend on pose and curvature and heading rate, and also pathwise + * velocity. of these only pathwise velocity changes during the scheduling. + * + * @return a positive number + */ + double amax(double q, double v); + } + + private final Constraint m_c; + + public Scheduler100(Constraint c) { + m_c = c; + } + + public double[] schedule(double[] q) { + + int n = q.length - 1; + + // initial dt + // new dt is never less than this so choose a small number + double[] dt = new double[n + 1]; + for (int i = 0; i < n; ++i) { + dt[i] = 0.10 / n; + } + + // velocity constraint + for (int i = 1; i < n + 1; ++i) { + // dx is never negative + double dx = q[i] - q[i - 1]; + // v is never negative + double v = dx / dt[i - 1]; + double vmax = m_c.vmax(q[i]); + double newdt = dx / vmax; + // only slower + dt[i - 1] = Math.max(dt[i - 1], newdt); + } + + // accel constraint forward, using backward finite differences + for (int i = 2; i < n + 1; ++i) { + double v0 = (q[i - 1] - q[i - 2]) / dt[i - 2]; + double dx = q[i] - q[i - 1]; + while (true) { + double v = dx / dt[i - 1]; + double a = (v - v0) / dt[i - 1]; + if (a > 0) { + double amax = m_c.amax(q[i], v); + if (a > amax) { + double newdt = solve(amax, v0, -dx); + // only slower + if (newdt > dt[i - 1]) { + dt[i - 1] = newdt; + continue; + } + } + } else if (a < 0) { + double amin = m_c.amin(q[i], v); + if (a < amin) { + double newdt = solve(amin, v0, -dx); + // only slower + if (newdt > dt[i - 1]) { + dt[i - 1] = newdt; + continue; + } + } + } + break; + } + } + + // accel constraint backward, using forward finite differences + for (int i = n - 2; i >= 0; --i) { + double v0 = (q[i + 2] - q[i + 1]) / dt[i + 1]; + double dx = q[i + 1] - q[i]; + while (true) { + double v = dx / dt[i]; + double a = (v0 - v) / dt[i]; + if (a > 0) { + double amax = m_c.amax(q[i], v); + if (a > amax) { + double newdt = solve(amax, -v0, dx); + // only slower + if (newdt > dt[i]) { + dt[i] = newdt; + continue; + } + } + } else if (a < 0) { + double amin = m_c.amin(q[i], v); + if (a < amin) { + double newdt = solve(amin, -v0, dx); + // only slower + if (newdt > dt[i]) { + dt[i] = newdt; + continue; + } + } + } + break; + } + } + return dt; + } + + private static double solve(double A, double B, double C) { + List soln = Math100.solveQuadratic(A, B, C); + return choose(soln); + } + + /** + * choose smallest non-negative solution + * dt is never negative ... and i think also should never be zero ... + */ + private static double choose(List soln) { + double x0 = Double.POSITIVE_INFINITY; + for (double x : soln) { + if (x >= 0 && x < x0) + x0 = x; + } + if (Double.isFinite(x0)) + return x0; + + // System.out.println("no solution"); + return 0; + + // throw new IllegalArgumentException(); + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index 8a1a6affe..2984e90fa 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -3,7 +3,6 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.optimization.GoldenSectionSearch; import org.team100.lib.trajectory.path.spline.SplineR1; import org.team100.lib.util.Math100; @@ -150,9 +149,9 @@ void testSimple3() { * * always positive. */ - double qdotmax(double s) { - if (s > 0.45 && s < 0.55) - return 0.5; + double qdotmax(double q) { + // if (q > 0.45 && q < 0.55) + // return 0.5; return 1.0; } @@ -162,9 +161,9 @@ void testSimple3() { * In reality, depends on pose and curvature and heading rate, and also pathwise * velocity. of these only pathwise velocity changes during the scheduling. */ - double qdotdotmax(double s, double v) { - if (s < 0.1) // soft start - return (s * 50) - v; + double qdotdotmax(double q, double v) { + if (q < 0.02) // soft start + return q*10; return 5.0 - v; } @@ -177,8 +176,8 @@ void testSimple3() { * @returns a negative number */ double qdotdotmin(double s, double v) { - if (s > 0.9) // soft stop - return (1 - s) * -50 + v; + // if (s > 1.4) // soft stop + // return (1 - s) * -50 + v; return -10.0 + v; } @@ -336,15 +335,21 @@ void testSimple4() { */ @Test void testSimple5() { - int n = 100; - SplineR1 q = SplineR1.get(0, 1, 0, 0, 0, 0); + SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); // these are the spline sample points; they can be // in arbitrary locations. s is immutable. - double[] s = new double[n + 1]; - for (int i = 0; i < n + 1; ++i) { - s[i] = (double) i / n; + double[] s = new double[101]; + for (int i = 0; i < 101; ++i) { + s[i] = (double) i / 100; } + double[] q = new double[s.length]; + for (int i = 0; i < s.length; ++i) { + q[i] = spline.getPosition(s[i]); + } + + int n = q.length - 1; + // initial dt // new dt is never less than this so choose a small number double[] dt = new double[n + 1]; @@ -355,10 +360,10 @@ void testSimple5() { // velocity constraint for (int i = 1; i < n + 1; ++i) { // dx is never negative - double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + double dx = q[i] - q[i - 1]; // v is never negative double v = dx / dt[i - 1]; - double vmax = qdotmax(s[i]); + double vmax = qdotmax(q[i]); double newdt = dx / vmax; // only slower dt[i - 1] = Math.max(dt[i - 1], newdt); @@ -366,41 +371,28 @@ void testSimple5() { // accel constraint forward, using backward finite differences for (int i = 2; i < n + 1; ++i) { - double v0 = (q.getPosition(s[i - 1]) - q.getPosition(s[i - 2])) / dt[i - 2]; - double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + double v0 = (q[i - 1] - q[i - 2]) / dt[i - 2]; + double dx = q[i] - q[i - 1]; while (true) { double v = dx / dt[i - 1]; - // System.out.printf("i %d v %f\n", i, v); double a = (v - v0) / dt[i - 1]; if (a > 0) { - double amax = qdotdotmax(s[i], v); + double amax = qdotdotmax(q[i], v); if (a > amax) { - // System.out.printf("forward pos s %f a %f\n", s[i], a); - double A = amax; - double B = v0; - double C = -dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); + double newdt = solve(amax, v0, -dx); // only slower if (newdt > dt[i - 1]) { dt[i - 1] = newdt; - // System.out.printf("newdt %f\n", newdt); continue; } } } else if (a < 0) { - double amin = qdotdotmin(s[i], v); + double amin = qdotdotmin(q[i], v); if (a < amin) { - // System.out.printf("forward neg s %f a %f\n", s[i], a); - double A = amin; - double B = v0; - double C = -dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); + double newdt = solve(amin, v0, -dx); // only slower if (newdt > dt[i - 1]) { dt[i - 1] = newdt; - // System.out.printf("newdt %f\n", newdt); continue; } } @@ -411,20 +403,15 @@ void testSimple5() { // accel constraint backward, using forward finite differences for (int i = n - 2; i >= 0; --i) { - double v0 = (q.getPosition(s[i + 2]) - q.getPosition(s[i + 1])) / dt[i + 1]; - double dx = q.getPosition(s[i + 1]) - q.getPosition(s[i]); + double v0 = (q[i + 2] - q[i + 1]) / dt[i + 1]; + double dx = q[i + 1] - q[i]; while (true) { double v = dx / dt[i]; double a = (v0 - v) / dt[i]; if (a > 0) { - double amax = qdotdotmax(s[i], v); + double amax = qdotdotmax(q[i], v); if (a > amax) { - // System.out.printf("backward pos s %f v %f v0 %f a %f\n", s[i], v, v0, a); - double A = amax; - double B = -v0; - double C = dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); + double newdt = solve(amax, -v0, dx); // only slower if (newdt > dt[i]) { dt[i] = newdt; @@ -432,14 +419,9 @@ void testSimple5() { } } } else if (a < 0) { - double amin = qdotdotmin(s[i], v); + double amin = qdotdotmin(q[i], v); if (a < amin) { - // System.out.printf("backward neg s %f v %f v0 %f a %f\n", s[i], v, v0, a); - double A = amin; - double B = -v0; - double C = dx; - List soln = Math100.solveQuadratic(A, B, C); - double newdt = choose(soln); + double newdt = solve(amin, -v0, dx); // only slower if (newdt > dt[i]) { dt[i] = newdt; @@ -452,10 +434,10 @@ void testSimple5() { } // integrate and dump the result - System.out.println("s, dt, t, x, v, a"); + System.out.println("t, x, v, a"); double t = 0; for (int i = 0; i < n + 1; ++i) { - double x = q.getPosition(s[i]); + double x = q[i]; double v = 0; double a = 0; if (i > 0) { @@ -463,22 +445,27 @@ void testSimple5() { } // compute v using backward finite difference if (i > 0) { - double dx = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + double dx = q[i] - q[i - 1]; v = dx / dt[i - 1]; } // compute a using backward finite difference if (i > 1) { - double dx0 = q.getPosition(s[i]) - q.getPosition(s[i - 1]); + double dx0 = q[i] - q[i - 1]; double v0 = dx0 / dt[i - 1]; - double dx1 = q.getPosition(s[i - 1]) - q.getPosition(s[i - 2]); + double dx1 = q[i - 1] - q[i - 2]; double v1 = dx1 / dt[i - 2]; a = (v0 - v1) / dt[i - 1]; } - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", - s[i], dt[i], t, x, v, a); + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", + t, x, v, a); } } + private double solve(double A, double B, double C) { + List soln = Math100.solveQuadratic(A, B, C); + return choose(soln); + } + /** * choose smallest non-negative solution * dt is never negative ... and i think also should never be zero ... diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java new file mode 100644 index 000000000..7b2242438 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java @@ -0,0 +1,86 @@ +package org.team100.lib.trajectory.timing; + +import org.junit.jupiter.api.Test; +import org.team100.lib.trajectory.path.spline.SplineR1; + +public class SchedulerTest { + + Scheduler100.Constraint constraint = new Scheduler100.Constraint() { + @Override + public double vmax(double q) { + if (q > 0.45 && q < 0.55) + return 0.5; + return 1.0; + } + + @Override + public double amax(double q, double v) { + if (q < 0.01) // soft start + return (q * 500); + return 5.0 - v; + } + + @Override + public double amin(double q, double v) { + if (q > 0.99) // soft stop + return (1 - q) * -500; + return -10.0 + v; + } + }; + + @Test + void test0() { + int n = 100; + SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); + // these are the spline sample points; they can be + // in arbitrary locations. s is immutable. + double[] s = new double[n + 1]; + for (int i = 0; i < n + 1; ++i) { + s[i] = (double) i / n; + } + + // positions + double[] q = new double[s.length]; + for (int i = 0; i < s.length; ++i) { + q[i] = spline.getPosition(s[i]); + } + + Scheduler100 scheduler = new Scheduler100(constraint); + + double[] dt = scheduler.schedule(q); + + dump(q, dt); + } + + public static void dump(double[] q, double[] dt) { + int n = q.length - 1; + + // integrate and dump the result + System.out.println("t, x, v, a"); + double t = 0; + for (int i = 0; i < n + 1; ++i) { + double x = q[i]; + double v = 0; + double a = 0; + if (i > 0) { + t += dt[i - 1]; + } + // compute v using backward finite difference + if (i > 0) { + double dx = q[i] - q[i - 1]; + v = dx / dt[i - 1]; + } + // compute a using backward finite difference + if (i > 1) { + double dx0 = q[i] - q[i - 1]; + double v0 = dx0 / dt[i - 1]; + double dx1 = q[i - 1] - q[i - 2]; + double v1 = dx1 / dt[i - 2]; + a = (v0 - v1) / dt[i - 1]; + } + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", + t, x, v, a); + } + } + +} From d30fb89d4d096a37fef9cc8796bbeea0245ba06c Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 16:31:19 -0800 Subject: [PATCH 25/42] fix the build --- .../kinodynamics/limiter/SwerveUtil.java | 2 +- .../team100/lib/trajectory/path/Path100.java | 1 + .../trajectory/timing/ConstantConstraint.java | 2 +- .../trajectory/timing/ConstrainedState.java | 2 +- .../trajectory/timing/ScheduleGenerator.java | 48 ++++++++++++++++--- .../timing/SwerveDriveDynamicsConstraint.java | 2 +- .../lib/trajectory/Trajectory100Test.java | 8 ++-- .../lib/trajectory/TrajectoryPlannerTest.java | 33 +++++++++++++ .../trajectory/timing/DirectScheduleTest.java | 17 ------- .../timing/TrajectoryVelocityProfileTest.java | 2 +- 10 files changed, 86 insertions(+), 31 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java index b12e033d1..65208de7d 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtil.java @@ -7,7 +7,7 @@ import org.team100.lib.util.Math100; public class SwerveUtil { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; /** * At low speed, accel is limited by the current limiters. diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 08adbbced..053ba6c00 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -31,6 +31,7 @@ public Path100(final List states) { Pose2dWithMotion p1 = getPoint(i); // use the distance metric that includes rotation double dist = GeometryUtil.doubleGeodesicDistance(p0, p1); + // double dist = p0.distanceM(p1); m_distances[i] = m_distances[i - 1] + dist; } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java index 2bb34f33a..0f5ce75c4 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java @@ -7,7 +7,7 @@ /** Trivial constraint for testing. */ public class ConstantConstraint implements TimingConstraint { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private final Mutable m_maxVelocity; private final Mutable m_maxAccel; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index 6e18334ce..80d28b71d 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -7,7 +7,7 @@ import org.team100.lib.util.Math100; class ConstrainedState { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; // using MAX_VALUE tickles some bugs private static final double MAX_V = 100; private static final double MAX_A = 100; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index b8ea25be9..ba5a2cbbf 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -12,7 +12,7 @@ * schedule. */ public class ScheduleGenerator { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final double EPSILON = 1e-6; /** this is the default, in order to make the constraints set the actual */ private static final double HIGH_ACCEL = 1000; @@ -49,14 +49,18 @@ private List resample(Path100 path, double step) throws Timing double maxDistance = path.getMaxDistance(); if (maxDistance == 0) throw new IllegalArgumentException(); - int num_states = (int) Math.ceil(maxDistance / step + 1); + int num_states = (int) Math.ceil(maxDistance / step) + 1; + if (DEBUG) + System.out.printf("resample max distance %f step %f num states %d f %f\n", + maxDistance, step, num_states, maxDistance / step); List samples = new ArrayList<>(num_states); for (int i = 0; i < num_states; ++i) { // the dtheta and curvature come from here and are never changed. // the values here are just interpolated from the original values. - Pose2dWithMotion state = path.sample(Math.min(i * step, maxDistance)); + double d = Math.min(i * step, maxDistance); + Pose2dWithMotion state = path.sample(d); if (DEBUG) - System.out.printf("RESAMPLE: %d %s\n", i, state); + System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state); samples.add(state); } return samples; @@ -72,9 +76,28 @@ public Trajectory100 timeParameterizeTrajectory( double end_vel) throws TimingException { if (samples.size() < 3) throw new IllegalArgumentException("must have at least three samples"); + if (DEBUG) { + System.out.println("samples"); + for (Pose2dWithMotion p2 : samples) { + System.out.println(p2); + } + } List constrainedStates = forwardPass(samples, start_vel); + if (DEBUG) { + System.out.println("after forward"); + for (ConstrainedState cs : constrainedStates) { + System.out.println(cs); + } + } Pose2dWithMotion lastState = samples.get(samples.size() - 1); backwardsPass(lastState, end_vel, constrainedStates); + if (DEBUG) { + System.out.println("after backward"); + for (ConstrainedState cs : constrainedStates) { + System.out.println(cs); + } + } + return integrate(constrainedStates); } @@ -90,14 +113,19 @@ public Trajectory100 timeParameterizeTrajectory( * acceleration during the backward pass (by slowing down the predecessor). */ private List forwardPass(List samples, double start_vel) { + + // note below we look again at this sample. I think this exists + // only to supply the start velocity. ConstrainedState predecessor = new ConstrainedState(samples.get(0), 0); predecessor.setVelocityM_S(start_vel); predecessor.setMinAccel(-HIGH_ACCEL); predecessor.setMaxAccel(HIGH_ACCEL); + // work forward through the samples List constrainedStates = new ArrayList<>(samples.size()); - for (int i = 1; i < samples.size(); ++i) { + // constrainedStates.add(predecessor); + for (int i = 0; i < samples.size(); ++i) { Pose2dWithMotion sample = samples.get(i); double dsM = sample.distanceM(predecessor.getState()); if (DEBUG) @@ -300,7 +328,9 @@ private static double dt( if (Math.abs(v0) > EPSILON) { return ds / v0; } - throw new TimingException(); + // not moving at all so dt is always zero + return 0; + // throw new TimingException(String.format("%f %f %f %f", v0, v1, ds, accel)); } /** @@ -369,5 +399,11 @@ public static double accel(double v0, double v1, double ds) { } public static class TimingException extends Exception { + public TimingException() { + } + + public TimingException(String s) { + super(s); + } } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java index 1511e286b..ed2119feb 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -20,7 +20,7 @@ * omega limit calculation is correct. */ public class SwerveDriveDynamicsConstraint implements TimingConstraint { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private final SwerveKinodynamics m_limits; private final Mutable vScale; private final Mutable aScale; diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 274abb846..613b78d3a 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -83,7 +83,7 @@ void testSample() { Trajectory100 trajectory = planner.restToRest(waypoints); - assertEquals(1.417, trajectory.duration(), DELTA); + assertEquals(1.418, trajectory.duration(), DELTA); TimedPose sample = trajectory.sample(0); assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(1); @@ -115,8 +115,10 @@ void testSampleThoroughly() { TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 trajectory = planner.restToRest(waypoints); + if (DEBUG) + System.out.println(trajectory); - assertEquals(1.417, trajectory.duration(), DELTA); + assertEquals(1.418, trajectory.duration(), DELTA); assertEquals(0.000, trajectory.sample(0.0).state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0.010, trajectory.sample(0.1).state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0.040, trajectory.sample(0.2).state().getPose().pose().getTranslation().getX(), DELTA); @@ -138,7 +140,7 @@ void testSampleThoroughly() { /** * Does the index help? No. * - * Does interpolation help, relative to just sampling the spline directly? No. + * Does interpolation help, relative to just sampling the spline directly? No. * * There's no need to run this all the time */ diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index de5328c21..f78fef41d 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -51,6 +51,39 @@ void testLinear() { TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); assertEquals(12, t.length()); + TimedPose tp = t.getPoint(0); + // start at zero velocity + assertEquals(0, tp.velocityM_S(), DELTA); + TimedPose p = t.getPoint(6); + assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); + } + + @Test + void testLinearRealistic() { + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1.2), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1.2)); + // these are the same as StraightLineTrajectoryTest. + SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); + List constraints = List.of( + new ConstantConstraint(logger, 1, 1, limits), + new SwerveDriveDynamicsConstraint(logger, limits, 1, 1), + new YawRateConstraint(logger, limits, 0.2), + new CapsizeAccelerationConstraint(logger, limits, 0.2)); + TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + Trajectory100 t = planner.restToRest(waypoints); + assertEquals(12, t.length()); + TimedPose tp = t.getPoint(0); + assertEquals(0, tp.velocityM_S(), DELTA); TimedPose p = t.getPoint(6); assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index 2984e90fa..b8b3f52d4 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -98,23 +98,6 @@ void testSimple1() { } } - @Test - void testSimple2() { - SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); - InterpolatingDoubleTreeMap m = new InterpolatingDoubleTreeMap(); - for (double s = 0; s <= 1; s += 0.05) { - double q = spline.getPosition(s); - m.put(q, s); - } - System.out.println("q, s"); - for (double q = 0; q <= 1.001; q += 0.01) { - double s = m.get(q); - double qprime = spline.getVelocity(s); - double qprimeprime = spline.getAcceleration(s); - System.out.printf("%5.3f, %5.3f, %5.3f\n", q, s); - } - } - @Test void testSimple3() { SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 90e422dbe..7d6e6df0d 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -28,7 +28,7 @@ * https://docs.google.com/spreadsheets/d/16UUCCz-qcPz_YZMnsJnVkVO1KGp5zHCOVo7EoJct2nA/edit?gid=0#gid=0 */ public class TrajectoryVelocityProfileTest implements Timeless { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); // A five-meter straight line. From 819d88f8678db8a46568055f4aade203b3cb7830 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 20:22:05 -0800 Subject: [PATCH 26/42] simplify trajectory constraints --- .../timing/CapsizeAccelerationConstraint.java | 41 +++++++---- .../trajectory/timing/ConstantConstraint.java | 17 ++--- .../trajectory/timing/ConstrainedState.java | 13 +--- .../trajectory/timing/DiamondConstraint.java | 14 ++-- .../timing/SwerveDriveDynamicsConstraint.java | 18 ++--- .../trajectory/timing/TimingConstraint.java | 68 ++++--------------- .../trajectory/timing/TorqueConstraint.java | 17 +++-- .../timing/VelocityLimitRegionConstraint.java | 16 +++-- .../trajectory/timing/YawRateConstraint.java | 24 +++++-- ...CentripetalAccelerationConstraintTest.java | 30 ++++---- .../timing/ConstantConstraintTest.java | 6 +- .../timing/DiamondConstraintTest.java | 12 ++-- .../timing/ScheduleGeneratorTest.java | 45 +++++++----- .../SwerveDriveDynamicsConstraintTest.java | 28 ++++---- .../timing/TorqueConstraintTest.java | 24 +++---- .../VelocityLimitRegionConstraintTest.java | 12 ++-- .../timing/YawRateConstraintTest.java | 18 +++-- 17 files changed, 199 insertions(+), 204 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java index 9ab7e4f07..dd3b62ccd 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java @@ -38,11 +38,31 @@ public CapsizeAccelerationConstraint( * If the curvature is zero, this will return infinity. */ @Override - public NonNegativeDouble getMaxVelocity(final Pose2dWithMotion state) { + public double maxV(final Pose2dWithMotion state) { double mMaxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); double radius = 1 / state.getCurvature(); // abs is used here to make sure sqrt is happy. - return new NonNegativeDouble(Math.sqrt(Math.abs(mMaxCentripetalAccel * radius))); + return Math.sqrt(Math.abs(mMaxCentripetalAccel * radius)); + } + + @Override + public double maxAccel(Pose2dWithMotion state, double velocity) { + double alongsq = alongSq(state, velocity); + if (alongsq < 0) { + // too fast for the curvature, can't speed up + return 0; + } + return Math.sqrt(alongsq); + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + double alongsq = alongSq(state, velocity); + if (alongsq < 0) { + // too fast for the curvature, slowing down is ok + return -m_limits.getMaxDriveDecelerationM_S2() * m_scale.getAsDouble(); + } + return -Math.sqrt(alongsq); } /** @@ -56,19 +76,10 @@ public NonNegativeDouble getMaxVelocity(final Pose2dWithMotion state) { * so * along = sqrt(total^2 - v^4/r^2) */ - @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocity) { - double mMaxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); - + private double alongSq(Pose2dWithMotion state, double velocity) { + double maxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); double radius = 1 / state.getCurvature(); - double centripetalAccel = velocity * velocity / radius; - double alongsq = mMaxCentripetalAccel * mMaxCentripetalAccel - centripetalAccel * centripetalAccel; - if (alongsq < 0) { - // if you're here, you're violating the velocity constraint above, - // and you should try to gently slow down. - return new MinMaxAcceleration(-m_limits.getMaxDriveDecelerationM_S2() * m_scale.getAsDouble(), 0); - } - double along = Math.sqrt(alongsq); - return new MinMaxAcceleration(-along, along); + double actualCentripetalAccel = velocity * velocity / radius; + return maxCentripetalAccel * maxCentripetalAccel - actualCentripetalAccel * actualCentripetalAccel; } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java index 0f5ce75c4..80007975b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java @@ -7,7 +7,6 @@ /** Trivial constraint for testing. */ public class ConstantConstraint implements TimingConstraint { - private static final boolean DEBUG = false; private final Mutable m_maxVelocity; private final Mutable m_maxAccel; @@ -22,15 +21,17 @@ public ConstantConstraint(LoggerFactory log, double vScale, double aScale, Swerv } @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - return new NonNegativeDouble(m_maxVelocity.getAsDouble()); + public double maxV(Pose2dWithMotion state) { + return m_maxVelocity.getAsDouble(); } @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocityM_S) { - if (DEBUG) - System.out.printf("CONSTANT CONSTRAINT %f\n", m_maxAccel.getAsDouble()); - return new MinMaxAcceleration(-m_maxAccel.getAsDouble(), m_maxAccel.getAsDouble()); + public double maxAccel(Pose2dWithMotion state, double velocityM_S) { + return m_maxAccel.getAsDouble(); + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return -m_maxAccel.getAsDouble(); } - } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index 80d28b71d..6daee0a86 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -3,8 +3,6 @@ import java.util.List; import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.trajectory.timing.TimingConstraint.NonNegativeDouble; -import org.team100.lib.util.Math100; class ConstrainedState { private static final boolean DEBUG = false; @@ -33,8 +31,7 @@ public ConstrainedState(Pose2dWithMotion state, double distance) { */ public void clampVelocity(List constraints) { for (TimingConstraint constraint : constraints) { - NonNegativeDouble constraintVel = constraint.getMaxVelocity(m_state); - double value = constraintVel.getValue(); + double value = constraint.maxV(m_state); value = Math.min(getVelocityM_S(), value); if (DEBUG) System.out.printf("VELOCITY CONSTRAINT %s %5.3f\n", @@ -48,12 +45,8 @@ public void clampVelocity(List constraints) { */ public void clampAccel(List constraints) { for (TimingConstraint constraint : constraints) { - TimingConstraint.MinMaxAcceleration min_max_accel = constraint - .getMinMaxAcceleration(m_state, getVelocityM_S()); - double minAccel = Math100.notNaN(min_max_accel.getMinAccel()); - m_minAccelM_S2 = Math.max(m_minAccelM_S2, minAccel); - double maxAccel = Math100.notNaN(min_max_accel.getMaxAccel()); - m_maxAccelM_S2 = Math.min(m_maxAccelM_S2, maxAccel); + m_minAccelM_S2 = Math.max(m_minAccelM_S2, constraint.maxDecel(m_state, getVelocityM_S())); + m_maxAccelM_S2 = Math.min(m_maxAccelM_S2, constraint.maxAccel(m_state, getVelocityM_S())); if (DEBUG) System.out.printf("ACCEL CONSTRAINT %s %5.3f %5.3f\n", constraint.getClass().getSimpleName(), diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java index cb3a68f3a..8166670b7 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java @@ -33,7 +33,7 @@ public DiamondConstraint(LoggerFactory parent, double maxVX, double maxVY, doubl } @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { + public double maxV(Pose2dWithMotion state) { Rotation2d course = state.getPose().course().toRotation(); Rotation2d heading = state.getPose().pose().getRotation(); Rotation2d strafe = course.minus(heading); @@ -41,14 +41,18 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { // https://en.wikipedia.org/wiki/Superellipse double a = m_maxVelocityX.getAsDouble(); double b = m_maxVelocityY.getAsDouble(); - double r = 1 / (Math.abs(strafe.getCos() / a) + Math.abs(strafe.getSin() / b)); - return new NonNegativeDouble(r); + return 1 / (Math.abs(strafe.getCos() / a) + Math.abs(strafe.getSin() / b)); } @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocityM_S) { + public double maxAccel(Pose2dWithMotion state, double velocityM_S) { // TODO: this should also have a diamond shape - return new MinMaxAcceleration(-m_maxAccel.getAsDouble(), m_maxAccel.getAsDouble()); + return m_maxAccel.getAsDouble(); + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return -m_maxAccel.getAsDouble(); } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java index ed2119feb..627455361 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -42,7 +42,7 @@ public SwerveDriveDynamicsConstraint( * speed allowed (m/s) that maintains the target spatial heading rate. */ @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { + public double maxV(Pose2dWithMotion state) { // First check instantaneous velocity and compute a limit based on drive // velocity. Rotation2d course = state.getPose().course().toRotation(); @@ -64,7 +64,7 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { for (SwerveModuleState100 module : module_states.all()) { max_vel = Math.min(max_vel, maxV() / Math.abs(module.speedMetersPerSecond())); } - return new NonNegativeDouble(max_vel); + return max_vel; } double maxV() { @@ -79,20 +79,22 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { * @see SwerveUtil.getAccelLimit() */ @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocity) { + public double maxAccel(Pose2dWithMotion state, double velocity) { if (Double.isNaN(velocity)) throw new IllegalArgumentException(); - // min accel is stronger than max accel - double minAccel = -1.0 * maxA(); double maxAccel = SwerveUtil.minAccel(m_limits, 1, 1, velocity); // i think this only works because it's not *exactly* zero at full speed. if (Math.abs(maxAccel) < 1e-3) maxAccel = 0.0; if (DEBUG) System.out.printf("SWERVE CONSTRAINT %f\n", maxAccel); - return new MinMaxAcceleration( - minAccel, - maxAccel); + return maxAccel; + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + // min accel is stronger than max accel + return -1.0 * maxA(); } private double maxA() { diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java index 4450dac6e..2d7a18d49 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java @@ -10,65 +10,23 @@ */ public interface TimingConstraint { /** - * Maximum allowed velocity m/s. This is always non-negative. + * Maximum allowed pathwise velocity, m/s. + * + * Always positive. */ - NonNegativeDouble getMaxVelocity(Pose2dWithMotion state); - - class NonNegativeDouble { - private final double m_value; - - public NonNegativeDouble(double value) { - if (value < 0) - throw new IllegalArgumentException(); - m_value = value; - } - - public double getValue() { - return m_value; - } - } + double maxV(Pose2dWithMotion state); /** - * Minimum and maximum allowed acceleration m/s^2. + * Maximum allowed pathwise acceleration, m/s^2. * - * The acceleration here is purely *along* the path, it doesn't have anything to - * do with cross-track accelerations due to curvature. + * Always positive. */ - MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocityM_S); - - class MinMaxAcceleration { - public static final MinMaxAcceleration NO_LIMITS = new MinMaxAcceleration(); - - private final double m_minAccel; - private final double m_maxAccel; + double maxAccel(Pose2dWithMotion state, double velocityM_S); - private MinMaxAcceleration() { - this(Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); - } - - /** - * @param minAccel a negative number - * @param maxAccel a positive number - */ - public MinMaxAcceleration(double minAccel, double maxAccel) { - if (minAccel > 0) { - throw new IllegalArgumentException(); - } - if (maxAccel < 0) { - throw new IllegalArgumentException(); - } - m_minAccel = minAccel; - m_maxAccel = maxAccel; - } - - /** Always negative (or zero). */ - public double getMinAccel() { - return m_minAccel; - } - - /** Always positive (or zero). */ - public double getMaxAccel() { - return m_maxAccel; - } - } + /** + * Maximum allowed pathwise deceleration, m/s^2. + * + * Always negative. + */ + double maxDecel(Pose2dWithMotion state, double velocityM_S); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java index 655c67250..2bd4aa255 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java @@ -46,14 +46,23 @@ public TorqueConstraint(double maxTorque) { } @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { + public double maxV(Pose2dWithMotion state) { // Do not constrain velocity. - return new NonNegativeDouble(Double.POSITIVE_INFINITY); + return Double.POSITIVE_INFINITY; } @Override - public MinMaxAcceleration getMinMaxAcceleration( + public double maxAccel( Pose2dWithMotion state, double velocityM_S) { + return getA(state); + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return -getA(state); + } + + private double getA(Pose2dWithMotion state) { WaypointSE2 pose = state.getPose(); Rotation2d course = pose.course().toRotation(); // acceleration unit vector @@ -65,6 +74,6 @@ public MinMaxAcceleration getMinMaxAcceleration( System.out.printf("Torque Constraint a: %6.3f p: %s r: %6.3f course: %6.3f\n", a, pose, r.getNorm(), course.getRadians()); } - return new MinMaxAcceleration(-a, a); + return a; } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java index 04d72bee5..485d443ca 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java @@ -24,19 +24,23 @@ public VelocityLimitRegionConstraint( } @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { + public double maxV(Pose2dWithMotion state) { final Translation2d translation = state.getPose().pose().getTranslation(); if (translation.getX() <= m_max.getX() && translation.getX() >= m_min.getX() && translation.getY() <= m_max.getY() && translation.getY() >= m_min.getY()) { - return new NonNegativeDouble(m_limit); + return m_limit; } - return new NonNegativeDouble(Double.POSITIVE_INFINITY); + return Double.POSITIVE_INFINITY; } @Override - public TimingConstraint.MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, - double velocity) { - return MinMaxAcceleration.NO_LIMITS; + public double maxAccel(Pose2dWithMotion state, double velocity) { + return Double.POSITIVE_INFINITY; + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return Double.NEGATIVE_INFINITY; } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java index b5bc97250..358b00efa 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java @@ -37,19 +37,29 @@ public YawRateConstraint(LoggerFactory log, SwerveKinodynamics limits, double sc } @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { + public double maxV(Pose2dWithMotion state) { // Heading rate in rad/m - final double heading_rate = state.getHeadingRateRad_M(); + double heading_rate = state.getHeadingRateRad_M(); // rad/s / rad/m => m/s. - return new NonNegativeDouble(m_maxOmegaRad_S.getAsDouble() / Math.abs(heading_rate)); + return m_maxOmegaRad_S.getAsDouble() / Math.abs(heading_rate); } @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, double velocity) { + public double maxAccel(Pose2dWithMotion state, double velocity) { + // TODO: this is wrong // Heading rate in rad/m - final double heading_rate = state.getHeadingRateRad_M(); + double heading_rate = state.getHeadingRateRad_M(); // rad/s^2 / rad/m => m/s^2 - double limitM_S = m_maxAlphaRad_S2.getAsDouble() / Math.abs(heading_rate); - return new MinMaxAcceleration(-limitM_S, limitM_S); + return m_maxAlphaRad_S2.getAsDouble() / Math.abs(heading_rate); } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + // TODO: this is wrong + // Heading rate in rad/m + double heading_rate = state.getHeadingRateRad_M(); + // rad/s^2 / rad/m => m/s^2 + return -(m_maxAlphaRad_S2.getAsDouble() / Math.abs(heading_rate)); + } + } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java index 478ed0b06..27566bed3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java @@ -33,9 +33,9 @@ void testSimple() { new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1); // motionless, so 100% of the capsize accel is available - assertEquals(-8.166, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); - assertEquals(8.166, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(2.857, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(-8.166, c.maxDecel(p, 0), DELTA); + assertEquals(8.166, c.maxAccel(p, 0), DELTA); + assertEquals(2.857, c.maxV(p), DELTA); } @Test @@ -52,9 +52,9 @@ void testSimpleMoving() { new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1); // moving, only some of the capsize accel is available - assertEquals(-5.257, c.getMinMaxAcceleration(p, 2.5).getMinAccel(), DELTA); - assertEquals(5.257, c.getMinMaxAcceleration(p, 2.5).getMaxAccel(), DELTA); - assertEquals(2.857, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(-5.257, c.maxDecel(p, 2.5), DELTA); + assertEquals(5.257, c.maxAccel(p, 2.5), DELTA); + assertEquals(2.857, c.maxV(p), DELTA); } @Test @@ -71,9 +71,9 @@ void testSimpleOverspeed() { new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1); // above the velocity limit - assertEquals(-1, c.getMinMaxAcceleration(p, 3).getMinAccel(), DELTA); - assertEquals(0, c.getMinMaxAcceleration(p, 3).getMaxAccel(), DELTA); - assertEquals(2.857, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(-1, c.maxDecel(p, 3), DELTA); + assertEquals(0, c.maxAccel(p, 3), DELTA); + assertEquals(2.857, c.maxV(p), DELTA); } @Test @@ -88,9 +88,9 @@ void testSimple2() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 1); - assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); - assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(2.021, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(-4.083, c.maxDecel(p, 0), DELTA); + assertEquals(4.083, c.maxAccel(p, 0), DELTA); + assertEquals(2.021, c.maxV(p), DELTA); } @Test @@ -105,9 +105,9 @@ void testStraightLine() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0); - assertEquals(-4.083, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); - assertEquals(4.083, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(-4.083, c.maxDecel(p, 0), DELTA); + assertEquals(4.083, c.maxAccel(p, 0), DELTA); + assertEquals(Double.POSITIVE_INFINITY, c.maxV(p), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java index 4a2d0bedd..089586dce 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java @@ -24,7 +24,7 @@ void testVelocity() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0); - assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(2, c.maxV(state), DELTA); } @Test @@ -34,8 +34,8 @@ void testAccel() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0); - assertEquals(-3, c.getMinMaxAcceleration(state, 1).getMinAccel(), DELTA); - assertEquals(3, c.getMinMaxAcceleration(state, 1).getMaxAccel(), DELTA); + assertEquals(-3, c.maxDecel(state, 1), DELTA); + assertEquals(3, c.maxAccel(state, 1), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java index 2900c344f..64cfd2d35 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java @@ -26,19 +26,19 @@ void testSquare() { new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0); // moving purely in x, get the x number - assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(1, c.maxV(state), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0); // moving purely in y, get the y number - assertEquals(1, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(1, c.maxV(state), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2), 0, 0); // moving diagonally, get less. - assertEquals(0.707, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(0.707, c.maxV(state), DELTA); } @Test @@ -49,19 +49,19 @@ void testVelocity() { new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0); // moving purely in x, get the x number - assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(2, c.maxV(state), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0); // moving purely in y, get the y number - assertEquals(3, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(3, c.maxV(state), DELTA); state = new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2), 0, 0); // moving diagonally, get less. - assertEquals(1.697, c.getMaxVelocity(state).getValue(), DELTA); + assertEquals(1.697, c.maxV(state), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index bc456ce59..e3bef2b4c 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -26,7 +26,6 @@ import org.team100.lib.trajectory.TrajectoryPlotter; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; -import org.team100.lib.trajectory.timing.TimingConstraint.MinMaxAcceleration; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -88,13 +87,14 @@ public void checkTrajectory( TimedPose prev_state = null; for (TimedPose state : traj.getPoints()) { for (final TimingConstraint constraint : constraints) { - assertTrue(state.velocityM_S() - EPSILON <= constraint.getMaxVelocity(state.state()).getValue()); - final MinMaxAcceleration accel_limits = constraint.getMinMaxAcceleration(state.state(), - state.velocityM_S()); - assertTrue(state.acceleration() - EPSILON <= accel_limits.getMaxAccel(), - String.format("%f %f", state.acceleration(), accel_limits.getMaxAccel())); - assertTrue(state.acceleration() + EPSILON >= accel_limits.getMinAccel(), - String.format("%f %f", state.acceleration(), accel_limits.getMinAccel())); + assertTrue(state.velocityM_S() - EPSILON <= constraint.maxV(state.state())); + assertTrue(state.acceleration() - EPSILON <= constraint.maxAccel( + state.state(), state.velocityM_S()), + String.format("%f %f", state.acceleration(), constraint.maxAccel( + state.state(), state.velocityM_S()))); + assertTrue(state.acceleration() + EPSILON >= constraint.maxDecel(state.state(), state.velocityM_S()), + String.format("%f %f", state.acceleration(), + constraint.maxDecel(state.state(), state.velocityM_S()))); } if (prev_state != null) { assertEquals(state.velocityM_S(), @@ -209,18 +209,22 @@ void testConditionalVelocityConstraint() { class ConditionalTimingConstraint implements TimingConstraint { @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { + public double maxV(Pose2dWithMotion state) { if (state.getPose().pose().getTranslation().getX() >= 24.0) { - return new NonNegativeDouble(5.0); + return 5.0; } else { - return new NonNegativeDouble(Double.POSITIVE_INFINITY); + return Double.POSITIVE_INFINITY; } } @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, - double velocity) { - return new TimingConstraint.MinMaxAcceleration(Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + public double maxAccel(Pose2dWithMotion state, double velocity) { + return Double.POSITIVE_INFINITY; + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return Double.NEGATIVE_INFINITY; } } @@ -236,14 +240,19 @@ void testConditionalAccelerationConstraint() { class ConditionalTimingConstraint implements TimingConstraint { @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - return new NonNegativeDouble(Double.POSITIVE_INFINITY); + public double maxV(Pose2dWithMotion state) { + return Double.POSITIVE_INFINITY; } @Override - public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, + public double maxAccel(Pose2dWithMotion state, double velocity) { - return new TimingConstraint.MinMaxAcceleration(-10.0, 10.0 / velocity); + return 10.0 / velocity; + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return -10.0; } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java index 076222a85..38afbc7cf 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java @@ -10,7 +10,6 @@ import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; -import org.team100.lib.trajectory.timing.TimingConstraint.MinMaxAcceleration; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -25,25 +24,25 @@ void testVelocity() { SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1); // motionless - double m = c.getMaxVelocity(new Pose2dWithMotion( + double m = c.maxV(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0)).getValue(); + 0, 0)); assertEquals(5, m, DELTA); // moving in +x, no curvature, no rotation - m = c.getMaxVelocity(new Pose2dWithMotion( + m = c.maxV(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0)).getValue(); + 0, 0)); // max allowed velocity is full speed assertEquals(5, m, DELTA); // moving in +x, 5 rad/meter - m = c.getMaxVelocity(new Pose2dWithMotion( + m = c.maxV(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 5, 0)).getValue(); + 5, 0)); // at 5 rad/m with 0.5m sides the fastest you can go is 1.55 m/s. assertEquals(1.925, m, DELTA); @@ -56,9 +55,7 @@ void testVelocity() { WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 11.313708, 0); - m = c.getMaxVelocity( - state) - .getValue(); + m = c.maxV(state); // verify corner velocity is full scale assertEquals(5, c.maxV()); // this should be feasible; note it's not exactly 1 due to discretization @@ -71,11 +68,10 @@ void testAccel() { SwerveKinodynamics kinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1); // this is constant - MinMaxAcceleration m = c.getMinMaxAcceleration(new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0), 0); - assertEquals(-20, m.getMinAccel(), DELTA); - assertEquals(10, m.getMaxAccel(), DELTA); + Pose2d p = new Pose2d(0, 0, new Rotation2d(0)); + Pose2dWithMotion p2 = new Pose2dWithMotion( + WaypointSE2.irrotational(p, 0, 1.2), 0, 0); + assertEquals(-20, c.maxDecel(p2, 0), DELTA); + assertEquals(10, c.maxAccel(p2, 0), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java index 37b31b6c6..09c7e1064 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java @@ -21,8 +21,8 @@ void testRadial() { new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), 0, 0); // no tangential motion => no limit - assertEquals(Double.NEGATIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(Double.POSITIVE_INFINITY, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); + assertEquals(Double.NEGATIVE_INFINITY, jc.maxDecel(state, 0), DELTA); + assertEquals(Double.POSITIVE_INFINITY, jc.maxAccel(state, 0), DELTA); } @Test @@ -34,8 +34,8 @@ void testTangential() { new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0); // tangential motion at 1 m - assertEquals(-1, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(1, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); + assertEquals(-1, jc.maxDecel(state, 0), DELTA); + assertEquals(1, jc.maxAccel(state, 0), DELTA); } @Test @@ -47,8 +47,8 @@ void testInclined() { new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 4, 1.2), 0, 0); // motion at 45 deg => higher limit - assertEquals(-1.414, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(1.414, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); + assertEquals(-1.414, jc.maxDecel(state, 0), DELTA); + assertEquals(1.414, jc.maxAccel(state, 0), DELTA); } @Test @@ -60,8 +60,8 @@ void testFar() { new Pose2d(2, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0); // more r => lower limit - assertEquals(-0.5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(0.5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); + assertEquals(-0.5, jc.maxDecel(state, 0), DELTA); + assertEquals(0.5, jc.maxAccel(state, 0), DELTA); } @Test @@ -73,8 +73,8 @@ void testFar2() { new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0); // more r => lower limit - assertEquals(-0.333, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(0.333, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); + assertEquals(-0.333, jc.maxDecel(state, 0), DELTA); + assertEquals(0.333, jc.maxAccel(state, 0), DELTA); } @Test @@ -86,7 +86,7 @@ void testRealistic() { new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2), 0, 0); // should match the constant constraint at around 1 m - assertEquals(-5, jc.getMinMaxAcceleration(state, 0).getMinAccel(), DELTA); - assertEquals(5, jc.getMinMaxAcceleration(state, 0).getMaxAccel(), DELTA); + assertEquals(-5, jc.maxDecel(state, 0), DELTA); + assertEquals(5, jc.maxAccel(state, 0), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java index c4e37f9fb..d3793c8ba 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java @@ -23,9 +23,9 @@ void testOutside() { new Pose2d(-1, -1, new Rotation2d(0)), 0, 1.2), 0, // spatial, so rad/m 0); - assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); - assertEquals(Double.POSITIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(Double.NEGATIVE_INFINITY, c.maxDecel(p, 0), DELTA); + assertEquals(Double.POSITIVE_INFINITY, c.maxAccel(p, 0), DELTA); + assertEquals(Double.POSITIVE_INFINITY, c.maxV(p), DELTA); } @Test @@ -38,9 +38,9 @@ void testInside() { new Pose2d(0.5, 0.5, new Rotation2d(0)), 0, 1.2), 0, // spatial, so rad/m 0); - assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); - assertEquals(Double.POSITIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(1, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(Double.NEGATIVE_INFINITY, c.maxDecel(p, 0), DELTA); + assertEquals(Double.POSITIVE_INFINITY, c.maxAccel(p, 0), DELTA); + assertEquals(1, c.maxV(p), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java index 65e29e445..a3bdc77b6 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java @@ -31,9 +31,9 @@ void testNormal() { WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m 0); - assertEquals(-8.485, c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); - assertEquals(8.485, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(2.828, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(-8.485, c.maxDecel(p, 0), DELTA); + assertEquals(8.485, c.maxAccel(p, 0), DELTA); + assertEquals(2.828, c.maxV(p), DELTA); } @Test @@ -46,7 +46,7 @@ void testVelocity2() { new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 1, // spatial, so rad/m 0); - assertEquals(5.656, c.getMaxVelocity(p).getValue(), DELTA); + assertEquals(5.656, c.maxV(p), DELTA); } @Test @@ -62,10 +62,9 @@ void testAccel() { 1, 0); // there is an accel limit. - assertEquals(-8.485, - c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); + assertEquals(-8.485, c.maxDecel(p, 0), DELTA); assertEquals(8.485, - c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); + c.maxAccel(p, 0), DELTA); } @Test @@ -80,9 +79,8 @@ void testAccel2() { 1, // spatial, so rad/m 0); // this number is still quite high even with a low scale. - assertEquals(-16.971, - c.getMinMaxAcceleration(p, 0).getMinAccel(), DELTA); + assertEquals(-16.971, c.maxDecel(p, 0), DELTA); assertEquals(16.971, - c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); + c.maxAccel(p, 0), DELTA); } } From 8972b8ae952935beb358b9b8d3ddb90b62eca927 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 20:54:35 -0800 Subject: [PATCH 27/42] fix curvature for turn-in-place --- .../path/spline/HolonomicSpline.java | 9 ++-- .../path/spline/HolonomicSplineTest.java | 46 +++++++++++++++++++ 2 files changed, 52 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index bec9c2bc9..db862195b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -212,7 +212,7 @@ private double getVelocity(double s) { // // // now yields SE(2) L2 norm, not just cartesian. - return Math.sqrt(dx*dx+dy*dy+dtheta*dtheta); + return Math.sqrt(dx * dx + dy * dy + dtheta * dtheta); } /** @@ -221,11 +221,14 @@ private double getVelocity(double s) { * Note the denominator is distance in this case, not the parameter, p. * but the argument to this function *is* the parameter, s. :-) */ - private double getCurvature(double s) { + double getCurvature(double s) { double dx = dx(s); double dy = dy(s); double ddx = ddx(s); double ddy = ddy(s); - return (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.sqrt((dx * dx + dy * dy))); + double d = dx * dx + dy * dy; + if (d <= 0) + return 0; + return (dx * ddy - ddx * dy) / Math.pow(d, 1.5); } } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index ebf62b4cc..6fd2c5512 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -36,6 +36,52 @@ class HolonomicSplineTest implements Timeless { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); + @Test + void testCurvature() { + // straight line, zero curvature. + HolonomicSpline s = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1)); + assertEquals(0, s.getCurvature(0.5), DELTA); + + // left turn + s = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1)); + assertEquals(0.950, s.getCurvature(0.5), DELTA); + + // rotation in place yields zero curvature since there is no x/y motion, so + // curvature has no meaning. + s = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(0, 0, 1), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d(1)), + new DirectionSE2(0, 0, 1), 1)); + assertEquals(0, s.getCurvature(0.5), DELTA); + } + @Test void testCourse() { Rotation2d course = new Rotation2d(Math.PI / 4); From 9252b13dcb04db3daaea4acbf8d14c5facdcec6e Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 22:05:51 -0800 Subject: [PATCH 28/42] more trajectories --- .../team100/lib/geometry/DirectionSE2.java | 2 + .../team100/lib/geometry/GeometryUtil.java | 48 ++++++ .../path/spline/HolonomicSpline.java | 10 +- .../lib/geometry/DirectionSE2Test.java | 14 ++ .../team100/lib/geometry/WaypointSE2Test.java | 25 +++ .../team100/lib/util/GeometryUtilTest.java | 161 ++++++++++++++++++ 6 files changed, 257 insertions(+), 3 deletions(-) create mode 100644 lib/src/test/java/org/team100/lib/geometry/DirectionSE2Test.java create mode 100644 lib/src/test/java/org/team100/lib/geometry/WaypointSE2Test.java diff --git a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java index a3d57f090..49fe97389 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -25,6 +25,8 @@ public class DirectionSE2 { public DirectionSE2(double px, double py, double ptheta) { double h = Math.sqrt(px * px + py * py + ptheta * ptheta); + if (h < 1e-6) + throw new IllegalArgumentException("zero direction is not allowed"); x = px / h; y = py / h; theta = ptheta / h; diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 825ebb38b..058939f9b 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -30,6 +30,54 @@ public class GeometryUtil { private GeometryUtil() { } + /** + * Change in course per change in position. + * + * The inverse radius of the circle fit to the three points. + * + * This is to replace the spline-derived curvature. + * + * https://en.wikipedia.org/wiki/Menger_curvature + * + * https://hratliff.com/files/curvature_calculations_and_circle_fitting.pdf + */ + public static double mengerCurvature(Translation2d t0, Translation2d t1, Translation2d t2) { + double x1 = t0.getX(); + double x2 = t1.getX(); + double x3 = t2.getX(); + double y1 = t0.getY(); + double y2 = t1.getY(); + double y3 = t2.getY(); + double dx12 = x2 - x1; + double dx23 = x3 - x2; + double dx13 = x1 - x3; + double dy12 = y2 - y1; + double dy23 = y3 - y2; + double dy13 = y1 - y3; + double num = 2 * Math.abs(dx12 * dy23 - dy12 * dx23); + double den = Math.sqrt((dx12 * dx12 + dy12 * dy12) + * (dx23 * dx23 + dy23 * dy23) + * (dx13 * dx13 + dy13 * dy13)); + if (den < 1e-6) { + // this isn't really zero + return 0; + } + return num / den; + } + + /** + * Change in heading per change in position, to replace the spline-derived + * heading rate. + */ + public static double headingRatio(Pose2d p0, Pose2d p1) { + Rotation2d h0 = p0.getRotation(); + Rotation2d h1 = p1.getRotation(); + double d = doubleGeodesicDistance(p0, p1); + if (Math.abs(d) < 1e-6) + return 0; + return h1.minus(h0).getRadians() / d; + } + /** Return a projected onto the direction of b, retaining the omega of a */ public static ChassisSpeeds project(ChassisSpeeds a, ChassisSpeeds b) { double norm = norm(b); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index db862195b..44f7af9e1 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -147,8 +147,10 @@ private double getDHeading(double s) { /** * Change in heading per distance traveled, i.e. spatial change in heading. * dtheta/ds (radians/meter). + * + * TODO: elsewhere this is combined with R2 pathwise velocity, so this is wrong. */ - private double getDHeadingDs(double s) { + public double getDHeadingDs(double s) { return getDHeading(s) / getVelocity(s); } @@ -221,14 +223,16 @@ private double getVelocity(double s) { * Note the denominator is distance in this case, not the parameter, p. * but the argument to this function *is* the parameter, s. :-) */ - double getCurvature(double s) { + public double getCurvature(double s) { double dx = dx(s); double dy = dy(s); double ddx = ddx(s); double ddy = ddy(s); double d = dx * dx + dy * dy; - if (d <= 0) + if (d <= 0) { + // this isn't really zero return 0; + } return (dx * ddy - ddx * dy) / Math.pow(d, 1.5); } } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/geometry/DirectionSE2Test.java b/lib/src/test/java/org/team100/lib/geometry/DirectionSE2Test.java new file mode 100644 index 000000000..988b66a32 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/geometry/DirectionSE2Test.java @@ -0,0 +1,14 @@ +package org.team100.lib.geometry; + +import static org.junit.jupiter.api.Assertions.assertThrows; + +import org.junit.jupiter.api.Test; + +public class DirectionSE2Test { + @Test + void testCourse() { + // zero direction is an error + assertThrows(IllegalArgumentException.class, () -> new DirectionSE2(0, 0, 0)); + } + +} diff --git a/lib/src/test/java/org/team100/lib/geometry/WaypointSE2Test.java b/lib/src/test/java/org/team100/lib/geometry/WaypointSE2Test.java new file mode 100644 index 000000000..1f1027083 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/geometry/WaypointSE2Test.java @@ -0,0 +1,25 @@ +package org.team100.lib.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class WaypointSE2Test { + private static final double DELTA = 0.001; + + @Test + void testCourse() { + WaypointSE2 w0 = new WaypointSE2( + new Pose2d(new Translation2d(), new Rotation2d()), + new DirectionSE2(0, 0, 1), 1); + + assertEquals(0, w0.course().x, DELTA); + assertEquals(0, w0.course().y, DELTA); + assertEquals(1, w0.course().theta, DELTA); + } + +} diff --git a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java index 5560ce9d8..2be7af73f 100644 --- a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java @@ -5,7 +5,10 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Quaternion; @@ -17,8 +20,166 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; class GeometryUtilTest { + private static final boolean DEBUG = false; private static final double DELTA = 0.001; + @Test + void testCurvature() { + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1)); + // verify one point + { + double splineCurvature = spline.getCurvature(0.5); + assertEquals(0.950, splineCurvature, DELTA); + Pose2d p0 = spline.getPose2d(0.49); + Pose2d p1 = spline.getPose2d(0.5); + Pose2d p2 = spline.getPose2d(0.51); + double mengerCurvature = GeometryUtil.mengerCurvature( + p0.getTranslation(), p1.getTranslation(), p2.getTranslation()); + assertEquals(0.950, mengerCurvature, DELTA); + } + // verify all the points + double DS = 0.01; + for (double s = DS; s <= 1 - DS; s += DS) { + double splineCurvature = spline.getCurvature(s); + Pose2d p0 = spline.getPose2d(s - DS); + Pose2d p1 = spline.getPose2d(s); + Pose2d p2 = spline.getPose2d(s + DS); + double mengerCurvature = GeometryUtil.mengerCurvature( + p0.getTranslation(), p1.getTranslation(), p2.getTranslation()); + if (DEBUG) + System.out.printf("%f %f %f %f\n", s, splineCurvature, mengerCurvature, + splineCurvature - mengerCurvature); + // error scales with ds. + assertEquals(splineCurvature, mengerCurvature, 0.001); + } + } + + @Test + void testCurvature2() { + // no curve + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1)); + double splineCurvature = spline.getCurvature(0.5); + assertEquals(0, splineCurvature, DELTA); + Pose2d p0 = spline.getPose2d(0.49); + Pose2d p1 = spline.getPose2d(0.5); + Pose2d p2 = spline.getPose2d(0.51); + double mengerCurvature = GeometryUtil.mengerCurvature( + p0.getTranslation(), p1.getTranslation(), p2.getTranslation()); + assertEquals(0, mengerCurvature, DELTA); + } + + @Test + void testCurvature3() { + // turn in place + WaypointSE2 w0 = new WaypointSE2( + new Pose2d(new Translation2d(), new Rotation2d()), + new DirectionSE2(0, 0, 1), 1); + WaypointSE2 w1 = new WaypointSE2( + new Pose2d(new Translation2d(), new Rotation2d(1)), + new DirectionSE2(0, 0, 1), 1); + HolonomicSpline spline = new HolonomicSpline(w0, w1); + + double splineCurvature = spline.getCurvature(0.5); + assertEquals(0, splineCurvature, DELTA); + Pose2d p0 = spline.getPose2d(0.49); + Pose2d p1 = spline.getPose2d(0.5); + Pose2d p2 = spline.getPose2d(0.51); + double mengerCurvature = GeometryUtil.mengerCurvature( + p0.getTranslation(), p1.getTranslation(), p2.getTranslation()); + assertEquals(0, mengerCurvature, DELTA); + } + + @Test + void testHeadingRate() { + // note spline rotation rate is not constant, to make it more interesting + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 1), 1)); + { + double splineHR = spline.getDHeadingDs(0.5); + assertEquals(0.869, splineHR, DELTA); + Pose2d p0 = spline.getPose2d(0.49); + Pose2d p1 = spline.getPose2d(0.51); + double discreteHR = GeometryUtil.headingRatio(p0, p1); + assertEquals(0.869, discreteHR, DELTA); + } + double DS = 0.001; + for (double s = DS; s <= 1 - DS; s += DS) { + double splineHR = spline.getDHeadingDs(s); + Pose2d p0 = spline.getPose2d(s - DS); + Pose2d p1 = spline.getPose2d(s + DS); + double discreteHR = GeometryUtil.headingRatio(p0, p1); + if (DEBUG) + System.out.printf("%f %f %f %f\n", s, splineHR, discreteHR, splineHR - discreteHR); + // error scales with ds + assertEquals(splineHR, discreteHR, 0.00001); + } + } + + @Test + void testHeadingRate2() { + // turning in place + HolonomicSpline spline = new HolonomicSpline( + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(0, 0, 1), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d(1)), + new DirectionSE2(0, 0, 1), 1)); + { + double splineHR = spline.getDHeadingDs(0.5); + // this is heading change per L2 metric; pure rotation = 1 + assertEquals(1, splineHR, DELTA); + Pose2d p0 = spline.getPose2d(0.49); + Pose2d p1 = spline.getPose2d(0.51); + double discreteHR = GeometryUtil.headingRatio(p0, p1); + assertEquals(1, discreteHR, DELTA); + } + double DS = 0.001; + for (double s = DS; s <= 1 - DS; s += DS) { + double splineHR = spline.getDHeadingDs(s); + Pose2d p0 = spline.getPose2d(s - DS); + Pose2d p1 = spline.getPose2d(s + DS); + double discreteHR = GeometryUtil.headingRatio(p0, p1); + if (DEBUG) + System.out.printf("%f %f %f %f\n", s, splineHR, discreteHR, splineHR - discreteHR); + // error scales with ds + assertEquals(splineHR, discreteHR, 0.00001); + } + } + @Test void testProject() { { From cb80c3802b881acb9a098386bba1a8db60d479c4 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 22:33:17 -0800 Subject: [PATCH 29/42] fix the build --- .../team100/lib/trajectory/TrajectoryPlotter.java | 4 ---- .../team100/lib/trajectory/path/PathFactoryTest.java | 12 ++++++------ 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java index 7e4992e40..37c3ed391 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -4,7 +4,6 @@ import java.awt.Frame; import java.util.List; import java.util.function.Supplier; -import java.util.function.ToDoubleFunction; import javax.swing.BoxLayout; import javax.swing.JDialog; @@ -24,9 +23,6 @@ import org.jfree.data.xy.XYSeriesCollection; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.spline.HolonomicSpline; -import org.team100.lib.trajectory.timing.TimedPose; - -import edu.wpi.first.math.geometry.Pose2d; public class TrajectoryPlotter { public static final boolean SHOW = false; diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index eb625fdee..d4de1e5a3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -121,7 +121,7 @@ void testSpin() { TrajectoryPlotter.plot(path, 0.1); } - /** Hard corners work now. */ + /** Hard corners once again do not work. */ @Test void testActualCorner() { List waypoints = List.of( @@ -135,11 +135,11 @@ void testActualCorner() { new Translation2d(1, 0), new Rotation2d()), new DirectionSE2(1, 0, 0), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - new Rotation2d()), - new DirectionSE2(0, 1, 0), 1), + // new WaypointSE2( + // new Pose2d( + // new Translation2d(1, 0), + // new Rotation2d()), + // new DirectionSE2(0, 1, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 1), From 6cdc218587de6f8a6a2f97fc8f79340116c00d33 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 22:38:03 -0800 Subject: [PATCH 30/42] blarg --- .../lib/trajectory/TrajectoryPlotter.java | 2 +- .../lib/trajectory/path/PathFactoryTest.java | 22 ++++++++++++------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java index 37c3ed391..bbbe90d82 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -25,7 +25,7 @@ import org.team100.lib.trajectory.path.spline.HolonomicSpline; public class TrajectoryPlotter { - public static final boolean SHOW = false; + public static final boolean SHOW = true; private static final int SIZE = 500; private final TrajectoryToVectorSeries converter; diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index d4de1e5a3..82d373386 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -121,7 +121,12 @@ void testSpin() { TrajectoryPlotter.plot(path, 0.1); } - /** Hard corners once again do not work. */ + /** + * Hard corners once again do not work. + * + * It *does* kinda work if the rotation axis is doing anything, which seems + * dumb. + */ @Test void testActualCorner() { List waypoints = List.of( @@ -134,12 +139,12 @@ void testActualCorner() { new Pose2d( new Translation2d(1, 0), new Rotation2d()), - new DirectionSE2(1, 0, 0), 1), - // new WaypointSE2( - // new Pose2d( - // new Translation2d(1, 0), - // new Rotation2d()), - // new DirectionSE2(0, 1, 0), 1), + new DirectionSE2(0, 0, 1), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(0, 0, 1), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 1), @@ -230,7 +235,8 @@ void testDx() { List motion = PathFactory.parameterizeSplines(splines, 0.001, 0.001, 0.001); for (Pose2dWithMotion p : motion) { if (DEBUG) - System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), p.getPose().pose().getTranslation().getY()); + System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), + p.getPose().pose().getTranslation().getY()); } } From c2f4ffd495ce0085c335c0afb068378f2ad0dd53 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 23 Dec 2025 22:40:28 -0800 Subject: [PATCH 31/42] do not plot --- .../test/java/org/team100/lib/trajectory/TrajectoryPlotter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java index bbbe90d82..37c3ed391 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -25,7 +25,7 @@ import org.team100.lib.trajectory.path.spline.HolonomicSpline; public class TrajectoryPlotter { - public static final boolean SHOW = true; + public static final boolean SHOW = false; private static final int SIZE = 500; private final TrajectoryToVectorSeries converter; From ddb5a7d2926be4b626a91786ac84e0524a316bbb Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 24 Dec 2025 11:51:48 -0800 Subject: [PATCH 32/42] cleaning --- .../team100/lib/geometry/GeometryUtil.java | 15 +++--------- .../lib/geometry/Pose2dWithMotion.java | 24 +++++++++---------- .../java/org/team100/lib/util/Math100.java | 1 + .../org/team100/lib/util/Math100Test.java | 8 ------- 4 files changed, 16 insertions(+), 32 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 058939f9b..035748afb 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -226,16 +226,6 @@ public static double distance(Rotation2d a, final Rotation2d other) { return a.unaryMinus().rotateBy(other).getRadians(); } - public static Rotation2d interpolate2(Rotation2d a, final Rotation2d b, double x) { - if (x <= 0.0) { - return a; - } else if (x >= 1.0) { - return b; - } - double angle_diff = a.unaryMinus().rotateBy(b).getRadians(); - return a.rotateBy(Rotation2d.fromRadians(angle_diff * x)); - } - /** Straight-line (not constant-twist) interpolation. */ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { if (x <= 0.0) { @@ -249,11 +239,11 @@ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { Rotation2d bR = b.getRotation(); // each translation axis is interpolated separately Translation2d lerpT = aT.interpolate(bT, x); - // Rotation2d lerpR = aR.interpolate(bR, x); - Rotation2d lerpR = interpolate2(aR, bR, x); + Rotation2d lerpR = aR.interpolate(bR, x); return new Pose2d(lerpT, lerpR); } + /** Linear interpolation of each component */ public static DirectionSE2 interpolate(DirectionSE2 a, DirectionSE2 b, double x) { return new DirectionSE2( MathUtil.interpolate(a.x, b.x, x), @@ -261,6 +251,7 @@ public static DirectionSE2 interpolate(DirectionSE2 a, DirectionSE2 b, double x) MathUtil.interpolate(a.theta, b.theta, x)); } + /** straight-line interpolation of pose, linear interpolation of course */ public static WaypointSE2 interpolate( WaypointSE2 a, WaypointSE2 b, diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 8c4e84c61..6f7fa00c8 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -15,28 +15,28 @@ public class Pose2dWithMotion { private static final boolean DEBUG = false; /** Pose and course. */ - private final WaypointSE2 m_pose; + private final WaypointSE2 m_waypoint; /** Change in heading per meter of motion, rad/m. */ private final double m_headingRate; /** Change in course per change in distance, rad/m. */ private final double m_curvatureRad_M; /** - * @param pose location and heading and direction of travel + * @param waypoint location and heading and direction of travel * @param headingRate change in heading, per meter traveled * @param curvatureRad_M change in course per meter traveled. */ public Pose2dWithMotion( - WaypointSE2 pose, + WaypointSE2 waypoint, double headingRate, double curvatureRad_M) { - m_pose = pose; + m_waypoint = waypoint; m_headingRate = headingRate; m_curvatureRad_M = curvatureRad_M; } public WaypointSE2 getPose() { - return m_pose; + return m_waypoint; } /** @@ -56,7 +56,7 @@ public double getCurvature() { /** This no longer uses a constant-twist arc, it's a straight line. */ public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) { return new Pose2dWithMotion( - GeometryUtil.interpolate(m_pose, other.m_pose, x), + GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x), MathUtil.interpolate(m_headingRate, other.m_headingRate, x), Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x)); } @@ -76,7 +76,7 @@ public double distanceM(Pose2dWithMotion other) { } public double distanceCartesian(Pose2dWithMotion other) { - return m_pose.pose().getTranslation().getDistance(other.m_pose.pose().getTranslation()); + return m_waypoint.pose().getTranslation().getDistance(other.m_waypoint.pose().getTranslation()); } public boolean equals(Object other) { @@ -87,7 +87,7 @@ public boolean equals(Object other) { } Pose2dWithMotion p2dwc = (Pose2dWithMotion) other; - if (!m_pose.equals(p2dwc.m_pose)) { + if (!m_waypoint.equals(p2dwc.m_waypoint)) { if (DEBUG) System.out.println("wrong waypoint"); return false; @@ -108,10 +108,10 @@ public boolean equals(Object other) { public String toString() { return String.format( "x %5.3f, y %5.3f, theta %5.3f, course %s, dtheta %5.3f, curvature %5.3f", - m_pose.pose().getTranslation().getX(), - m_pose.pose().getTranslation().getY(), - m_pose.pose().getRotation().getRadians(), - m_pose.course(), + m_waypoint.pose().getTranslation().getX(), + m_waypoint.pose().getTranslation().getY(), + m_waypoint.pose().getRotation().getRadians(), + m_waypoint.course(), m_headingRate, m_curvatureRad_M); } diff --git a/lib/src/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index dad4eb7bb..8ff0e51b0 100644 --- a/lib/src/main/java/org/team100/lib/util/Math100.java +++ b/lib/src/main/java/org/team100/lib/util/Math100.java @@ -51,6 +51,7 @@ public static double limit(double v, double min, double max) { return Math.min(max, Math.max(min, v)); } + /** Linear interpolation between a and b */ public static double interpolate(double a, double b, double x) { if (x == 0) return a; diff --git a/lib/src/test/java/org/team100/lib/util/Math100Test.java b/lib/src/test/java/org/team100/lib/util/Math100Test.java index c6bada2fd..d0c8e5e8c 100644 --- a/lib/src/test/java/org/team100/lib/util/Math100Test.java +++ b/lib/src/test/java/org/team100/lib/util/Math100Test.java @@ -38,14 +38,6 @@ void testSolveQuadraticZero() { assertEquals(0, roots.size()); } - - - - - - - - @Test void testGetMinDistance() { double measurement = 4; From e5f1b065c8f25218cad8c5a14e2af86207bf87d8 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 24 Dec 2025 12:18:01 -0800 Subject: [PATCH 33/42] organize metrics --- .../team100/lib/geometry/GeometryUtil.java | 95 +--------------- .../org/team100/lib/geometry/Metrics.java | 107 ++++++++++++++++++ .../lib/geometry/Pose2dWithMotion.java | 2 +- .../localization/AprilTagRobotLocalizer.java | 4 +- .../team100/lib/trajectory/path/Path100.java | 4 +- .../lib/trajectory/path/PathFactory.java | 3 +- .../path/spline/HolonomicSpline.java | 4 +- .../path/spline/HolonomicSpline3d.java | 4 +- .../{util => geometry}/GeometryUtilTest.java | 62 +--------- .../org/team100/lib/geometry/MetricsTest.java | 66 +++++++++++ .../org/team100/lib/geometry/TestSE2Math.java | 10 +- .../optimization/GoldenSectionSearchTest.java | 6 +- .../lib/optimization/GradientDescentTest.java | 5 +- .../lib/optimization/TernarySearchTest.java | 5 +- .../lib/trajectory/TrajectoryTest.java | 4 +- .../path/spline/HolonomicSplineTest.java | 3 +- 16 files changed, 209 insertions(+), 175 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/geometry/Metrics.java rename lib/src/test/java/org/team100/lib/{util => geometry}/GeometryUtilTest.java (86%) create mode 100644 lib/src/test/java/org/team100/lib/geometry/MetricsTest.java diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 035748afb..3fba2e9b0 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -25,7 +25,7 @@ * Lots of utility functions. */ public class GeometryUtil { - private static final boolean DEBUG = false; + static final boolean DEBUG = false; private GeometryUtil() { } @@ -72,7 +72,7 @@ public static double mengerCurvature(Translation2d t0, Translation2d t1, Transla public static double headingRatio(Pose2d p0, Pose2d p1) { Rotation2d h0 = p0.getRotation(); Rotation2d h1 = p1.getRotation(); - double d = doubleGeodesicDistance(p0, p1); + double d = Metrics.doubleGeodesicDistance(p0, p1); if (Math.abs(d) < 1e-6) return 0; return h1.minus(h0).getRadians() / d; @@ -80,7 +80,7 @@ public static double headingRatio(Pose2d p0, Pose2d p1) { /** Return a projected onto the direction of b, retaining the omega of a */ public static ChassisSpeeds project(ChassisSpeeds a, ChassisSpeeds b) { - double norm = norm(b); + double norm = Metrics.translationalNorm(b); if (norm < 1e-9) { // there's no target course, bail out return a; @@ -187,31 +187,6 @@ public static boolean isParallel(Rotation2d a, Rotation2d b) { || Math.abs(a.getRadians() - WrapRadians(b.getRadians() + Math.PI)) <= 1e-12; } - /** - * The norm of the translational part of the twist. Note this does not match the - * path length for nonzero omega. - */ - public static double norm(Twist2d a) { - return Math.hypot(a.dx, a.dy); - } - - /* All components of the twist. */ - public static double normL2(Twist2d a) { - return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); - } - - public static double norm(Twist3d t) { - Vector v = VecBuilder.fill(t.dx, t.dy, t.dz, t.rx, t.ry, t.rz); - return v.norm(); - } - - public static double norm(ChassisSpeeds a) { - // Common case of dy == 0 - if (a.vyMetersPerSecond == 0.0) - return Math.abs(a.vxMetersPerSecond); - return Math.hypot(a.vxMetersPerSecond, a.vyMetersPerSecond); - } - public static boolean near(ChassisSpeeds a, ChassisSpeeds b) { return MathUtil.isNear(a.vxMetersPerSecond, b.vxMetersPerSecond, 1e-6) && MathUtil.isNear(a.vyMetersPerSecond, b.vyMetersPerSecond, 1e-6) @@ -222,10 +197,6 @@ public static Rotation2d flip(Rotation2d a) { return new Rotation2d(MathUtil.angleModulus(a.getRadians() + Math.PI)); } - public static double distance(Rotation2d a, final Rotation2d other) { - return a.unaryMinus().rotateBy(other).getRadians(); - } - /** Straight-line (not constant-twist) interpolation. */ public static Pose2d interpolate(Pose2d a, Pose2d b, double x) { if (x <= 0.0) { @@ -301,62 +272,6 @@ public static Rotation3d interpolate(Rotation3d a, Rotation3d b, double x) { MathUtil.interpolate(a.getZ(), b.getZ(), x)); } - // TODO: move this whole section about distances to a separate class. - // - // https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf - - public static double distance(PoseWithCurvature a, PoseWithCurvature b) { - // this is not used - return norm(slog(transformBy(inverse(a.poseMeters), b.poseMeters))); - } - - /** - * Distance along the arc between the two poses (in either order) produced by a - * constant twist. - */ - public static double distanceM(Pose2d a, Pose2d b) { - return norm(slog(transformBy(inverse(a), b))); - } - - /** - * Double-geodesic combines the angular distance with the translational - * distance, weighting 1 radian equal to 1 meter. - * - * This is not the geodesic distance, which is zero for spin-in-place. It's just - * the L2 norm for all three dimensions. - * - * TODO: adjustable weights - * - * Note the Chirikjian paper below suggests using mass and inertia for weighting - * - * @see https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf - * @see https://rpk.lcsr.jhu.edu/wp-content/uploads/2017/08/Partial-Bi-Invariance-of-SE3-Metrics1.pdf - */ - public static double doubleGeodesicDistance(Pose2d a, Pose2d b) { - Translation2d tDiff = a.getTranslation().minus(b.getTranslation()); - double tSqDist = dot(tDiff, tDiff); - double aDiff = a.getRotation().minus(b.getRotation()).getRadians(); - if (DEBUG) - System.out.printf("double geodesic distance t %f a %f\n", tSqDist, aDiff * aDiff); - return Math.sqrt(aDiff * aDiff + tSqDist); - } - - public static double doubleGeodesicDistance(Pose2dWithMotion a, Pose2dWithMotion b) { - return doubleGeodesicDistance(a.getPose(), b.getPose()); - } - - public static double doubleGeodesicDistance(WaypointSE2 a, WaypointSE2 b) { - return doubleGeodesicDistance(a.pose(), b.pose()); - } - - public static double distanceM(Translation2d a, Translation2d b) { - return a.getDistance(b); - } - - public static double distanceM(Translation3d a, Translation3d b) { - return a.getDistance(b); - } - public static Translation2d inverse(Translation2d a) { return new Translation2d(-a.getX(), -a.getY()); } @@ -377,7 +292,7 @@ public static boolean poseWithCurvatureEquals(PoseWithCurvature a, PoseWithCurva /** direction of the translational part of the twist */ public static Optional getCourse(Twist2d t) { - if (norm(t) > 1e-12) { + if (Metrics.translationalNorm(t) > 1e-12) { return Optional.of(new Rotation2d(t.dx, t.dy)); } else { return Optional.empty(); @@ -386,7 +301,7 @@ public static Optional getCourse(Twist2d t) { /** robot-relative course */ public static Optional getCourse(ChassisSpeeds t) { - if (norm(t) > 1e-12) { + if (Metrics.translationalNorm(t) > 1e-12) { return Optional.of(new Rotation2d(t.vxMetersPerSecond, t.vyMetersPerSecond)); } else { return Optional.empty(); diff --git a/lib/src/main/java/org/team100/lib/geometry/Metrics.java b/lib/src/main/java/org/team100/lib/geometry/Metrics.java new file mode 100644 index 000000000..4f1523c0d --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/Metrics.java @@ -0,0 +1,107 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N6; +import edu.wpi.first.math.spline.PoseWithCurvature; + +/** + * Various distance metrics and norms. + */ +public class Metrics { + + /** + * Distance along the arc between the two poses (in either order) produced by a + * constant twist. + */ + public static double distanceM(Pose2d a, Pose2d b) { + return translationalNorm( + GeometryUtil.slog( + GeometryUtil.transformBy(GeometryUtil.inverse(a), b))); + } + + /** + * The norm of the translational part of the twist. Note this does not match the + * path length for nonzero omega. + */ + public static double translationalNorm(Twist2d a) { + return Math.hypot(a.dx, a.dy); + } + + /* All components of the twist. */ + public static double l2Norm(Twist2d a) { + return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); + } + + /** All components of the twist */ + public static double l2Norm(Twist3d t) { + Vector v = VecBuilder.fill(t.dx, t.dy, t.dz, t.rx, t.ry, t.rz); + return v.norm(); + } + + /** Angle from a to b */ + public static double distanceRad(Rotation2d a, Rotation2d b) { + return b.minus(a).getRadians(); + } + + /** Just translational velocity */ + public static double translationalNorm(ChassisSpeeds a) { + // Common case (for tank drives) of dy == 0 + if (a.vyMetersPerSecond == 0.0) + return Math.abs(a.vxMetersPerSecond); + return Math.hypot(a.vxMetersPerSecond, a.vyMetersPerSecond); + } + + /** + * Translational part of the twist from a to b. + * + * https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + */ + public static double distance(PoseWithCurvature a, PoseWithCurvature b) { + return translationalNorm( + GeometryUtil.slog(GeometryUtil.transformBy(GeometryUtil.inverse(a.poseMeters), b.poseMeters))); + } + + /** + * Double-geodesic combines the angular distance with the translational + * distance, weighting 1 radian equal to 1 meter. + * + * This is not the geodesic distance, which is zero for spin-in-place. It's just + * the L2 norm for all three dimensions. + * + * TODO: adjustable weights + * + * Note the Chirikjian paper below suggests using mass and inertia for weighting + * + * I thought this would be a good idea for generalizing path distances to SE(2), + * but now I'm not sure. + * + * @see https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + * @see https://rpk.lcsr.jhu.edu/wp-content/uploads/2017/08/Partial-Bi-Invariance-of-SE3-Metrics1.pdf + */ + public static double doubleGeodesicDistance(Pose2d a, Pose2d b) { + Translation2d tDiff = a.getTranslation().minus(b.getTranslation()); + double tSqDist = GeometryUtil.dot(tDiff, tDiff); + double aDiff = a.getRotation().minus(b.getRotation()).getRadians(); + if (GeometryUtil.DEBUG) + System.out.printf("double geodesic distance t %f a %f\n", tSqDist, aDiff * aDiff); + return Math.sqrt(aDiff * aDiff + tSqDist); + } + + /** Double geodesic distance between the pose components */ + public static double doubleGeodesicDistance(WaypointSE2 a, WaypointSE2 b) { + return doubleGeodesicDistance(a.pose(), b.pose()); + } + + /** Double geodesic distance between the pose components */ + public static double doubleGeodesicDistance(Pose2dWithMotion a, Pose2dWithMotion b) { + return doubleGeodesicDistance(a.getPose(), b.getPose()); + } + +} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 6f7fa00c8..477a6632a 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -68,7 +68,7 @@ public double distanceM(Pose2dWithMotion other) { // dheading/dt thing to work. // // - return GeometryUtil.doubleGeodesicDistance(this, other); + return Metrics.doubleGeodesicDistance(this, other); // // // return diff --git a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java index 7f91792ec..54f64aba6 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java @@ -433,9 +433,7 @@ static double[] visionMeasurementStdDevs(double distanceM) { private static double distance(Pose2d a, Pose2d b) { // the translation distance is a little quicker to calculate and we don't care // about the "twist" curve measurement - return GeometryUtil.distanceM( - a.getTranslation(), - b.getTranslation()); + return a.getTranslation().getDistance(b.getTranslation()); } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 053ba6c00..17a6ef826 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -3,7 +3,7 @@ import java.util.ArrayList; import java.util.List; -import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.timing.ScheduleGenerator; @@ -30,7 +30,7 @@ public Path100(final List states) { Pose2dWithMotion p0 = getPoint(i - 1); Pose2dWithMotion p1 = getPoint(i); // use the distance metric that includes rotation - double dist = GeometryUtil.doubleGeodesicDistance(p0, p1); + double dist = Metrics.doubleGeodesicDistance(p0, p1); // double dist = p0.distanceM(p1); m_distances[i] = m_distances[i - 1] + dist; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 145664e08..df29d3e1e 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -4,6 +4,7 @@ import java.util.List; import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.path.spline.HolonomicSpline; @@ -110,7 +111,7 @@ private static void getSegmentArc( if (Math.abs(error.getTranslation().getX()) > maxDx || Math.abs(error.getTranslation().getY()) > maxDy || Math.abs(error.getRotation().getRadians()) > maxDTheta - || GeometryUtil.normL2(twist_half) > maxNorm) { + || Metrics.l2Norm(twist_half) > maxNorm) { // add a point in between // note the extra condition to avoid points too far apart. getSegmentArc(spline, rv, t0, thalf, maxDx, maxDy, maxDTheta); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 44f7af9e1..699e7c728 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -1,7 +1,7 @@ package org.team100.lib.trajectory.path.spline; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.WaypointSE2; @@ -62,7 +62,7 @@ public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { // Distance metric includes both translation and rotation. This is not // the geodesic distance, which is zero for spin-in-place. It's just the // L2 norm for all three dimensions. - double distance = GeometryUtil.doubleGeodesicDistance(p0.pose(), p1.pose()); + double distance = Metrics.doubleGeodesicDistance(p0.pose(), p1.pose()); if (DEBUG) System.out.printf("distance %f\n", distance); double scale0 = p0.scale() * distance; diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java index 3036b996a..ec28340b2 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java @@ -47,8 +47,8 @@ public HolonomicSpline3d(Pose3dWithDirection p0, Pose3dWithDirection p1) { } public HolonomicSpline3d(Pose3dWithDirection p0, Pose3dWithDirection p1, double mN0, double mN1) { - double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); - double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); + double scale0 = mN0 * p0.translation().getDistance(p1.translation()); + double scale1 = mN1 * p0.translation().getDistance(p1.translation()); DirectionSE3 course0 = p0.course(); DirectionSE3 course1 = p1.course(); diff --git a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java similarity index 86% rename from lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java rename to lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java index 2be7af73f..62a2b26c1 100644 --- a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java @@ -1,13 +1,10 @@ -package org.team100.lib.util; +package org.team100.lib.geometry; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; @@ -249,66 +246,13 @@ void testSlog() { @Test void testDistance2d() { - assertEquals(1, GeometryUtil.distanceM( - new Translation2d(1, 0), new Translation2d(0, 0)), DELTA); + assertEquals(1, new Translation2d(1, 0).getDistance(new Translation2d(0, 0)), DELTA); } @Test void testDistance3d() { - assertEquals(1, GeometryUtil.distanceM( - new Translation3d(1, 0, 0), new Translation3d(0, 0, 0)), DELTA); - } - - @Test - void testDistance() { - // same pose => 0 - assertEquals(0, - GeometryUtil.distanceM( - new Pose2d(1, 0, Rotation2d.kZero), - new Pose2d(1, 0, Rotation2d.kZero)), - DELTA); - // 1d distance assertEquals(1, - GeometryUtil.distanceM( - new Pose2d(0, 0, Rotation2d.kZero), - new Pose2d(1, 0, Rotation2d.kZero)), - DELTA); - // 2d distance - assertEquals(1.414, - GeometryUtil.distanceM( - new Pose2d(0, 1, Rotation2d.kZero), - new Pose2d(1, 0, Rotation2d.kZero)), - DELTA); - // rotation means a little arc, so the path length is a little longer. - assertEquals(1.111, - GeometryUtil.distanceM( - new Pose2d(0, 0, Rotation2d.kZero), - new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), - DELTA); - // the arc in this case is the entire quarter circle - assertEquals(1.571, - GeometryUtil.distanceM( - new Pose2d(0, 1, Rotation2d.kZero), - new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), - DELTA); - // order doesn't matter - assertEquals(1.571, - GeometryUtil.distanceM( - new Pose2d(1, 0, Rotation2d.kCCW_Pi_2), - new Pose2d(0, 1, Rotation2d.kZero)), - DELTA); - // pure rotation yields zero distance, which isn't really what we want. - assertEquals(0, - GeometryUtil.distanceM( - new Pose2d(0, 0, Rotation2d.kZero), - new Pose2d(0, 0, Rotation2d.kCCW_90deg)), - DELTA); - // use double geodesic distance to fix that. - assertEquals(1.571, - GeometryUtil.doubleGeodesicDistance( - new Pose2d(0, 0, Rotation2d.kZero), - new Pose2d(0, 0, Rotation2d.kCCW_90deg)), - DELTA); + new Translation3d(1, 0, 0).getDistance(new Translation3d(0, 0, 0)), DELTA); } @Test diff --git a/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java b/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java new file mode 100644 index 000000000..83536fdfb --- /dev/null +++ b/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java @@ -0,0 +1,66 @@ +package org.team100.lib.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.GeometryUtil; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class MetricsTest { + private static final double DELTA = 0.001; + + @Test + void testDistance() { + // same pose => 0 + assertEquals(0, + Metrics.distanceM( + new Pose2d(1, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), + DELTA); + // 1d distance + assertEquals(1, + Metrics.distanceM( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), + DELTA); + // 2d distance + assertEquals(1.414, + Metrics.distanceM( + new Pose2d(0, 1, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), + DELTA); + // rotation means a little arc, so the path length is a little longer. + assertEquals(1.111, + Metrics.distanceM( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), + DELTA); + // the arc in this case is the entire quarter circle + assertEquals(1.571, + Metrics.distanceM( + new Pose2d(0, 1, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), + DELTA); + // order doesn't matter + assertEquals(1.571, + Metrics.distanceM( + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2), + new Pose2d(0, 1, Rotation2d.kZero)), + DELTA); + // pure rotation yields zero distance, which isn't really what we want. + assertEquals(0, + Metrics.distanceM( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(0, 0, Rotation2d.kCCW_90deg)), + DELTA); + // use double geodesic distance to fix that. + assertEquals(1.571, + Metrics.doubleGeodesicDistance( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(0, 0, Rotation2d.kCCW_90deg)), + DELTA); + } + +} diff --git a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java index 4ecf7ef40..840f21f09 100644 --- a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java +++ b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java @@ -296,8 +296,8 @@ void testTwist3() { assertEquals(0.0, between.dy, EPSILON); assertEquals(1, between.dtheta, EPSILON); // in this case, the distances are the same. - assertEquals(1, GeometryUtil.normL2(between), EPSILON); - assertEquals(1, GeometryUtil.doubleGeodesicDistance(start, end), EPSILON); + assertEquals(1, Metrics.l2Norm(between), EPSILON); + assertEquals(1, Metrics.doubleGeodesicDistance(start, end), EPSILON); } @Test @@ -310,10 +310,10 @@ void testTwist4() { assertEquals(-0.5, between.dy, EPSILON); assertEquals(1, between.dtheta, EPSILON); // in this case, the distances are NOT the same. - assertEquals(1.445, GeometryUtil.normL2(between), 0.001); - assertEquals(1.414, GeometryUtil.doubleGeodesicDistance(start, end), 0.001); + assertEquals(1.445, Metrics.l2Norm(between), 0.001); + assertEquals(1.414, Metrics.doubleGeodesicDistance(start, end), 0.001); // this just seems wrong - assertEquals(1.043, GeometryUtil.distanceM(start, end), 0.001); + assertEquals(1.043, Metrics.distanceM(start, end), 0.001); // this is just the cartesian part assertEquals(1, start.getTranslation().getDistance(end.getTranslation()), 0.001); } diff --git a/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java b/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java index ec7e00289..558e125da 100644 --- a/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java +++ b/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java @@ -5,7 +5,7 @@ import java.util.function.DoubleUnaryOperator; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; @@ -78,7 +78,7 @@ void testPose() { DoubleUnaryOperator f = (x) -> { Pose3d sample = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(axis, x)); Twist3d t = desired.log(sample); - return GeometryUtil.norm(t); + return Metrics.l2Norm(t); }; GoldenSectionSearch s = new GoldenSectionSearch(f, 1e-12, 100); assertEquals(1.0, s.solve(-Math.PI, Math.PI), 1e-12); @@ -92,7 +92,7 @@ void testPosePerformance() { DoubleUnaryOperator f = (x) -> { Pose3d sample = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(axis, x)); Twist3d t = desired.log(sample); - return GeometryUtil.norm(t); + return Metrics.l2Norm(t); }; GoldenSectionSearch s = new GoldenSectionSearch(f, 1e-3, 100); diff --git a/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java b/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java index dbcefa898..ada75d3f6 100644 --- a/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java +++ b/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java @@ -6,6 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -77,7 +78,7 @@ void testPose() { Function, Double> f = (x) -> { Pose3d sample = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(axis, x.get(0))); Twist3d t = desired.log(sample); - return GeometryUtil.norm(t); + return Metrics.l2Norm(t); }; GradientDescent s = new GradientDescent<>(Nat.N1(), f, 1e-12, 100); Vector x = VecBuilder.fill(0); @@ -92,7 +93,7 @@ void testPosePerformance() { Function, Double> f = (x) -> { Pose3d sample = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(axis, x.get(0))); Twist3d t = desired.log(sample); - return GeometryUtil.norm(t); + return Metrics.l2Norm(t); }; GradientDescent s = new GradientDescent<>(Nat.N1(), f, 1e-3, 100); diff --git a/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java b/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java index ab7fcc765..bc4e922cb 100644 --- a/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java +++ b/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java @@ -6,6 +6,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; @@ -78,7 +79,7 @@ void testPose() { DoubleUnaryOperator f = (x) -> { Pose3d sample = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(axis, x)); Twist3d t = desired.log(sample); - return GeometryUtil.norm(t); + return Metrics.l2Norm(t); }; TernarySearch s = new TernarySearch(f, 1e-12, 100); assertEquals(1.0, s.solve(-Math.PI, Math.PI), 1e-12); @@ -92,7 +93,7 @@ void testPosePerformance() { DoubleUnaryOperator f = (x) -> { Pose3d sample = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(axis, x)); Twist3d t = desired.log(sample); - return GeometryUtil.norm(t); + return Metrics.l2Norm(t); }; TernarySearch s = new TernarySearch(f, 1e-3, 100); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 7c482eb25..3102c1122 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -116,7 +116,7 @@ void testDheading() { DirectionSE2 course0 = p0.state().getPose().course(); DirectionSE2 course1 = p1.state().getPose().course(); p1.state().getPose().pose().log(p0.state().getPose().pose()); - double dcourse1 = GeometryUtil.norm(course1.minus(course0)); + double dcourse1 = Metrics.translationalNorm(course1.minus(course0)); // double dcourse1 = GeometryUtil.normL2(course1.minus(course0)); double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians(); double radius = dcourse / distance; diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 6fd2c5512..dbb525f65 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -10,6 +10,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; @@ -350,7 +351,7 @@ void testDheading() { double dheading = heading1.minus(heading0).getRadians(); DirectionSE2 course0 = p0.getPose().course(); DirectionSE2 course1 = p1.getPose().course(); - double curve = GeometryUtil.normL2(course1.minus(course0)); + double curve = Metrics.l2Norm(course1.minus(course0)); // this value is wrong because we use the l2 distance, // so it's kind of counting the rotation twice. double dheadingDx = dheading / l2distance; From c01000ace0db93e8a4e385ee67748585be74bab4 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 24 Dec 2025 14:37:49 -0800 Subject: [PATCH 34/42] go back to pathwise distances to match curvature metric --- .../org/team100/lib/geometry/DeltaSE2.java | 2 +- .../team100/lib/geometry/GeometryUtil.java | 3 +- .../org/team100/lib/geometry/Metrics.java | 109 ++++++++++-------- .../lib/geometry/Pose2dWithMotion.java | 57 ++++----- .../java/org/team100/lib/state/ControlR3.java | 2 +- .../team100/lib/trajectory/Trajectory100.java | 8 +- .../team100/lib/trajectory/path/Path100.java | 11 +- .../lib/trajectory/path/PathFactory.java | 2 +- .../timing/CapsizeAccelerationConstraint.java | 4 +- .../trajectory/timing/ConstrainedState.java | 7 +- .../trajectory/timing/ScheduleGenerator.java | 13 ++- .../lib/trajectory/timing/TimedPose.java | 52 ++++----- .../lib/geometry/GeometryUtilTest.java | 11 -- .../org/team100/lib/geometry/MetricsTest.java | 21 ++-- .../org/team100/lib/geometry/TestSE2Math.java | 15 +-- .../lib/trajectory/Trajectory100Test.java | 6 + .../lib/trajectory/TrajectoryTest.java | 18 +-- .../lib/trajectory/path/PathFactoryTest.java | 2 +- .../path/spline/HolonomicSplineTest.java | 15 +-- .../lib/trajectory/timing/TimedStateTest.java | 12 +- 20 files changed, 170 insertions(+), 200 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java b/lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java index f3101e0f7..78bdf59f3 100644 --- a/lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java +++ b/lib/src/main/java/org/team100/lib/geometry/DeltaSE2.java @@ -9,7 +9,7 @@ * This is just a container for the difference between two poses. * * This treats the dimensions as independent, i.e. in the R3 tangent space, - * not the SE(2) manifold. + * not the SE(2) manifold. Actually it's more like R2xS1, independently. * * The SE(2) difference represents a *geodesic* in SE(2), and for differences * that include rotation, this will appear as a curved path -- usually not what diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 3fba2e9b0..90b10eedc 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -66,8 +66,7 @@ public static double mengerCurvature(Translation2d t0, Translation2d t1, Transla } /** - * Change in heading per change in position, to replace the spline-derived - * heading rate. + * Change in heading per change in position. */ public static double headingRatio(Pose2d p0, Pose2d p1) { Rotation2d h0 = p0.getRotation(); diff --git a/lib/src/main/java/org/team100/lib/geometry/Metrics.java b/lib/src/main/java/org/team100/lib/geometry/Metrics.java index 4f1523c0d..eca646cd4 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Metrics.java +++ b/lib/src/main/java/org/team100/lib/geometry/Metrics.java @@ -3,13 +3,11 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N6; -import edu.wpi.first.math.spline.PoseWithCurvature; /** * Various distance metrics and norms. @@ -17,70 +15,92 @@ public class Metrics { /** - * Distance along the arc between the two poses (in either order) produced by a - * constant twist. + * The distance between translational components. Ignores rotation entirely. */ - public static double distanceM(Pose2d a, Pose2d b) { - return translationalNorm( - GeometryUtil.slog( - GeometryUtil.transformBy(GeometryUtil.inverse(a), b))); + public static double translationalDistance(Pose2d a, Pose2d b) { + return a.getTranslation().getDistance(b.getTranslation()); } /** - * The norm of the translational part of the twist. Note this does not match the - * path length for nonzero omega. + * Projection of the shortest distance in SE(2) between two poses into the R2 + * (xy) plane. + * + * It doesn't count the rotational part of the distance per se, just the effect + * of the rotation on the planar part. + * + * A geodesic is the shortest distance (constant twist) in the SE(2) manifold, + * which takes a path that looks like a little arc in the x/y plane if there is + * a non-zero theta component. + * + * This function returns the R2 path length of that little arc. + * + * https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + * + * For distance that ignores the SE(2) coupling, see DeltaSE2, which treats + * SE(2) as R2xS1. */ - public static double translationalNorm(Twist2d a) { - return Math.hypot(a.dx, a.dy); + public static double projectedDistance(Pose2d a, Pose2d b) { + return translationalNorm(a.log(b)); } - /* All components of the twist. */ - public static double l2Norm(Twist2d a) { - return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); + /** + * L2 norm of only the translational part of the twist; this is the pathwise + * length of the little arc. + */ + public static double translationalNorm(Twist2d a) { + return Math.hypot(a.dx, a.dy); } - /** All components of the twist */ - public static double l2Norm(Twist3d t) { - Vector v = VecBuilder.fill(t.dx, t.dy, t.dz, t.rx, t.ry, t.rz); - return v.norm(); + /** The magnitude of the translational velocity. */ + public static double translationalNorm(ChassisSpeeds a) { + return Math.hypot(a.vxMetersPerSecond, a.vyMetersPerSecond); } - /** Angle from a to b */ - public static double distanceRad(Rotation2d a, Rotation2d b) { - return b.minus(a).getRadians(); - } + ///////////////////////////////////////////////////////////////// + /// + /// DANGER ZONE + /// + /// Don't use anything below here unless you really know what you're doing - /** Just translational velocity */ - public static double translationalNorm(ChassisSpeeds a) { - // Common case (for tank drives) of dy == 0 - if (a.vyMetersPerSecond == 0.0) - return Math.abs(a.vxMetersPerSecond); - return Math.hypot(a.vxMetersPerSecond, a.vyMetersPerSecond); + /* + * L2 norm of all components of the twist. + * + * Note that the components don't use the same units, so this norm can be + * confusing. + * + * Don't use it for anything where you compare it to xy planar distances or + * velocities. + */ + static double l2Norm(Twist2d a) { + return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); } /** - * Translational part of the twist from a to b. + * L2 norm of all components of the twist. * - * https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf + * Note that the components don't use the same units, so this norm can be + * confusing. + * + * It's useful for optimization, because it captures all the dimensions, and + * zero really is zero, but don't use it for anything else. */ - public static double distance(PoseWithCurvature a, PoseWithCurvature b) { - return translationalNorm( - GeometryUtil.slog(GeometryUtil.transformBy(GeometryUtil.inverse(a.poseMeters), b.poseMeters))); + public static double l2Norm(Twist3d t) { + Vector v = VecBuilder.fill(t.dx, t.dy, t.dz, t.rx, t.ry, t.rz); + return v.norm(); } - /** + /** * Double-geodesic combines the angular distance with the translational * distance, weighting 1 radian equal to 1 meter. * - * This is not the geodesic distance, which is zero for spin-in-place. It's just - * the L2 norm for all three dimensions. + * This is not the projected geodesic distance, which is zero for spin-in-place. + * It's just the L2 norm for all three dimensions. * * TODO: adjustable weights * * Note the Chirikjian paper below suggests using mass and inertia for weighting * - * I thought this would be a good idea for generalizing path distances to SE(2), - * but now I'm not sure. + * Don't compare this distance to R2 (xy) planar distances. * * @see https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf * @see https://rpk.lcsr.jhu.edu/wp-content/uploads/2017/08/Partial-Bi-Invariance-of-SE3-Metrics1.pdf @@ -93,15 +113,4 @@ public static double doubleGeodesicDistance(Pose2d a, Pose2d b) { System.out.printf("double geodesic distance t %f a %f\n", tSqDist, aDiff * aDiff); return Math.sqrt(aDiff * aDiff + tSqDist); } - - /** Double geodesic distance between the pose components */ - public static double doubleGeodesicDistance(WaypointSE2 a, WaypointSE2 b) { - return doubleGeodesicDistance(a.pose(), b.pose()); - } - - /** Double geodesic distance between the pose components */ - public static double doubleGeodesicDistance(Pose2dWithMotion a, Pose2dWithMotion b) { - return doubleGeodesicDistance(a.getPose(), b.getPose()); - } - } diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 477a6632a..d174b8ade 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -4,34 +4,27 @@ import edu.wpi.first.math.MathUtil; -/** - * WaypointSE2 and curvature in SE(2). Curvature is a unit vector describing how - * direction changes with the spline parameter. - * - * * the spatial rate of change in heading - * * the spatial rate of change in course - * * the spatial rate of change in course curvature - */ +/** WaypointSE2 with heading rate and curvature. */ public class Pose2dWithMotion { private static final boolean DEBUG = false; /** Pose and course. */ private final WaypointSE2 m_waypoint; /** Change in heading per meter of motion, rad/m. */ - private final double m_headingRate; + private final double m_headingRateRad_M; /** Change in course per change in distance, rad/m. */ private final double m_curvatureRad_M; /** - * @param waypoint location and heading and direction of travel - * @param headingRate change in heading, per meter traveled - * @param curvatureRad_M change in course per meter traveled. + * @param waypoint location and heading and direction of travel + * @param headingRateRad_M change in heading, per meter traveled + * @param curvatureRad_M change in course per meter traveled. */ public Pose2dWithMotion( WaypointSE2 waypoint, - double headingRate, + double headingRateRad_M, double curvatureRad_M) { m_waypoint = waypoint; - m_headingRate = headingRate; + m_headingRateRad_M = headingRateRad_M; m_curvatureRad_M = curvatureRad_M; } @@ -45,38 +38,32 @@ public WaypointSE2 getPose() { * If you want radians per second, multiply by velocity (meters per second). */ public double getHeadingRateRad_M() { - return m_headingRate; + return m_headingRateRad_M; } /** Radians per meter, which is the reciprocal of the radius. */ - public double getCurvature() { + public double getCurvatureRad_M() { return m_curvatureRad_M; } - /** This no longer uses a constant-twist arc, it's a straight line. */ + /** + * Linear interpolation of each component separately. + * + * Not a constant-twist arc. + */ public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) { return new Pose2dWithMotion( GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x), - MathUtil.interpolate(m_headingRate, other.m_headingRate, x), + MathUtil.interpolate(m_headingRateRad_M, other.m_headingRateRad_M, x), Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x)); } - /** This now uses double-geodesic distance, i.e. L2 norm including rotation. */ - public double distanceM(Pose2dWithMotion other) { - // - // this should match HolonomicSpline.getVelocity() for the - // dheading/dt thing to work. - // - // - return Metrics.doubleGeodesicDistance(this, other); - // - // - // return - // m_pose.pose().getTranslation().getDistance(other.m_pose.pose().getTranslation()); - } - + /** + * R2 (xy) planar distance only (IGNORES ROTATION) so that planar + * velocity and curvature works correctly. + */ public double distanceCartesian(Pose2dWithMotion other) { - return m_waypoint.pose().getTranslation().getDistance(other.m_waypoint.pose().getTranslation()); + return Metrics.translationalDistance(m_waypoint.pose(), other.m_waypoint.pose()); } public boolean equals(Object other) { @@ -92,7 +79,7 @@ public boolean equals(Object other) { System.out.println("wrong waypoint"); return false; } - if (!Math100.epsilonEquals(m_headingRate, p2dwc.m_headingRate)) { + if (!Math100.epsilonEquals(m_headingRateRad_M, p2dwc.m_headingRateRad_M)) { if (DEBUG) System.out.println("wrong heading rate"); return false; @@ -112,7 +99,7 @@ public String toString() { m_waypoint.pose().getTranslation().getY(), m_waypoint.pose().getRotation().getRadians(), m_waypoint.course(), - m_headingRate, + m_headingRateRad_M, m_curvatureRad_M); } diff --git a/lib/src/main/java/org/team100/lib/state/ControlR3.java b/lib/src/main/java/org/team100/lib/state/ControlR3.java index 155afdbd7..f3029dc52 100644 --- a/lib/src/main/java/org/team100/lib/state/ControlR3.java +++ b/lib/src/main/java/org/team100/lib/state/ControlR3.java @@ -141,7 +141,7 @@ public static ControlR3 fromTimedPose(TimedPose timedPose) { double thetaa = timedPose.state().getHeadingRateRad_M() * accelM_s_s; // centripetal accel = v^2/r = v^2 * curvature - double curvRad_M = timedPose.state().getCurvature(); + double curvRad_M = timedPose.state().getCurvatureRad_M(); double centripetalAccelM_s_s = velocityM_s * velocityM_s * curvRad_M; double xCa = -1.0 * course.getSin() * centripetalAccelM_s_s; double yCa = course.getCos() * centripetalAccelM_s_s; diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java index a99d6d78a..89cbac683 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java @@ -46,12 +46,12 @@ public TimedPose sample(final double timeS) { final TimedPose ceil = getPoint(i); if (ceil.getTimeS() >= timeS) { final TimedPose floor = getPoint(i - 1); - double betweenPoints = ceil.getTimeS() - floor.getTimeS(); - if (Math.abs(betweenPoints) <= 1e-12) { + double span = ceil.getTimeS() - floor.getTimeS(); + if (Math.abs(span) <= 1e-12) { return ceil; } - double t = (timeS - floor.getTimeS()) / betweenPoints; - return floor.interpolate2(ceil, t); + double delta_t = timeS - floor.getTimeS(); + return floor.interpolate(ceil, delta_t); } } throw new IllegalStateException("impossible trajectory: " + toString()); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 17a6ef826..476920e49 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -14,7 +14,10 @@ */ public class Path100 { private final List m_points; - /** double geodesic distance which is meters and radians */ + /** + * Double geodesic distance which is meters and radians. + * Do not use this distance to compute planar velocity. + */ private final double[] m_distances; public Path100(final List states) { @@ -29,9 +32,9 @@ public Path100(final List states) { m_points.add(states.get(i)); Pose2dWithMotion p0 = getPoint(i - 1); Pose2dWithMotion p1 = getPoint(i); - // use the distance metric that includes rotation - double dist = Metrics.doubleGeodesicDistance(p0, p1); - // double dist = p0.distanceM(p1); + // using the distance metric that includes rotation makes it possible + // to have rotational paths + double dist = Metrics.doubleGeodesicDistance(p0.getPose().pose(), p1.getPose().pose()); m_distances[i] = m_distances[i - 1] + dist; } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index df29d3e1e..5d69dc5d5 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -111,7 +111,7 @@ private static void getSegmentArc( if (Math.abs(error.getTranslation().getX()) > maxDx || Math.abs(error.getTranslation().getY()) > maxDy || Math.abs(error.getRotation().getRadians()) > maxDTheta - || Metrics.l2Norm(twist_half) > maxNorm) { + || Metrics.translationalNorm(twist_half) > maxNorm) { // add a point in between // note the extra condition to avoid points too far apart. getSegmentArc(spline, rv, t0, thalf, maxDx, maxDy, maxDTheta); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java index dd3b62ccd..ff195017b 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java @@ -40,7 +40,7 @@ public CapsizeAccelerationConstraint( @Override public double maxV(final Pose2dWithMotion state) { double mMaxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); - double radius = 1 / state.getCurvature(); + double radius = 1 / state.getCurvatureRad_M(); // abs is used here to make sure sqrt is happy. return Math.sqrt(Math.abs(mMaxCentripetalAccel * radius)); } @@ -78,7 +78,7 @@ public double maxDecel(Pose2dWithMotion state, double velocity) { */ private double alongSq(Pose2dWithMotion state, double velocity) { double maxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); - double radius = 1 / state.getCurvature(); + double radius = 1 / state.getCurvatureRad_M(); double actualCentripetalAccel = velocity * velocity / radius; return maxCentripetalAccel * maxCentripetalAccel - actualCentripetalAccel * actualCentripetalAccel; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index 6daee0a86..afbdefb01 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -60,15 +60,12 @@ public Pose2dWithMotion getState() { return m_state; } - /** this should be L2 distance */ + /** Pathwise distance */ public double getDistanceM() { return m_distanceM; } - /** - * if we're using L2 norm then this isn't meters and isn't always even cartesian - * at all - */ + /** Pathwise velocity */ public double getVelocityM_S() { return m_velocityM_S; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index ba5a2cbbf..6533f263f 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -124,10 +124,9 @@ private List forwardPass(List samples, doubl // work forward through the samples List constrainedStates = new ArrayList<>(samples.size()); - // constrainedStates.add(predecessor); for (int i = 0; i < samples.size(); ++i) { Pose2dWithMotion sample = samples.get(i); - double dsM = sample.distanceM(predecessor.getState()); + double dsM = sample.distanceCartesian(predecessor.getState()); if (DEBUG) System.out.printf("i%d dsM %f\n", i, dsM); ConstrainedState constrainedState = new ConstrainedState( @@ -146,10 +145,12 @@ private List forwardPass(List samples, doubl } private void forwardWork(ConstrainedState s0, ConstrainedState s1) { - // constant-twist path length between states - // actually this is now the double-geodesic metric (L2 for all 3 dimensions) - // which means it is not just meters. - double dsM = s1.getState().distanceM(s0.getState()); + // R2 translation distance between states + // not constant-twist arc + // not double-geodesic with rotation + // Just translation, so that the pathwise velocity matches + // the curvature in the state. + double dsM = s1.getState().distanceCartesian(s0.getState()); // We may need to iterate to find the maximum end velocity and common // acceleration, since acceleration limits may be a function of velocity. diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java index 21b0738fc..c7f1dbd73 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java @@ -3,8 +3,6 @@ import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.util.Math100; -import edu.wpi.first.math.MathUtil; - /** * Represents a state within a 2d holonomic trajectory, i.e. with heading * independent from course. @@ -16,15 +14,9 @@ public class TimedPose { private final Pose2dWithMotion m_state; /** Time we achieve this state. */ private final double m_timeS; - - /** - * ds/dt - * - * if we're using L2 norm then this isn't meters and isn't always even cartesian - * at all - */ + /** Instantaneous pathwise velocity, m/s. */ private final double m_velocityM_S; - /** d^2s/dt^2 */ + /** Instantaneous pathwise (not centripetal) acceleration, m/s^2. */ private double m_accelM_S_S; public TimedPose( @@ -42,10 +34,12 @@ public Pose2dWithMotion state() { return m_state; } + /** Time we achieve this state. */ public double getTimeS() { return m_timeS; } + /** Instantaneous pathwise velocity, m/s. */ public double velocityM_S() { return m_velocityM_S; } @@ -55,7 +49,7 @@ void set_acceleration(double acceleration) { m_accelM_S_S = acceleration; } - /** this means acceleration along the path, not centripetal acceleration. */ + /** Instantaneous pathwise (not centripetal) acceleration, m/s^2. */ public double acceleration() { return m_accelM_S_S; } @@ -69,39 +63,37 @@ public String toString() { m_accelM_S_S); } - public TimedPose interpolate2(TimedPose other, double x) { - final double new_t = MathUtil.interpolate(m_timeS, other.m_timeS, x); - final double delta_t = new_t - getTimeS(); - if (delta_t < 0.0) { - return other.interpolate2(this, 1.0 - x); - } - boolean reversing = m_velocityM_S < 0.0 || (Math.abs(m_velocityM_S - 0.0) <= 1e-12 && acceleration() < 0.0); - final double new_v = m_velocityM_S + m_accelM_S_S * delta_t; - final double new_s = (reversing ? -1.0 : 1.0) - * (m_velocityM_S * delta_t + .5 * m_accelM_S_S * delta_t * delta_t); + /** + * Linear interpolation by time. + * + * Velocity of this state is the initial velocity. + * Acceleration of this state is constant through the whole arc. + */ + public TimedPose interpolate(TimedPose other, double delta_t) { + double tLerp = m_timeS + delta_t; + double vLerp = m_velocityM_S + m_accelM_S_S * delta_t; + double pathwiseDistance = m_velocityM_S * delta_t + 0.5 * m_accelM_S_S * delta_t * delta_t; - double interpolant = new_s / m_state.distanceM(other.m_state); + double distanceBetween = m_state.distanceCartesian(other.m_state); + double interpolant = pathwiseDistance / distanceBetween; if (Double.isNaN(interpolant)) { interpolant = 1.0; } + if (DEBUG) + System.out.printf("tlerp %f\n", tLerp); return new TimedPose( m_state.interpolate(other.m_state, interpolant), - new_t, - new_v, + tLerp, + vLerp, m_accelM_S_S); } - /** L2 norm */ - public double distance(TimedPose other) { - return m_state.distanceM(other.m_state); - } - + /** Translation only, ignores rotation */ public double distanceCartesian(TimedPose other) { return m_state.distanceCartesian(other.m_state); } - @Override public boolean equals(final Object other) { if (!(other instanceof TimedPose)) { diff --git a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java index 62a2b26c1..706a708d0 100644 --- a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java @@ -244,17 +244,6 @@ void testSlog() { assertEquals(1.571, twist.dtheta, DELTA); } - @Test - void testDistance2d() { - assertEquals(1, new Translation2d(1, 0).getDistance(new Translation2d(0, 0)), DELTA); - } - - @Test - void testDistance3d() { - assertEquals(1, - new Translation3d(1, 0, 0).getDistance(new Translation3d(0, 0, 0)), DELTA); - } - @Test void testCollinear() { assertTrue(GeometryUtil.isColinear( diff --git a/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java b/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java index 83536fdfb..7eb14297c 100644 --- a/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java +++ b/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java @@ -3,7 +3,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,43 +14,43 @@ public class MetricsTest { void testDistance() { // same pose => 0 assertEquals(0, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(1, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)), DELTA); // 1d distance assertEquals(1, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)), DELTA); // 2d distance assertEquals(1.414, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(0, 1, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)), DELTA); // rotation means a little arc, so the path length is a little longer. assertEquals(1.111, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), DELTA); // the arc in this case is the entire quarter circle assertEquals(1.571, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(0, 1, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), DELTA); // order doesn't matter assertEquals(1.571, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(1, 0, Rotation2d.kCCW_Pi_2), new Pose2d(0, 1, Rotation2d.kZero)), DELTA); // pure rotation yields zero distance, which isn't really what we want. assertEquals(0, - Metrics.distanceM( + Metrics.projectedDistance( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(0, 0, Rotation2d.kCCW_90deg)), DELTA); @@ -61,6 +60,12 @@ void testDistance() { new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(0, 0, Rotation2d.kCCW_90deg)), DELTA); + // pure rotation translation => zero + assertEquals(0.0, + Metrics.translationalDistance( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(0, 0, Rotation2d.kCCW_90deg)), + DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java index 840f21f09..f398e4637 100644 --- a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java +++ b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java @@ -295,9 +295,9 @@ void testTwist3() { assertEquals(0, between.dx, EPSILON); assertEquals(0.0, between.dy, EPSILON); assertEquals(1, between.dtheta, EPSILON); - // in this case, the distances are the same. - assertEquals(1, Metrics.l2Norm(between), EPSILON); + assertEquals(0, Metrics.translationalNorm(between), EPSILON); assertEquals(1, Metrics.doubleGeodesicDistance(start, end), EPSILON); + assertEquals(0, Metrics.translationalDistance(start, end), EPSILON); } @Test @@ -308,13 +308,10 @@ void testTwist4() { Twist2d between = start.log(end); assertEquals(0.915, between.dx, 0.001); assertEquals(-0.5, between.dy, EPSILON); - assertEquals(1, between.dtheta, EPSILON); - // in this case, the distances are NOT the same. - assertEquals(1.445, Metrics.l2Norm(between), 0.001); + assertEquals(1.0, between.dtheta, EPSILON); + assertEquals(1.043, Metrics.translationalNorm(between), 0.001); assertEquals(1.414, Metrics.doubleGeodesicDistance(start, end), 0.001); - // this just seems wrong - assertEquals(1.043, Metrics.distanceM(start, end), 0.001); - // this is just the cartesian part - assertEquals(1, start.getTranslation().getDistance(end.getTranslation()), 0.001); + assertEquals(1.0, Metrics.translationalDistance(start, end), 0.001); + assertEquals(1.043, Metrics.projectedDistance(start, end), 0.001); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 613b78d3a..cfbfc383e 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -118,6 +118,12 @@ void testSampleThoroughly() { if (DEBUG) System.out.println(trajectory); + if (DEBUG) { + for (double t = 0; t < 1.5; t += 0.1) { + System.out.printf("%5.3f %s\n", t, trajectory.sample(t)); + } + } + assertEquals(1.418, trajectory.duration(), DELTA); assertEquals(0.000, trajectory.sample(0.0).state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0.010, trajectory.sample(0.1).state().getPose().pose().getTranslation().getX(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 3102c1122..9723551f6 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -72,9 +72,7 @@ void testCircle() { TrajectoryPlanner p = new TrajectoryPlanner(c); Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); - // TrajectoryPlotter.plot(trajectory, 0.25); - } @Test @@ -100,8 +98,6 @@ void testDheading() { "t, intrinsic_heading_dt, heading_dt, intrinsic_ca, extrinsic_ca, extrinsic v, intrinsic v, dcourse, dcourse1"); for (double t = 0.04; t < duration; t += 0.04) { TimedPose p1 = trajectory.sample(t); - double distance = p1.distance(p0); - double distanceCartesian = p1.distanceCartesian(p0); Rotation2d heading0 = p0.state().getPose().pose().getRotation(); Rotation2d heading1 = p1.state().getPose().pose().getRotation(); double dheading = heading1.minus(heading0).getRadians(); @@ -117,23 +113,17 @@ void testDheading() { DirectionSE2 course1 = p1.state().getPose().course(); p1.state().getPose().pose().log(p0.state().getPose().pose()); double dcourse1 = Metrics.translationalNorm(course1.minus(course0)); - // double dcourse1 = GeometryUtil.normL2(course1.minus(course0)); double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians(); - double radius = dcourse / distance; - // derived v is pretty close - double v = distance / 0.04; - double extrinsicCa = v * v / radius; - double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.state().getCurvature(); + double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.state().getCurvatureRad_M(); + if (DEBUG) - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", t, intrinsicDheadingDt, dheadingDt, - intrinsicCa, extrinsicCa, - v, p0.velocityM_S(), + intrinsicCa, p0.velocityM_S(), dcourse, dcourse1); p0 = p1; } - // TrajectoryPlotter.plot(trajectory, 0.25); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 82d373386..5c78aab29 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -180,7 +180,7 @@ void testComposite() { new DirectionSE2(1, 0, 0), 1)); Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(foo, 0.1); - assertEquals(28, foo.length(), 0.001); + assertEquals(18, foo.length(), 0.001); } @Test diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index dbb525f65..55cb73f77 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -9,7 +9,6 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.WaypointSE2; @@ -344,28 +343,24 @@ void testDheading() { "s, p0_heading_rate, p0_curvature, distance, post_hoc_heading_rate, post_hoc_curvature, post_hoc_heading_rate2, post_hoc_curvature2"); for (double s = 0.01; s <= 1.0; s += 0.01) { Pose2dWithMotion p1 = spline.getPose2dWithMotion(s); - double l2distance = p1.distanceM(p0); double cartesianDistance = p1.distanceCartesian(p0); Rotation2d heading0 = p0.getPose().pose().getRotation(); Rotation2d heading1 = p1.getPose().pose().getRotation(); double dheading = heading1.minus(heading0).getRadians(); DirectionSE2 course0 = p0.getPose().course(); DirectionSE2 course1 = p1.getPose().course(); - double curve = Metrics.l2Norm(course1.minus(course0)); - // this value is wrong because we use the l2 distance, - // so it's kind of counting the rotation twice. - double dheadingDx = dheading / l2distance; - double curveDx = curve / l2distance; + double curve = Metrics.translationalNorm(course1.minus(course0)); // this value matches the intrinsic one since it just uses // cartesian distance in the denominator. double dheadingDx2 = dheading / cartesianDistance; double curveDx2 = curve / cartesianDistance; + if (DEBUG) System.out.printf( - "%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f \n", - s, p0.getHeadingRateRad_M(), p0.getCurvature(), - l2distance, dheadingDx, curveDx, dheadingDx2, curveDx2); + "%5.3f, %5.3f, %5.3f, %5.3f, %5.3f \n", + s, p0.getHeadingRateRad_M(), p0.getCurvatureRad_M(), + dheadingDx2, curveDx2); p0 = p1; } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index 6b42dbb3a..869a040b9 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -30,16 +30,16 @@ void test() { 0, 0), 1.0, 1.0, 0.0); - TimedPose i0 = start_state.interpolate2(end_state, 0.0); - assertEquals(start_state, i0); - assertEquals(end_state, start_state.interpolate2(end_state, 1.0)); - assertEquals(end_state, end_state.interpolate2(start_state, 0.0)); - assertEquals(start_state, end_state.interpolate2(start_state, 1.0)); + // endpoints + assertEquals(start_state, start_state.interpolate(end_state, 0.0)); + assertEquals(end_state, start_state.interpolate(end_state, 1.0)); - TimedPose intermediate_state = start_state.interpolate2(end_state, 0.5); + // halfway between the states by time + TimedPose intermediate_state = start_state.interpolate(end_state, 0.5); assertEquals(0.5, intermediate_state.getTimeS(), EPSILON); assertEquals(start_state.acceleration(), intermediate_state.acceleration(), EPSILON); assertEquals(0.5, intermediate_state.velocityM_S(), EPSILON); + // close to the start state by distance assertEquals(0.125, intermediate_state.state().getPose().pose().getTranslation().getX(), EPSILON); } } From 8e23d41985f6383713f83b6591a8b7a6086946e6 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 24 Dec 2025 18:24:57 -0800 Subject: [PATCH 35/42] finish making trajectory corner not work --- .../frc2025/Swerve/Auto/GoToCoralStation.java | 1 - .../org/team100/frc2025/robot/Binder.java | 1 - .../CalgamesArm/TrajectoryJointTest.java | 2 +- .../team100/lib/geometry/GeometryUtil.java | 30 ++-- .../org/team100/lib/geometry/Metrics.java | 4 +- .../localization/AprilTagRobotLocalizer.java | 1 - .../team100/lib/logging/LoggerFactory.java | 14 +- .../reference/r3/TrajectoryReferenceR3.java | 4 +- .../java/org/team100/lib/state/ControlR3.java | 4 +- .../java/org/team100/lib/state/ModelR3.java | 4 +- .../r3/commands/GoToPosePosition.java | 1 - .../tank/commands/FixedTrajectory.java | 6 +- .../tank/commands/ToPoseWithTrajectory.java | 6 +- .../java/org/team100/lib/trajectory/NEW.md | 134 ------------------ .../java/org/team100/lib/trajectory/README.md | 6 +- .../team100/lib/trajectory/Trajectory100.java | 27 ++-- .../team100/lib/trajectory/path/Path100.java | 29 +++- .../lib/trajectory/path/PathFactory.java | 10 +- .../path/spline/HolonomicSpline.java | 6 +- .../trajectory/timing/ScheduleGenerator.java | 31 ++-- .../{TimedPose.java => TimedState.java} | 14 +- .../TrajectoryVisualization.java | 4 +- .../r3/FeedforwardControllerR3Test.java | 1 - .../r3/FullStateControllerR3Test.java | 44 +++--- .../lib/geometry/GeometryUtilTest.java | 40 +----- .../lib/optimization/GradientDescentTest.java | 1 - .../lib/optimization/TernarySearchTest.java | 1 - .../prr/AnalyticalJacobianTest.java | 6 +- .../lib/subsystems/prr/JacobianTest.java | 6 +- .../DriveToPoseWithTrajectoryTest.java | 4 +- .../subsystems/swerve/SwerveControlTest.java | 26 ++-- .../subsystems/swerve/SwerveModelTest.java | 26 ++-- .../lib/trajectory/Trajectory100Test.java | 47 +++--- .../lib/trajectory/TrajectoryPlannerTest.java | 62 ++------ .../lib/trajectory/TrajectoryTest.java | 38 ++--- .../trajectory/TrajectoryToVectorSeries.java | 13 +- .../lib/trajectory/path/Path100Test.java | 98 ------------- .../lib/trajectory/path/PathFactoryTest.java | 55 ++----- .../path/spline/HolonomicSplineTest.java | 130 ++--------------- .../trajectory/timing/DirectScheduleTest.java | 2 - .../timing/ScheduleGeneratorTest.java | 26 ++-- .../lib/trajectory/timing/TimedStateTest.java | 6 +- 42 files changed, 279 insertions(+), 692 deletions(-) delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/NEW.md rename lib/src/main/java/org/team100/lib/trajectory/timing/{TimedPose.java => TimedState.java} (91%) diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index 95cd44136..5287f7884 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -6,7 +6,6 @@ import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.CoralStation; -import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; diff --git a/comp/src/main/java/org/team100/frc2025/robot/Binder.java b/comp/src/main/java/org/team100/frc2025/robot/Binder.java index 9fc29c6ad..9c908d349 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Binder.java @@ -7,7 +7,6 @@ import org.team100.frc2025.Climber.ClimberCommands; import org.team100.frc2025.CommandGroups.MoveToAlgaePosition; -import org.team100.frc2025.CommandGroups.ScoreSmart.ScoreCoralSmartLuke; import org.team100.frc2025.Swerve.ManualWithBargeAssist; import org.team100.frc2025.Swerve.ManualWithProfiledReefLock; import org.team100.frc2025.Swerve.Auto.BigLoop; diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java index 623860b1c..41ef39a85 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -63,7 +63,7 @@ void homeToL4() { .println( "t, x, y, r, vx, vy, vr, ax, ay, ar, q1, q2, q3, q1dot, q2dot, q3dot, q1ddot, q2ddot, q3ddot"); for (double tt = 0; tt < t.duration(); tt += 0.02) { - ControlR3 m = ControlR3.fromTimedPose(t.sample(tt)); + ControlR3 m = ControlR3.fromTimedState(t.sample(tt)); Pose2d p = m.pose(); VelocitySE2 v = m.velocity(); AccelerationSE2 a = m.acceleration(); diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 90b10eedc..b0e217c11 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -65,18 +65,6 @@ public static double mengerCurvature(Translation2d t0, Translation2d t1, Transla return num / den; } - /** - * Change in heading per change in position. - */ - public static double headingRatio(Pose2d p0, Pose2d p1) { - Rotation2d h0 = p0.getRotation(); - Rotation2d h1 = p1.getRotation(); - double d = Metrics.doubleGeodesicDistance(p0, p1); - if (Math.abs(d) < 1e-6) - return 0; - return h1.minus(h0).getRadians() / d; - } - /** Return a projected onto the direction of b, retaining the omega of a */ public static ChassisSpeeds project(ChassisSpeeds a, ChassisSpeeds b) { double norm = Metrics.translationalNorm(b); @@ -377,4 +365,22 @@ public static Vector toVec(Translation2d t) { public static Vector toVec3(Translation2d t) { return VecBuilder.fill(t.getX(), t.getY(), 0); } + + ///////////////////////////////////////////////////////////////// + /// + /// DANGER ZONE + /// + /// Don't use anything below here unless you really know what you're doing + /// + /** + * Change in heading per change in position. + */ + static double headingRatio(Pose2d p0, Pose2d p1) { + Rotation2d h0 = p0.getRotation(); + Rotation2d h1 = p1.getRotation(); + double d = Metrics.doubleGeodesicDistance(p0, p1); + if (Math.abs(d) < 1e-6) + return 0; + return h1.minus(h0).getRadians() / d; + } } diff --git a/lib/src/main/java/org/team100/lib/geometry/Metrics.java b/lib/src/main/java/org/team100/lib/geometry/Metrics.java index eca646cd4..2b7e7d050 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Metrics.java +++ b/lib/src/main/java/org/team100/lib/geometry/Metrics.java @@ -71,7 +71,7 @@ public static double translationalNorm(ChassisSpeeds a) { * Don't use it for anything where you compare it to xy planar distances or * velocities. */ - static double l2Norm(Twist2d a) { + public static double l2Norm(Twist2d a) { return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); } @@ -105,7 +105,7 @@ public static double l2Norm(Twist3d t) { * @see https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf * @see https://rpk.lcsr.jhu.edu/wp-content/uploads/2017/08/Partial-Bi-Invariance-of-SE3-Metrics1.pdf */ - public static double doubleGeodesicDistance(Pose2d a, Pose2d b) { + static double doubleGeodesicDistance(Pose2d a, Pose2d b) { Translation2d tDiff = a.getTranslation().minus(b.getTranslation()); double tSqDist = GeometryUtil.dot(tDiff, tDiff); double aDiff = a.getRotation().minus(b.getRotation()).getRadians(); diff --git a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java index 54f64aba6..450f91978 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java @@ -7,7 +7,6 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java index e053465a8..b8cf0269a 100644 --- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java +++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java @@ -26,7 +26,7 @@ import org.team100.lib.subsystems.prr.JointVelocities; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePosition100; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePositions; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -449,14 +449,14 @@ public Rotation2dLogger rotation2dLogger(Level level, String leaf) { return new Rotation2dLogger(level, leaf); } - public class TimedPoseLogger { + public class TimedStateLogger { private final Level m_level; private final Pose2dWithMotionLogger m_pose2dWithMotionLogger; private final DoubleLogger m_timeLogger; private final DoubleLogger m_velocityLogger; private final DoubleLogger m_accelLogger; - TimedPoseLogger(Level level, String leaf) { + TimedStateLogger(Level level, String leaf) { m_level = level; m_pose2dWithMotionLogger = pose2dWithMotionLogger(level, join(leaf, "posestate")); m_timeLogger = doubleLogger(level, join(leaf, "time")); @@ -464,10 +464,10 @@ public class TimedPoseLogger { m_accelLogger = doubleLogger(level, join(leaf, "accel")); } - public void log(Supplier vals) { + public void log(Supplier vals) { if (!allow(m_level)) return; - TimedPose val = vals.get(); + TimedState val = vals.get(); m_pose2dWithMotionLogger.log(val::state); m_timeLogger.log(val::getTimeS); m_velocityLogger.log(val::velocityM_S); @@ -476,8 +476,8 @@ public void log(Supplier vals) { } } - public TimedPoseLogger timedPoseLogger(Level level, String leaf) { - return new TimedPoseLogger(level, leaf); + public TimedStateLogger timedPoseLogger(Level level, String leaf) { + return new TimedStateLogger(level, leaf); } public class PoseWithCurvatureLogger { diff --git a/lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java b/lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java index a23ad41bb..e58df8be1 100644 --- a/lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java +++ b/lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java @@ -64,7 +64,7 @@ public boolean done() { @Override public ModelR3 goal() { - ModelR3 goal = ControlR3.fromTimedPose(m_trajectory.getLastPoint()).model(); + ModelR3 goal = ControlR3.fromTimedState(m_trajectory.getLastPoint()).model(); m_log_goal.log(() -> goal); return goal; } @@ -78,6 +78,6 @@ private double progress() { } private ControlR3 sample(double t) { - return ControlR3.fromTimedPose(m_trajectory.sample(t)); + return ControlR3.fromTimedState(m_trajectory.sample(t)); } } diff --git a/lib/src/main/java/org/team100/lib/state/ControlR3.java b/lib/src/main/java/org/team100/lib/state/ControlR3.java index f3029dc52..874b8d4fa 100644 --- a/lib/src/main/java/org/team100/lib/state/ControlR3.java +++ b/lib/src/main/java/org/team100/lib/state/ControlR3.java @@ -3,7 +3,7 @@ import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -124,7 +124,7 @@ public Control100 theta() { * * Correctly accounts for centripetal acceleration. */ - public static ControlR3 fromTimedPose(TimedPose timedPose) { + public static ControlR3 fromTimedState(TimedState timedPose) { double xx = timedPose.state().getPose().pose().getTranslation().getX(); double yx = timedPose.state().getPose().pose().getTranslation().getY(); double thetax = timedPose.state().getPose().pose().getRotation().getRadians(); diff --git a/lib/src/main/java/org/team100/lib/state/ModelR3.java b/lib/src/main/java/org/team100/lib/state/ModelR3.java index edfabfb07..7816cc8e3 100644 --- a/lib/src/main/java/org/team100/lib/state/ModelR3.java +++ b/lib/src/main/java/org/team100/lib/state/ModelR3.java @@ -5,7 +5,7 @@ import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -129,7 +129,7 @@ public Model100 theta() { /** * Transform timed pose into swerve state. */ - public static ModelR3 fromTimedPose(TimedPose timedPose) { + public static ModelR3 fromTimedState(TimedState timedPose) { Pose2dWithMotion state = timedPose.state(); WaypointSE2 pose = state.getPose(); Translation2d translation = pose.pose().getTranslation(); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java index 15a75137a..0b78d5d75 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java @@ -3,7 +3,6 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java index d999f24dc..b2fd65f14 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java @@ -6,7 +6,7 @@ import org.team100.lib.framework.TimedRobot100; import org.team100.lib.subsystems.tank.TankDrive; import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.controller.LTVUnicycleController; @@ -59,9 +59,9 @@ public void execute() { return; // current for position error double t = progress(); - TimedPose current = m_trajectory.sample(t); + TimedState current = m_trajectory.sample(t); // next for feedforward (and selecting K) - TimedPose next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S); + TimedState next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S); Pose2d currentPose = m_drive.getPose(); Pose2d poseReference = current.state().getPose().pose(); double velocityReference = next.velocityM_S(); diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java index 2cf44c051..4335fea49 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java @@ -10,7 +10,7 @@ import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.controller.LTVUnicycleController; @@ -61,9 +61,9 @@ public void execute() { return; // current for position error double t = progress(); - TimedPose current = m_trajectory.sample(t); + TimedState current = m_trajectory.sample(t); // next for feedforward (and selecting K) - TimedPose next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S); + TimedState next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S); Pose2d currentPose = m_drive.getPose(); Pose2d poseReference = current.state().getPose().pose(); double velocityReference = next.velocityM_S(); diff --git a/lib/src/main/java/org/team100/lib/trajectory/NEW.md b/lib/src/main/java/org/team100/lib/trajectory/NEW.md deleted file mode 100644 index cd35aeded..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/NEW.md +++ /dev/null @@ -1,134 +0,0 @@ -# New Trajectories - -## Background -Prior to 2026, Team 100 trajectories were implemented using code adapted from -254's 2022 repo. This solution was holonomic (unlike the WPI version), and included -optimization of inner knots to minimize change in curvature. The 254 code was written -in a very complex style, so the adaptation was a significant departure, but it still -contained many idiosyncrasies. - -It also supported many things that just didn't seem to matter very much. All our actual -trajectories for the past 2 seasons have had exactly one segment, so the optimizer -never even ran. Even in tests it doesn't seem to do anything that can't be achieved through -careful tuning, which we do anyway. And there was a class of things it couldn't do: -paths with corners, paths with only rotation, very short paths. It computed path-wise -velocity and acceleration, and had separate mechanisms for heading rate and -course rate, probably a remnant of a previous "tank" drive version. -In any case, none of it was anything any student spent any time looking closely at, -it was just a black box. - -For 2026, we should have a trajectory generation system that the students can understand, -end-to-end. It should be much simpler, and without any of the 254 remnants. - -## Design - -As before, there will be several stages: - -* Given a series of waypoints, make a spline -* Sample the spline to make a piecewise linear path with short-enough segments, close enough to the real spline -* Assign timing, velocity, and acceleration to each point in the path - -The representation will use SE(2), rather than the mix of R2 and SE(2) we used before. -This means that the distance metric will include rotation: for now we'll use the -L2 norm on all three components, with equal weighting. We might come back to the -weighting if it seems to matter. - -The SE(2) implementation will mean that rotation-only paths will work exactly the -same as linear or blended ones. - -Each step in the process will be completely separate, unlike the previous version, -where the spline parameter (and derivatives) were used as part of the later steps. -Thus the scheduler should be able to work on *any* sequence of poses. - -## Splines - -The spline generator is the only thing that uses the direction in the waypoints; -after that, there's just a list of poses. - -## Sampling - -Previously there were two stages of sampling: bisection to get secants -close to the spline, and then walking the secant lines to get segments close -together. This design wasn't great: - -* S-shaped curves could fail the bisection process -- the middle point is "close" -even though others are not. -* walking the secants is inferior to just bisecting more - -The new version just bisects with both criteria (midpoint closeness and segment shortness) - -## Interpolation - -Originally, interpolation between samples integrated geodesics in the SE(2) manifold, -yielding scallops. This behavior was partially removed in 2025, and the new version -removes it entirely. There's no reason for the path between samples to have -constant direction in SE(2). - -The path between samples *does* have constant *acceleration*, because that makes -everything easier. - -## Velocity - -All three components of velocity of each sample are used for feedforward -and for constraints. Because the acceleration within each segment is constant, -the velocities can be interpolated from the velocities at the endpoints, -or integrated from the starting end, given the acceleration at that point. - -The previous scheduler computed (scalar, pathwise) velocity from the -(constrained) acceleration along a segment. Heading velocity was -computed from the pathwise velocity using the direction of travel -("heading rate per meter"). This obviously won't work when the -pathwise velocity is zero, as it would be for turn-in-place segments. - -For these computations, yet another annotation of the path is used, -with `ConstrainedState`, which remembers (pathwise, scalar, mutable) -velocity and acceleration. - -Accordingly, the sampled datatype needs to have SE(2) velocity, whether -the integration or interpolation approach is used. - -## Acceleration - -All three components of acceleration of each sample are affected -by constraints, during scheduling. - -It would also be nice to use acceleration for feedforward, like we do -for simpler mechanisms. - -The acceleration within each segment is constant, -by definition, but it can't be calculated by looking at the timing of the -endpoint poses alone. It *is* calculable given the velocities at the -endpoints. - -The previous scheduler computed (scalar, pathwise) acceleration, -and recorded it in `TimedPose.` - -Both pathwise and centripetal acceleration are available in the Control -copy of the TimedPose, where it seems to be used in the Jacobian -(for planar mechanisms) -- it would be good to use this for driving too. - -Accordingly, the sampled datatype needs to have the SE(2) acceleration too. - -## Constraints - -The previous design had separate constraint implmentations for path-wise -and centripetal acceleration, a remnant of its "tank" drive roots. The -actual acceleration constraint is some sort of truncated circle: - -* radius of the circle is determined by capsize force or carpet grip. -* truncation of the circle is determined by velocity -- back-EMF limits motor torque. - -At each step in scheduling, the directions of the velocity and acceleration -vectors are known, so the constraint API should supply them. The notion -of "centripital" acceleration should disappear. - -The previous design also separated the cartesian and rotational components -of the velocity constraint; these should be combined. - -## Scheduling - -Given a sequence of poses, the scheduler annotates each with its velocity, -and creates an arc of constant acceleration linking each pair. - -The result is not a series of SE(2) twists; each of the three -components are considered indepenent (to avoid scallops). diff --git a/lib/src/main/java/org/team100/lib/trajectory/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md index ab353d251..5041bd050 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/README.md @@ -15,9 +15,9 @@ The process of constructing a trajectory has three stages: 3. Construct a list of points interpolated along the secant lines, such that the points aren't too far apart. -4. Using a list of kinodynamic constraints (see `lib.trajectory.timing`), assign a time for each point. The resulting list of `TimedPose`s is created by `ScheduleGenerator`, producing `Trajectory100`. +4. Using a list of kinodynamic constraints (see `lib.trajectory.timing`), assign a time for each point. The resulting list of `TimedState`s is created by `ScheduleGenerator`, producing `Trajectory100`. -To use a trajectory, you `sample()` it, with time (in seconds) as the parameter. The resulting `TimedPose` is interpolated between from the list above. +To use a trajectory, you `sample()` it, with time (in seconds) as the parameter. The resulting `TimedState` is interpolated between from the list above. If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains, it will work well enough to set the course and heading to be the same at each waypoint. @@ -72,7 +72,7 @@ constrained states. ## Sampling -at runtime, the `TimedPose` list is sampled by time, which means finding floor and ceiling +at runtime, the `TimedState` list is sampled by time, which means finding floor and ceiling states and interpolating. The interpolant is the fraction of time between states, which is used to derive the fraction of distance between states (i.e. along the secant line), which is used as the interpolant between poses. Note the acceleration is not interpolated diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java index 89cbac683..74b6fa346 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java @@ -3,13 +3,13 @@ import java.util.ArrayList; import java.util.List; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; /** * A list of timed poses. */ public class Trajectory100 { - private final List m_points; + private final List m_points; private final double m_duration; public Trajectory100() { @@ -18,21 +18,20 @@ public Trajectory100() { } /** First timestamp must be zero. */ - public Trajectory100(final List states) { + public Trajectory100(final List states) { m_points = states; m_duration = m_points.get(m_points.size() - 1).getTimeS(); } /** - * Interpolate a TimedPose. - * - * This scans the whole trajectory for every sample, but most of the time - * is the interpolation; I tried a TreeMap index and it only saved a few - * nanoseconds per call. + * Interpolate a TimedState. * * @param timeS start is zero. */ - public TimedPose sample(final double timeS) { + public TimedState sample(double timeS) { + // This scans the whole trajectory for every sample, but most of the time + // is the interpolation; I tried a TreeMap index and it only saved a few + // nanoseconds per call. if (isEmpty()) throw new IllegalStateException("can't sample an empty trajectory"); if (timeS >= m_duration) { @@ -43,9 +42,9 @@ public TimedPose sample(final double timeS) { } for (int i = 1; i < length(); ++i) { - final TimedPose ceil = getPoint(i); + final TimedState ceil = getPoint(i); if (ceil.getTimeS() >= timeS) { - final TimedPose floor = getPoint(i - 1); + final TimedState floor = getPoint(i - 1); double span = ceil.getTimeS() - floor.getTimeS(); if (Math.abs(span) <= 1e-12) { return ceil; @@ -70,7 +69,7 @@ public int length() { return m_points.size(); } - public TimedPose getLastPoint() { + public TimedState getLastPoint() { return m_points.get(length() - 1); } @@ -78,11 +77,11 @@ public double duration() { return m_duration; } - public List getPoints() { + public List getPoints() { return m_points; } - public TimedPose getPoint(int index) { + public TimedState getPoint(int index) { return m_points.get(index); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 476920e49..7cc7dbb53 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -7,16 +7,24 @@ import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.timing.ScheduleGenerator; +import edu.wpi.first.math.geometry.Twist2d; + /** * Represents a 2d holonomic path, i.e. with heading independent from course. * * There's no timing information here. For that, see Trajectory100. */ public class Path100 { + // if an interpolated point is more than this far from an endpoint, + // it indicates the endpoints are too far apart, including too far apart + // in rotation, which is an aspect of the path scheduling that the + // scheduler can't see + // TODO: make this a constructor parameter. + private static final double INTERPOLATION_LIMIT = 0.3; private final List m_points; /** - * Double geodesic distance which is meters and radians. - * Do not use this distance to compute planar velocity. + * Translational distance, just the xy plane, not the Twist arc + * or anything else, just xy distance. */ private final double[] m_distances; @@ -32,9 +40,7 @@ public Path100(final List states) { m_points.add(states.get(i)); Pose2dWithMotion p0 = getPoint(i - 1); Pose2dWithMotion p1 = getPoint(i); - // using the distance metric that includes rotation makes it possible - // to have rotational paths - double dist = Metrics.doubleGeodesicDistance(p0.getPose().pose(), p1.getPose().pose()); + double dist = Metrics.translationalDistance(p0.getPose().pose(), p1.getPose().pose()); m_distances[i] = m_distances[i - 1] + dist; } } @@ -85,7 +91,18 @@ public Pose2dWithMotion sample(double distance) throws ScheduleGenerator.TimingE if (d1 >= distance) { // Found the bracket. double s = (distance - d0) / d; - return p0.interpolate(p1, s); + Pose2dWithMotion lerp = p0.interpolate(p1, s); + // disallow corners + Twist2d t0 = p0.getPose().course().minus(lerp.getPose().course()); + double l0 = Metrics.l2Norm(t0); + Twist2d t1 = p1.getPose().course().minus(lerp.getPose().course()); + double l1 = Metrics.l2Norm(t1); + if (Math.max(l0, l1) > INTERPOLATION_LIMIT) + throw new IllegalStateException( + String.format( + "Interpolated value too far away, p0=%s, p1=%s, t0=%s t1=%s. This usually indicates a sharp corner in the path, which is not allowed.", + p0, p1, t0, t1)); + return lerp; } } throw new ScheduleGenerator.TimingException(); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index 5d69dc5d5..d6be39471 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -108,10 +108,18 @@ private static void getSegmentArc( // difference between twist and sample Transform2d error = phalf_predicted.minus(phalf); + Pose2dWithMotion p20 = spline.getPose2dWithMotion(t0); + Pose2dWithMotion p21 = spline.getPose2dWithMotion(t1); + Twist2d p2t = p20.getPose().course().minus(p21.getPose().course()); + + // checks both translational and l2 norms + // also checks change in course if (Math.abs(error.getTranslation().getX()) > maxDx || Math.abs(error.getTranslation().getY()) > maxDy || Math.abs(error.getRotation().getRadians()) > maxDTheta - || Metrics.translationalNorm(twist_half) > maxNorm) { + || Metrics.translationalNorm(twist_full) > maxNorm + || Metrics.l2Norm(twist_full) > maxNorm + || Metrics.l2Norm(p2t) > maxNorm) { // add a point in between // note the extra condition to avoid points too far apart. getSegmentArc(spline, rv, t0, thalf, maxDx, maxDy, maxDTheta); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 699e7c728..566446680 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -59,10 +59,8 @@ public class HolonomicSpline { * @param p1 ending pose */ public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { - // Distance metric includes both translation and rotation. This is not - // the geodesic distance, which is zero for spin-in-place. It's just the - // L2 norm for all three dimensions. - double distance = Metrics.doubleGeodesicDistance(p0.pose(), p1.pose()); + // Translation distance in the xy plane. + double distance = Metrics.translationalDistance(p0.pose(), p1.pose()); if (DEBUG) System.out.printf("distance %f\n", distance); double scale0 = p0.scale() * distance; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 6533f263f..0dd8e0933 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -48,7 +48,7 @@ public Trajectory100 timeParameterizeTrajectory( private List resample(Path100 path, double step) throws TimingException { double maxDistance = path.getMaxDistance(); if (maxDistance == 0) - throw new IllegalArgumentException(); + throw new IllegalArgumentException("max distance must be greater than zero"); int num_states = (int) Math.ceil(maxDistance / step) + 1; if (DEBUG) System.out.printf("resample max distance %f step %f num states %d f %f\n", @@ -113,20 +113,25 @@ public Trajectory100 timeParameterizeTrajectory( * acceleration during the backward pass (by slowing down the predecessor). */ private List forwardPass(List samples, double start_vel) { - - // note below we look again at this sample. I think this exists + + // note below we look again at this sample. I think this exists // only to supply the start velocity. ConstrainedState predecessor = new ConstrainedState(samples.get(0), 0); predecessor.setVelocityM_S(start_vel); predecessor.setMinAccel(-HIGH_ACCEL); predecessor.setMaxAccel(HIGH_ACCEL); - // work forward through the samples List constrainedStates = new ArrayList<>(samples.size()); for (int i = 0; i < samples.size(); ++i) { Pose2dWithMotion sample = samples.get(i); double dsM = sample.distanceCartesian(predecessor.getState()); + if (i > 0 && i < samples.size() - 1 && dsM < 1e-6) { + // the first distance is zero because of the weird loop structure. + // the last distance can be zero if the step size exactly divides the path + // length + throw new IllegalStateException("zero distance not allowed"); + } if (DEBUG) System.out.printf("i%d dsM %f\n", i, dsM); ConstrainedState constrainedState = new ConstrainedState( @@ -215,7 +220,8 @@ private void backwardsPass( List constrainedStates) { // "successor" comes before in the backwards walk. start with the last state. ConstrainedState endState = constrainedStates.get(constrainedStates.size() - 1); - ConstrainedState successor = new ConstrainedState(lastState, endState.getDistanceM()); + ConstrainedState successor = new ConstrainedState( + lastState, endState.getDistanceM()); successor.setVelocityM_S(end_velocity); successor.setMinAccel(-HIGH_ACCEL); successor.setMaxAccel(HIGH_ACCEL); @@ -284,17 +290,18 @@ private void backwardsWork(ConstrainedState s0, ConstrainedState s1) { * last state accel is always zero, which might be wrong. */ private static Trajectory100 integrate(List states) throws TimingException { - List poses = new ArrayList<>(states.size()); - double time = 0.0; // time along path - // this should be L2 distance - double distance = 0.0; // distance along path + List poses = new ArrayList<>(states.size()); + double time = 0.0; + // distance along path + // for turn-in-place, + double distance = 0.0; double v0 = 0.0; if (DEBUG) System.out.println("i, v0, v1, ds"); for (int i = 0; i < states.size(); ++i) { ConstrainedState state = states.get(i); - final double ds = state.getDistanceM() - distance; - final double v1 = state.getVelocityM_S(); + double ds = state.getDistanceM() - distance; + double v1 = state.getVelocityM_S(); if (DEBUG) System.out.printf("%d, %5.3f, %5.3f, %5.3f\n", i, v0, v1, ds); double dt = 0.0; @@ -307,7 +314,7 @@ private static Trajectory100 integrate(List states) throws Tim if (Double.isNaN(time) || Double.isInfinite(time)) { throw new TimingException(); } - poses.add(new TimedPose(state.getState(), time, v1, 0)); + poses.add(new TimedState(state.getState(), time, v1, 0)); v0 = v1; distance = state.getDistanceM(); } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java similarity index 91% rename from lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java rename to lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java index c7f1dbd73..34c100f84 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java @@ -9,7 +9,7 @@ * * The timing fields are set by the ScheduleGenerator. */ -public class TimedPose { +public class TimedState { private static final boolean DEBUG = false; private final Pose2dWithMotion m_state; /** Time we achieve this state. */ @@ -19,7 +19,7 @@ public class TimedPose { /** Instantaneous pathwise (not centripetal) acceleration, m/s^2. */ private double m_accelM_S_S; - public TimedPose( + public TimedState( Pose2dWithMotion state, double t, double velocity, @@ -69,7 +69,7 @@ public String toString() { * Velocity of this state is the initial velocity. * Acceleration of this state is constant through the whole arc. */ - public TimedPose interpolate(TimedPose other, double delta_t) { + public TimedState interpolate(TimedState other, double delta_t) { double tLerp = m_timeS + delta_t; double vLerp = m_velocityM_S + m_accelM_S_S * delta_t; double pathwiseDistance = m_velocityM_S * delta_t + 0.5 * m_accelM_S_S * delta_t * delta_t; @@ -82,7 +82,7 @@ public TimedPose interpolate(TimedPose other, double delta_t) { if (DEBUG) System.out.printf("tlerp %f\n", tLerp); - return new TimedPose( + return new TimedState( m_state.interpolate(other.m_state, interpolant), tLerp, vLerp, @@ -90,18 +90,18 @@ public TimedPose interpolate(TimedPose other, double delta_t) { } /** Translation only, ignores rotation */ - public double distanceCartesian(TimedPose other) { + public double distanceCartesian(TimedState other) { return m_state.distanceCartesian(other.m_state); } @Override public boolean equals(final Object other) { - if (!(other instanceof TimedPose)) { + if (!(other instanceof TimedState)) { if (DEBUG) System.out.println("wrong type"); return false; } - TimedPose ts = (TimedPose) other; + TimedState ts = (TimedState) other; if (!m_state.equals(ts.m_state)) { if (DEBUG) System.out.println("wrong state"); diff --git a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java index 4edc72556..b73070566 100644 --- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java +++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java @@ -7,7 +7,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.Trajectory; @@ -31,7 +31,7 @@ public void setViz(Trajectory100 trajectory) { private static double[] fromTrajectory100(Trajectory100 m_trajectory) { double[] arr = new double[m_trajectory.length() * 3]; int ndx = 0; - for (TimedPose p : m_trajectory.getPoints()) { + for (TimedState p : m_trajectory.getPoints()) { WaypointSE2 pose = p.state().getPose(); arr[ndx + 0] = pose.pose().getTranslation().getX(); arr[ndx + 1] = pose.pose().getTranslation().getY(); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java index e7e72ef4a..e507a3784 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java @@ -6,7 +6,6 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java index e0a3ed094..9a8fe6155 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java @@ -17,7 +17,7 @@ import org.team100.lib.state.Model100; import org.team100.lib.state.ModelR3; import org.team100.lib.testing.Timeless; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -178,7 +178,7 @@ void testErrZero() { 0.01, 0.02); ModelR3 measurement = new ModelR3(); ModelR3 currentReference = ModelR3 - .fromTimedPose(new TimedPose( + .fromTimedState(new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -196,7 +196,7 @@ void testErrXAhead() { 0.01, 0.02); ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 - .fromTimedPose(new TimedPose(new Pose2dWithMotion( + .fromTimedState(new TimedState(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0), 0, 0, 0)); @@ -212,7 +212,7 @@ void testErrXBehind() { 0.01, 0.02); ModelR3 measurement = new ModelR3(new Pose2d(0, 0, new Rotation2d())); ModelR3 currentReference = ModelR3 - .fromTimedPose(new TimedPose(new Pose2dWithMotion( + .fromTimedState(new TimedState(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), 0, 0), 0, 0, 0)); @@ -229,7 +229,7 @@ void testErrXAheadWithRotation() { 0.01, 0.02); ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d(1))); ModelR3 currentReference = ModelR3 - .fromTimedPose(new TimedPose(new Pose2dWithMotion( + .fromTimedState(new TimedState(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(1)), 0, 1.2), 0, 0), 0, 0, 0)); @@ -258,7 +258,7 @@ void testErrorAhead() { // constant speed double acceleration = 0; // we're exactly on the setpoint so zero error - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 positionError = controller.positionError(measurement, currentReference); assertEquals(0, positionError.getX(), DELTA); assertEquals(0, positionError.getY(), DELTA); @@ -282,7 +282,7 @@ void testErrorSideways() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 positionError = controller.positionError(measurement, currentReference); assertEquals(1, positionError.getX(), DELTA); assertEquals(0, positionError.getY(), DELTA); @@ -306,7 +306,7 @@ void testVelocityErrorZero() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); VelocitySE2 error = controller.velocityError(measurement, currentReference); // we're exactly on the setpoint so zero error assertEquals(0, error.x(), DELTA); @@ -335,7 +335,7 @@ void testVelocityErrorAhead() { // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); VelocitySE2 error = controller.velocityError(measurement, currentReference); // error should include both components assertEquals(1, error.x(), DELTA); @@ -358,9 +358,9 @@ void testFeedForwardAhead() { double velocity = 1; // constant speed double acceleration = 0; - TimedPose setpoint = new TimedPose(state, t, velocity, acceleration); + TimedState setpoint = new TimedState(state, t, velocity, acceleration); // feedforward should be straight ahead, no rotation. - ControlR3 nextReference = ControlR3.fromTimedPose(setpoint); + ControlR3 nextReference = ControlR3.fromTimedState(setpoint); VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -382,9 +382,9 @@ void testFeedForwardSideways() { double velocity = 1; // constant speed double acceleration = 0; - TimedPose setpoint = new TimedPose(state, t, velocity, acceleration); + TimedState setpoint = new TimedState(state, t, velocity, acceleration); // feedforward should be -y, robot relative, no rotation. - ControlR3 nextReference = ControlR3.fromTimedPose(setpoint); + ControlR3 nextReference = ControlR3.fromTimedState(setpoint); VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -406,7 +406,7 @@ void testFeedForwardTurning() { double velocity = 1; // constant speed double acceleration = 0; - ControlR3 nextReference = ControlR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ControlR3 nextReference = ControlR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); VelocitySE2 speeds = controller.feedforward(nextReference); // feedforward should be ahead and rotating. assertEquals(1, speeds.x(), DELTA); @@ -439,7 +439,7 @@ void testFeedbackAhead() { currentState, new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // we're exactly on the setpoint so zero feedback @@ -473,7 +473,7 @@ void testFeedbackAheadPlusY() { currentState, new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // setpoint should be negative y @@ -507,7 +507,7 @@ void testFeedbackAheadPlusTheta() { currentState, new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // robot is on the setpoint in translation @@ -543,7 +543,7 @@ void testFeedbackSideways() { ModelR3 measurement = new ModelR3( currentState, new VelocitySE2(1, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // on target @@ -576,7 +576,7 @@ void testFeedbackSidewaysPlusY() { ModelR3 measurement = new ModelR3( currentState, new VelocitySE2(1, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // feedback is -y field relative @@ -610,7 +610,7 @@ void testFullFeedbackAhead() { ModelR3 measurement = new ModelR3( currentState, new VelocitySE2(1, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); VelocitySE2 speeds = controller.fullFeedback(perr, verr); @@ -646,7 +646,7 @@ void testFullFeedbackSideways() { ModelR3 measurement = new ModelR3( currentPose, new VelocitySE2(0.5, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); VelocitySE2 speeds = controller.fullFeedback(perr, verr); @@ -684,7 +684,7 @@ void testFullFeedbackSidewaysWithRotation() { // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); + ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); // feedforward should be straight ahead, no rotation. DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); diff --git a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java index 706a708d0..327c3bed7 100644 --- a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java @@ -122,47 +122,11 @@ void testHeadingRate() { new DirectionSE2(1, 0, 1), 1)); { double splineHR = spline.getDHeadingDs(0.5); - assertEquals(0.869, splineHR, DELTA); + assertEquals(0.811, splineHR, DELTA); Pose2d p0 = spline.getPose2d(0.49); Pose2d p1 = spline.getPose2d(0.51); double discreteHR = GeometryUtil.headingRatio(p0, p1); - assertEquals(0.869, discreteHR, DELTA); - } - double DS = 0.001; - for (double s = DS; s <= 1 - DS; s += DS) { - double splineHR = spline.getDHeadingDs(s); - Pose2d p0 = spline.getPose2d(s - DS); - Pose2d p1 = spline.getPose2d(s + DS); - double discreteHR = GeometryUtil.headingRatio(p0, p1); - if (DEBUG) - System.out.printf("%f %f %f %f\n", s, splineHR, discreteHR, splineHR - discreteHR); - // error scales with ds - assertEquals(splineHR, discreteHR, 0.00001); - } - } - - @Test - void testHeadingRate2() { - // turning in place - HolonomicSpline spline = new HolonomicSpline( - new WaypointSE2( - new Pose2d( - new Translation2d(), - new Rotation2d()), - new DirectionSE2(0, 0, 1), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(), - new Rotation2d(1)), - new DirectionSE2(0, 0, 1), 1)); - { - double splineHR = spline.getDHeadingDs(0.5); - // this is heading change per L2 metric; pure rotation = 1 - assertEquals(1, splineHR, DELTA); - Pose2d p0 = spline.getPose2d(0.49); - Pose2d p1 = spline.getPose2d(0.51); - double discreteHR = GeometryUtil.headingRatio(p0, p1); - assertEquals(1, discreteHR, DELTA); + assertEquals(0.811, discreteHR, DELTA); } double DS = 0.001; for (double s = DS; s <= 1 - DS; s += DS) { diff --git a/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java b/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java index ada75d3f6..263e1cf61 100644 --- a/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java +++ b/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java @@ -5,7 +5,6 @@ import java.util.function.Function; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Metrics; import edu.wpi.first.math.Nat; diff --git a/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java b/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java index bc4e922cb..99f8e25b0 100644 --- a/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java +++ b/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java @@ -5,7 +5,6 @@ import java.util.function.DoubleUnaryOperator; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Metrics; import edu.wpi.first.math.VecBuilder; diff --git a/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java b/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java index d69e55936..831f42b8b 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java @@ -15,7 +15,7 @@ import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -258,8 +258,8 @@ void testTrajectory() { double d = t.duration(); double dt = d / 20; for (double time = 0; time < d; time += dt) { - TimedPose tp = t.sample(time); - ModelR3 sm = ModelR3.fromTimedPose(tp); + TimedState tp = t.sample(time); + ModelR3 sm = ModelR3.fromTimedState(tp); Pose2d p = sm.pose(); VelocitySE2 v = sm.velocity(); EAWConfig c = k.inverse(p); diff --git a/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java b/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java index 19f472d5f..c9a037607 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java @@ -16,7 +16,7 @@ import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -235,8 +235,8 @@ void testTrajectory() { double d = t.duration(); double dt = d / 20; for (double time = 0; time < d; time += dt) { - TimedPose tp = t.sample(time); - ModelR3 sm = ModelR3.fromTimedPose(tp); + TimedState tp = t.sample(time); + ModelR3 sm = ModelR3.fromTimedState(tp); Pose2d p = sm.pose(); VelocitySE2 v = sm.velocity(); EAWConfig c = k.inverse(p); diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java index 324f35ae3..bd915db5a 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java @@ -21,7 +21,7 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import org.team100.lib.visualization.TrajectoryVisualization; @@ -55,7 +55,7 @@ void testSimple() throws IOException { drive, (start, end) -> new Trajectory100( List.of( - new TimedPose( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index c6d7988f9..4a9c13d0b 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -7,7 +7,7 @@ import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ControlR3; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -26,9 +26,9 @@ void testTransform() { } @Test - void testTimedPose() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -43,9 +43,9 @@ void testTimedPose() { } @Test - void testTimedPose2() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState2() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -60,9 +60,9 @@ void testTimedPose2() { } @Test - void testTimedPose3() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState3() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -78,9 +78,9 @@ void testTimedPose3() { /** +x motion, positive curvature => +y accel. */ @Test - void testTimedPose4() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState4() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index 710ac0508..ff266cef2 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -8,7 +8,7 @@ import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.state.ModelR3; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,9 +27,9 @@ void testTransform() { } @Test - void testTimedPose() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -44,9 +44,9 @@ void testTimedPose() { } @Test - void testTimedPose2() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState2() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -61,9 +61,9 @@ void testTimedPose2() { } @Test - void testTimedPose3() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState3() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -79,9 +79,9 @@ void testTimedPose3() { /** +x motion, positive curvature => +y accel. */ @Test - void testTimedPose4() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState4() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index cfbfc383e..6d0ebdee8 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -18,7 +18,7 @@ import org.team100.lib.trajectory.path.spline.HolonomicSpline; import org.team100.lib.trajectory.timing.ScheduleGenerator; import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; @@ -51,7 +51,7 @@ void testPreviewAndAdvance() { Trajectory100 trajectory = planner.restToRest(waypoints); - TimedPose sample = trajectory.sample(0); + TimedState sample = trajectory.sample(0); assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(1); @@ -84,7 +84,7 @@ void testSample() { Trajectory100 trajectory = planner.restToRest(waypoints); assertEquals(1.418, trajectory.duration(), DELTA); - TimedPose sample = trajectory.sample(0); + TimedState sample = trajectory.sample(0); assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(1); assertEquals(0.825, sample.state().getPose().pose().getTranslation().getX(), DELTA); @@ -97,7 +97,6 @@ void testSample() { */ @Test void testSampleThoroughly() { - List waypoints = List.of( new WaypointSE2( new Pose2d( @@ -125,22 +124,26 @@ void testSampleThoroughly() { } assertEquals(1.418, trajectory.duration(), DELTA); - assertEquals(0.000, trajectory.sample(0.0).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.010, trajectory.sample(0.1).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.040, trajectory.sample(0.2).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.090, trajectory.sample(0.3).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.160, trajectory.sample(0.4).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.250, trajectory.sample(0.5).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.360, trajectory.sample(0.6).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.487, trajectory.sample(0.7).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.618, trajectory.sample(0.8).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.732, trajectory.sample(0.9).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.825, trajectory.sample(1.0).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.899, trajectory.sample(1.1).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.953, trajectory.sample(1.2).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0.987, trajectory.sample(1.3).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(1.000, trajectory.sample(1.4).state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(1.000, trajectory.sample(1.5).state().getPose().pose().getTranslation().getX(), DELTA); + check(trajectory, 0.0, 0.000); + check(trajectory, 0.1, 0.010); + check(trajectory, 0.2, 0.040); + check(trajectory, 0.3, 0.090); + check(trajectory, 0.4, 0.160); + check(trajectory, 0.5, 0.250); + check(trajectory, 0.6, 0.360); + check(trajectory, 0.7, 0.487); + check(trajectory, 0.8, 0.618); + check(trajectory, 0.9, 0.732); + check(trajectory, 1.0, 0.825); + check(trajectory, 1.1, 0.899); + check(trajectory, 1.2, 0.953); + check(trajectory, 1.3, 0.987); + check(trajectory, 1.4, 1.000); + check(trajectory, 1.5, 1.000); + } + + private void check(Trajectory100 trajectory, double t, double x) { + assertEquals(x, trajectory.sample(t).state().getPose().pose().getTranslation().getX(), DELTA); } /** @@ -183,7 +186,7 @@ void testSamplePerformance() throws TimingException { Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.02, 0.2, 0.1); - assertEquals(23.063, path.getMaxDistance(), 0.001); + assertEquals(22.734, path.getMaxDistance(), 0.001); start = System.nanoTime(); for (int rep = 0; rep < reps; ++rep) { @@ -206,7 +209,7 @@ void testSamplePerformance() throws TimingException { Trajectory100 trajectory = generator.timeParameterizeTrajectory(path, 0.1, 0, 0); - assertEquals(232, trajectory.length()); + assertEquals(229, trajectory.length()); start = System.nanoTime(); for (int rep = 0; rep < reps; ++rep) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index f78fef41d..868742769 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -8,8 +8,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -20,7 +20,7 @@ import org.team100.lib.trajectory.timing.CapsizeAccelerationConstraint; import org.team100.lib.trajectory.timing.ConstantConstraint; import org.team100.lib.trajectory.timing.SwerveDriveDynamicsConstraint; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -51,10 +51,10 @@ void testLinear() { TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); assertEquals(12, t.length()); - TimedPose tp = t.getPoint(0); + TimedState tp = t.getPoint(0); // start at zero velocity assertEquals(0, tp.velocityM_S(), DELTA); - TimedPose p = t.getPoint(6); + TimedState p = t.getPoint(6); assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -82,45 +82,13 @@ void testLinearRealistic() { TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); assertEquals(12, t.length()); - TimedPose tp = t.getPoint(0); + TimedState tp = t.getPoint(0); assertEquals(0, tp.velocityM_S(), DELTA); - TimedPose p = t.getPoint(6); + TimedState p = t.getPoint(6); assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } - @Test - void testBackingUp() { - List waypoints = List.of( - new WaypointSE2( - new Pose2d( - new Translation2d(0, 0), - Rotation2d.kZero), - new DirectionSE2(-1, 0, 0), 1.2), - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - Rotation2d.kZero), - new DirectionSE2(1, 0, 0), 1.2)); - SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); - - // these are the same as StraightLineTrajectoryTest. - List constraints = List.of( - new ConstantConstraint(logger, 1, 1, limits), - new SwerveDriveDynamicsConstraint(logger, limits, 1, 1), - new YawRateConstraint(logger, limits, 0.2), - new CapsizeAccelerationConstraint(logger, limits, 0.2)); - double start_vel = 1; - double end_vel = 0; - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - Trajectory100 t = planner.generateTrajectory( - waypoints, start_vel, end_vel); - TimedPose p = t.getPoint(6); - assertEquals(0.272, p.state().getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); - - } - /** * 0.23 ms on my machine. * @@ -156,7 +124,7 @@ void testPerformance() { System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations); } assertEquals(18, t.length()); - TimedPose p = t.getPoint(6); + TimedState p = t.getPoint(6); assertEquals(0.585, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -181,7 +149,7 @@ void testRestToRest() { double maxDriveAccelerationM_S2 = swerveKinodynamics.getMaxDriveAccelerationM_S2(); assertEquals(5, maxDriveVelocityM_S); assertEquals(10, maxDriveAccelerationM_S2); - for (TimedPose p : trajectory.getPoints()) { + for (TimedState p : trajectory.getPoints()) { assertTrue(p.velocityM_S() - 0.001 <= maxDriveVelocityM_S, String.format("%f %f", p.velocityM_S(), maxDriveVelocityM_S)); assertTrue(p.acceleration() - 0.001 <= maxDriveAccelerationM_S2, @@ -200,18 +168,6 @@ void testMovingToRest() { assertEquals(1.176, traj.duration(), DELTA); } - @Test - void testBackingUp2() { - SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); - List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(-1, 0, 0)); - Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); - Trajectory100 traj = planner.movingToRest(start, end); - TrajectoryPlotter.plot(traj, 0.1); - assertEquals(1.549, traj.duration(), DELTA); - } - @Test void test2d() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); @@ -220,7 +176,7 @@ void test2d() { ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 1, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); - assertEquals(2.869, traj.duration(), DELTA); + assertEquals(3.337, traj.duration(), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 9723551f6..22d16f5e3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -1,5 +1,7 @@ package org.team100.lib.trajectory; +import static org.junit.jupiter.api.Assertions.assertTrue; + import java.util.List; import org.junit.jupiter.api.Test; @@ -10,7 +12,7 @@ import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -18,12 +20,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -/** - * These pop up GUI windows, so leave them commented out when you check in. - */ public class TrajectoryTest { private static final boolean DEBUG = false; - private static final boolean SHOW = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); /** @@ -41,8 +39,18 @@ void testSimple() throws InterruptedException { Trajectory100 t = p.restToRest( new Pose2d(0, 0, new Rotation2d()), new Pose2d(10, 1, new Rotation2d())); - if (SHOW) - new TrajectoryPlotter(0.5).plot("simple", t); + new TrajectoryPlotter(0.5).plot("simple", t); + } + + /** Turning in place does not work */ + @Test + void testTurnInPlace() throws InterruptedException { + TrajectoryPlanner p = new TrajectoryPlanner( + List.of(new ConstantConstraint(log, 1, 0.1))); + Trajectory100 t = p.restToRest( + new Pose2d(0, 0, new Rotation2d()), + new Pose2d(0, 0, new Rotation2d(1))); + assertTrue(t.isEmpty()); } @Test @@ -50,7 +58,7 @@ void testCircle() { // see HolonomicSplineTest.testCircle(); // this is to see how to create the dtheta and curvature // without the spline. - double scale = 0.9; + double scale = 1.3; WaypointSE2 p0 = new WaypointSE2( new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), new DirectionSE2(0, 1, 1), scale); @@ -77,7 +85,7 @@ void testCircle() { @Test void testDheading() { - double scale = 0.9; + double scale = 1.3; WaypointSE2 w0 = new WaypointSE2( new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), new DirectionSE2(0, 1, 1), scale); @@ -92,12 +100,12 @@ void testDheading() { TrajectoryPlanner p = new TrajectoryPlanner(c); Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); double duration = trajectory.duration(); - TimedPose p0 = trajectory.sample(0); + TimedState p0 = trajectory.sample(0); if (DEBUG) System.out.println( "t, intrinsic_heading_dt, heading_dt, intrinsic_ca, extrinsic_ca, extrinsic v, intrinsic v, dcourse, dcourse1"); for (double t = 0.04; t < duration; t += 0.04) { - TimedPose p1 = trajectory.sample(t); + TimedState p1 = trajectory.sample(t); Rotation2d heading0 = p0.state().getPose().pose().getRotation(); Rotation2d heading1 = p1.state().getPose().pose().getRotation(); double dheading = heading1.minus(heading0).getRadians(); @@ -115,7 +123,7 @@ void testDheading() { double dcourse1 = Metrics.translationalNorm(course1.minus(course0)); double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians(); double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.state().getCurvatureRad_M(); - + if (DEBUG) System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", t, intrinsicDheadingDt, dheadingDt, @@ -154,8 +162,7 @@ void testCurved() throws InterruptedException { new Rotation2d(-Math.PI / 2)), new DirectionSE2(0, 1, 0), 1)); Trajectory100 t = p.restToRest(waypoints); - if (SHOW) - new TrajectoryPlotter(0.5).plot("curved", t); + new TrajectoryPlotter(0.5).plot("curved", t); } /** @@ -186,7 +193,6 @@ void testMultipleWaypoints() throws InterruptedException { new Rotation2d(-Math.PI / 2)), new DirectionSE2(0, 1, 0), 1)); Trajectory100 t = p.restToRest(waypoints); - if (SHOW) - new TrajectoryPlotter(0.3).plot("multiple", t); + new TrajectoryPlotter(0.3).plot("multiple", t); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java index debe90508..f01e0d705 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java @@ -3,11 +3,12 @@ import org.jfree.data.xy.VectorSeries; import org.jfree.data.xy.XYSeries; import org.team100.lib.geometry.WaypointSE2; -import org.team100.lib.trajectory.timing.TimedPose; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Rotation2d; public class TrajectoryToVectorSeries { + private static final boolean DEBUG = false; private static final int POINTS = 20; /** Length of the vector indicating heading */ @@ -21,9 +22,11 @@ public TrajectoryToVectorSeries(double scale) { public VectorSeries convert(String name, Trajectory100 t) { VectorSeries s = new VectorSeries(name); double duration = t.duration(); + if (DEBUG) + System.out.printf("duration %f\n", duration); double dt = duration / POINTS; for (double time = 0; time < duration; time += dt) { - TimedPose p = t.sample(time); + TimedState p = t.sample(time); WaypointSE2 pp = p.state().getPose(); double x = pp.pose().getTranslation().getX(); double y = pp.pose().getTranslation().getY(); @@ -31,6 +34,8 @@ public VectorSeries convert(String name, Trajectory100 t) { double dx = m_scale * heading.getCos(); double dy = m_scale * heading.getSin(); s.add(x, y, dx, dy); + if (DEBUG) + System.out.printf("%f\n", time); } return s; } @@ -45,7 +50,7 @@ public static XYSeries x(String name, Trajectory100 trajectory) { double duration = trajectory.duration(); double dt = duration / POINTS; for (double t = 0; t <= duration + 0.0001; t += dt) { - TimedPose p = trajectory.sample(t); + TimedState p = trajectory.sample(t); WaypointSE2 pp = p.state().getPose(); double x = pp.pose().getTranslation().getX(); series.add(t, x); @@ -63,7 +68,7 @@ public static XYSeries xdot(String name, Trajectory100 trajectory) { double duration = trajectory.duration(); double dt = duration / POINTS; for (double t = 0; t <= duration + 0.0001; t += dt) { - TimedPose p = trajectory.sample(t); + TimedState p = trajectory.sample(t); Rotation2d course = p.state().getPose().course().toRotation(); double velocityM_s = p.velocityM_S(); System.out.println(velocityM_s); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 11adaab9f..863bdd555 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -8,20 +8,15 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.path.spline.HolonomicSpline; -import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; class Path100Test { - private static final double DELTA = 0.001; - private static final boolean DEBUG = false; private static final List HEADINGS = Arrays.asList( GeometryUtil.fromDegrees(0), @@ -79,97 +74,4 @@ void testStateAccessors() { assertEquals(HEADINGS.get(3), traj.getPoint(3).getPose().pose().getRotation()); } - /** - * Note that many of the results here are "wrong" because the waypoints aren't - * correctly specified. - */ - @Test - void test() throws TimingException { - List waypoints = Arrays.asList( - new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(0.0, 0.0, new Rotation2d(Math.toRadians(0))), 0, 1.2), - 0.1, 0), - new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(24.0, 0.0, new Rotation2d(Math.toRadians(30))), 0, 1.2), - 0.1, 0), - new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(36.0, 0.0, new Rotation2d(Math.toRadians(60))), Math.PI / 2, 1.2), - 1e6, 0), - new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(36.0, 24.0, new Rotation2d(Math.toRadians(60))), 0, 1.2), - 0.1, 0), - new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(60.0, 24.0, new Rotation2d(Math.toRadians(180))), 0, 1.2), - 0.1, 0)); - - // Create the reference trajectory (straight line motion between waypoints). - Path100 path = new Path100(waypoints); - - if (DEBUG) { - System.out.println("d, x, y, heading, course"); - for (int d = 0; d < 90; ++d) { - Pose2dWithMotion s = path.sample(d); - Pose2d p = s.getPose().pose(); - System.out.printf("%d, %6.3f, %6.3f, %6.3f, %6.3f\n", - d, p.getX(), p.getY(), p.getRotation().getRadians(), - s.getPose().course().toRotation().getRadians()); - } - } - - // constant-twist arcs - assertEquals(84.108, path.getMaxDistance(), DELTA); - - // initial sample is exactly at the start - Pose2dWithMotion sample0 = path.sample(0.0); - assertEquals(0, sample0.getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0, sample0.getPose().pose().getTranslation().getY(), DELTA); - - // course is +x - assertEquals(0, sample0.getPose().course().toRotation().getDegrees()); - - // heading is 0 - assertEquals(0, sample0.getPose().pose().getRotation().getDegrees()); - - Pose2dWithMotion sample12 = path.sample(12.0); - assertEquals(11.997, sample12.getPose().pose().getTranslation().getX(), DELTA); - assertEquals(0, sample12.getPose().pose().getTranslation().getY(), DELTA); - - // course should be +x - assertEquals(0, sample12.getPose().course().toRotation().getDegrees(), DELTA); - - assertEquals(14.996, sample12.getPose().pose().getRotation().getDegrees(), DELTA); - - Pose2dWithMotion sample5 = path.sample(48); - assertEquals(36, sample5.getPose().pose().getTranslation().getX(), DELTA); - assertEquals(11.983, sample5.getPose().pose().getTranslation().getY(), DELTA); - assertEquals(45.082, sample5.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(60, sample5.getPose().pose().getRotation().getDegrees(), DELTA); - - Pose2dWithMotion sample6 = path.sample(60); - assertEquals(36, sample6.getPose().pose().getTranslation().getX(), DELTA); - assertEquals(23.983, sample6.getPose().pose().getTranslation().getY(), DELTA); - assertEquals(0.041, sample6.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(60, sample6.getPose().pose().getRotation().getDegrees(), DELTA); - - Pose2dWithMotion sample72 = path.sample(72.0); - assertEquals(47.937, sample72.getPose().pose().getTranslation().getX(), DELTA); - assertEquals(24, sample72.getPose().pose().getTranslation().getY(), DELTA); - - // course should be +x - assertEquals(0, sample72.getPose().course().toRotation().getDegrees(), DELTA); - - assertEquals(119.687, sample72.getPose().pose().getRotation().getDegrees(), DELTA); - - Pose2dWithMotion sample8 = path.sample(84); - assertEquals(59.892, sample8.getPose().pose().getTranslation().getX(), DELTA); - assertEquals(24, sample8.getPose().pose().getTranslation().getY(), DELTA); - assertEquals(0, sample8.getPose().course().toRotation().getDegrees(), DELTA); - assertEquals(179.460, sample8.getPose().pose().getRotation().getDegrees(), DELTA); - - } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 5c78aab29..7ec5ee4d5 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -24,27 +24,6 @@ public class PathFactoryTest implements Timeless { private static final boolean DEBUG = false; private static final double DELTA = 0.01; - @Test - void testBackingUp() { - List waypoints = List.of( - new WaypointSE2( - new Pose2d( - new Translation2d(0, 0), - Rotation2d.kZero), - new DirectionSE2(-1, 0, 0), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - Rotation2d.kZero), - new DirectionSE2(1, 0, 0), 1)); - Path100 path = PathFactory.pathFromWaypoints( - waypoints, - 0.0127, - 0.0127, - Math.toRadians(1.0)); - assertEquals(14, path.length()); - } - /** Preserves the tangent at the corner and so makes a little "S" */ @Test void testCorner() { @@ -66,13 +45,13 @@ void testCorner() { new DirectionSE2(0, 1, 0), 1)); Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - assertEquals(17, path.length()); + assertEquals(54, path.length()); Pose2dWithMotion p = path.getPoint(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = path.getPoint(8); - assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } @@ -92,18 +71,18 @@ void testLinear() { new DirectionSE2(1, 0, 0), 1)); Path100 path = PathFactory.pathFromWaypoints( waypoints, 0.01, 0.01, 0.1); - assertEquals(9, path.length()); + assertEquals(17, path.length()); Pose2dWithMotion p = path.getPoint(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); p = path.getPoint(8); - assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } - /** Turning in place works now. */ + /** Turning in place once again does not work. */ @Test void testSpin() { List waypoints = List.of( @@ -117,16 +96,10 @@ void testSpin() { new Translation2d(0, 0), Rotation2d.kCCW_90deg), new DirectionSE2(0, 0, 1), 1)); - Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - TrajectoryPlotter.plot(path, 0.1); + assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); } - /** - * Hard corners once again do not work. - * - * It *does* kinda work if the rotation axis is doing anything, which seems - * dumb. - */ + /** Hard corners once again do not work. */ @Test void testActualCorner() { List waypoints = List.of( @@ -150,8 +123,7 @@ void testActualCorner() { new Translation2d(1, 1), new Rotation2d()), new DirectionSE2(0, 1, 0), 1)); - Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - TrajectoryPlotter.plot(path, 0.1); + assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); } @Test @@ -163,11 +135,6 @@ void testComposite() { new Translation2d(0, 0), new Rotation2d()), new DirectionSE2(1, 0, 0), 1), - new WaypointSE2( - new Pose2d( - new Translation2d(1, 0), - new Rotation2d()), - new DirectionSE2(1, 0, 0), 1), new WaypointSE2( new Pose2d( new Translation2d(1, 0), @@ -180,7 +147,7 @@ void testComposite() { new DirectionSE2(1, 0, 0), 1)); Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(foo, 0.1); - assertEquals(18, foo.length(), 0.001); + assertEquals(59, foo.length(), 0.001); } @Test @@ -276,9 +243,9 @@ void testPerformance() { System.out.printf("total duration ms: %5.3f\n", totalDurationMs); System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations); } - assertEquals(17, t.length()); + assertEquals(33, t.length()); Pose2dWithMotion p = t.getPoint(4); - assertEquals(0.417, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.211, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 55cb73f77..f258f81ae 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -1,8 +1,7 @@ package org.team100.lib.trajectory.path.spline; import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertFalse; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.assertThrows; import java.util.ArrayList; import java.util.List; @@ -23,10 +22,10 @@ import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.CapsizeAccelerationConstraint; +import org.team100.lib.trajectory.timing.ConstantConstraint; import org.team100.lib.trajectory.timing.ScheduleGenerator; import org.team100.lib.trajectory.timing.TimingConstraint; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -189,7 +188,7 @@ void testRotationSoft() { assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); // high rotation rate in the middle - assertEquals(0.979, p.getHeadingRateRad_M(), DELTA); + assertEquals(0.915, p.getHeadingRateRad_M(), DELTA); p = s.getPose2dWithMotion(1); assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); @@ -280,10 +279,6 @@ void spin() { List splines = new ArrayList<>(); splines.add(s0); - - assertTrue(verifyC1(splines)); - assertTrue(verifyC2(splines)); - TrajectoryPlotter.plot(splines, 0.1); } @@ -291,11 +286,7 @@ void spin() { void testCircle() { // four splines that make a circle should have nice even curvature and velocity // throughout. The circle is centered at zero, the heading always points there. - // - // This now uses the specification of SE(2) "course" which includes rotation. - // - // I fiddled with the scale to make a pretty good circle. - double scale = 0.9; + double scale = 1.3; WaypointSE2 p0 = new WaypointSE2( new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), new DirectionSE2(0, 1, 1), scale); @@ -313,15 +304,7 @@ void testCircle() { HolonomicSpline s2 = new HolonomicSpline(p2, p3); HolonomicSpline s3 = new HolonomicSpline(p3, p0); List splines = List.of(s0, s1, s2, s3); - - // before - assertTrue(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) - assertTrue(verifyC2(splines)); - - checkCircle(splines, 0.011, 0.005); - - // optimize and compare + checkCircle(splines, 0.008, 0.006); TrajectoryPlotter.plot(splines, 0.1); } @@ -355,7 +338,6 @@ void testDheading() { double dheadingDx2 = dheading / cartesianDistance; double curveDx2 = curve / cartesianDistance; - if (DEBUG) System.out.printf( "%5.3f, %5.3f, %5.3f, %5.3f, %5.3f \n", @@ -385,8 +367,10 @@ private void checkCircle(List splines, double rangeError, doubl actualAzimuthError = Math.max(actualAzimuthError, Math.abs(error.getRadians())); } } - assertEquals(0, actualRangeError, rangeError); - assertEquals(0, actualAzimuthError, azimuthError); + assertEquals(0, actualRangeError, rangeError, + String.format("range actual %f allowed %f", actualRangeError, rangeError)); + assertEquals(0, actualAzimuthError, azimuthError, + String.format("azimuth actual %f expected %f", actualAzimuthError, azimuthError)); } @@ -419,13 +403,7 @@ void testLine() { List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); - TrajectoryPlotter.plot(splines, 0.1); - - // spline joints are not C1 smooth - assertFalse(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) - assertTrue(verifyC2(splines)); } /** @@ -465,17 +443,14 @@ void testPath0() { List splines = new ArrayList<>(); splines.add(s01); splines.add(s12); - - assertTrue(verifyC1(splines)); - assertTrue(verifyC2(splines)); - TrajectoryPlotter.plot(splines, 0.1); } @Test void testMismatchedXYDerivatives() { - // corners seem to be allowed, but yield un-traversable trajectories - // with infinite accelerations at the corners, so they should be prohibited. + // because path generation never looks across spline boundaries, + // it is possible to make sharp corners at the "knots." But you can't + // make a trajectory with these corners, the scheduler will fail. // this goes straight ahead to (1,0) // derivatives point straight ahead @@ -514,25 +489,13 @@ void testMismatchedXYDerivatives() { System.out.printf("spline %s\n", s); } - // spline joints are C1 smooth (i.e. same slope) - assertFalse(verifyC1(splines)); - // spline joints are C2 smooth (i.e. same curvature) - assertTrue(verifyC2(splines)); - Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05)); if (DEBUG) System.out.printf("path %s\n", path); - List constraints = new ArrayList<>(); + List constraints = List.of(new ConstantConstraint(logger, 1, 1)); ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); - Trajectory100 trajectory = scheduleGenerator.timeParameterizeTrajectory(path, - 0.05, 0, 0); - - TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); - plotter.plot("plot", trajectory); - - if (DEBUG) - System.out.printf("trajectory %s\n", trajectory); - + assertThrows(IllegalStateException.class, () -> scheduleGenerator.timeParameterizeTrajectory(path, + 0.05, 0, 0)); } @Test @@ -595,67 +558,4 @@ void testEntryVelocity() { if (DEBUG) System.out.printf("trajectory %s\n", trajectory); } - - /** - * True if adjacent spline endpoints have (nearly) identical derivative terms. - */ - static boolean verifyC1(List splines) { - if (splines.size() < 2) - return true; - for (int i = 0; i < splines.size() - 1; ++i) { - HolonomicSpline s0 = splines.get(i); - HolonomicSpline s1 = splines.get(i + 1); - if (!MathUtil.isNear(s0.dx(1), s1.dx(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad x C1 %f %f\n", s0.dx(1), s1.dx(0)); - } - return false; - } - if (!MathUtil.isNear(s0.dy(1), s1.dy(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad y C1 %f %f\n", s0.dy(1), s1.dy(0)); - } - return false; - } - if (!MathUtil.isNear(s0.dtheta(1), s1.dtheta(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad theta C1 %f %f\n", s0.dtheta(1), s1.dtheta(0)); - } - return false; - } - } - return true; - } - - /** - * True if adjacent spline endpoints have (nearly) identical second-derivative - * terms. - */ - static boolean verifyC2(List splines) { - if (splines.size() < 2) - return true; - for (int i = 0; i < splines.size() - 1; ++i) { - HolonomicSpline s0 = splines.get(i); - HolonomicSpline s1 = splines.get(i + 1); - if (!MathUtil.isNear(s0.ddx(1), s1.ddx(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad x C2 %f %f\n", s0.ddx(1), s1.ddx(0)); - } - return false; - } - if (!MathUtil.isNear(s0.ddy(1), s1.ddy(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad y C2 %f %f\n", s0.ddy(1), s1.ddy(0)); - } - return false; - } - if (!MathUtil.isNear(s0.ddtheta(1), s1.ddtheta(0), 1e-6)) { - if (DEBUG) { - System.out.printf("bad theta C2 %f %f\n", s0.ddtheta(1), s1.ddtheta(0)); - } - return false; - } - } - return true; - } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java index b8b3f52d4..eeaf6a9a3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java @@ -6,8 +6,6 @@ import org.team100.lib.trajectory.path.spline.SplineR1; import org.team100.lib.util.Math100; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; - public class DirectScheduleTest { private static final double EPSILON = 1e-6; diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index e3bef2b4c..c2d60c664 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -3,6 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.ArrayList; @@ -23,7 +24,6 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.trajectory.TrajectoryPlotter; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; @@ -84,8 +84,8 @@ public void checkTrajectory( // Go state by state, verifying all constraints are satisfied and integration is // correct. - TimedPose prev_state = null; - for (TimedPose state : traj.getPoints()) { + TimedState prev_state = null; + for (TimedState state : traj.getPoints()) { for (final TimingConstraint constraint : constraints) { assertTrue(state.velocityM_S() - EPSILON <= constraint.maxV(state.state())); assertTrue(state.acceleration() - EPSILON <= constraint.maxAccel( @@ -107,11 +107,10 @@ public void checkTrajectory( } /** - * Turning in place works now. + * Turning in place does not work.k */ @Test void testJustTurningInPlace() { - Path100 path = new Path100(Arrays.asList( new Pose2dWithMotion( new WaypointSE2( @@ -124,25 +123,18 @@ void testJustTurningInPlace() { new DirectionSE2(0, 0, 1), 1), 1, 0))); - // our distance metric combines meters and radians with equal weight - // because this is pure rotation, the "distance" here is in radians. - assertEquals(Math.PI, path.getMaxDistance(), DELTA); + assertEquals(0, path.getMaxDistance(), DELTA); + assertEquals(2, path.length()); if (DEBUG) System.out.printf("PATH:\n%s\n", path); - TrajectoryPlotter.plot(path, 0.1); List constraints = new ArrayList(); ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory( + assertThrows(IllegalArgumentException.class, ()-> u.timeParameterizeTrajectory( path, 1.0, 0.0, - 0.0); - if (DEBUG) - System.out.printf("TRAJ:\n%s\n", traj); - - assertFalse(traj.isEmpty()); - + 0.0)); } /** @@ -334,7 +326,7 @@ void testPerformance() { System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations); } assertEquals(18, t.length()); - TimedPose p = t.getPoint(6); + TimedState p = t.getPoint(6); assertEquals(0.575, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index 869a040b9..4699826e4 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java @@ -15,7 +15,7 @@ class TimedStateTest { @Test void test() { // At (0,0,0), t=0, v=0, acceleration=1 - TimedPose start_state = new TimedPose( + TimedState start_state = new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -23,7 +23,7 @@ void test() { 0.0, 0.0, 1.0); // At (.5,0,0), t=1, v=1, acceleration=0 - TimedPose end_state = new TimedPose( + TimedState end_state = new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0.5, 0, new Rotation2d(0)), 0, 1.2), @@ -35,7 +35,7 @@ void test() { assertEquals(end_state, start_state.interpolate(end_state, 1.0)); // halfway between the states by time - TimedPose intermediate_state = start_state.interpolate(end_state, 0.5); + TimedState intermediate_state = start_state.interpolate(end_state, 0.5); assertEquals(0.5, intermediate_state.getTimeS(), EPSILON); assertEquals(start_state.acceleration(), intermediate_state.acceleration(), EPSILON); assertEquals(0.5, intermediate_state.velocityM_S(), EPSILON); From 8cda8961a897e418e6e292b5f14d5163df060ffa Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Wed, 24 Dec 2025 18:36:48 -0800 Subject: [PATCH 36/42] paring down schedule generator WIP --- .../trajectory/timing/ConstrainedState.java | 101 ++----------- .../trajectory/timing/ScheduleGenerator.java | 140 +++++++----------- 2 files changed, 65 insertions(+), 176 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index afbdefb01..4a2835a0b 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -1,100 +1,19 @@ package org.team100.lib.trajectory.timing; -import java.util.List; - import org.team100.lib.geometry.Pose2dWithMotion; class ConstrainedState { - private static final boolean DEBUG = false; - // using MAX_VALUE tickles some bugs - private static final double MAX_V = 100; - private static final double MAX_A = 100; - - private final Pose2dWithMotion m_state; - /** Cumulative distance along the path */ - private final double m_distanceM; - - private double m_velocityM_S; - private double m_minAccelM_S2; - private double m_maxAccelM_S2; + public final Pose2dWithMotion state; + public final double distance; + public double velocity; + public double decel; + public double accel; public ConstrainedState(Pose2dWithMotion state, double distance) { - m_state = state; - m_distanceM = distance; - setVelocityM_S(MAX_V); - setMinAccel(-MAX_A); - setMaxAccel(MAX_A); - } - - /** - * Clamp state velocity to constraints. - */ - public void clampVelocity(List constraints) { - for (TimingConstraint constraint : constraints) { - double value = constraint.maxV(m_state); - value = Math.min(getVelocityM_S(), value); - if (DEBUG) - System.out.printf("VELOCITY CONSTRAINT %s %5.3f\n", - constraint.getClass().getSimpleName(), value); - setVelocityM_S(value); - } - } - - /** - * Clamp constraint state accelerations to the constraints. - */ - public void clampAccel(List constraints) { - for (TimingConstraint constraint : constraints) { - m_minAccelM_S2 = Math.max(m_minAccelM_S2, constraint.maxDecel(m_state, getVelocityM_S())); - m_maxAccelM_S2 = Math.min(m_maxAccelM_S2, constraint.maxAccel(m_state, getVelocityM_S())); - if (DEBUG) - System.out.printf("ACCEL CONSTRAINT %s %5.3f %5.3f\n", - constraint.getClass().getSimpleName(), - m_minAccelM_S2, - m_maxAccelM_S2); - } - - } - - public Pose2dWithMotion getState() { - return m_state; - } - - /** Pathwise distance */ - public double getDistanceM() { - return m_distanceM; - } - - /** Pathwise velocity */ - public double getVelocityM_S() { - return m_velocityM_S; - } - - public void setVelocityM_S(double velocityM_S) { - if (Double.isNaN(velocityM_S)) - throw new IllegalArgumentException(); - m_velocityM_S = velocityM_S; - } - - public double getMinAccel() { - return m_minAccelM_S2; - } - - public void setMinAccel(double minAccelM_S2) { - m_minAccelM_S2 = minAccelM_S2; - } - - public double getMaxAccel() { - return m_maxAccelM_S2; - } - - public void setMaxAccel(double maxAccelM_S2) { - m_maxAccelM_S2 = maxAccelM_S2; - } - - @Override - public String toString() { - return m_state.toString() + ", distance: " + m_distanceM + ", vel: " + getVelocityM_S() + ", " + - "min_acceleration: " + m_minAccelM_S2 + ", max_acceleration: " + m_maxAccelM_S2; + this.state = state; + this.distance = distance; + velocity = 100; + decel = -100; + accel = 100; } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 0dd8e0933..441d85eb3 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -76,27 +76,11 @@ public Trajectory100 timeParameterizeTrajectory( double end_vel) throws TimingException { if (samples.size() < 3) throw new IllegalArgumentException("must have at least three samples"); - if (DEBUG) { - System.out.println("samples"); - for (Pose2dWithMotion p2 : samples) { - System.out.println(p2); - } - } + List constrainedStates = forwardPass(samples, start_vel); - if (DEBUG) { - System.out.println("after forward"); - for (ConstrainedState cs : constrainedStates) { - System.out.println(cs); - } - } + Pose2dWithMotion lastState = samples.get(samples.size() - 1); backwardsPass(lastState, end_vel, constrainedStates); - if (DEBUG) { - System.out.println("after backward"); - for (ConstrainedState cs : constrainedStates) { - System.out.println(cs); - } - } return integrate(constrainedStates); } @@ -117,35 +101,31 @@ private List forwardPass(List samples, doubl // note below we look again at this sample. I think this exists // only to supply the start velocity. ConstrainedState predecessor = new ConstrainedState(samples.get(0), 0); - predecessor.setVelocityM_S(start_vel); - predecessor.setMinAccel(-HIGH_ACCEL); - predecessor.setMaxAccel(HIGH_ACCEL); + predecessor.velocity = start_vel; + predecessor.decel = -HIGH_ACCEL; + predecessor.accel = HIGH_ACCEL; // work forward through the samples List constrainedStates = new ArrayList<>(samples.size()); for (int i = 0; i < samples.size(); ++i) { Pose2dWithMotion sample = samples.get(i); - double dsM = sample.distanceCartesian(predecessor.getState()); + double dsM = sample.distanceCartesian(predecessor.state); if (i > 0 && i < samples.size() - 1 && dsM < 1e-6) { // the first distance is zero because of the weird loop structure. // the last distance can be zero if the step size exactly divides the path // length throw new IllegalStateException("zero distance not allowed"); } - if (DEBUG) - System.out.printf("i%d dsM %f\n", i, dsM); + ConstrainedState constrainedState = new ConstrainedState( - sample, dsM + predecessor.getDistanceM()); + sample, dsM + predecessor.distance); constrainedStates.add(constrainedState); + forwardWork(predecessor, constrainedState); + predecessor = constrainedState; } - if (DEBUG) { - System.out.println("after forward pass:"); - for (int i = 0; i < constrainedStates.size(); ++i) { - System.out.printf("%d, %s\n", i, constrainedStates.get(i)); - } - } + return constrainedStates; } @@ -155,57 +135,49 @@ private void forwardWork(ConstrainedState s0, ConstrainedState s1) { // not double-geodesic with rotation // Just translation, so that the pathwise velocity matches // the curvature in the state. - double dsM = s1.getState().distanceCartesian(s0.getState()); + double dsM = s1.state.distanceCartesian(s0.state); // We may need to iterate to find the maximum end velocity and common // acceleration, since acceleration limits may be a function of velocity. while (true) { // first try the previous state accel to get the new state velocity - double v1 = v1(s0.getVelocityM_S(), s0.getMaxAccel(), dsM); - if (DEBUG) - System.out.printf("initial v1 %5.3f\n", v1); - s1.setVelocityM_S(v1); + double v1 = v1(s0.velocity, s0.accel, dsM); + + s1.velocity = v1; // also use max accels for the new state accels - s1.setMinAccel(-HIGH_ACCEL); - s1.setMaxAccel(HIGH_ACCEL); + s1.decel = -HIGH_ACCEL; + s1.accel = HIGH_ACCEL; // reduce velocity according to constraints - s1.clampVelocity(m_constraints); + for (TimingConstraint constraint : m_constraints) { + s1.velocity = Math.min(s1.velocity, constraint.maxV(s1.state)); + } - if (DEBUG) - System.out.printf("accel before %5.3f\n", s1.getMaxAccel()); - // reduce accel according to constraints - // in the failure case, this is clamped to zero, - // because the velocity is very high. - // and that's because the preceding accel is the near-infinite value - // which means we exceed the max in one time step - // so the key is to not do that, i think. - s1.clampAccel(m_constraints); - if (DEBUG) - System.out.printf("accel after %5.3f\n", s1.getMaxAccel()); + for (TimingConstraint constraint1 : m_constraints) { + s1.decel = Math.max(s1.decel, constraint1.maxDecel(s1.state, s1.velocity)); + s1.accel = Math.min(s1.accel, constraint1.maxAccel(s1.state, s1.velocity)); + } // motionless if (Math.abs(dsM) < EPSILON) { return; } - double accel = accel(s0.getVelocityM_S(), s1.getVelocityM_S(), dsM); + double accel = accel(s0.velocity, s1.velocity, dsM); // in the failure case, max accel is zero so this always fails. - if (accel > s1.getMaxAccel() + EPSILON) { + if (accel > s1.accel + EPSILON) { // implied accel is too high because v1 is too high, perhaps because // a0 was too high, try again with the (lower) constrained value // // if the constrained value is zero, this doesn't work at all. - if (DEBUG) - System.out.printf("accel too high %5.3f, %5.3f, %5.3f\n", - accel, s0.getMaxAccel(), s1.getMaxAccel()); - s0.setMaxAccel(s1.getMaxAccel()); + + s0.accel = s1.accel; continue; } - if (accel > s0.getMinAccel() + EPSILON) { + if (accel > s0.decel + EPSILON) { // set the previous state accel to whatever the constrained velocity implies - s0.setMaxAccel(accel); + s0.accel = accel; } return; } @@ -221,29 +193,26 @@ private void backwardsPass( // "successor" comes before in the backwards walk. start with the last state. ConstrainedState endState = constrainedStates.get(constrainedStates.size() - 1); ConstrainedState successor = new ConstrainedState( - lastState, endState.getDistanceM()); - successor.setVelocityM_S(end_velocity); - successor.setMinAccel(-HIGH_ACCEL); - successor.setMaxAccel(HIGH_ACCEL); + lastState, endState.distance); + successor.velocity = end_velocity; + successor.decel = -HIGH_ACCEL; + successor.accel = HIGH_ACCEL; // work backwards through the states list for (int i = constrainedStates.size() - 1; i >= 0; --i) { ConstrainedState constrainedState = constrainedStates.get(i); + backwardsWork(constrainedState, successor); + successor = constrainedState; } - if (DEBUG) { - System.out.println("after backwards pass:"); - for (int i = 0; i < constrainedStates.size(); ++i) { - System.out.printf("%d, %s\n", i, constrainedStates.get(i)); - } - } + } /** s0 is earlier, s1 is "successor", we're walking backwards. */ private void backwardsWork(ConstrainedState s0, ConstrainedState s1) { // backwards (negative) distance from successor to initial state. - double ds = s0.getDistanceM() - s1.getDistanceM(); + double ds = s0.distance - s1.distance; if (ds > 0) { // must be negative if we're walking backwards. throw new IllegalStateException(); @@ -253,18 +222,21 @@ private void backwardsWork(ConstrainedState s0, ConstrainedState s1) { // s0 velocity can't be more than the accel implies // so this is actually an estimate for v0 // min a is negative, ds is negative, so v0 is faster than v1 - double v0 = v1(s1.getVelocityM_S(), s1.getMinAccel(), ds); + double v0 = v1(s1.velocity, s1.decel, ds); - if (s0.getVelocityM_S() <= v0) { + if (s0.velocity <= v0) { // s0 v is slower than implied v0, which means // that actual accel is larger than the min, so we're fine // No new limits to impose. return; } // s0 v is too fast, turn it down to obey v1 min accel. - s0.setVelocityM_S(v0); + s0.velocity = v0; - s0.clampAccel(m_constraints); + for (TimingConstraint constraint : m_constraints) { + s0.decel = Math.max(s0.decel, constraint.maxDecel(s0.state, s0.velocity)); + s0.accel = Math.min(s0.accel, constraint.maxAccel(s0.state, s0.velocity)); + } // motionless if (Math.abs(ds) < EPSILON) { @@ -272,14 +244,14 @@ private void backwardsWork(ConstrainedState s0, ConstrainedState s1) { } // implied accel using the constrained v0 - double accel = accel(s1.getVelocityM_S(), s0.getVelocityM_S(), ds); - if (accel < s0.getMinAccel() - EPSILON) { + double accel = accel(s1.velocity, s0.velocity, ds); + if (accel < s0.decel - EPSILON) { // accel is too low which implies that s1 accel is too low, try again - s1.setMinAccel(s0.getMinAccel()); + s1.decel = s0.decel; continue; } // set final accel to the implied value - s1.setMinAccel(accel); + s1.decel = accel; return; } } @@ -296,14 +268,12 @@ private static Trajectory100 integrate(List states) throws Tim // for turn-in-place, double distance = 0.0; double v0 = 0.0; - if (DEBUG) - System.out.println("i, v0, v1, ds"); + for (int i = 0; i < states.size(); ++i) { ConstrainedState state = states.get(i); - double ds = state.getDistanceM() - distance; - double v1 = state.getVelocityM_S(); - if (DEBUG) - System.out.printf("%d, %5.3f, %5.3f, %5.3f\n", i, v0, v1, ds); + double ds = state.distance - distance; + double v1 = state.velocity; + double dt = 0.0; if (i > 0) { double prevAccel = accel(v0, v1, ds); @@ -314,9 +284,9 @@ private static Trajectory100 integrate(List states) throws Tim if (Double.isNaN(time) || Double.isInfinite(time)) { throw new TimingException(); } - poses.add(new TimedState(state.getState(), time, v1, 0)); + poses.add(new TimedState(state.state, time, v1, 0)); v0 = v1; - distance = state.getDistanceM(); + distance = state.distance; } return new Trajectory100(poses); } From 94212283e6555d0aabe0163c2ff5cb5f83692f5b Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 25 Dec 2025 00:02:13 -0800 Subject: [PATCH 37/42] cutting up trajectory code WIP --- .../trajectory/timing/ScheduleGenerator.java | 363 +++++++----------- .../java/org/team100/lib/util/Math100.java | 61 +++ .../timing/ScheduleGeneratorTest.java | 84 +--- .../org/team100/lib/util/Math100Test.java | 85 ++++ 4 files changed, 282 insertions(+), 311 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 441d85eb3..1c070b369 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -6,6 +6,7 @@ import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.path.Path100; +import org.team100.lib.util.Math100; /** * Given a path, produces a trajectory, which includes the path and adds a @@ -76,40 +77,30 @@ public Trajectory100 timeParameterizeTrajectory( double end_vel) throws TimingException { if (samples.size() < 3) throw new IllegalArgumentException("must have at least three samples"); - - List constrainedStates = forwardPass(samples, start_vel); - - Pose2dWithMotion lastState = samples.get(samples.size() - 1); - backwardsPass(lastState, end_vel, constrainedStates); - - return integrate(constrainedStates); - } - - /** - * Forward pass. - * - * We look at pairs of consecutive states, where the start state has already - * been velocity parameterized (though we may adjust the velocity downwards - * during the backwards pass). We wish to find an acceleration that is - * admissible at both the start and end state, as well as an admissible end - * velocity. If there is no admissible end velocity or acceleration, we set the - * end velocity to the state's maximum allowed velocity and will repair the - * acceleration during the backward pass (by slowing down the predecessor). - */ - private List forwardPass(List samples, double start_vel) { - // note below we look again at this sample. I think this exists // only to supply the start velocity. - ConstrainedState predecessor = new ConstrainedState(samples.get(0), 0); - predecessor.velocity = start_vel; - predecessor.decel = -HIGH_ACCEL; - predecessor.accel = HIGH_ACCEL; + ConstrainedState s0 = new ConstrainedState(samples.get(0), 0); + s0.velocity = start_vel; + s0.decel = -HIGH_ACCEL; + s0.accel = HIGH_ACCEL; + + // + // Forward pass. + // + // We look at pairs of consecutive states, where the start state has already + // been velocity parameterized (though we may adjust the velocity downwards + // during the backwards pass). We wish to find an acceleration that is + // admissible at both the start and end state, as well as an admissible end + // velocity. If there is no admissible end velocity or acceleration, we set the + // end velocity to the state's maximum allowed velocity and will repair the + // acceleration during the backward pass (by slowing down the predecessor). + // // work forward through the samples List constrainedStates = new ArrayList<>(samples.size()); for (int i = 0; i < samples.size(); ++i) { Pose2dWithMotion sample = samples.get(i); - double dsM = sample.distanceCartesian(predecessor.state); + double dsM = sample.distanceCartesian(s0.state); if (i > 0 && i < samples.size() - 1 && dsM < 1e-6) { // the first distance is zero because of the weird loop structure. // the last distance can be zero if the step size exactly divides the path @@ -117,166 +108,152 @@ private List forwardPass(List samples, doubl throw new IllegalStateException("zero distance not allowed"); } - ConstrainedState constrainedState = new ConstrainedState( - sample, dsM + predecessor.distance); - constrainedStates.add(constrainedState); - - forwardWork(predecessor, constrainedState); - - predecessor = constrainedState; - } - - return constrainedStates; - } - - private void forwardWork(ConstrainedState s0, ConstrainedState s1) { - // R2 translation distance between states - // not constant-twist arc - // not double-geodesic with rotation - // Just translation, so that the pathwise velocity matches - // the curvature in the state. - double dsM = s1.state.distanceCartesian(s0.state); - - // We may need to iterate to find the maximum end velocity and common - // acceleration, since acceleration limits may be a function of velocity. - while (true) { - // first try the previous state accel to get the new state velocity - double v1 = v1(s0.velocity, s0.accel, dsM); - - s1.velocity = v1; - - // also use max accels for the new state accels - s1.decel = -HIGH_ACCEL; - s1.accel = HIGH_ACCEL; - - // reduce velocity according to constraints - for (TimingConstraint constraint : m_constraints) { - s1.velocity = Math.min(s1.velocity, constraint.maxV(s1.state)); + ConstrainedState s1 = new ConstrainedState(sample, dsM + s0.distance); + constrainedStates.add(s1); + + // R2 translation distance between states + // not constant-twist arc + // not double-geodesic with rotation + // Just translation, so that the pathwise velocity matches + // the curvature in the state. + double dsM1 = s1.state.distanceCartesian(s0.state); + + // We may need to iterate to find the maximum end velocity and common + // acceleration, since acceleration limits may be a function of velocity. + while (true) { + // first try the previous state accel to get the new state velocity + double v1 = Math100.v1(s0.velocity, s0.accel, dsM1); + + s1.velocity = v1; + + // also use max accels for the new state accels + s1.decel = -HIGH_ACCEL; + s1.accel = HIGH_ACCEL; + + // reduce velocity according to constraints + for (TimingConstraint constraint : m_constraints) { + s1.velocity = Math.min(s1.velocity, constraint.maxV(s1.state)); + } + + for (TimingConstraint constraint1 : m_constraints) { + s1.decel = Math.max(s1.decel, constraint1.maxDecel(s1.state, s1.velocity)); + s1.accel = Math.min(s1.accel, constraint1.maxAccel(s1.state, s1.velocity)); + } + + // motionless + if (Math.abs(dsM1) < EPSILON) { + break; + } + + double accel = Math100.accel(s0.velocity, s1.velocity, dsM1); + // in the failure case, max accel is zero so this always fails. + if (accel > s1.accel + EPSILON) { + // implied accel is too high because v1 is too high, perhaps because + // a0 was too high, try again with the (lower) constrained value + // + // if the constrained value is zero, this doesn't work at all. + + s0.accel = s1.accel; + continue; + } + if (accel > s0.decel + EPSILON) { + // set the previous state accel to whatever the constrained velocity implies + s0.accel = accel; + } + break; } - for (TimingConstraint constraint1 : m_constraints) { - s1.decel = Math.max(s1.decel, constraint1.maxDecel(s1.state, s1.velocity)); - s1.accel = Math.min(s1.accel, constraint1.maxAccel(s1.state, s1.velocity)); - } + s0 = s1; + } - // motionless - if (Math.abs(dsM) < EPSILON) { - return; - } + - double accel = accel(s0.velocity, s1.velocity, dsM); - // in the failure case, max accel is zero so this always fails. - if (accel > s1.accel + EPSILON) { - // implied accel is too high because v1 is too high, perhaps because - // a0 was too high, try again with the (lower) constrained value - // - // if the constrained value is zero, this doesn't work at all. + Pose2dWithMotion lastState = samples.get(samples.size() - 1); - s0.accel = s1.accel; - continue; - } - if (accel > s0.decel + EPSILON) { - // set the previous state accel to whatever the constrained velocity implies - s0.accel = accel; - } - return; - } - } + // + // Backwards pass + // - /** - * Backwards pass - */ - private void backwardsPass( - Pose2dWithMotion lastState, - double end_velocity, - List constrainedStates) { // "successor" comes before in the backwards walk. start with the last state. ConstrainedState endState = constrainedStates.get(constrainedStates.size() - 1); - ConstrainedState successor = new ConstrainedState( + ConstrainedState s1 = new ConstrainedState( lastState, endState.distance); - successor.velocity = end_velocity; - successor.decel = -HIGH_ACCEL; - successor.accel = HIGH_ACCEL; + s1.velocity = end_vel; + s1.decel = -HIGH_ACCEL; + s1.accel = HIGH_ACCEL; // work backwards through the states list for (int i = constrainedStates.size() - 1; i >= 0; --i) { - ConstrainedState constrainedState = constrainedStates.get(i); - - backwardsWork(constrainedState, successor); + ConstrainedState s01 = constrainedStates.get(i); - successor = constrainedState; - } - - } - - /** s0 is earlier, s1 is "successor", we're walking backwards. */ - private void backwardsWork(ConstrainedState s0, ConstrainedState s1) { - // backwards (negative) distance from successor to initial state. - double ds = s0.distance - s1.distance; - if (ds > 0) { - // must be negative if we're walking backwards. - throw new IllegalStateException(); - } - - while (true) { - // s0 velocity can't be more than the accel implies - // so this is actually an estimate for v0 - // min a is negative, ds is negative, so v0 is faster than v1 - double v0 = v1(s1.velocity, s1.decel, ds); - - if (s0.velocity <= v0) { - // s0 v is slower than implied v0, which means - // that actual accel is larger than the min, so we're fine - // No new limits to impose. - return; + // backwards (negative) distance from successor to initial state. + double ds = s01.distance - s1.distance; + if (ds > 0) { + // must be negative if we're walking backwards. + throw new IllegalStateException(); } - // s0 v is too fast, turn it down to obey v1 min accel. - s0.velocity = v0; - for (TimingConstraint constraint : m_constraints) { - s0.decel = Math.max(s0.decel, constraint.maxDecel(s0.state, s0.velocity)); - s0.accel = Math.min(s0.accel, constraint.maxAccel(s0.state, s0.velocity)); + while (true) { + // s0 velocity can't be more than the accel implies + // so this is actually an estimate for v0 + // min a is negative, ds is negative, so v0 is faster than v1 + double v0 = Math100.v1(s1.velocity, s1.decel, ds); + + if (s01.velocity <= v0) { + // s0 v is slower than implied v0, which means + // that actual accel is larger than the min, so we're fine + // No new limits to impose. + break; + } + // s0 v is too fast, turn it down to obey v1 min accel. + s01.velocity = v0; + + for (TimingConstraint constraint : m_constraints) { + s01.decel = Math.max(s01.decel, constraint.maxDecel(s01.state, s01.velocity)); + s01.accel = Math.min(s01.accel, constraint.maxAccel(s01.state, s01.velocity)); + } + + // motionless + if (Math.abs(ds) < EPSILON) { + break; + } + + // implied accel using the constrained v0 + double accel = Math100.accel(s1.velocity, s01.velocity, ds); + if (accel < s01.decel - EPSILON) { + // accel is too low which implies that s1 accel is too low, try again + s1.decel = s01.decel; + continue; + } + // set final accel to the implied value + s1.decel = accel; + break; } - // motionless - if (Math.abs(ds) < EPSILON) { - return; - } - - // implied accel using the constrained v0 - double accel = accel(s1.velocity, s0.velocity, ds); - if (accel < s0.decel - EPSILON) { - // accel is too low which implies that s1 accel is too low, try again - s1.decel = s0.decel; - continue; - } - // set final accel to the implied value - s1.decel = accel; - return; + s1 = s01; } - } - /** - * Integrate the constrained states forward in time to obtain the TimedStates. - * - * last state accel is always zero, which might be wrong. - */ - private static Trajectory100 integrate(List states) throws TimingException { - List poses = new ArrayList<>(states.size()); + // + // Integrate the constrained states forward in time to obtain the TimedStates. + // + // last state accel is always zero, which might be wrong. + // + + List poses = new ArrayList<>(constrainedStates.size()); double time = 0.0; // distance along path // for turn-in-place, double distance = 0.0; double v0 = 0.0; - for (int i = 0; i < states.size(); ++i) { - ConstrainedState state = states.get(i); + for (int i = 0; i < constrainedStates.size(); ++i) { + ConstrainedState state = constrainedStates.get(i); double ds = state.distance - distance; double v1 = state.velocity; double dt = 0.0; if (i > 0) { - double prevAccel = accel(v0, v1, ds); + double prevAccel = Math100.accel(v0, v1, ds); poses.get(i - 1).set_acceleration(prevAccel); dt = dt(v0, v1, ds, prevAccel); } @@ -288,6 +265,7 @@ private static Trajectory100 integrate(List states) throws Tim v0 = v1; distance = state.distance; } + return new Trajectory100(poses); } @@ -311,77 +289,6 @@ private static double dt( // throw new TimingException(String.format("%f %f %f %f", v0, v1, ds, accel)); } - /** - * Return final velocity, v1, given initial velocity, v0, and acceleration over - * distance ds. - * - * v1 = sqrt(v0^2 + 2ads) - * - * note a can be negative. - * - * note ds can be negative, which implies backwards time - * - * @param v0 initial velocity - * @param a acceleration - * @param ds distance - * @return final velocity - */ - static double v1(double v0, double a, double ds) { - /* - * a = dv/dt - * v = ds/dt - * dt = ds/v - * a = v dv/ds - * a = v (v1-v0)/ds - * v = (v0+v1)/2 - * a = (v0+v1)(v1-v0)/2ds - * a = (v1^2 - v0^2)/2ds - * 2*a*ds = v1^2 - v0^2 - * v1 = sqrt(v0^2 + 2*a*ds) - */ - return Math.sqrt(v0 * v0 + 2.0 * a * ds); - } - - /** - * Return acceleration implied by the change in velocity (v0 to v1) - * over the distance, ds. - * - * a = (v1^2 - v0^2) / 2ds - * - * note ds can be negative, which implies negative time. - * - * @param v0 initial velocity - * @param v1 final velocity - * @param ds distance - */ - public static double accel(double v0, double v1, double ds) { - if (Math.abs(ds) < 1e-6) { - // prevent division by zero - return 0; - } - /* - * a = dv/dt - * v = ds/dt - * dt = ds/v - * a = v dv/ds - * a = v (v1-v0)/ds - * v = (v0+v1)/2 - * a = (v0+v1)(v1-v0)/2ds - * a = (v1^2 - v0^2)/2ds - */ - double a = (v1 * v1 - v0 * v0) / (2.0 * ds); - double dv = v1 - v0; - // this can be negative, which indicates that v1 precedes v0 - double dt = a / dv; - return a; - } - public static class TimingException extends Exception { - public TimingException() { - } - - public TimingException(String s) { - super(s); - } } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index 8ff0e51b0..85db7b215 100644 --- a/lib/src/main/java/org/team100/lib/util/Math100.java +++ b/lib/src/main/java/org/team100/lib/util/Math100.java @@ -95,4 +95,65 @@ public static double throwIfOutOfRange(double x, double minX, double maxX) { return x; } + /** + * Return acceleration implied by the change in velocity (v0 to v1) + * over the distance, ds. + * + * a = (v1^2 - v0^2) / 2ds + * + * note ds can be negative, which implies negative time. + * + * @param v0 initial velocity + * @param v1 final velocity + * @param ds distance + */ + public static double accel(double v0, double v1, double ds) { + if (Math.abs(ds) < 1e-6) { + // prevent division by zero + return 0; + } + /* + * a = dv/dt + * v = ds/dt + * dt = ds/v + * a = v dv/ds + * a = v (v1-v0)/ds + * v = (v0+v1)/2 + * a = (v0+v1)(v1-v0)/2ds + * a = (v1^2 - v0^2)/2ds + */ + return (v1 * v1 - v0 * v0) / (2.0 * ds); + } + + /** + * Return final velocity, v1, given initial velocity, v0, and acceleration over + * distance ds. + * + * v1 = sqrt(v0^2 + 2ads) + * + * note a can be negative. + * + * note ds can be negative, which implies backwards time + * + * @param v0 initial velocity + * @param a acceleration + * @param ds distance + * @return final velocity + */ + public static double v1(double v0, double a, double ds) { + /* + * a = dv/dt + * v = ds/dt + * dt = ds/v + * a = v dv/ds + * a = v (v1-v0)/ds + * v = (v0+v1)/2 + * a = (v0+v1)(v1-v0)/2ds + * a = (v1^2 - v0^2)/2ds + * 2*a*ds = v1^2 - v0^2 + * v1 = sqrt(v0^2 + 2*a*ds) + */ + return Math.sqrt(v0 * v0 + 2.0 * a * ds); + } + } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index c2d60c664..a0d41180a 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -11,12 +11,9 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.geometry.StateSE2; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -130,7 +127,7 @@ void testJustTurningInPlace() { List constraints = new ArrayList(); ScheduleGenerator u = new ScheduleGenerator(constraints); - assertThrows(IllegalArgumentException.class, ()-> u.timeParameterizeTrajectory( + assertThrows(IllegalArgumentException.class, () -> u.timeParameterizeTrajectory( path, 1.0, 0.0, @@ -254,33 +251,6 @@ public double maxDecel(Pose2dWithMotion state, double velocity) { assertNotNull(timed_traj); } - @Test - void testAccel() { - // average v = 0.5 - // dv = 1 - assertEquals(0.5, ScheduleGenerator.accel(0, 1, 1.0), 0.001); - assertEquals(1.0, ScheduleGenerator.accel(0, 1, 0.5), 0.001); - // average v = 1.5 - // dv = 1 - assertEquals(1.5, ScheduleGenerator.accel(1, 2, 1.0), 0.001); - // same case, backwards - assertEquals(1.5, ScheduleGenerator.accel(2, 1, -1.0), 0.001); - } - - @Test - void testV1() { - // no v or a => no new v - assertEquals(0.0, ScheduleGenerator.v1(0, 0, 1.0)); - // no a => keep old v - assertEquals(1.0, ScheduleGenerator.v1(1, 0, 1.0)); - // a = 0.5 for 1 => final v is 1 - assertEquals(1.0, ScheduleGenerator.v1(0, 0.5, 1.0)); - // same case, backwards - assertEquals(0.0, ScheduleGenerator.v1(1.0, 0.5, -1.0)); - // backwards with negative accel - assertEquals(1.0, ScheduleGenerator.v1(0.0, -0.5, -1.0)); - } - /** * 0.16 ms on my machine. * @@ -332,56 +302,4 @@ void testPerformance() { } - /** - * Fundamental math. - * - * Components are treated as independent. - */ - @Test - void testBasic0() { - // Given two states, find the acceleration between them. - StateSE2 s0 = new StateSE2( - new Pose2d(0, 0, new Rotation2d(0)), - new VelocitySE2(0, 0, 0)); - StateSE2 s1 = new StateSE2( - new Pose2d(1, 0, new Rotation2d(0)), - new VelocitySE2(1, 0, 0)); - AccelerationSE2 a = new AccelerationSE2( - ScheduleGenerator.accel( - s0.vel().x(), s1.vel().x(), s1.pose().getX() - s0.pose().getX()), - ScheduleGenerator.accel( - s0.vel().y(), s1.vel().y(), s1.pose().getY() - s0.pose().getY()), - ScheduleGenerator.accel( - s0.vel().theta(), s1.vel().theta(), - s1.pose().getRotation().minus(s0.pose().getRotation()).getRadians())); - // 1 meter at 1 m/s, a=0.5 m/s, t= 2 - assertEquals(0.5, a.x(), DELTA); - assertEquals(0, a.y(), DELTA); - assertEquals(0, a.theta(), DELTA); - } - - @Test - void testBasic1() { - // This case makes no sense. - StateSE2 s0 = new StateSE2( - new Pose2d(0, 0, new Rotation2d(0)), - new VelocitySE2(0, 0, 0)); - StateSE2 s1 = new StateSE2( - new Pose2d(1, 0, new Rotation2d(0)), // <<< positive position - new VelocitySE2(-1, 0, 0)); // <<< negative velocity - AccelerationSE2 a = new AccelerationSE2( - ScheduleGenerator.accel( - s0.vel().x(), s1.vel().x(), s1.pose().getX() - s0.pose().getX()), - ScheduleGenerator.accel( - s0.vel().y(), s1.vel().y(), s1.pose().getY() - s0.pose().getY()), - ScheduleGenerator.accel( - s0.vel().theta(), s1.vel().theta(), - s1.pose().getRotation().minus(s0.pose().getRotation()).getRadians())); - // 1 meter at 1 m/s, a=0.5 m/s, t= 2 - // this acceleration is for negative time, i.e. from s1 to s0, decelerating. - assertEquals(0.5, a.x(), DELTA); - assertEquals(0, a.y(), DELTA); - assertEquals(0, a.theta(), DELTA); - } - } diff --git a/lib/src/test/java/org/team100/lib/util/Math100Test.java b/lib/src/test/java/org/team100/lib/util/Math100Test.java index d0c8e5e8c..59eaf29fd 100644 --- a/lib/src/test/java/org/team100/lib/util/Math100Test.java +++ b/lib/src/test/java/org/team100/lib/util/Math100Test.java @@ -7,6 +7,12 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.StateSE2; +import org.team100.lib.geometry.VelocitySE2; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; class Math100Test { private static final double DELTA = 0.001; @@ -46,4 +52,83 @@ void testGetMinDistance() { assertEquals(2 * Math.PI, d, DELTA); } + @Test + void testAccel() { + // average v = 0.5 + // dv = 1 + assertEquals(0.5, Math100.accel(0, 1, 1.0), 0.001); + assertEquals(1.0, Math100.accel(0, 1, 0.5), 0.001); + // average v = 1.5 + // dv = 1 + assertEquals(1.5, Math100.accel(1, 2, 1.0), 0.001); + // same case, backwards + assertEquals(1.5, Math100.accel(2, 1, -1.0), 0.001); + } + + /** + * Fundamental math. + * + * Components are treated as independent. + */ + @Test + void testBasic0() { + // Given two states, find the acceleration between them. + StateSE2 s0 = new StateSE2( + new Pose2d(0, 0, new Rotation2d(0)), + new VelocitySE2(0, 0, 0)); + StateSE2 s1 = new StateSE2( + new Pose2d(1, 0, new Rotation2d(0)), + new VelocitySE2(1, 0, 0)); + AccelerationSE2 a = new AccelerationSE2( + Math100.accel( + s0.vel().x(), s1.vel().x(), s1.pose().getX() - s0.pose().getX()), + Math100.accel( + s0.vel().y(), s1.vel().y(), s1.pose().getY() - s0.pose().getY()), + Math100.accel( + s0.vel().theta(), s1.vel().theta(), + s1.pose().getRotation().minus(s0.pose().getRotation()).getRadians())); + // 1 meter at 1 m/s, a=0.5 m/s, t= 2 + assertEquals(0.5, a.x(), DELTA); + assertEquals(0, a.y(), DELTA); + assertEquals(0, a.theta(), DELTA); + } + + @Test + void testBasic1() { + // This case makes no sense. + StateSE2 s0 = new StateSE2( + new Pose2d(0, 0, new Rotation2d(0)), + new VelocitySE2(0, 0, 0)); + StateSE2 s1 = new StateSE2( + new Pose2d(1, 0, new Rotation2d(0)), // <<< positive position + new VelocitySE2(-1, 0, 0)); // <<< negative velocity + AccelerationSE2 a = new AccelerationSE2( + Math100.accel( + s0.vel().x(), s1.vel().x(), s1.pose().getX() - s0.pose().getX()), + Math100.accel( + s0.vel().y(), s1.vel().y(), s1.pose().getY() - s0.pose().getY()), + Math100.accel( + s0.vel().theta(), s1.vel().theta(), + s1.pose().getRotation().minus(s0.pose().getRotation()).getRadians())); + // 1 meter at 1 m/s, a=0.5 m/s, t= 2 + // this acceleration is for negative time, i.e. from s1 to s0, decelerating. + assertEquals(0.5, a.x(), DELTA); + assertEquals(0, a.y(), DELTA); + assertEquals(0, a.theta(), DELTA); + } + + @Test + void testV1() { + // no v or a => no new v + assertEquals(0.0, Math100.v1(0, 0, 1.0)); + // no a => keep old v + assertEquals(1.0, Math100.v1(1, 0, 1.0)); + // a = 0.5 for 1 => final v is 1 + assertEquals(1.0, Math100.v1(0, 0.5, 1.0)); + // same case, backwards + assertEquals(0.0, Math100.v1(1.0, 0.5, -1.0)); + // backwards with negative accel + assertEquals(1.0, Math100.v1(0.0, -0.5, -1.0)); + } + } From 088e53efbcc320b58a5197c763de68f2110f4980 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 25 Dec 2025 09:37:54 -0800 Subject: [PATCH 38/42] cutting up schedule generator WIP --- .../team100/lib/trajectory/path/Path100.java | 26 ++ .../trajectory/timing/ConstrainedState.java | 8 +- .../trajectory/timing/ScheduleGenerator.java | 340 +++++++++--------- .../AprilTagRobotLocalizerTest.java | 6 +- .../timing/TrajectoryVelocityProfileTest.java | 6 +- 5 files changed, 196 insertions(+), 190 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 7cc7dbb53..18280325c 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -6,6 +6,7 @@ import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.timing.ScheduleGenerator; +import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; import edu.wpi.first.math.geometry.Twist2d; @@ -108,6 +109,30 @@ public Pose2dWithMotion sample(double distance) throws ScheduleGenerator.TimingE throw new ScheduleGenerator.TimingException(); } + /** + * Samples the entire path evenly by distance. + */ + public Pose2dWithMotion[] resample(double step) throws ScheduleGenerator.TimingException { + double maxDistance = getMaxDistance(); + if (maxDistance == 0) + throw new IllegalArgumentException("max distance must be greater than zero"); + int num_states = (int) Math.ceil(maxDistance / step) + 1; + if (ScheduleGenerator.DEBUG) + System.out.printf("resample max distance %f step %f num states %d f %f\n", + maxDistance, step, num_states, maxDistance / step); + Pose2dWithMotion[] samples = new Pose2dWithMotion[num_states]; + for (int i = 0; i < num_states; ++i) { + // the dtheta and curvature come from here and are never changed. + // the values here are just interpolated from the original values. + double d = Math.min(i * step, maxDistance); + Pose2dWithMotion state = sample(d); + if (ScheduleGenerator.DEBUG) + System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state); + samples[i] =state; + } + return samples; + } + @Override public String toString() { StringBuilder builder = new StringBuilder(); @@ -119,4 +144,5 @@ public String toString() { } return builder.toString(); } + } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java index 4a2835a0b..5bba9874e 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java @@ -1,17 +1,11 @@ package org.team100.lib.trajectory.timing; -import org.team100.lib.geometry.Pose2dWithMotion; - class ConstrainedState { - public final Pose2dWithMotion state; - public final double distance; public double velocity; public double decel; public double accel; - public ConstrainedState(Pose2dWithMotion state, double distance) { - this.state = state; - this.distance = distance; + public ConstrainedState() { velocity = 100; decel = -100; accel = 100; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 1c070b369..094e71204 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -13,20 +13,22 @@ * schedule. */ public class ScheduleGenerator { - private static final boolean DEBUG = false; + public static class TimingException extends Exception { + } + + public static final boolean DEBUG = false; private static final double EPSILON = 1e-6; /** this is the default, in order to make the constraints set the actual */ private static final double HIGH_ACCEL = 1000; private final List m_constraints; - /** If you want a max velocity or accel constraint, use ConstantConstraint. */ public ScheduleGenerator(List constraints) { m_constraints = constraints; } /** - * Samples the path evenly by distance, and then assign times to each sample. + * Samples the path evenly by distance, then assigns a time to each sample. */ public Trajectory100 timeParameterizeTrajectory( Path100 path, @@ -34,7 +36,7 @@ public Trajectory100 timeParameterizeTrajectory( double start_vel, double end_vel) { try { - List samples = resample(path, step); + Pose2dWithMotion[] samples = path.resample(step); return timeParameterizeTrajectory(samples, start_vel, end_vel); } catch (TimingException e) { e.printStackTrace(); @@ -43,43 +45,26 @@ public Trajectory100 timeParameterizeTrajectory( } } - /** - * Samples the path evenly by distance. - */ - private List resample(Path100 path, double step) throws TimingException { - double maxDistance = path.getMaxDistance(); - if (maxDistance == 0) - throw new IllegalArgumentException("max distance must be greater than zero"); - int num_states = (int) Math.ceil(maxDistance / step) + 1; - if (DEBUG) - System.out.printf("resample max distance %f step %f num states %d f %f\n", - maxDistance, step, num_states, maxDistance / step); - List samples = new ArrayList<>(num_states); - for (int i = 0; i < num_states; ++i) { - // the dtheta and curvature come from here and are never changed. - // the values here are just interpolated from the original values. - double d = Math.min(i * step, maxDistance); - Pose2dWithMotion state = path.sample(d); - if (DEBUG) - System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state); - samples.add(state); - } - return samples; - } - /** * input is some set of samples (could be evenly sampled or not), output is * these same samples with time. */ public Trajectory100 timeParameterizeTrajectory( - List samples, + Pose2dWithMotion[] samples, double start_vel, double end_vel) throws TimingException { - if (samples.size() < 3) + int n = samples.length; + if (n < 3) throw new IllegalArgumentException("must have at least three samples"); + + double distances[] = new double[n]; + // note below we look again at this sample. I think this exists // only to supply the start velocity. - ConstrainedState s0 = new ConstrainedState(samples.get(0), 0); + Pose2dWithMotion previousPose = samples[0]; + ConstrainedState s0 = new ConstrainedState(); + distances[0] = 0; + double previousDistance = 0; s0.velocity = start_vel; s0.decel = -HIGH_ACCEL; s0.accel = HIGH_ACCEL; @@ -97,140 +82,145 @@ public Trajectory100 timeParameterizeTrajectory( // // work forward through the samples - List constrainedStates = new ArrayList<>(samples.size()); - for (int i = 0; i < samples.size(); ++i) { - Pose2dWithMotion sample = samples.get(i); - double dsM = sample.distanceCartesian(s0.state); - if (i > 0 && i < samples.size() - 1 && dsM < 1e-6) { - // the first distance is zero because of the weird loop structure. - // the last distance can be zero if the step size exactly divides the path - // length - throw new IllegalStateException("zero distance not allowed"); - } - - ConstrainedState s1 = new ConstrainedState(sample, dsM + s0.distance); - constrainedStates.add(s1); - - // R2 translation distance between states - // not constant-twist arc - // not double-geodesic with rotation - // Just translation, so that the pathwise velocity matches - // the curvature in the state. - double dsM1 = s1.state.distanceCartesian(s0.state); - - // We may need to iterate to find the maximum end velocity and common - // acceleration, since acceleration limits may be a function of velocity. - while (true) { - // first try the previous state accel to get the new state velocity - double v1 = Math100.v1(s0.velocity, s0.accel, dsM1); - - s1.velocity = v1; - - // also use max accels for the new state accels - s1.decel = -HIGH_ACCEL; - s1.accel = HIGH_ACCEL; - - // reduce velocity according to constraints - for (TimingConstraint constraint : m_constraints) { - s1.velocity = Math.min(s1.velocity, constraint.maxV(s1.state)); + ConstrainedState[] constrainedStates = new ConstrainedState[n]; + { + for (int i = 0; i < n; ++i) { + double arclength = samples[i].distanceCartesian(previousPose); + if (i > 0 && i < n - 1 && arclength < 1e-6) { + // the first distance is zero because of the weird loop structure. + // the last distance can be zero if the step size exactly divides the path + // length + throw new IllegalStateException("zero distance not allowed"); } - for (TimingConstraint constraint1 : m_constraints) { - s1.decel = Math.max(s1.decel, constraint1.maxDecel(s1.state, s1.velocity)); - s1.accel = Math.min(s1.accel, constraint1.maxAccel(s1.state, s1.velocity)); - } - - // motionless - if (Math.abs(dsM1) < EPSILON) { + distances[i] = arclength + previousDistance; + + constrainedStates[i] = new ConstrainedState(); + + // R2 translation distance between states + // not constant-twist arc + // not double-geodesic with rotation + // Just translation, so that the pathwise velocity matches + // the curvature in the state. + double arclength1 = samples[i].distanceCartesian(previousPose); + + // We may need to iterate to find the maximum end velocity and common + // acceleration, since acceleration limits may be a function of velocity. + while (true) { + // first try the previous state accel to get the new state velocity + double v1 = Math100.v1(s0.velocity, s0.accel, arclength1); + + constrainedStates[i].velocity = v1; + + // also use max accels for the new state accels + constrainedStates[i].decel = -HIGH_ACCEL; + constrainedStates[i].accel = HIGH_ACCEL; + + // reduce velocity according to constraints + for (TimingConstraint constraint : m_constraints) { + constrainedStates[i].velocity = Math.min(constrainedStates[i].velocity, + constraint.maxV(samples[i])); + } + + for (TimingConstraint constraint1 : m_constraints) { + constrainedStates[i].decel = Math.max(constrainedStates[i].decel, + constraint1.maxDecel(samples[i], constrainedStates[i].velocity)); + constrainedStates[i].accel = Math.min(constrainedStates[i].accel, + constraint1.maxAccel(samples[i], constrainedStates[i].velocity)); + } + + // motionless, which can happen at the end + if (Math.abs(arclength1) < EPSILON) { + break; + } + + double accel = Math100.accel(s0.velocity, constrainedStates[i].velocity, arclength1); + // in the failure case, max accel is zero so this always fails. + if (accel > constrainedStates[i].accel + EPSILON) { + // implied accel is too high because v1 is too high, perhaps because + // a0 was too high, try again with the (lower) constrained value + // + // if the constrained value is zero, this doesn't work at all. + + s0.accel = constrainedStates[i].accel; + continue; + } + if (accel > s0.decel + EPSILON) { + // set the previous state accel to whatever the constrained velocity implies + s0.accel = accel; + } break; } - double accel = Math100.accel(s0.velocity, s1.velocity, dsM1); - // in the failure case, max accel is zero so this always fails. - if (accel > s1.accel + EPSILON) { - // implied accel is too high because v1 is too high, perhaps because - // a0 was too high, try again with the (lower) constrained value - // - // if the constrained value is zero, this doesn't work at all. - - s0.accel = s1.accel; - continue; - } - if (accel > s0.decel + EPSILON) { - // set the previous state accel to whatever the constrained velocity implies - s0.accel = accel; - } - break; + s0 = constrainedStates[i]; + previousPose = samples[i]; + previousDistance = distances[i]; } - - s0 = s1; } - - - Pose2dWithMotion lastState = samples.get(samples.size() - 1); - // // Backwards pass // - // "successor" comes before in the backwards walk. start with the last state. - ConstrainedState endState = constrainedStates.get(constrainedStates.size() - 1); - ConstrainedState s1 = new ConstrainedState( - lastState, endState.distance); - s1.velocity = end_vel; - s1.decel = -HIGH_ACCEL; - s1.accel = HIGH_ACCEL; - - // work backwards through the states list - for (int i = constrainedStates.size() - 1; i >= 0; --i) { - ConstrainedState s01 = constrainedStates.get(i); - - // backwards (negative) distance from successor to initial state. - double ds = s01.distance - s1.distance; - if (ds > 0) { - // must be negative if we're walking backwards. - throw new IllegalStateException(); - } - - while (true) { - // s0 velocity can't be more than the accel implies - // so this is actually an estimate for v0 - // min a is negative, ds is negative, so v0 is faster than v1 - double v0 = Math100.v1(s1.velocity, s1.decel, ds); - - if (s01.velocity <= v0) { - // s0 v is slower than implied v0, which means - // that actual accel is larger than the min, so we're fine - // No new limits to impose. - break; + { + // "successor" comes before in the backwards walk. start with the last state. + double successorDistance = distances[n - 1]; + ConstrainedState s1 = new ConstrainedState(); + s1.velocity = end_vel; + s1.decel = -HIGH_ACCEL; + s1.accel = HIGH_ACCEL; + + // work backwards through the states list + for (int i = n - 1; i >= 0; --i) { + // backwards (negative) distance from successor to initial state. + double dq = distances[i] - successorDistance; + if (dq > 0) { + // must be negative if we're walking backwards. + throw new IllegalStateException(); } - // s0 v is too fast, turn it down to obey v1 min accel. - s01.velocity = v0; - for (TimingConstraint constraint : m_constraints) { - s01.decel = Math.max(s01.decel, constraint.maxDecel(s01.state, s01.velocity)); - s01.accel = Math.min(s01.accel, constraint.maxAccel(s01.state, s01.velocity)); - } - - // motionless - if (Math.abs(ds) < EPSILON) { + while (true) { + // s0 velocity can't be more than the accel implies + // so this is actually an estimate for v0 + // min a is negative, dq is negative, so v0 is faster than v1 + double v0 = Math100.v1(s1.velocity, s1.decel, dq); + + if (constrainedStates[i].velocity <= v0) { + // s0 v is slower than implied v0, which means + // that actual accel is larger than the min, so we're fine + // No new limits to impose. + break; + } + // s0 v is too fast, turn it down to obey v1 min accel. + constrainedStates[i].velocity = v0; + + for (TimingConstraint constraint : m_constraints) { + constrainedStates[i].decel = Math.max(constrainedStates[i].decel, + constraint.maxDecel(samples[i], constrainedStates[i].velocity)); + constrainedStates[i].accel = Math.min(constrainedStates[i].accel, + constraint.maxAccel(samples[i], constrainedStates[i].velocity)); + } + + // motionless, which can happen at the end + if (Math.abs(dq) < EPSILON) { + break; + } + + // implied accel using the constrained v0 + double accel = Math100.accel(s1.velocity, constrainedStates[i].velocity, dq); + if (accel < constrainedStates[i].decel - EPSILON) { + // accel is too low which implies that s1 accel is too low, try again + s1.decel = constrainedStates[i].decel; + continue; + } + // set final accel to the implied value + s1.decel = accel; break; } - // implied accel using the constrained v0 - double accel = Math100.accel(s1.velocity, s01.velocity, ds); - if (accel < s01.decel - EPSILON) { - // accel is too low which implies that s1 accel is too low, try again - s1.decel = s01.decel; - continue; - } - // set final accel to the implied value - s1.decel = accel; - break; + s1 = constrainedStates[i]; + successorDistance = distances[i]; } - - s1 = s01; } // @@ -239,31 +229,30 @@ public Trajectory100 timeParameterizeTrajectory( // last state accel is always zero, which might be wrong. // - List poses = new ArrayList<>(constrainedStates.size()); - double time = 0.0; - // distance along path - // for turn-in-place, - double distance = 0.0; - double v0 = 0.0; - - for (int i = 0; i < constrainedStates.size(); ++i) { - ConstrainedState state = constrainedStates.get(i); - double ds = state.distance - distance; - double v1 = state.velocity; - - double dt = 0.0; - if (i > 0) { - double prevAccel = Math100.accel(v0, v1, ds); - poses.get(i - 1).set_acceleration(prevAccel); - dt = dt(v0, v1, ds, prevAccel); - } - time += dt; - if (Double.isNaN(time) || Double.isInfinite(time)) { - throw new TimingException(); + List poses = new ArrayList<>(n); + { + double time = 0.0; + double distance = 0.0; + double v0 = 0.0; + + for (int i = 0; i < n; ++i) { + double dq = distances[i] - distance; + double v1 = constrainedStates[i].velocity; + + double dt = 0.0; + if (i > 0) { + double prevAccel = Math100.accel(v0, v1, dq); + poses.get(i - 1).set_acceleration(prevAccel); + dt = dt(v0, v1, dq, prevAccel); + } + time += dt; + if (Double.isNaN(time) || Double.isInfinite(time)) { + throw new TimingException(); + } + poses.add(new TimedState(samples[i], time, v1, 0)); + v0 = v1; + distance = distances[i]; } - poses.add(new TimedState(state.state, time, v1, 0)); - v0 = v1; - distance = state.distance; } return new Trajectory100(poses); @@ -271,24 +260,21 @@ public Trajectory100 timeParameterizeTrajectory( /** * If accelerating, find the time to go from v0 to v1. Otherwise find the time - * to go distance ds at speed v0. + * to go distance dq at speed v0. */ private static double dt( double v0, double v1, - double ds, + double dq, double accel) throws TimingException { if (Math.abs(accel) > EPSILON) { return (v1 - v0) / accel; } if (Math.abs(v0) > EPSILON) { - return ds / v0; + return dq / v0; } // not moving at all so dt is always zero return 0; // throw new TimingException(String.format("%f %f %f %f", v0, v1, ds, accel)); } - - public static class TimingException extends Exception { - } } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java index a29ed9738..c9017426b 100644 --- a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java +++ b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java @@ -81,7 +81,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { inst.startClient4("tag_finder24"); // wait for the NT thread - Thread.sleep(5); + Thread.sleep(100); assertTrue(inst.isConnected()); StructArrayTopic topic = inst.getStructArrayTopic( @@ -92,7 +92,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { (long) Takt.get() * 1000000); // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); inst.flush(); assertTrue(poseEstimate.isEmpty()); @@ -111,7 +111,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { (long) Takt.get() * 1000000); // wait for NT rate-limiting - Thread.sleep(100); + Thread.sleep(200); inst.flush(); localizer.update(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 7d6e6df0d..06a0b37b9 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -32,13 +32,13 @@ public class TrajectoryVelocityProfileTest implements Timeless { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); // A five-meter straight line. - public static final List WAYPOINTS = Arrays.asList( + public static final Pose2dWithMotion[] WAYPOINTS = new Pose2dWithMotion[] { new Pose2dWithMotion(WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( new Pose2d(2.5, 0, new Rotation2d(0)), 0, 1.2), 0, 0), new Pose2dWithMotion(WaypointSE2.irrotational( - new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0)); + new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0) }; // No rotation. public static final List HEADINGS = List.of( @@ -101,7 +101,7 @@ void testAuto() throws TimingException { List constraints = timing.testAuto(logger); ScheduleGenerator u = new ScheduleGenerator(constraints); Trajectory100 traj = u.timeParameterizeTrajectory( - new Path100(WAYPOINTS), 0.5, 0, 0); + new Path100(Arrays.asList(WAYPOINTS)), 0.5, 0, 0); // Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); print(traj); } From da0ddbe5c85fd71b5aa4900acaaf3d22ca225f7b Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 25 Dec 2025 10:17:44 -0800 Subject: [PATCH 39/42] remove ConstrainedState --- .../lib/geometry/Pose2dWithMotion.java | 3 +- .../trajectory/timing/ConstrainedState.java | 13 --- .../trajectory/timing/ScheduleGenerator.java | 98 +++++++++---------- 3 files changed, 47 insertions(+), 67 deletions(-) delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index d174b8ade..41fbfd40b 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -60,7 +60,8 @@ public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) { /** * R2 (xy) planar distance only (IGNORES ROTATION) so that planar - * velocity and curvature works correctly. + * velocity and curvature works correctly. Not the twist arclength. + * Not the double-geodesic L2 thing. Just XY translation hypot. */ public double distanceCartesian(Pose2dWithMotion other) { return Metrics.translationalDistance(m_waypoint.pose(), other.m_waypoint.pose()); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java deleted file mode 100644 index 5bba9874e..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ /dev/null @@ -1,13 +0,0 @@ -package org.team100.lib.trajectory.timing; - -class ConstrainedState { - public double velocity; - public double decel; - public double accel; - - public ConstrainedState() { - velocity = 100; - decel = -100; - accel = 100; - } -} \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 094e71204..b09853ecd 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -58,18 +58,19 @@ public Trajectory100 timeParameterizeTrajectory( throw new IllegalArgumentException("must have at least three samples"); double distances[] = new double[n]; + double velocities[] = new double[n]; + double decels[] = new double[n]; + double accels[] = new double[n]; // note below we look again at this sample. I think this exists // only to supply the start velocity. Pose2dWithMotion previousPose = samples[0]; - ConstrainedState s0 = new ConstrainedState(); distances[0] = 0; double previousDistance = 0; - s0.velocity = start_vel; - s0.decel = -HIGH_ACCEL; - s0.accel = HIGH_ACCEL; - - // + double previousVelocity = start_vel; + double previousDecel = -HIGH_ACCEL; + double previousAccel = HIGH_ACCEL; + // Forward pass. // // We look at pairs of consecutive states, where the start state has already @@ -79,12 +80,10 @@ public Trajectory100 timeParameterizeTrajectory( // velocity. If there is no admissible end velocity or acceleration, we set the // end velocity to the state's maximum allowed velocity and will repair the // acceleration during the backward pass (by slowing down the predecessor). - // - // work forward through the samples - ConstrainedState[] constrainedStates = new ConstrainedState[n]; { for (int i = 0; i < n; ++i) { + double arclength = samples[i].distanceCartesian(previousPose); if (i > 0 && i < n - 1 && arclength < 1e-6) { // the first distance is zero because of the weird loop structure. @@ -94,81 +93,72 @@ public Trajectory100 timeParameterizeTrajectory( } distances[i] = arclength + previousDistance; - - constrainedStates[i] = new ConstrainedState(); - - // R2 translation distance between states - // not constant-twist arc - // not double-geodesic with rotation - // Just translation, so that the pathwise velocity matches - // the curvature in the state. - double arclength1 = samples[i].distanceCartesian(previousPose); + velocities[i] = 100; // We may need to iterate to find the maximum end velocity and common // acceleration, since acceleration limits may be a function of velocity. while (true) { // first try the previous state accel to get the new state velocity - double v1 = Math100.v1(s0.velocity, s0.accel, arclength1); + double v1 = Math100.v1(previousVelocity, previousAccel, arclength); - constrainedStates[i].velocity = v1; + velocities[i] = v1; // also use max accels for the new state accels - constrainedStates[i].decel = -HIGH_ACCEL; - constrainedStates[i].accel = HIGH_ACCEL; + decels[i] = -HIGH_ACCEL; + accels[i] = HIGH_ACCEL; // reduce velocity according to constraints for (TimingConstraint constraint : m_constraints) { - constrainedStates[i].velocity = Math.min(constrainedStates[i].velocity, + velocities[i] = Math.min(velocities[i], constraint.maxV(samples[i])); } for (TimingConstraint constraint1 : m_constraints) { - constrainedStates[i].decel = Math.max(constrainedStates[i].decel, - constraint1.maxDecel(samples[i], constrainedStates[i].velocity)); - constrainedStates[i].accel = Math.min(constrainedStates[i].accel, - constraint1.maxAccel(samples[i], constrainedStates[i].velocity)); + decels[i] = Math.max(decels[i], constraint1.maxDecel(samples[i], velocities[i])); + accels[i] = Math.min(accels[i], constraint1.maxAccel(samples[i], velocities[i])); + } // motionless, which can happen at the end - if (Math.abs(arclength1) < EPSILON) { + if (Math.abs(arclength) < EPSILON) { break; } - double accel = Math100.accel(s0.velocity, constrainedStates[i].velocity, arclength1); + double accel = Math100.accel(previousVelocity, velocities[i], arclength); // in the failure case, max accel is zero so this always fails. - if (accel > constrainedStates[i].accel + EPSILON) { + if (accel > accels[i] + EPSILON) { // implied accel is too high because v1 is too high, perhaps because // a0 was too high, try again with the (lower) constrained value // // if the constrained value is zero, this doesn't work at all. - s0.accel = constrainedStates[i].accel; + previousAccel = accels[i]; continue; } - if (accel > s0.decel + EPSILON) { + if (accel > previousDecel + EPSILON) { // set the previous state accel to whatever the constrained velocity implies - s0.accel = accel; + previousAccel = accel; } break; } - s0 = constrainedStates[i]; previousPose = samples[i]; previousDistance = distances[i]; + previousVelocity = velocities[i]; + previousDecel = decels[i]; + previousAccel = accels[i]; } } // // Backwards pass // - { // "successor" comes before in the backwards walk. start with the last state. double successorDistance = distances[n - 1]; - ConstrainedState s1 = new ConstrainedState(); - s1.velocity = end_vel; - s1.decel = -HIGH_ACCEL; - s1.accel = HIGH_ACCEL; + double successorVelocity = end_vel; + double successorDecel = -HIGH_ACCEL; + double successorAccel = HIGH_ACCEL; // work backwards through the states list for (int i = n - 1; i >= 0; --i) { @@ -183,22 +173,20 @@ public Trajectory100 timeParameterizeTrajectory( // s0 velocity can't be more than the accel implies // so this is actually an estimate for v0 // min a is negative, dq is negative, so v0 is faster than v1 - double v0 = Math100.v1(s1.velocity, s1.decel, dq); + double v0 = Math100.v1(successorVelocity, successorDecel, dq); - if (constrainedStates[i].velocity <= v0) { + if (velocities[i] <= v0) { // s0 v is slower than implied v0, which means // that actual accel is larger than the min, so we're fine // No new limits to impose. break; } // s0 v is too fast, turn it down to obey v1 min accel. - constrainedStates[i].velocity = v0; + velocities[i] = v0; for (TimingConstraint constraint : m_constraints) { - constrainedStates[i].decel = Math.max(constrainedStates[i].decel, - constraint.maxDecel(samples[i], constrainedStates[i].velocity)); - constrainedStates[i].accel = Math.min(constrainedStates[i].accel, - constraint.maxAccel(samples[i], constrainedStates[i].velocity)); + decels[i] = Math.max(decels[i], constraint.maxDecel(samples[i], velocities[i])); + accels[i] = Math.min(accels[i], constraint.maxAccel(samples[i], velocities[i])); } // motionless, which can happen at the end @@ -207,19 +195,21 @@ public Trajectory100 timeParameterizeTrajectory( } // implied accel using the constrained v0 - double accel = Math100.accel(s1.velocity, constrainedStates[i].velocity, dq); - if (accel < constrainedStates[i].decel - EPSILON) { + double accel = Math100.accel(successorVelocity, velocities[i], dq); + if (accel < decels[i] - EPSILON) { // accel is too low which implies that s1 accel is too low, try again - s1.decel = constrainedStates[i].decel; + successorDecel = decels[i]; continue; } // set final accel to the implied value - s1.decel = accel; + successorDecel = accel; break; } - s1 = constrainedStates[i]; successorDistance = distances[i]; + successorVelocity = velocities[i]; + successorDecel = decels[i]; + successorAccel = accels[i]; } } @@ -228,16 +218,18 @@ public Trajectory100 timeParameterizeTrajectory( // // last state accel is always zero, which might be wrong. // - List poses = new ArrayList<>(n); { double time = 0.0; double distance = 0.0; + // + // TODO i think this should be start_vel + // double v0 = 0.0; for (int i = 0; i < n; ++i) { double dq = distances[i] - distance; - double v1 = constrainedStates[i].velocity; + double v1 = velocities[i]; double dt = 0.0; if (i > 0) { From f5df326482bd5c3042dc9fb0b31e120873cd5ddb Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 25 Dec 2025 10:23:26 -0800 Subject: [PATCH 40/42] cleaning --- .../trajectory/timing/ScheduleGenerator.java | 30 +++++++------------ 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index b09853ecd..7e4f3ac31 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -62,15 +62,6 @@ public Trajectory100 timeParameterizeTrajectory( double decels[] = new double[n]; double accels[] = new double[n]; - // note below we look again at this sample. I think this exists - // only to supply the start velocity. - Pose2dWithMotion previousPose = samples[0]; - distances[0] = 0; - double previousDistance = 0; - double previousVelocity = start_vel; - double previousDecel = -HIGH_ACCEL; - double previousAccel = HIGH_ACCEL; - // Forward pass. // // We look at pairs of consecutive states, where the start state has already @@ -82,6 +73,11 @@ public Trajectory100 timeParameterizeTrajectory( // acceleration during the backward pass (by slowing down the predecessor). { + Pose2dWithMotion previousPose = samples[0]; + double previousDistance = 0; + double previousVelocity = start_vel; + double previousDecel = -HIGH_ACCEL; + double previousAccel = HIGH_ACCEL; for (int i = 0; i < n; ++i) { double arclength = samples[i].distanceCartesian(previousPose); @@ -109,8 +105,7 @@ public Trajectory100 timeParameterizeTrajectory( // reduce velocity according to constraints for (TimingConstraint constraint : m_constraints) { - velocities[i] = Math.min(velocities[i], - constraint.maxV(samples[i])); + velocities[i] = Math.min(velocities[i], constraint.maxV(samples[i])); } for (TimingConstraint constraint1 : m_constraints) { @@ -222,27 +217,22 @@ public Trajectory100 timeParameterizeTrajectory( { double time = 0.0; double distance = 0.0; - // - // TODO i think this should be start_vel - // double v0 = 0.0; for (int i = 0; i < n; ++i) { double dq = distances[i] - distance; - double v1 = velocities[i]; - double dt = 0.0; if (i > 0) { - double prevAccel = Math100.accel(v0, v1, dq); + double prevAccel = Math100.accel(v0, velocities[i], dq); poses.get(i - 1).set_acceleration(prevAccel); - dt = dt(v0, v1, dq, prevAccel); + dt = dt(v0, velocities[i], dq, prevAccel); } time += dt; if (Double.isNaN(time) || Double.isInfinite(time)) { throw new TimingException(); } - poses.add(new TimedState(samples[i], time, v1, 0)); - v0 = v1; + poses.add(new TimedState(samples[i], time, velocities[i], 0)); + v0 = velocities[i]; distance = distances[i]; } } From 91feb5c17efc96f852c49e56a095bd46d4fd448b Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 25 Dec 2025 21:59:52 -0800 Subject: [PATCH 41/42] bleah --- .../org/team100/lib/geometry/Metrics.java | 6 +- .../lib/geometry/Pose2dWithMotion.java | 4 +- .../kinodynamics/SwerveKinodynamics.java | 3 + .../team100/lib/trajectory/Trajectory100.java | 17 +++++ .../timing/CapsizeAccelerationConstraint.java | 29 +++++--- .../trajectory/timing/ScheduleGenerator.java | 67 ++++++++++--------- .../java/org/team100/lib/util/Math100.java | 26 +++---- .../lib/trajectory/TrajectoryPlannerTest.java | 32 ++++++++- 8 files changed, 125 insertions(+), 59 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/geometry/Metrics.java b/lib/src/main/java/org/team100/lib/geometry/Metrics.java index 2b7e7d050..2a71d889b 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Metrics.java +++ b/lib/src/main/java/org/team100/lib/geometry/Metrics.java @@ -16,6 +16,8 @@ public class Metrics { /** * The distance between translational components. Ignores rotation entirely. + * + * Always non-negative. */ public static double translationalDistance(Pose2d a, Pose2d b) { return a.getTranslation().getDistance(b.getTranslation()); @@ -71,7 +73,7 @@ public static double translationalNorm(ChassisSpeeds a) { * Don't use it for anything where you compare it to xy planar distances or * velocities. */ - public static double l2Norm(Twist2d a) { + public static double l2Norm(Twist2d a) { return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); } @@ -89,7 +91,7 @@ public static double l2Norm(Twist3d t) { return v.norm(); } - /** + /** * Double-geodesic combines the angular distance with the translational * distance, weighting 1 radian equal to 1 meter. * diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java index 41fbfd40b..e5bea460d 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -60,8 +60,10 @@ public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) { /** * R2 (xy) planar distance only (IGNORES ROTATION) so that planar - * velocity and curvature works correctly. Not the twist arclength. + * velocity and curvature works correctly. Not the twist arclength. * Not the double-geodesic L2 thing. Just XY translation hypot. + * + * Always non-negative. */ public double distanceCartesian(Pose2dWithMotion other) { return Metrics.translationalDistance(m_waypoint.pose(), other.m_waypoint.pose()); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java index 27eb4e885..29c96c00b 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java @@ -38,6 +38,7 @@ public class SwerveKinodynamics { private final double m_backtrack; private final double m_wheelbase; private final double m_frontoffset; + // TODO: make this adjustable (e.g. by elevator height) private final double m_vcg; /** Diagonal distance from center to wheel. */ private final double m_radius; @@ -187,6 +188,8 @@ public double getMaxAngleAccelRad_S2() { /** * Acceleration which will tip the robot onto two wheels, m/s^2. Computed from * vertical center of gravity and frame size. + * + * TODO: make this adjustable (e.g. by elevator height) */ public double getMaxCapsizeAccelM_S2() { return 9.8 * (m_fulcrum / m_vcg); diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java index 74b6fa346..1c7f59a77 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java @@ -3,8 +3,12 @@ import java.util.ArrayList; import java.util.List; +import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.trajectory.timing.TimedState; +import edu.wpi.first.math.geometry.Pose2d; + /** * A list of timed poses. */ @@ -96,4 +100,17 @@ public String toString() { } return builder.toString(); } + + /** For cutting-and-pasting into a spreadsheet */ + public void dump() { + System.out.println("i, t, v, a, k, x, y"); + for (int i = 0; i < length(); ++i) { + TimedState ts = getPoint(i); + Pose2dWithMotion pwm = ts.state(); + WaypointSE2 w = pwm.getPose(); + Pose2d p = w.pose(); + System.out.printf("%d, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + i, ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(), p.getX(), p.getY()); + } + } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java index ff195017b..ad5bb35a9 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java @@ -9,8 +9,10 @@ * Velocity limit based on curvature and the capsize limit (scaled). */ public class CapsizeAccelerationConstraint implements TimingConstraint { - private final SwerveKinodynamics m_limits; + private static final boolean DEBUG = true; private final Mutable m_scale; + private final double m_maxCentripetalAccel; + private final double m_maxDecel; /** * Use the factory. @@ -26,8 +28,16 @@ public CapsizeAccelerationConstraint( SwerveKinodynamics limits, double scale) { LoggerFactory log = parent.type(this); - m_limits = limits; m_scale = new Mutable(log, "scale", scale); + m_maxCentripetalAccel = limits.getMaxCapsizeAccelM_S2(); + m_maxDecel = -limits.getMaxDriveDecelerationM_S2(); + } + + public CapsizeAccelerationConstraint(LoggerFactory parent, double centripetal, double decel) { + LoggerFactory log = parent.type(this); + m_scale = new Mutable(log, "scale", 1); + m_maxCentripetalAccel = centripetal; + m_maxDecel = -decel; } /** @@ -39,17 +49,17 @@ public CapsizeAccelerationConstraint( */ @Override public double maxV(final Pose2dWithMotion state) { - double mMaxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); double radius = 1 / state.getCurvatureRad_M(); // abs is used here to make sure sqrt is happy. - return Math.sqrt(Math.abs(mMaxCentripetalAccel * radius)); + return Math.sqrt(Math.abs(m_maxCentripetalAccel * m_scale.getAsDouble() * radius)); } @Override public double maxAccel(Pose2dWithMotion state, double velocity) { double alongsq = alongSq(state, velocity); if (alongsq < 0) { - // too fast for the curvature, can't speed up + if (DEBUG) + System.out.println("too fast for the curvature, can't speed up"); return 0; } return Math.sqrt(alongsq); @@ -59,8 +69,9 @@ public double maxAccel(Pose2dWithMotion state, double velocity) { public double maxDecel(Pose2dWithMotion state, double velocity) { double alongsq = alongSq(state, velocity); if (alongsq < 0) { - // too fast for the curvature, slowing down is ok - return -m_limits.getMaxDriveDecelerationM_S2() * m_scale.getAsDouble(); + if (DEBUG) + System.out.println("too fast for the curvature, slowing down is ok"); + return m_maxDecel * m_scale.getAsDouble(); } return -Math.sqrt(alongsq); } @@ -77,9 +88,9 @@ public double maxDecel(Pose2dWithMotion state, double velocity) { * along = sqrt(total^2 - v^4/r^2) */ private double alongSq(Pose2dWithMotion state, double velocity) { - double maxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); double radius = 1 / state.getCurvatureRad_M(); double actualCentripetalAccel = velocity * velocity / radius; - return maxCentripetalAccel * maxCentripetalAccel - actualCentripetalAccel * actualCentripetalAccel; + return m_maxCentripetalAccel * m_scale.getAsDouble() * m_maxCentripetalAccel * m_scale.getAsDouble() + - actualCentripetalAccel * actualCentripetalAccel; } } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index 7e4f3ac31..a80d3f50b 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -16,7 +16,7 @@ public class ScheduleGenerator { public static class TimingException extends Exception { } - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; private static final double EPSILON = 1e-6; /** this is the default, in order to make the constraints set the actual */ private static final double HIGH_ACCEL = 1000; @@ -57,21 +57,13 @@ public Trajectory100 timeParameterizeTrajectory( if (n < 3) throw new IllegalArgumentException("must have at least three samples"); + // distance monotonically increases double distances[] = new double[n]; double velocities[] = new double[n]; double decels[] = new double[n]; double accels[] = new double[n]; // Forward pass. - // - // We look at pairs of consecutive states, where the start state has already - // been velocity parameterized (though we may adjust the velocity downwards - // during the backwards pass). We wish to find an acceleration that is - // admissible at both the start and end state, as well as an admissible end - // velocity. If there is no admissible end velocity or acceleration, we set the - // end velocity to the state's maximum allowed velocity and will repair the - // acceleration during the backward pass (by slowing down the predecessor). - { Pose2dWithMotion previousPose = samples[0]; double previousDistance = 0; @@ -79,7 +71,7 @@ public Trajectory100 timeParameterizeTrajectory( double previousDecel = -HIGH_ACCEL; double previousAccel = HIGH_ACCEL; for (int i = 0; i < n; ++i) { - + // arclength is never negative but can be zero. double arclength = samples[i].distanceCartesian(previousPose); if (i > 0 && i < n - 1 && arclength < 1e-6) { // the first distance is zero because of the weird loop structure. @@ -90,6 +82,8 @@ public Trajectory100 timeParameterizeTrajectory( distances[i] = arclength + previousDistance; velocities[i] = 100; + decels[i] = -HIGH_ACCEL; + accels[i] = HIGH_ACCEL; // We may need to iterate to find the maximum end velocity and common // acceleration, since acceleration limits may be a function of velocity. @@ -100,18 +94,27 @@ public Trajectory100 timeParameterizeTrajectory( velocities[i] = v1; // also use max accels for the new state accels - decels[i] = -HIGH_ACCEL; - accels[i] = HIGH_ACCEL; + // decels[i] = -HIGH_ACCEL; + // accels[i] = HIGH_ACCEL; // reduce velocity according to constraints for (TimingConstraint constraint : m_constraints) { - velocities[i] = Math.min(velocities[i], constraint.maxV(samples[i])); + double constraintV = constraint.maxV(samples[i]); + if (DEBUG) { + System.out.printf("i %d constraint %s v %f\n", + i, constraint.getClass().getSimpleName(), constraintV); + } + velocities[i] = Math.min(velocities[i], constraintV); } for (TimingConstraint constraint1 : m_constraints) { - decels[i] = Math.max(decels[i], constraint1.maxDecel(samples[i], velocities[i])); + // removing this breaks TrajectoryPlannerTest.test2d. + // which is an s-shaped thing with variable curvature. + // the passing test is definitely wrong, and the failing + // test is wrong too. + // decels[i] = Math.max(decels[i], constraint1.maxDecel(samples[i], + // velocities[i])); accels[i] = Math.min(accels[i], constraint1.maxAccel(samples[i], velocities[i])); - } // motionless, which can happen at the end @@ -159,16 +162,14 @@ public Trajectory100 timeParameterizeTrajectory( for (int i = n - 1; i >= 0; --i) { // backwards (negative) distance from successor to initial state. double dq = distances[i] - successorDistance; - if (dq > 0) { - // must be negative if we're walking backwards. - throw new IllegalStateException(); - } while (true) { // s0 velocity can't be more than the accel implies // so this is actually an estimate for v0 // min a is negative, dq is negative, so v0 is faster than v1 double v0 = Math100.v1(successorVelocity, successorDecel, dq); + if (DEBUG) + System.out.printf("i %d v0 %f\n", i, v0); if (velocities[i] <= v0) { // s0 v is slower than implied v0, which means @@ -176,7 +177,9 @@ public Trajectory100 timeParameterizeTrajectory( // No new limits to impose. break; } - // s0 v is too fast, turn it down to obey v1 min accel. + // v is too fast + if (DEBUG) + System.out.printf("v too fast i %d v_i %f v0 %f\n", i, velocities[i], v0); velocities[i] = v0; for (TimingConstraint constraint : m_constraints) { @@ -215,25 +218,25 @@ public Trajectory100 timeParameterizeTrajectory( // List poses = new ArrayList<>(n); { - double time = 0.0; - double distance = 0.0; - double v0 = 0.0; + double runningTotalTime = 0.0; + double previousDistance = 0.0; + double previousVelocity = 0.0; for (int i = 0; i < n; ++i) { - double dq = distances[i] - distance; + double dq = distances[i] - previousDistance; double dt = 0.0; if (i > 0) { - double prevAccel = Math100.accel(v0, velocities[i], dq); + double prevAccel = Math100.accel(previousVelocity, velocities[i], dq); poses.get(i - 1).set_acceleration(prevAccel); - dt = dt(v0, velocities[i], dq, prevAccel); + dt = dt(previousVelocity, velocities[i], dq, prevAccel); } - time += dt; - if (Double.isNaN(time) || Double.isInfinite(time)) { + runningTotalTime += dt; + if (Double.isNaN(runningTotalTime) || Double.isInfinite(runningTotalTime)) { throw new TimingException(); } - poses.add(new TimedState(samples[i], time, velocities[i], 0)); - v0 = velocities[i]; - distance = distances[i]; + poses.add(new TimedState(samples[i], runningTotalTime, velocities[i], 0)); + previousVelocity = velocities[i]; + previousDistance = distances[i]; } } diff --git a/lib/src/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index 85db7b215..f2b7fbaab 100644 --- a/lib/src/main/java/org/team100/lib/util/Math100.java +++ b/lib/src/main/java/org/team100/lib/util/Math100.java @@ -97,32 +97,32 @@ public static double throwIfOutOfRange(double x, double minX, double maxX) { /** * Return acceleration implied by the change in velocity (v0 to v1) - * over the distance, ds. + * over the distance, dx. * - * a = (v1^2 - v0^2) / 2ds + * a = (v1^2 - v0^2) / 2dx * - * note ds can be negative, which implies negative time. + * note dx can be negative, which implies negative time. * * @param v0 initial velocity * @param v1 final velocity - * @param ds distance + * @param dx distance */ - public static double accel(double v0, double v1, double ds) { - if (Math.abs(ds) < 1e-6) { + public static double accel(double v0, double v1, double dx) { + if (Math.abs(dx) < 1e-6) { // prevent division by zero return 0; } /* * a = dv/dt - * v = ds/dt - * dt = ds/v - * a = v dv/ds - * a = v (v1-v0)/ds + * v = dx/dt + * dt = dx/v + * a = v dv/dx + * a = v (v1-v0)/dx * v = (v0+v1)/2 - * a = (v0+v1)(v1-v0)/2ds - * a = (v1^2 - v0^2)/2ds + * a = (v0+v1)(v1-v0)/2dx + * a = (v1^2 - v0^2)/2dx */ - return (v1 * v1 - v0 * v0) / (2.0 * ds); + return (v1 * v1 - v0 * v0) / (2.0 * dx); } /** diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 868742769..6e00d1a33 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -170,12 +170,40 @@ void testMovingToRest() { @Test void test2d() { - SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); - List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); + List constraints = List.of( + new ConstantConstraint(logger, 1, 1), + new CapsizeAccelerationConstraint(logger, 1, 1)); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 1, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); + traj.dump(); + assertEquals(3.337, traj.duration(), DELTA); + } + + // this should result in constant velocity, slowing for the curve, and then + // accelerating for the exit. + // + // but it doesn't, so there's something wrong. + // + // in both the "pass" and "fail" cases (with decel in the forward pass), this + // produces too-low accels in the straights, and too much accel variation in + // the curve. + @Test + void test2d2() { + List waypoints = List.of( + new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1), + new WaypointSE2(new Pose2d(1, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1), + new WaypointSE2(new Pose2d(2, 1, new Rotation2d()), new DirectionSE2(0, 1, 0), 1), + new WaypointSE2(new Pose2d(2, 2, new Rotation2d()), new DirectionSE2(0, 1, 0), 1)); + + List constraints = List.of( + new ConstantConstraint(logger, 1, 1), + new CapsizeAccelerationConstraint(logger, 0.5, 1)); + TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + + Trajectory100 traj = planner.generateTrajectory(waypoints, 1, 1); + traj.dump(); assertEquals(3.337, traj.duration(), DELTA); } From 4790bdeb87d35b086ac052244ef15e4052b6e0fe Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 26 Dec 2025 12:10:50 -0800 Subject: [PATCH 42/42] finish overhauling ScheduleGenerator --- ...veToPoseWithTrajectoryAndExitVelocity.java | 3 +- .../java/org/team100/lib/trajectory/README.md | 67 +-- .../lib/trajectory/TrajectoryPlanner.java | 1 + .../team100/lib/trajectory/path/Path100.java | 57 ++- .../lib/trajectory/path/PathFactory.java | 25 +- .../timing/CapsizeAccelerationConstraint.java | 14 +- .../lib/trajectory/timing/DirectSchedule.java | 96 ---- .../trajectory/timing/MultipleShooting.java | 85 ---- .../team100/lib/trajectory/timing/README.md | 10 +- .../trajectory/timing/ScheduleGenerator.java | 358 +++++++------ .../lib/trajectory/timing/Scheduler100.java | 166 ------- .../lib/trajectory/timing/TimedState.java | 12 +- .../java/org/team100/lib/util/Math100.java | 61 ++- .../r3/ReferenceControllerR3Test.java | 5 +- .../lib/trajectory/ParameterizationTest.java | 6 +- .../lib/trajectory/Trajectory100Test.java | 23 +- .../lib/trajectory/TrajectoryPlannerTest.java | 69 ++- .../lib/trajectory/path/Path100Test.java | 2 +- .../lib/trajectory/path/PathFactoryTest.java | 15 +- .../path/spline/HolonomicSplineTest.java | 40 +- .../trajectory/path/spline/SplineR1Test.java | 3 +- .../trajectory/timing/DirectScheduleTest.java | 469 ------------------ .../timing/MultipleShootingTest.java | 34 -- .../timing/ScheduleGeneratorTest.java | 27 +- .../lib/trajectory/timing/SchedulerTest.java | 86 ---- .../timing/TrajectoryVelocityProfileTest.java | 2 + 26 files changed, 405 insertions(+), 1331 deletions(-) delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java delete mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java delete mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java delete mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java index bc0a34f95..8408167f8 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java @@ -4,9 +4,8 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.geometry.DirectionR2; import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; diff --git a/lib/src/main/java/org/team100/lib/trajectory/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md index 5041bd050..3d72d2784 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/README.md @@ -11,7 +11,7 @@ The process of constructing a trajectory has three stages: 1. Construct a list of splines. Each spline joins two points smoothly. The spline parameter has no physical meaning, it's just 0.0 on one end and 1.0 on the other. See the `lib.trajectory.path.spline` package. -2. Construct a list of points along the spline, such that straight lines connecting the points ("secant lines") don't deviate too much from the true spline. (This uses recursive bisection.) These points will be close together where the curvature is high, and far apart along straighter sections of the spline. The list is created by `PathFactory`, producing `Path100`, which integrates along the list to find the distance. See the `lib.trajectory.path` package. +2. Construct a list of points along the spline, such that straight lines connecting the points ("secant lines") don't deviate too much from the true spline, and aren't too far apart from each other. (This uses recursive bisection.) These points will be close together where the curvature is high, and far apart along straighter sections of the spline. The list is created by `PathFactory`, producing `Path100`, which integrates along the list to find the distance. See the `lib.trajectory.path` package. 3. Construct a list of points interpolated along the secant lines, such that the points aren't too far apart. @@ -19,67 +19,4 @@ The process of constructing a trajectory has three stages: To use a trajectory, you `sample()` it, with time (in seconds) as the parameter. The resulting `TimedState` is interpolated between from the list above. -If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains, it will work well enough to set the course and heading to be the same at each waypoint. - -## Math - -The process above is confusing. Here's what it should do instead. - -1. make a spline based on the endpoints -2. construct a list of *spline parameter values* that satisfy the secant constraint -3. construct a discrete mapping from those spline parameters to time -4. at runtime, interpolate - -there could be some caching of the spline values to save some multiplications. - -the key is to not keep so many copies of the pose data around; there's no reason for it -since we don't do optimization anymore. - -The essential math here is as follows. - -The spline position is $q(s)$ for the parameter $s$ - -The velocity with respect to $s$ is $q'(s) = dq/ds$ - -The accleration with respect to $s$ is $q''(s) = d^2q/ds^2$ - -So we're constructing a function for $s(t)$ and its time derivatives, - -$\dot{s} = \dfrac{ds}{dt}$ - -$\ddot{s} = \dfrac{d^2s}{dt^2}$ - -Position is just the composite: -$x(t) = q(t) = q(s(t))$ - -so using the chain rule: -$v(t) = \dot{q}(t) = \dfrac{dq}{ds} \dfrac{ds}{dt} = q'(s(t))\dot{s}$ - -and using the product rule: -$a(t) = \ddot{q}(t) = q''(s)\dot{s}^2 + q'(s)\ddot{s}$ - -## Constructing $s(t)$ - -The way it works now, the last step in `timeParameterizeTrajectory` is to integrate through the -constrained states. - -* Each state has a "total distance so far" number; this is used to find the length of the previous segment. -* each state has a velocity number (real velocity wrt time). -* the acceleration during the previous segment is calculated based on the difference in velocity and the length. this acceleration is attached to the *previous* state, i.e. the state velocity is instantaneous but the acceleration applies to the entire following segment. -* The duration of the previous segment is calculated based on the difference in velocities and the derived acceleration above -* segment duration is cumulated -* each state is annotated with the cumulative time, the endpoint velocity, and zero for the acceleration (it is filled in on the next loop). - -## Sampling - -at runtime, the `TimedState` list is sampled by time, which means finding floor and ceiling -states and interpolating. The interpolant is the fraction of time between states, which -is used to derive the fraction of distance between states (i.e. along the secant line), -which is used as the interpolant between poses. Note the acceleration is not interpolated -since it is constant within a segment. - -Is caching states and interpolating faster than computing the spline on the fly? - -No! see `Trajectory100Test.testSamplePerformance` which shows that it's about the same, -around 200 ns per sample. The budget is 20000000 ns, so the sampling time is -one millionth of the budget. \ No newline at end of file +If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains, it will work well enough to set the course and heading to be the same at each waypoint. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index cdff9560d..8d1253c0d 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -225,6 +225,7 @@ public Trajectory100 generateTrajectory( // Create a path from splines. Path100 path = PathFactory.pathFromWaypoints( waypoints, + m_trajectoryStep, m_splineTolerance, m_splineTolerance, m_splineRotationTolerance); diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 18280325c..f16dc9fa6 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java @@ -6,7 +6,6 @@ import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.trajectory.timing.ScheduleGenerator; -import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; import edu.wpi.first.math.geometry.Twist2d; @@ -16,6 +15,7 @@ * There's no timing information here. For that, see Trajectory100. */ public class Path100 { + private static final boolean DEBUG = false; // if an interpolated point is more than this far from an endpoint, // it indicates the endpoints are too far apart, including too far apart // in rotation, which is an aspect of the path scheduling that the @@ -99,25 +99,49 @@ public Pose2dWithMotion sample(double distance) throws ScheduleGenerator.TimingE Twist2d t1 = p1.getPose().course().minus(lerp.getPose().course()); double l1 = Metrics.l2Norm(t1); if (Math.max(l0, l1) > INTERPOLATION_LIMIT) - throw new IllegalStateException( - String.format( - "Interpolated value too far away, p0=%s, p1=%s, t0=%s t1=%s. This usually indicates a sharp corner in the path, which is not allowed.", - p0, p1, t0, t1)); + System.out.printf( + "WARNING! Interpolated value too far away, p0=%s, p1=%s, t0=%s t1=%s. This usually indicates a sharp corner in the path, which is not allowed.", + p0, p1, t0, t1); return lerp; } } throw new ScheduleGenerator.TimingException(); } + /** Just returns the list of points with no further sampling. */ + public Pose2dWithMotion[] resample() throws ScheduleGenerator.TimingException { + return m_points.toArray(Pose2dWithMotion[]::new); + } + + @Override + public String toString() { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < length(); ++i) { + builder.append(i); + builder.append(": "); + builder.append(getPoint(i)); + builder.append(System.lineSeparator()); + } + return builder.toString(); + } + + //////////////////////////////////////////////////////////////////// + /// + /// DANGER ZONE + /// + /// Don't use anything here unless you know what you're doing. + /// + /** - * Samples the entire path evenly by distance. + * Samples the entire path evenly by distance. Since the spline parameterizer + * now contains a pathwise distance limit, you shouldn't need this anymore. */ - public Pose2dWithMotion[] resample(double step) throws ScheduleGenerator.TimingException { + Pose2dWithMotion[] resample(double step) throws ScheduleGenerator.TimingException { double maxDistance = getMaxDistance(); if (maxDistance == 0) throw new IllegalArgumentException("max distance must be greater than zero"); int num_states = (int) Math.ceil(maxDistance / step) + 1; - if (ScheduleGenerator.DEBUG) + if (DEBUG) System.out.printf("resample max distance %f step %f num states %d f %f\n", maxDistance, step, num_states, maxDistance / step); Pose2dWithMotion[] samples = new Pose2dWithMotion[num_states]; @@ -126,23 +150,10 @@ public Pose2dWithMotion[] resample(double step) throws ScheduleGenerator.TimingE // the values here are just interpolated from the original values. double d = Math.min(i * step, maxDistance); Pose2dWithMotion state = sample(d); - if (ScheduleGenerator.DEBUG) + if (DEBUG) System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state); - samples[i] =state; + samples[i] = state; } return samples; } - - @Override - public String toString() { - StringBuilder builder = new StringBuilder(); - for (int i = 0; i < length(); ++i) { - builder.append(i); - builder.append(": "); - builder.append(getPoint(i)); - builder.append(System.lineSeparator()); - } - return builder.toString(); - } - } diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java index d6be39471..9dce4431e 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java @@ -18,6 +18,7 @@ public class PathFactory { public static Path100 pathFromWaypoints( List waypoints, + double maxNorm, double maxDx, double maxDy, double maxDTheta) { @@ -25,7 +26,7 @@ public static Path100 pathFromWaypoints( for (int i = 1; i < waypoints.size(); ++i) { splines.add(new HolonomicSpline(waypoints.get(i - 1), waypoints.get(i))); } - return new Path100(parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); + return new Path100(parameterizeSplines(splines, maxNorm, maxDx, maxDy, maxDTheta)); } /** @@ -44,6 +45,7 @@ public static Path100 pathFromWaypoints( */ static List parameterizeSpline( HolonomicSpline s, + double maxNorm, double maxDx, double maxDy, double maxDTheta, @@ -53,13 +55,14 @@ static List parameterizeSpline( rv.add(s.getPose2dWithMotion(0.0)); double dt = (t1 - t0); for (double t = 0; t < t1; t += dt) { - PathFactory.getSegmentArc(s, rv, t, t + dt, maxDx, maxDy, maxDTheta); + PathFactory.getSegmentArc(s, maxNorm, rv, t, t + dt, maxDx, maxDy, maxDTheta); } return rv; } public static List parameterizeSplines( List splines, + double maxNorm, double maxDx, double maxDy, double maxDTheta) { @@ -71,7 +74,7 @@ public static List parameterizeSplines( HolonomicSpline s = splines.get(i); if (DEBUG) System.out.printf("SPLINE:\n%d\n%s\n", i, s); - List samples = parameterizeSpline(s, maxDx, maxDy, maxDTheta, 0.0, 1.0); + List samples = parameterizeSpline(s, maxNorm, maxDx, maxDy, maxDTheta, 0.0, 1.0); samples.remove(0); rv.addAll(samples); } @@ -79,21 +82,21 @@ public static List parameterizeSplines( } /** - * Recursive bisection to find a series of secant lines close to the real curve. + * Recursive bisection to find a series of secant lines close to the real curve, + * and with the points closer than maxNorm to each other, measured in L2 norm + * (i.e. x, y, heading), and also course. * * Note if the path is s-shaped, then bisection can find the middle :-) */ private static void getSegmentArc( HolonomicSpline spline, + double maxNorm, // max distance between points List rv, double t0, // [0,1] not time double t1, // [0,1] not time double maxDx, double maxDy, double maxDTheta) { - // points must be this close together - // TODO: make this a parameter. - final double maxNorm = 0.1; Pose2d p0 = spline.getPose2d(t0); double thalf = (t0 + t1) / 2; Pose2d phalf = spline.getPose2d(thalf); @@ -108,10 +111,12 @@ private static void getSegmentArc( // difference between twist and sample Transform2d error = phalf_predicted.minus(phalf); + // also prohibit large changes in direction between points Pose2dWithMotion p20 = spline.getPose2dWithMotion(t0); Pose2dWithMotion p21 = spline.getPose2dWithMotion(t1); Twist2d p2t = p20.getPose().course().minus(p21.getPose().course()); + // note the extra conditions to avoid points too far apart. // checks both translational and l2 norms // also checks change in course if (Math.abs(error.getTranslation().getX()) > maxDx @@ -121,9 +126,9 @@ private static void getSegmentArc( || Metrics.l2Norm(twist_full) > maxNorm || Metrics.l2Norm(p2t) > maxNorm) { // add a point in between - // note the extra condition to avoid points too far apart. - getSegmentArc(spline, rv, t0, thalf, maxDx, maxDy, maxDTheta); - getSegmentArc(spline, rv, thalf, t1, maxDx, maxDy, maxDTheta); + + getSegmentArc(spline, maxNorm, rv, t0, thalf, maxDx, maxDy, maxDTheta); + getSegmentArc(spline, maxNorm, rv, thalf, t1, maxDx, maxDy, maxDTheta); } else { // midpoint is close enough, this looks good rv.add(spline.getPose2dWithMotion(t1)); diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java index ad5bb35a9..3dc01d462 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java @@ -9,7 +9,7 @@ * Velocity limit based on curvature and the capsize limit (scaled). */ public class CapsizeAccelerationConstraint implements TimingConstraint { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private final Mutable m_scale; private final double m_maxCentripetalAccel; private final double m_maxDecel; @@ -49,7 +49,7 @@ public CapsizeAccelerationConstraint(LoggerFactory parent, double centripetal, d */ @Override public double maxV(final Pose2dWithMotion state) { - double radius = 1 / state.getCurvatureRad_M(); + double radius = 1 / Math.abs(state.getCurvatureRad_M()); // abs is used here to make sure sqrt is happy. return Math.sqrt(Math.abs(m_maxCentripetalAccel * m_scale.getAsDouble() * radius)); } @@ -73,7 +73,10 @@ public double maxDecel(Pose2dWithMotion state, double velocity) { System.out.println("too fast for the curvature, slowing down is ok"); return m_maxDecel * m_scale.getAsDouble(); } - return -Math.sqrt(alongsq); + double decel = -Math.sqrt(alongsq); + if (DEBUG) + System.out.printf("decel %f\n", decel); + return decel; } /** @@ -88,8 +91,11 @@ public double maxDecel(Pose2dWithMotion state, double velocity) { * along = sqrt(total^2 - v^4/r^2) */ private double alongSq(Pose2dWithMotion state, double velocity) { - double radius = 1 / state.getCurvatureRad_M(); + double radius = 1 / Math.abs(state.getCurvatureRad_M()); double actualCentripetalAccel = velocity * velocity / radius; + if (DEBUG) + System.out.printf("radius %f actual centripetal %f\n", + radius, actualCentripetalAccel); return m_maxCentripetalAccel * m_scale.getAsDouble() * m_maxCentripetalAccel * m_scale.getAsDouble() - actualCentripetalAccel * actualCentripetalAccel; } diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java deleted file mode 100644 index 6ba489577..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/DirectSchedule.java +++ /dev/null @@ -1,96 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import org.team100.lib.trajectory.path.spline.SplineR1; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; - -/** - * Experiment with schedule map, a stand-off function for the spline parameter - * as a function of time. - * - * Uses https://en.wikipedia.org/wiki/Finite_difference - * - * This matches the notation in README.md - */ -public class DirectSchedule { - private static final double EPSILON = 1e-6; - - private final SplineR1 m_spline; - private final InterpolatingDoubleTreeMap m_sdot; - // integrates sdot - private final InterpolatingDoubleTreeMap m_s; - private double m_maxT; - - public DirectSchedule(SplineR1 spline) { - m_spline = spline; - m_sdot = new InterpolatingDoubleTreeMap(); - m_s = new InterpolatingDoubleTreeMap(); - } - - public void put(double t, double sdot) { - m_sdot.put(t, sdot); - m_maxT = Math.max(m_maxT, t); - // keep s in sync - integrate(); - } - - private void integrate() { - double s = 0; - m_s.clear(); - double dt = 0.001; - m_s.put(0.0, 0.0); - for (double t = dt; t <= m_maxT; t += dt) { - double sdot = sdot(t); - s += sdot * dt; - m_s.put(t, s); - } - } - - /** q(t) position for time t */ - public double x(double t) { - return q(s(t)); - } - - /** qdot(t) */ - public double v(double t) { - return qprime(s(t)) * sdot(t); - } - - /** qdotdot(t) */ - public double a(double t) { - return qprimeprime(s(t)) * sdot(t) * sdot(t) + qprime(s(t)) * sdotdot(t); - } - - /** position for parameter s */ - public double q(double s) { - s = MathUtil.clamp(s, 0, 1); - return m_spline.getPosition(s); - } - - /** dq/ds, derivative of position wrt parameter s */ - public double qprime(double s) { - s = MathUtil.clamp(s, 0, 1); - return m_spline.getVelocity(s); - } - - /** d^2q/ds^2, second derivative of postion wrt parameter s */ - public double qprimeprime(double s) { - s = MathUtil.clamp(s, 0, 1); - return m_spline.getAcceleration(s); - } - - /** spline parameter, s, for time t */ - public double s(double t) { - return m_s.get(t); - } - - public double sdot(double t) { - return m_sdot.get(t); - } - - public double sdotdot(double t) { - return (sdot(t + EPSILON) - sdot(t)) / EPSILON; - } - -} diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java b/lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java deleted file mode 100644 index eb4b528db..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/MultipleShooting.java +++ /dev/null @@ -1,85 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import java.util.List; - -import org.team100.lib.util.Math100; - -/** - * Experiment with a new implementation of the multiple shooting method. - * - * There are constraints on V and A, as well as continuity of X and V. - * - * @see https://en.wikipedia.org/wiki/Direct_multiple_shooting_method - * @see https://en.wikipedia.org/wiki/Trajectory_optimization - */ -public class MultipleShooting { - private final double m_maxV; - private final double m_maxA; - - public MultipleShooting(double maxV, double maxA) { - m_maxV = maxV; - m_maxA = maxA; - } - - public void solve(double[] x, double[] v, Double[] a) { - int n = x.length; - // estimate x_i from x_(i-1) - double[] x_est = new double[n]; - double[] dt_est = new double[n - 1]; - for (int i = 1; i < n; ++i) { - // forward integral - double dx = x[i] - x[i - 1]; - double v0 = v[i - 1]; - // choose max accel in direction of x1 if there's no a estimate - if (a[i - 1] == null) { - a[i - 1] = Math.signum(dx) * m_maxA; - } - double a0 = a[i - 1]; - // maybe instead of solving for t (because it can fail), just guess? - double A = 0.5 * a0; - double B = v0; - double C = -1.0 * dx; - List soln = Math100.solveQuadratic(A, B, C); - double dt = choose(soln); - dt_est[i - 1] = dt; - x_est[i] = x[i - 1] + v0 * dt + 0.5 * a0 * dt * dt; - } - // this contains v discontinuities - simulate(x, v, a, dt_est); - - } - - private void simulate(double[] x, double[] v, Double[] a, double[] dt_est) { - double DT = 0.01; - double t0 = 0; - System.out.println("t, a_t, v_t, x_t"); - for (int i = 0; i < x.length - 1; ++i) { - double x0 = x[i]; - double v0 = v[i]; - double a0 = a[i]; - for (double t = 0; t < dt_est[i]; t += DT) { - double v_t = v0 + a0 * t; - double x_t = x0 + v0 * t + 0.5 * a0 * t * t; - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", t0 + t, a0, v_t, x_t); - } - t0 += dt_est[i]; - } - } - - // choose smallest non-negative solution - private double choose(List soln) { - double x0 = Double.POSITIVE_INFINITY; - for (double x : soln) { - if (x >= 0 && x < x0) - x0 = x; - } - if (Double.isFinite(x0)) - return x0; - - // no solution - return 0; - - // throw new IllegalArgumentException(); - } - -} diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/README.md b/lib/src/main/java/org/team100/lib/trajectory/timing/README.md index ad5d0331c..8448596b7 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/README.md @@ -1,11 +1,3 @@ # lib.trajectory.timing -This package helps to create trajectory schedules. - -It is similar to the WPI package `math.trajectory.constraint`. - -The main difference is that the input here is based on paths, without any sense of time or speed. The input is about the path, the output is about time. - -In contrast, the WPI approach is to take timed input and produce adjusted timed output. - -Both approaches work. Do not try to mix them. +This package helps to create trajectory schedules. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java index a80d3f50b..2b2eabdcd 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java @@ -13,12 +13,15 @@ * schedule. */ public class ScheduleGenerator { + public static class TimingException extends Exception { } - public static final boolean DEBUG = true; + public static final boolean DEBUG = false; private static final double EPSILON = 1e-6; - /** this is the default, in order to make the constraints set the actual */ + + /** Defaults to make the constraints set the actual. */ + private static final double HIGH_V = 100; private static final double HIGH_ACCEL = 1000; private final List m_constraints; @@ -36,7 +39,8 @@ public Trajectory100 timeParameterizeTrajectory( double start_vel, double end_vel) { try { - Pose2dWithMotion[] samples = path.resample(step); + // Pose2dWithMotion[] samples = path.resample(step); + Pose2dWithMotion[] samples = path.resample(); return timeParameterizeTrajectory(samples, start_vel, end_vel); } catch (TimingException e) { e.printStackTrace(); @@ -46,220 +50,200 @@ public Trajectory100 timeParameterizeTrajectory( } /** - * input is some set of samples (could be evenly sampled or not), output is - * these same samples with time. + * Input is some set of samples (could be evenly sampled or not). + * + * Output is these same samples with time. */ public Trajectory100 timeParameterizeTrajectory( Pose2dWithMotion[] samples, double start_vel, - double end_vel) throws TimingException { - int n = samples.length; - if (n < 3) - throw new IllegalArgumentException("must have at least three samples"); - - // distance monotonically increases - double distances[] = new double[n]; - double velocities[] = new double[n]; - double decels[] = new double[n]; - double accels[] = new double[n]; - - // Forward pass. - { - Pose2dWithMotion previousPose = samples[0]; - double previousDistance = 0; - double previousVelocity = start_vel; - double previousDecel = -HIGH_ACCEL; - double previousAccel = HIGH_ACCEL; - for (int i = 0; i < n; ++i) { - // arclength is never negative but can be zero. - double arclength = samples[i].distanceCartesian(previousPose); - if (i > 0 && i < n - 1 && arclength < 1e-6) { - // the first distance is zero because of the weird loop structure. - // the last distance can be zero if the step size exactly divides the path - // length - throw new IllegalStateException("zero distance not allowed"); - } - - distances[i] = arclength + previousDistance; - velocities[i] = 100; - decels[i] = -HIGH_ACCEL; - accels[i] = HIGH_ACCEL; - - // We may need to iterate to find the maximum end velocity and common - // acceleration, since acceleration limits may be a function of velocity. - while (true) { - // first try the previous state accel to get the new state velocity - double v1 = Math100.v1(previousVelocity, previousAccel, arclength); - - velocities[i] = v1; - - // also use max accels for the new state accels - // decels[i] = -HIGH_ACCEL; - // accels[i] = HIGH_ACCEL; - - // reduce velocity according to constraints - for (TimingConstraint constraint : m_constraints) { - double constraintV = constraint.maxV(samples[i]); - if (DEBUG) { - System.out.printf("i %d constraint %s v %f\n", - i, constraint.getClass().getSimpleName(), constraintV); - } - velocities[i] = Math.min(velocities[i], constraintV); - } + double end_vel) { + double[] distances = getDistances(samples); + double[] velocities = getVelocities(samples, start_vel, end_vel, distances); + double[] accels = getAccels(distances, velocities); + double[] runningTime = getRunningTime(distances, velocities, accels); + List timedStates = getTimedStates(samples, velocities, accels, runningTime); + return new Trajectory100(timedStates); + } - for (TimingConstraint constraint1 : m_constraints) { - // removing this breaks TrajectoryPlannerTest.test2d. - // which is an s-shaped thing with variable curvature. - // the passing test is definitely wrong, and the failing - // test is wrong too. - // decels[i] = Math.max(decels[i], constraint1.maxDecel(samples[i], - // velocities[i])); - accels[i] = Math.min(accels[i], constraint1.maxAccel(samples[i], velocities[i])); - } + /** + * Creates a list of timed states. + */ + private List getTimedStates( + Pose2dWithMotion[] samples, double[] velocities, double[] accels, double[] runningTime) { + int n = samples.length; + List timedStates = new ArrayList<>(n); + for (int i = 0; i < n; ++i) { + timedStates.add(new TimedState(samples[i], runningTime[i], velocities[i], accels[i])); + } + return timedStates; + } - // motionless, which can happen at the end - if (Math.abs(arclength) < EPSILON) { - break; - } + /** + * Computes duration of each arc and accumulate. Assigns a time to each point. + */ + private double[] getRunningTime(double[] distances, double[] velocities, double[] accels) { + int n = distances.length; + double[] runningTime = new double[n]; + for (int i = 1; i < n; ++i) { + double arcLength = distances[i] - distances[i - 1]; + double dt = dt(velocities[i - 1], velocities[i], arcLength, accels[i - 1]); + runningTime[i] = runningTime[i - 1] + dt; + } + return runningTime; + } - double accel = Math100.accel(previousVelocity, velocities[i], arclength); - // in the failure case, max accel is zero so this always fails. - if (accel > accels[i] + EPSILON) { - // implied accel is too high because v1 is too high, perhaps because - // a0 was too high, try again with the (lower) constrained value - // - // if the constrained value is zero, this doesn't work at all. + /** + * Computes average accel based on distance of each arc and velocity at each + * point. + * + * Accel is attached to the *start* of each arc ([i] not [i+1]) + * + * The very last accel is always zero, but it's never used since it describes + * samples off the end of the trajectory. + */ + private double[] getAccels(double[] distances, double[] velocities) { + int n = distances.length; + double[] accels = new double[n]; + for (int i = 0; i < n - 1; ++i) { + double arcLength = distances[i + 1] - distances[i]; + accels[i] = Math100.accel(velocities[i], velocities[i + 1], arcLength); + } + return accels; + } - previousAccel = accels[i]; - continue; - } - if (accel > previousDecel + EPSILON) { - // set the previous state accel to whatever the constrained velocity implies - previousAccel = accel; - } - break; - } + /** + * Assigns a velocity to each sample, using velocity, accel, and decel + * constraints. + */ + private double[] getVelocities( + Pose2dWithMotion[] samples, double start_vel, double end_vel, double[] distances) { + double velocities[] = new double[samples.length]; + forward(samples, start_vel, distances, velocities); + backward(samples, end_vel, distances, velocities); + if (start_vel > velocities[0]) { + System.out.printf("WARNING: start velocity %f is higher than constrained velocity %f\n", + start_vel, velocities[0]); + } + return velocities; + } - previousPose = samples[i]; - previousDistance = distances[i]; - previousVelocity = velocities[i]; - previousDecel = decels[i]; - previousAccel = accels[i]; + /** + * Computes velocities[i+1] using velocity and acceleration constraints using + * the + * state at i. + */ + private void forward( + Pose2dWithMotion[] samples, double start_vel, double[] distances, double[] velocities) { + int n = samples.length; + velocities[0] = start_vel; + for (int i = 0; i < n - 1; ++i) { + double arclength = distances[i + 1] - distances[i]; + if (Math.abs(arclength) < EPSILON) { + // zero-length arcs have the same state at both ends + velocities[i + 1] = velocities[i]; + break; + } + // velocity constraint depends only on state + double maxV = velocityConstraint(samples[i + 1]); + // start with the maximum velocity + velocities[i + 1] = maxV; + // reduce velocity to fit under the acceleration constraint + double impliedAccel = Math100.accel(velocities[i], velocities[i + 1], arclength); + double maxAccel = accelConstraint(samples[i], velocities[i]); + if (impliedAccel > maxAccel + EPSILON) { + velocities[i + 1] = Math100.v1(velocities[i], maxAccel, arclength); } } + } - // - // Backwards pass - // - { - // "successor" comes before in the backwards walk. start with the last state. - double successorDistance = distances[n - 1]; - double successorVelocity = end_vel; - double successorDecel = -HIGH_ACCEL; - double successorAccel = HIGH_ACCEL; - - // work backwards through the states list - for (int i = n - 1; i >= 0; --i) { - // backwards (negative) distance from successor to initial state. - double dq = distances[i] - successorDistance; - - while (true) { - // s0 velocity can't be more than the accel implies - // so this is actually an estimate for v0 - // min a is negative, dq is negative, so v0 is faster than v1 - double v0 = Math100.v1(successorVelocity, successorDecel, dq); - if (DEBUG) - System.out.printf("i %d v0 %f\n", i, v0); - - if (velocities[i] <= v0) { - // s0 v is slower than implied v0, which means - // that actual accel is larger than the min, so we're fine - // No new limits to impose. - break; - } - // v is too fast - if (DEBUG) - System.out.printf("v too fast i %d v_i %f v0 %f\n", i, velocities[i], v0); - velocities[i] = v0; - - for (TimingConstraint constraint : m_constraints) { - decels[i] = Math.max(decels[i], constraint.maxDecel(samples[i], velocities[i])); - accels[i] = Math.min(accels[i], constraint.maxAccel(samples[i], velocities[i])); - } - - // motionless, which can happen at the end - if (Math.abs(dq) < EPSILON) { - break; - } - - // implied accel using the constrained v0 - double accel = Math100.accel(successorVelocity, velocities[i], dq); - if (accel < decels[i] - EPSILON) { - // accel is too low which implies that s1 accel is too low, try again - successorDecel = decels[i]; - continue; - } - // set final accel to the implied value - successorDecel = accel; - break; - } - - successorDistance = distances[i]; - successorVelocity = velocities[i]; - successorDecel = decels[i]; - successorAccel = accels[i]; + /** + * Adjusts velocities[i] for decel constraint based on the state at i+1. + * + * This isn't strictly correct since the decel constraint should operate at i, + * but walking backwards through the path, only i+1 is available. + */ + private void backward( + Pose2dWithMotion[] samples, double end_vel, double[] distances, double[] velocities) { + int n = samples.length; + velocities[n - 1] = end_vel; + for (int i = n - 2; i >= 0; --i) { + double arclength = distances[i + 1] - distances[i]; + if (Math.abs(arclength) < EPSILON) { + // already handled this case + break; + } + double impliedAccel = Math100.accel(velocities[i], velocities[i + 1], arclength); + // Apply the decel constraint at the end of the segment since it is feasible. + double maxDecel = decelConstraint(samples[i + 1], velocities[i + 1]); + if (impliedAccel < maxDecel - EPSILON) { + velocities[i] = Math100.v0(velocities[i + 1], maxDecel, arclength); } } + } - // - // Integrate the constrained states forward in time to obtain the TimedStates. - // - // last state accel is always zero, which might be wrong. - // - List poses = new ArrayList<>(n); - { - double runningTotalTime = 0.0; - double previousDistance = 0.0; - double previousVelocity = 0.0; + /** + * Computes the length of each arc and accumulates. + */ + private double[] getDistances(Pose2dWithMotion[] samples) { + int n = samples.length; + double distances[] = new double[n]; + for (int i = 1; i < n; ++i) { + double arclength = samples[i].distanceCartesian(samples[i - 1]); + distances[i] = arclength + distances[i - 1]; + } + return distances; + } - for (int i = 0; i < n; ++i) { - double dq = distances[i] - previousDistance; - double dt = 0.0; - if (i > 0) { - double prevAccel = Math100.accel(previousVelocity, velocities[i], dq); - poses.get(i - 1).set_acceleration(prevAccel); - dt = dt(previousVelocity, velocities[i], dq, prevAccel); - } - runningTotalTime += dt; - if (Double.isNaN(runningTotalTime) || Double.isInfinite(runningTotalTime)) { - throw new TimingException(); - } - poses.add(new TimedState(samples[i], runningTotalTime, velocities[i], 0)); - previousVelocity = velocities[i]; - previousDistance = distances[i]; - } + /** + * Returns the lowest (i.e. closest to zero) velocity constraint from the list + * of constraints. Always positive or zero. + */ + private double velocityConstraint(Pose2dWithMotion sample) { + double minVelocity = HIGH_V; + for (TimingConstraint constraint : m_constraints) { + minVelocity = Math.min(minVelocity, constraint.maxV(sample)); } + return minVelocity; + } - return new Trajectory100(poses); + /** + * Returns the lowest (i.e. closest to zero) acceleration constraint from the + * list of constraints. Always positive or zero. + */ + private double accelConstraint(Pose2dWithMotion sample, double velocity) { + double minAccel = HIGH_ACCEL; + for (TimingConstraint constraint : m_constraints) { + minAccel = Math.min(minAccel, constraint.maxAccel(sample, velocity)); + } + return minAccel; } /** - * If accelerating, find the time to go from v0 to v1. Otherwise find the time - * to go distance dq at speed v0. + * Returns the highest (i.e. closest to zero) deceleration constraint from the + * list of constraints. Always negative or zero. */ + private double decelConstraint(Pose2dWithMotion sample, double velocity) { + double maxDecel = -HIGH_ACCEL; + for (TimingConstraint constraint : m_constraints) { + maxDecel = Math.max(maxDecel, constraint.maxDecel(sample, velocity)); + } + return maxDecel; + } + private static double dt( double v0, double v1, - double dq, - double accel) throws TimingException { + double arcLength, + double accel) { if (Math.abs(accel) > EPSILON) { + // If accelerating, find the time to go from v0 to v1. return (v1 - v0) / accel; } if (Math.abs(v0) > EPSILON) { - return dq / v0; + // If moving, find the time to go distance dq at speed v0. + return arcLength / v0; } - // not moving at all so dt is always zero return 0; - // throw new TimingException(String.format("%f %f %f %f", v0, v1, ds, accel)); } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java b/lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java deleted file mode 100644 index e819a9d1c..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/Scheduler100.java +++ /dev/null @@ -1,166 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import java.util.List; - -import org.team100.lib.util.Math100; - -/** - * Assign timestamps waypoints. - * - * This is the same approach as the old ScheduleGenerator: assign durations to - * the arcs between waypoints, using constraints on pathwise velocity and - * accel/decel, walk over the waypoints once forward, and - * once backward, then once more to add up the durations. - */ -public class Scheduler100 { - - public static interface Constraint { - /** - * Can depend on curvature and course and heading and heading rate, - * none of which vary during scheduling. - * - * @return a positive number. - */ - double vmax(double q); - - /** - * Since v is alwasy positive, amin means decel. - * - * Can depend on pose and curvature and heading rate, and also pathwise - * velocity. of these only pathwise velocity changes during the scheduling. - * - * @returns a negative number - */ - double amin(double q, double v); - - /** - * Since v is always positive, amax means accel. - * - * Can depend on pose and curvature and heading rate, and also pathwise - * velocity. of these only pathwise velocity changes during the scheduling. - * - * @return a positive number - */ - double amax(double q, double v); - } - - private final Constraint m_c; - - public Scheduler100(Constraint c) { - m_c = c; - } - - public double[] schedule(double[] q) { - - int n = q.length - 1; - - // initial dt - // new dt is never less than this so choose a small number - double[] dt = new double[n + 1]; - for (int i = 0; i < n; ++i) { - dt[i] = 0.10 / n; - } - - // velocity constraint - for (int i = 1; i < n + 1; ++i) { - // dx is never negative - double dx = q[i] - q[i - 1]; - // v is never negative - double v = dx / dt[i - 1]; - double vmax = m_c.vmax(q[i]); - double newdt = dx / vmax; - // only slower - dt[i - 1] = Math.max(dt[i - 1], newdt); - } - - // accel constraint forward, using backward finite differences - for (int i = 2; i < n + 1; ++i) { - double v0 = (q[i - 1] - q[i - 2]) / dt[i - 2]; - double dx = q[i] - q[i - 1]; - while (true) { - double v = dx / dt[i - 1]; - double a = (v - v0) / dt[i - 1]; - if (a > 0) { - double amax = m_c.amax(q[i], v); - if (a > amax) { - double newdt = solve(amax, v0, -dx); - // only slower - if (newdt > dt[i - 1]) { - dt[i - 1] = newdt; - continue; - } - } - } else if (a < 0) { - double amin = m_c.amin(q[i], v); - if (a < amin) { - double newdt = solve(amin, v0, -dx); - // only slower - if (newdt > dt[i - 1]) { - dt[i - 1] = newdt; - continue; - } - } - } - break; - } - } - - // accel constraint backward, using forward finite differences - for (int i = n - 2; i >= 0; --i) { - double v0 = (q[i + 2] - q[i + 1]) / dt[i + 1]; - double dx = q[i + 1] - q[i]; - while (true) { - double v = dx / dt[i]; - double a = (v0 - v) / dt[i]; - if (a > 0) { - double amax = m_c.amax(q[i], v); - if (a > amax) { - double newdt = solve(amax, -v0, dx); - // only slower - if (newdt > dt[i]) { - dt[i] = newdt; - continue; - } - } - } else if (a < 0) { - double amin = m_c.amin(q[i], v); - if (a < amin) { - double newdt = solve(amin, -v0, dx); - // only slower - if (newdt > dt[i]) { - dt[i] = newdt; - continue; - } - } - } - break; - } - } - return dt; - } - - private static double solve(double A, double B, double C) { - List soln = Math100.solveQuadratic(A, B, C); - return choose(soln); - } - - /** - * choose smallest non-negative solution - * dt is never negative ... and i think also should never be zero ... - */ - private static double choose(List soln) { - double x0 = Double.POSITIVE_INFINITY; - for (double x : soln) { - if (x >= 0 && x < x0) - x0 = x; - } - if (Double.isFinite(x0)) - return x0; - - // System.out.println("no solution"); - return 0; - - // throw new IllegalArgumentException(); - } - -} diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java index 34c100f84..fbbab2cd7 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java @@ -16,8 +16,11 @@ public class TimedState { private final double m_timeS; /** Instantaneous pathwise velocity, m/s. */ private final double m_velocityM_S; - /** Instantaneous pathwise (not centripetal) acceleration, m/s^2. */ - private double m_accelM_S_S; + /** + * Pathwise acceleration for the timespan after this state, m/s^2. It's computed + * by looking at the velocity of the next state, and the distance to get there. + */ + private final double m_accelM_S_S; public TimedState( Pose2dWithMotion state, @@ -44,11 +47,6 @@ public double velocityM_S() { return m_velocityM_S; } - /** accel is set based on the velocity of the next state, so we set it here. */ - void set_acceleration(double acceleration) { - m_accelM_S_S = acceleration; - } - /** Instantaneous pathwise (not centripetal) acceleration, m/s^2. */ public double acceleration() { return m_accelM_S_S; diff --git a/lib/src/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index f2b7fbaab..47641cc09 100644 --- a/lib/src/main/java/org/team100/lib/util/Math100.java +++ b/lib/src/main/java/org/team100/lib/util/Math100.java @@ -112,6 +112,7 @@ public static double accel(double v0, double v1, double dx) { // prevent division by zero return 0; } + /* * a = dv/dt * v = dx/dt @@ -127,33 +128,65 @@ public static double accel(double v0, double v1, double dx) { /** * Return final velocity, v1, given initial velocity, v0, and acceleration over - * distance ds. + * distance dx. * - * v1 = sqrt(v0^2 + 2ads) + * v1 = sqrt(v0^2 + 2adx) * * note a can be negative. * - * note ds can be negative, which implies backwards time + * note dx can be negative, which implies backwards time * * @param v0 initial velocity * @param a acceleration - * @param ds distance + * @param dx distance + * @return final velocity + */ + public static double v1(double v0, double a, double dx) { + /* + * a = dv/dt + * v = dx/dt + * dt = dx/v + * a = v dv/dx + * a = v (v1-v0)/dx + * v = (v0+v1)/2 + * a = (v0+v1)(v1-v0)/2dx + * a = (v1^2 - v0^2)/2dx + * 2*a*dx = v1^2 - v0^2 + * v1 = sqrt(v0^2 + 2*a*dx) + */ + return Math.sqrt(v0 * v0 + 2.0 * a * dx); + } + + /** + * Return initial velocity, v0, given final velocity, v1, and acceleration over + * distance dx. + * + * v0 = sqrt(v1^2 - 2adx) + * + * note a can be negative. + * + * note dx can be negative, which implies backwards time + * + * @param v1 final velocity + * @param a acceleration + * @param dx distance * @return final velocity */ - public static double v1(double v0, double a, double ds) { + + public static double v0(double v1, double a, double dx) { /* * a = dv/dt - * v = ds/dt - * dt = ds/v - * a = v dv/ds - * a = v (v1-v0)/ds + * v = dx/dt + * dt = dx/v + * a = v dv/dx + * a = v (v1-v0)/dx * v = (v0+v1)/2 - * a = (v0+v1)(v1-v0)/2ds - * a = (v1^2 - v0^2)/2ds - * 2*a*ds = v1^2 - v0^2 - * v1 = sqrt(v0^2 + 2*a*ds) + * a = (v0+v1)(v1-v0)/2dx + * a = (v1^2 - v0^2)/2dx + * 2*a*dx = v1^2 - v0^2 + * v0 = sqrt(v1^2 - 2*a*dx) */ - return Math.sqrt(v0 * v0 + 2.0 * a * ds); + return Math.sqrt(v1 * v1 - 2.0 * a * dx); } } diff --git a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java index d90c66436..aaf386b06 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java @@ -150,10 +150,11 @@ void testFieldRelativeTrajectory() { double start_vel = 0.0; double end_vel = 0.0; - Path100 path = PathFactory.pathFromWaypoints(waypoints, 2, 0.25, 0.1); + double stepSize = 2; + + Path100 path = PathFactory.pathFromWaypoints(waypoints, stepSize, 2, 0.25, 0.1); assertFalse(path.isEmpty()); - double stepSize = 2; ScheduleGenerator u = new ScheduleGenerator(Arrays.asList()); Trajectory100 trajectory = u.timeParameterizeTrajectory( path, diff --git a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java index be13114b5..86953c109 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java @@ -26,6 +26,7 @@ /** To visualize the different ways to parameterize a spline. */ public class ParameterizationTest { + private static final boolean DEBUG = false; private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); private static final Supplier renderer = () -> new StandardXYItemRenderer( @@ -113,7 +114,7 @@ void testPoses() { new DirectionSE2(1, 0, 0), 1)); List poses = PathFactory.parameterizeSplines( - List.of(spline), 0.02, 0.2, 0.1); + List.of(spline), 0.1, 0.02, 0.2, 0.1); XYSeries sx = PathToVectorSeries.x("spline", poses); XYDataset dataSet = TrajectoryPlotter.collect(sx); @@ -140,7 +141,8 @@ void testTrajectory() { Trajectory100 trajectory = p.generateTrajectory(waypoints, 0, 0); // this is wrong somehow - System.out.printf("TRAJECTORY\n%s\n", trajectory); + if (DEBUG) + System.out.printf("TRAJECTORY\n%s\n", trajectory); TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("trajectory", trajectory); diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 6d0ebdee8..9c07bd26f 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -82,12 +82,14 @@ void testSample() { TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 trajectory = planner.restToRest(waypoints); + if (DEBUG) + trajectory.dump(); - assertEquals(1.418, trajectory.duration(), DELTA); + assertEquals(1.415, trajectory.duration(), DELTA); TimedState sample = trajectory.sample(0); assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(1); - assertEquals(0.825, sample.state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.828, sample.state().getPose().pose().getTranslation().getX(), DELTA); sample = trajectory.sample(2); assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA); } @@ -123,7 +125,7 @@ void testSampleThoroughly() { } } - assertEquals(1.418, trajectory.duration(), DELTA); + assertEquals(1.415, trajectory.duration(), DELTA); check(trajectory, 0.0, 0.000); check(trajectory, 0.1, 0.010); check(trajectory, 0.2, 0.040); @@ -131,11 +133,11 @@ void testSampleThoroughly() { check(trajectory, 0.4, 0.160); check(trajectory, 0.5, 0.250); check(trajectory, 0.6, 0.360); - check(trajectory, 0.7, 0.487); - check(trajectory, 0.8, 0.618); - check(trajectory, 0.9, 0.732); - check(trajectory, 1.0, 0.825); - check(trajectory, 1.1, 0.899); + check(trajectory, 0.7, 0.490); + check(trajectory, 0.8, 0.622); + check(trajectory, 0.9, 0.734); + check(trajectory, 1.0, 0.828); + check(trajectory, 1.1, 0.901); check(trajectory, 1.2, 0.953); check(trajectory, 1.3, 0.987); check(trajectory, 1.4, 1.000); @@ -185,7 +187,7 @@ void testSamplePerformance() throws TimingException { // INTERPOLATE SPLINE POINTS (170 ns) Path100 path = PathFactory.pathFromWaypoints( - waypoints, 0.02, 0.2, 0.1); + waypoints, 0.1, 0.02, 0.2, 0.1); assertEquals(22.734, path.getMaxDistance(), 0.001); start = System.nanoTime(); @@ -208,8 +210,9 @@ void testSamplePerformance() throws TimingException { ScheduleGenerator generator = new ScheduleGenerator(constraints); Trajectory100 trajectory = generator.timeParameterizeTrajectory(path, 0.1, 0, 0); + TrajectoryPlotter.plot(trajectory, 1); - assertEquals(229, trajectory.length()); + assertEquals(313, trajectory.length()); start = System.nanoTime(); for (int rep = 0; rep < reps; ++rep) { diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 6e00d1a33..1b07fdd98 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -50,12 +50,12 @@ void testLinear() { List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); - assertEquals(12, t.length()); + assertEquals(17, t.length()); TimedState tp = t.getPoint(0); // start at zero velocity assertEquals(0, tp.velocityM_S(), DELTA); - TimedState p = t.getPoint(6); - assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); + TimedState p = t.getPoint(8); + assertEquals(0.5, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -81,11 +81,11 @@ void testLinearRealistic() { new CapsizeAccelerationConstraint(logger, limits, 0.2)); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); - assertEquals(12, t.length()); + assertEquals(17, t.length()); TimedState tp = t.getPoint(0); assertEquals(0, tp.velocityM_S(), DELTA); - TimedState p = t.getPoint(6); - assertEquals(0.6, p.state().getPose().pose().getTranslation().getX(), DELTA); + TimedState p = t.getPoint(8); + assertEquals(0.5, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -123,9 +123,9 @@ void testPerformance() { System.out.printf("total duration ms: %5.3f\n", totalDurationMs); System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations); } - assertEquals(18, t.length()); - TimedState p = t.getPoint(6); - assertEquals(0.585, p.state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(33, t.length()); + TimedState p = t.getPoint(12); + assertEquals(0.605, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @@ -168,34 +168,50 @@ void testMovingToRest() { assertEquals(1.176, traj.duration(), DELTA); } + /** + * This is a curve that goes +y, turns sharply towards +x, with a more gradual + * curve after that. + * + * Initial velocity is clamped (with a warning) + * Max braking to the apex + * Coast through the apex + * Max accel for about half of the rest + * Max decel to the end + */ @Test void test2d() { - List constraints = List.of( + List constraints = List.of( new ConstantConstraint(logger, 1, 1), new CapsizeAccelerationConstraint(logger, 1, 1)); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 1, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); - traj.dump(); - assertEquals(3.337, traj.duration(), DELTA); + if (DEBUG) + traj.dump(); + assertEquals(2.741, traj.duration(), DELTA); } - // this should result in constant velocity, slowing for the curve, and then - // accelerating for the exit. - // - // but it doesn't, so there's something wrong. - // - // in both the "pass" and "fail" cases (with decel in the forward pass), this - // produces too-low accels in the straights, and too much accel variation in - // the curve. + /** + * Straight for 1m, approximately a quarter-circle + * + * Constant velocity on the straight + * Maximum braking just before the start of the curve + * Declining braking as the curvature grows + * No braking at all at the apex + * Gradual acceleration exiting the curve + * Maximum acceleration at the exit, to the max V + * Constant velocity on the straight + * + * These curves should be symmetric around the apex + */ @Test void test2d2() { List waypoints = List.of( - new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1), - new WaypointSE2(new Pose2d(1, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1), - new WaypointSE2(new Pose2d(2, 1, new Rotation2d()), new DirectionSE2(0, 1, 0), 1), - new WaypointSE2(new Pose2d(2, 2, new Rotation2d()), new DirectionSE2(0, 1, 0), 1)); + new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3), + new WaypointSE2(new Pose2d(1, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3), + new WaypointSE2(new Pose2d(2, 1, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3), + new WaypointSE2(new Pose2d(2, 2, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3)); List constraints = List.of( new ConstantConstraint(logger, 1, 1), @@ -203,8 +219,9 @@ void test2d2() { TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 traj = planner.generateTrajectory(waypoints, 1, 1); - traj.dump(); - assertEquals(3.337, traj.duration(), DELTA); + if (DEBUG) + traj.dump(); + assertEquals(4.603, traj.duration(), DELTA); } } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 863bdd555..14e490e18 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -48,7 +48,7 @@ void testEmpty() { double maxDx = 0.1; double maxDy = 0.1; double maxDTheta = 0.1; - Path100 path = new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); + Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.1, maxDx, maxDy, maxDTheta)); assertEquals(0, path.length(), 0.001); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 7ec5ee4d5..3e82241a4 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -43,7 +43,7 @@ void testCorner() { new Translation2d(1, 1), new Rotation2d()), new DirectionSE2(0, 1, 0), 1)); - Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); + Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.1, 0.01, 0.01, 0.1); assertEquals(54, path.length()); Pose2dWithMotion p = path.getPoint(0); @@ -70,7 +70,7 @@ void testLinear() { new Rotation2d()), new DirectionSE2(1, 0, 0), 1)); Path100 path = PathFactory.pathFromWaypoints( - waypoints, 0.01, 0.01, 0.1); + waypoints, 0.1, 0.01, 0.01, 0.1); assertEquals(17, path.length()); Pose2dWithMotion p = path.getPoint(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); @@ -96,7 +96,7 @@ void testSpin() { new Translation2d(0, 0), Rotation2d.kCCW_90deg), new DirectionSE2(0, 0, 1), 1)); - assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); + assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints,0.1, 0.01, 0.01, 0.1)); } /** Hard corners once again do not work. */ @@ -123,7 +123,7 @@ void testActualCorner() { new Translation2d(1, 1), new Rotation2d()), new DirectionSE2(0, 1, 0), 1)); - assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); + assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints,0.1, 0.01, 0.01, 0.1)); } @Test @@ -145,7 +145,7 @@ void testComposite() { new Translation2d(2, 0), new Rotation2d(1)), new DirectionSE2(1, 0, 0), 1)); - Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); + Path100 foo = PathFactory.pathFromWaypoints(waypoints, 0.1, 0.01, 0.01, 0.1); TrajectoryPlotter.plot(foo, 0.1); assertEquals(59, foo.length(), 0.001); } @@ -164,7 +164,7 @@ void test() { new DirectionSE2(1, 5, 0), 1.2); HolonomicSpline s = new HolonomicSpline(p1, p2); - List samples = PathFactory.parameterizeSpline(s, 0.05, 0.05, 0.1, 0.0, 1.0); + List samples = PathFactory.parameterizeSpline(s, 0.1, 0.05, 0.05, 0.1, 0.0, 1.0); double arclength = 0; Pose2dWithMotion cur_pose = samples.get(0); @@ -199,7 +199,7 @@ void testDx() { Rotation2d.kZero), new DirectionSE2(0, 1, 0), 1)); List splines = List.of(s0); - List motion = PathFactory.parameterizeSplines(splines, 0.001, 0.001, 0.001); + List motion = PathFactory.parameterizeSplines(splines, 0.1, 0.001, 0.001, 0.001); for (Pose2dWithMotion p : motion) { if (DEBUG) System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), @@ -233,6 +233,7 @@ void testPerformance() { for (int i = 0; i < iterations; ++i) { t = PathFactory.pathFromWaypoints( waypoints, + 0.1, SPLINE_SAMPLE_TOLERANCE_M, SPLINE_SAMPLE_TOLERANCE_M, SPLINE_SAMPLE_TOLERANCE_RAD); diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index f258f81ae..de4b5f0ea 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -1,7 +1,6 @@ package org.team100.lib.trajectory.path.spline; import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertThrows; import java.util.ArrayList; import java.util.List; @@ -282,10 +281,17 @@ void spin() { TrajectoryPlotter.plot(splines, 0.1); } + /** + * Four splines that make an approximate circle. + * + * The heading rate is pretty constant, looking at the origin pretty closely. + * + * The path curvature is not constant, because that's how our splines work: the + * curvature is always zero at the ends. It does mostly range from about 0.75 to + * about 1.25, which is kinda close? + */ @Test void testCircle() { - // four splines that make a circle should have nice even curvature and velocity - // throughout. The circle is centered at zero, the heading always points there. double scale = 1.3; WaypointSE2 p0 = new WaypointSE2( new Pose2d(new Translation2d(1, 0), Rotation2d.k180deg), @@ -351,9 +357,11 @@ void testDheading() { private void checkCircle(List splines, double rangeError, double azimuthError) { double actualRangeError = 0; double actualAzimuthError = 0; - for (HolonomicSpline s : splines) { - for (double j = 0; j < 0.99; j += 0.1) { - Pose2d p = s.getPose2d(j); + if (DEBUG) + System.out.println("s, x, y, k, dh"); + for (HolonomicSpline spline : splines) { + for (double s = 0; s < 0.99; s += 0.01) { + Pose2d p = spline.getPose2d(s); // the position should be on the circle double range = p.getTranslation().getNorm(); actualRangeError = Math.max(actualRangeError, Math.abs(1.0 - range)); @@ -365,6 +373,12 @@ private void checkCircle(List splines, double rangeError, doubl // circle. // 3/10/25 i made generation coarser so it's less accurate. actualAzimuthError = Math.max(actualAzimuthError, Math.abs(error.getRadians())); + + double k = spline.getCurvature(s); + + double dh = spline.getDHeadingDs(s); + if (DEBUG) + System.out.printf("%f, %f, %f, %f, %f\n", s, p.getX(), p.getY(), k, dh); } } assertEquals(0, actualRangeError, rangeError, @@ -449,8 +463,7 @@ void testPath0() { @Test void testMismatchedXYDerivatives() { // because path generation never looks across spline boundaries, - // it is possible to make sharp corners at the "knots." But you can't - // make a trajectory with these corners, the scheduler will fail. + // it is possible to make sharp corners at the "knots." // this goes straight ahead to (1,0) // derivatives point straight ahead @@ -489,13 +502,16 @@ void testMismatchedXYDerivatives() { System.out.printf("spline %s\n", s); } - Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05)); + Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.1, 0.05, 0.05, 0.05)); if (DEBUG) System.out.printf("path %s\n", path); List constraints = List.of(new ConstantConstraint(logger, 1, 1)); ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); - assertThrows(IllegalStateException.class, () -> scheduleGenerator.timeParameterizeTrajectory(path, - 0.05, 0, 0)); + Trajectory100 traj = scheduleGenerator.timeParameterizeTrajectory(path, + 0.05, 0, 0); + if (DEBUG) + traj.dump(); + TrajectoryPlotter.plot(traj, 0.1); } @Test @@ -524,7 +540,7 @@ void testEntryVelocity() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("splines", splines); - List motion = PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05); + List motion = PathFactory.parameterizeSplines(splines, 0.1, 0.05, 0.05, 0.05); if (DEBUG) { for (Pose2dWithMotion p : motion) { System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java index 21ebd2c40..02a9a3658 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java @@ -37,7 +37,8 @@ void testSample() { } private void show(SplineR1 spline) { - System.out.println("t, x, v, a, j"); + if (DEBUG) + System.out.println("t, x, v, a, j"); for (double t = 0; t <= 1; t += 0.01) { double x = spline.getPosition(t); double v = spline.getVelocity(t); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java deleted file mode 100644 index eeaf6a9a3..000000000 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/DirectScheduleTest.java +++ /dev/null @@ -1,469 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import java.util.List; - -import org.junit.jupiter.api.Test; -import org.team100.lib.trajectory.path.spline.SplineR1; -import org.team100.lib.util.Math100; - -public class DirectScheduleTest { - private static final double EPSILON = 1e-6; - - @Test - void testSimple0() { - // constant velocity - // we want v=0 at the end, so the map here goes to zero at the end. - SplineR1 spline = SplineR1.get(0, 1, 1, 1, 0, 0); - DirectSchedule schedule = new DirectSchedule(spline); - // by hand, for amax = 2, vmax = 0.5 - schedule.put(-100.0, 1.0); - schedule.put(0.0, 0.0); - schedule.put(0.1, 0.2); - schedule.put(0.2, 0.4); - schedule.put(0.3, 0.5); - schedule.put(0.4, 0.5); - schedule.put(0.5, 0.5); - schedule.put(0.6, 0.5); - schedule.put(0.7, 0.5); - schedule.put(0.8, 0.5); - schedule.put(0.9, 0.5); - schedule.put(1.0, 0.5); - schedule.put(1.1, 0.5); - schedule.put(1.2, 0.5); - schedule.put(1.3, 0.5); - schedule.put(1.4, 0.5); - schedule.put(1.5, 0.5); - schedule.put(1.6, 0.5); - schedule.put(1.7, 0.5); - schedule.put(1.8, 0.5); - schedule.put(1.9, 0.5); - schedule.put(2.0, 0.5); - schedule.put(2.1, 0.4); - schedule.put(2.2, 0.2); - schedule.put(2.3, 0.0); - schedule.put(2.4, 0.0); - schedule.put(100.0, 0.0); - System.out.println("t, x, v, a"); - for (double t = 0; t <= 3.001; t += 0.1) { - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", - t, schedule.x(t), schedule.v(t), schedule.a(t)); - } - } - - @Test - void testSimple1() { - // dx = 0 at the ends - // we also want 0 at the ends, so the map is constant there - SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); - DirectSchedule schedule = new DirectSchedule(spline); - // by hand, for amax = 2, vmax = 0.5 - schedule.put(-100.0, 1.0); - schedule.put(0.0, 0.75); - schedule.put(0.1, 0.75); - schedule.put(0.2, 0.6); - schedule.put(0.3, 0.58); - schedule.put(0.4, 0.46); - schedule.put(0.5, 0.3); - schedule.put(0.6, 0.3); - schedule.put(0.7, 0.3); - schedule.put(0.8, 0.3); - schedule.put(0.9, 0.3); - schedule.put(1.0, 0.3); - schedule.put(1.1, 0.3); - schedule.put(1.2, 0.3); - schedule.put(1.3, 0.3); - schedule.put(1.4, 0.3); - schedule.put(1.5, 0.3); - schedule.put(1.6, 0.3); - schedule.put(1.7, 0.4); - schedule.put(1.8, 0.4); - schedule.put(1.9, 0.4); - schedule.put(2.0, 0.5); - schedule.put(2.1, 0.65); - schedule.put(2.2, 0.75); - schedule.put(2.3, 0.75); - schedule.put(2.4, 0.75); - schedule.put(2.5, 0.75); - schedule.put(2.6, 0.75); - schedule.put(2.7, 0.75); - schedule.put(2.8, 0.75); - schedule.put(100.0, 0.0); - System.out.println("t, x, v, a, qprime"); - for (double t = 0; t <= 3.001; t += 0.1) { - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", - t, schedule.x(t), schedule.v(t), schedule.a(t), - schedule.qprime(schedule.s(t))); - } - } - - @Test - void testSimple3() { - SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); - - double DS = 0.01; - double qq = 0; - System.out.println("s, q, qq, qError, qprime, qprimeprime"); - for (double s = 0; s <= 1.001; s += DS) { - double q = spline.getPosition(s); - double qprime = spline.getVelocity(s); - // https://en.wikipedia.org/wiki/Numerical_integration - if (s > 0) { - // trapezoid integration is good to about 50 ppm - // qq += (spline.getVelocity(s - DS) + spline.getVelocity(s)) * DS/2; - // simpsons rule is good to 2 ppb - qq += (spline.getVelocity(s - DS) - + 4 * spline.getVelocity(s - DS / 2) - + spline.getVelocity(s)) * DS / 6; - } - double qError = q - qq; - double qprimeprime = spline.getAcceleration(s); - System.out.printf("%10.8f, %10.8f, %10.8f, %12.10f, %10.8f, %10.8f\n", - s, q, qq, qError, qprime, qprimeprime); - } - } - - /** - * qdot constraint varies; for now it's a function of s. - * - * In reality, depends on curvature and course and heading and heading rate, - * none of which vary during scheduling. - * - * always positive. - */ - double qdotmax(double q) { - // if (q > 0.45 && q < 0.55) - // return 0.5; - return 1.0; - } - - /** - * a positive number - * - * In reality, depends on pose and curvature and heading rate, and also pathwise - * velocity. of these only pathwise velocity changes during the scheduling. - */ - double qdotdotmax(double q, double v) { - if (q < 0.02) // soft start - return q*10; - return 5.0 - v; - } - - /** - * Since v is never negative, qdotdotmin always means deceleration. - * - * In reality, depends on pose and curvature and heading rate, and also pathwise - * velocity. of these only pathwise velocity changes during the scheduling. - * - * @returns a negative number - */ - double qdotdotmin(double s, double v) { - // if (s > 1.4) // soft stop - // return (1 - s) * -50 + v; - return -10.0 + v; - } - - @Test - void testSimple4() { - int n = 100; - SplineR1 q = SplineR1.get(0, 1, 0, 0, 0, 0); - - // these are the spline sample points; they can be - // in arbitrary locations. s is immutable. - double[] s = new double[n + 1]; - for (int i = 0; i < n + 1; ++i) { - s[i] = (double) i / n; - } - // start with initial estimate of sdot - double[] sdot = new double[n + 1]; - for (int i = 0; i < n + 1; ++i) { - sdot[i] = 1.0; - } - - // velocity constraint - for (int i = 0; i < n + 1; ++i) { - // first derivative of q wrt parameter s - double qprimei = q.getVelocity(s[i]); - // second derivative of q wrt parameter s - double qprimeprimei = q.getAcceleration(s[i]); - // first derivative of q wrt time, using chain rule - // qdot = dq/ds * ds/dt - double qdoti = qprimei * sdot[i]; - // also sdot(t) = 1/tprime(s) - // adjust sdot so that qdot is under the constraint - double qdotmaxi = qdotmax(s[i]); - if (Math.abs(qdoti) > qdotmaxi) { - sdot[i] = qdotmaxi / qprimei; - } - } - - // accel constraint - for (int i = 1; i < n + 1; ++i) { - // previous - int j = i - 1; - // first derivative of q wrt parameter s - double qprimei = q.getVelocity(s[i]); - // second derivative of q wrt parameter s - double qprimeprimei = q.getAcceleration(s[i]); - - double ds = s[i] - s[j]; - - // trailing difference to get tprimeprime - double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; - // differentiate sdot to get sdotdot. - // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 - // sdotdot = -tprimeprime * sdot^3 - double sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; - - // chain rule - // d2q/dt2 = d2q/ds2 * (ds/dt)^2 + dq/ds * d2s/dt2 - // qdotdot = qprimeprime * sdot^2 + qprime * sdotdot - double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; - // adjust sdot so that qdotdot is under the constraint - double qdotdotmaxi = qdotdotmax(s[i], 0); - if (Math.abs(qdotdoti) > Math.abs(qdotdotmaxi)) { - double sdotnew = Math.sqrt((2 * qdotdotmaxi * ds + qprimei * sdot[j] * sdot[j]) - / (2 * qprimeprimei * ds + qprimei)); - if (sdotnew < sdot[i]) { - sdot[i] = sdotnew; - tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; - sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; - } - } - } - - // accel constraint backwards - for (int i = n - 1; i > 0; --i) { - // previous - int j = i + 1; - // first derivative of q wrt parameter s - // note negative sign - double qprimei = -q.getVelocity(s[i]); - // second derivative of q wrt parameter s - // note negative sign - double qprimeprimei = -q.getAcceleration(s[i]); - - // this is a negative number - double ds = s[i] - s[j]; - - // trailing difference to get tprimeprime - // this is a positive number - double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; - // differentiate sdot to get sdotdot. - // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 - // sdotdot = -tprimeprime * sdot^3 - double sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; - - // chain rule - // d2q/dt2 = d2q/ds2 * (ds/dt)^2 + dq/ds * d2s/dt2 - // qdotdot = qprimeprime * sdot^2 + qprime * sdotdot - double qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; - // adjust sdot so that qdotdot is under the constraint - double qdotdotmaxi = qdotdotmax(s[i], 0); - if (Math.abs(qdotdoti) > Math.abs(qdotdotmaxi)) { - double sdotnew = Math.sqrt((2 * qdotdotmaxi * ds + qprimei * sdot[j] * sdot[j]) - / (2 * qprimeprimei * ds + qprimei)); - if (sdotnew < sdot[i]) { - sdot[i] = sdotnew; - tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; - sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; - } - } - } - - // integrate and dump the result - System.out.println("t, s, sdot, sdotdot, q, qdot, qdotdot"); - double t = 0; - for (int i = 0; i < n + 1; ++i) { - - double qi = q.getPosition(s[i]); - double qprimei = q.getVelocity(s[i]); - double qprimeprimei = q.getAcceleration(s[i]); - double qdoti = qprimei * sdot[i]; - double sdotdoti = 0; - double qdotdoti = 0; - if (i > 0) { - int j = i - 1; - // Integrate. - // We have sdot(t) but we want to integrate over s to find t - // The derivative of the inverse is the reciprocal - // https://en.wikipedia.org/wiki/Inverse_function_rule - // sdot(t) = 1/tprime(s) - double ds = s[i] - s[j]; - // tprime = dt/ds - double tprimei = 1 / sdot[i]; - double dt1 = tprimei * ds; - - // trailing difference to get tprimeprime - double tprimeprimei = (1 / sdot[i] - 1 / sdot[j]) / ds; - // differentiate sdot to get sdotdot. - // d2s/dt2 = - d2t/ds2 * (ds/dt)^3 - sdotdoti = -tprimeprimei * sdot[i] * sdot[i] * sdot[i]; - qdotdoti = qprimeprimei * sdot[i] * sdot[i] + qprimei * sdotdoti; - - t += dt1; - } - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", - t, s[i], sdot[i], sdotdoti, qi, qdoti, qdotdoti); - } - } - - /** - * schedules dt directly, doesn't try to us spline velocity etc. - * - * makes no attempt to limit jerk; jerk limiting would require an iterative - * solver, which would be nice to avoid, and it's not that important - * in practice. - */ - @Test - void testSimple5() { - SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); - // these are the spline sample points; they can be - // in arbitrary locations. s is immutable. - double[] s = new double[101]; - for (int i = 0; i < 101; ++i) { - s[i] = (double) i / 100; - } - - double[] q = new double[s.length]; - for (int i = 0; i < s.length; ++i) { - q[i] = spline.getPosition(s[i]); - } - - int n = q.length - 1; - - // initial dt - // new dt is never less than this so choose a small number - double[] dt = new double[n + 1]; - for (int i = 0; i < n; ++i) { - dt[i] = 0.10 / n; - } - - // velocity constraint - for (int i = 1; i < n + 1; ++i) { - // dx is never negative - double dx = q[i] - q[i - 1]; - // v is never negative - double v = dx / dt[i - 1]; - double vmax = qdotmax(q[i]); - double newdt = dx / vmax; - // only slower - dt[i - 1] = Math.max(dt[i - 1], newdt); - } - - // accel constraint forward, using backward finite differences - for (int i = 2; i < n + 1; ++i) { - double v0 = (q[i - 1] - q[i - 2]) / dt[i - 2]; - double dx = q[i] - q[i - 1]; - while (true) { - double v = dx / dt[i - 1]; - double a = (v - v0) / dt[i - 1]; - if (a > 0) { - double amax = qdotdotmax(q[i], v); - if (a > amax) { - double newdt = solve(amax, v0, -dx); - // only slower - if (newdt > dt[i - 1]) { - dt[i - 1] = newdt; - continue; - } - } - } else if (a < 0) { - double amin = qdotdotmin(q[i], v); - if (a < amin) { - double newdt = solve(amin, v0, -dx); - // only slower - if (newdt > dt[i - 1]) { - dt[i - 1] = newdt; - continue; - } - } - } - break; - } - } - - // accel constraint backward, using forward finite differences - for (int i = n - 2; i >= 0; --i) { - double v0 = (q[i + 2] - q[i + 1]) / dt[i + 1]; - double dx = q[i + 1] - q[i]; - while (true) { - double v = dx / dt[i]; - double a = (v0 - v) / dt[i]; - if (a > 0) { - double amax = qdotdotmax(q[i], v); - if (a > amax) { - double newdt = solve(amax, -v0, dx); - // only slower - if (newdt > dt[i]) { - dt[i] = newdt; - continue; - } - } - } else if (a < 0) { - double amin = qdotdotmin(q[i], v); - if (a < amin) { - double newdt = solve(amin, -v0, dx); - // only slower - if (newdt > dt[i]) { - dt[i] = newdt; - continue; - } - } - } - break; - } - } - - // integrate and dump the result - System.out.println("t, x, v, a"); - double t = 0; - for (int i = 0; i < n + 1; ++i) { - double x = q[i]; - double v = 0; - double a = 0; - if (i > 0) { - t += dt[i - 1]; - } - // compute v using backward finite difference - if (i > 0) { - double dx = q[i] - q[i - 1]; - v = dx / dt[i - 1]; - } - // compute a using backward finite difference - if (i > 1) { - double dx0 = q[i] - q[i - 1]; - double v0 = dx0 / dt[i - 1]; - double dx1 = q[i - 1] - q[i - 2]; - double v1 = dx1 / dt[i - 2]; - a = (v0 - v1) / dt[i - 1]; - } - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", - t, x, v, a); - } - } - - private double solve(double A, double B, double C) { - List soln = Math100.solveQuadratic(A, B, C); - return choose(soln); - } - - /** - * choose smallest non-negative solution - * dt is never negative ... and i think also should never be zero ... - */ - private double choose(List soln) { - double x0 = Double.POSITIVE_INFINITY; - for (double x : soln) { - if (x >= 0 && x < x0) - x0 = x; - } - if (Double.isFinite(x0)) - return x0; - - // System.out.println("no solution"); - return 0; - - // throw new IllegalArgumentException(); - } - -} diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java deleted file mode 100644 index 1035fbd27..000000000 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/MultipleShootingTest.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import org.junit.jupiter.api.Test; - -public class MultipleShootingTest { - @Test - void test0() { - // x is fixed - double[] x = new double[] { 1, 2, 2 }; - // v endpoints are fixed - double[] v = new double[] { 0, 0, 0 }; - // a is between, has no fixed values - Double[] a = new Double[] { null, null }; - double maxV = 1; - double maxA = 1; - MultipleShooting shooter = new MultipleShooting(maxV, maxA); - shooter.solve(x, v, a); - } - - @Test - void test1() { - // x is fixed - double[] x = new double[] { 1, 2, 3 }; - // v endpoints are fixed - double[] v = new double[] { 0, 0, 0 }; - // a is between, has no fixed values - Double[] a = new Double[] { null, null }; - double maxV = 1; - double maxA = 1; - MultipleShooting shooter = new MultipleShooting(maxV, maxA); - shooter.solve(x, v, a); - } - -} diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java index a0d41180a..09d4fa6f8 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java @@ -3,7 +3,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.ArrayList; @@ -104,7 +103,7 @@ public void checkTrajectory( } /** - * Turning in place does not work.k + * Turning in place does not work, but it also doesn't fail. */ @Test void testJustTurningInPlace() { @@ -127,11 +126,12 @@ void testJustTurningInPlace() { List constraints = new ArrayList(); ScheduleGenerator u = new ScheduleGenerator(constraints); - assertThrows(IllegalArgumentException.class, () -> u.timeParameterizeTrajectory( + Trajectory100 traj = u.timeParameterizeTrajectory( path, 1.0, 0.0, - 0.0)); + 0.0); + assertEquals(0, traj.duration(), DELTA); } /** @@ -148,21 +148,21 @@ void testNoConstraints() { Trajectory100 timed_traj = buildAndCheckTrajectory(path, 1.0, new ArrayList(), 0.0, 0.0, 20.0, 5.0); - assertEquals(66, timed_traj.length()); + assertEquals(4, timed_traj.length()); // Trapezoidal profile. timed_traj = buildAndCheckTrajectory(path, 1.0, new ArrayList(), 0.0, 0.0, 10.0, 5.0); - assertEquals(66, timed_traj.length()); + assertEquals(4, timed_traj.length()); // Trapezoidal profile with start and end velocities. timed_traj = buildAndCheckTrajectory(path, 1.0, new ArrayList(), 5.0, 2.0, 10.0, 5.0); - assertEquals(66, timed_traj.length()); + assertEquals(4, timed_traj.length()); } /** @@ -178,18 +178,18 @@ void testCentripetalConstraint() { Trajectory100 timed_traj = buildAndCheckTrajectory(path, 1.0, List.of(new CapsizeAccelerationConstraint(logger, limits, 1.0)), 0.0, 0.0, 20.0, 5.0); - assertEquals(66, timed_traj.length()); + assertEquals(4, timed_traj.length()); assertNotNull(timed_traj); // Trapezoidal profile. timed_traj = buildAndCheckTrajectory(path, 1.0, new ArrayList(), 0.0, 0.0, 10.0, 5.0); - assertEquals(66, timed_traj.length()); + assertEquals(4, timed_traj.length()); // Trapezoidal profile with start and end velocities. timed_traj = buildAndCheckTrajectory(path, 1.0, new ArrayList(), 5.0, 2.0, 10.0, 5.0); - assertEquals(66, timed_traj.length()); + assertEquals(4, timed_traj.length()); } @Test @@ -277,6 +277,7 @@ void testPerformance() { Path100 path = PathFactory.pathFromWaypoints( waypoints, + TRAJECTORY_STEP_M, SPLINE_SAMPLE_TOLERANCE_M, SPLINE_SAMPLE_TOLERANCE_M, SPLINE_SAMPLE_TOLERANCE_RAD); @@ -295,9 +296,9 @@ void testPerformance() { System.out.printf("total duration ms: %5.3f\n", totalDurationMs); System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations); } - assertEquals(18, t.length()); - TimedState p = t.getPoint(6); - assertEquals(0.575, p.state().getPose().pose().getTranslation().getX(), DELTA); + assertEquals(33, t.length()); + TimedState p = t.getPoint(12); + assertEquals(0.605, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java deleted file mode 100644 index 7b2242438..000000000 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/SchedulerTest.java +++ /dev/null @@ -1,86 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import org.junit.jupiter.api.Test; -import org.team100.lib.trajectory.path.spline.SplineR1; - -public class SchedulerTest { - - Scheduler100.Constraint constraint = new Scheduler100.Constraint() { - @Override - public double vmax(double q) { - if (q > 0.45 && q < 0.55) - return 0.5; - return 1.0; - } - - @Override - public double amax(double q, double v) { - if (q < 0.01) // soft start - return (q * 500); - return 5.0 - v; - } - - @Override - public double amin(double q, double v) { - if (q > 0.99) // soft stop - return (1 - q) * -500; - return -10.0 + v; - } - }; - - @Test - void test0() { - int n = 100; - SplineR1 spline = SplineR1.get(0, 1, 0, 0, 0, 0); - // these are the spline sample points; they can be - // in arbitrary locations. s is immutable. - double[] s = new double[n + 1]; - for (int i = 0; i < n + 1; ++i) { - s[i] = (double) i / n; - } - - // positions - double[] q = new double[s.length]; - for (int i = 0; i < s.length; ++i) { - q[i] = spline.getPosition(s[i]); - } - - Scheduler100 scheduler = new Scheduler100(constraint); - - double[] dt = scheduler.schedule(q); - - dump(q, dt); - } - - public static void dump(double[] q, double[] dt) { - int n = q.length - 1; - - // integrate and dump the result - System.out.println("t, x, v, a"); - double t = 0; - for (int i = 0; i < n + 1; ++i) { - double x = q[i]; - double v = 0; - double a = 0; - if (i > 0) { - t += dt[i - 1]; - } - // compute v using backward finite difference - if (i > 0) { - double dx = q[i] - q[i - 1]; - v = dx / dt[i - 1]; - } - // compute a using backward finite difference - if (i > 1) { - double dx0 = q[i] - q[i - 1]; - double v0 = dx0 / dt[i - 1]; - double dx1 = q[i - 1] - q[i - 2]; - double v1 = dx1 / dt[i - 2]; - a = (v0 - v1) / dt[i - 1]; - } - System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f\n", - t, x, v, a); - } - } - -} diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 06a0b37b9..9878c7df1 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java @@ -81,6 +81,8 @@ void testConstantConstraint() throws TimingException { /** * This produces the desired current-limited exponential shape. + * + * TODO: this tickles a bug in the schedule generator dt(), fix it. */ @Test void testSwerveConstraint() throws TimingException {