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..9ba192f23 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.HolonomicPose2d; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.ConfigLogger; -import org.team100.lib.logging.LoggerFactory.GlobalAccelerationR3Logger; -import org.team100.lib.logging.LoggerFactory.GlobalVelocityR3Logger; +import org.team100.lib.logging.LoggerFactory.AccelerationSE2Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.logging.LoggerFactory.JointAccelerationsLogger; import org.team100.lib.logging.LoggerFactory.JointForceLogger; import org.team100.lib.logging.LoggerFactory.JointVelocitiesLogger; @@ -105,8 +105,8 @@ public class CalgamesMech extends SubsystemBase implements Music, PositionSubsys private final JointForceLogger m_log_jointF; private final Pose2dLogger m_log_pose; - private final GlobalVelocityR3Logger m_log_cartesianV; - private final GlobalAccelerationR3Logger m_log_cartesianA; + private final VelocitySE2Logger m_log_cartesianV; + private final AccelerationSE2Logger m_log_cartesianA; private final LinearMechanism m_elevatorFront; private final LinearMechanism m_elevatorBack; @@ -146,8 +146,8 @@ public CalgamesMech( LoggerFactory cartesianLog = parent.name("cartesian"); m_log_pose = cartesianLog.pose2dLogger(Level.DEBUG, "pose"); - m_log_cartesianV = cartesianLog.globalVelocityR3Logger(Level.DEBUG, "velocity"); - m_log_cartesianA = cartesianLog.globalAccelerationR3Logger(Level.DEBUG, "accel"); + m_log_cartesianV = cartesianLog.VelocitySE2Logger(Level.DEBUG, "velocity"); + m_log_cartesianA = cartesianLog.AccelerationSE2Logger(Level.DEBUG, "accel"); LoggerFactory elevatorbackLog = parent.name("elevatorBack"); LoggerFactory elevatorfrontLog = parent.name("elevatorFront"); @@ -317,14 +317,14 @@ public ModelR3 getState() { EAWConfig c = getConfig(); JointVelocities jv = getJointVelocity(); Pose2d p = m_kinematics.forward(c); - GlobalVelocityR3 v = m_jacobian.forward(c, jv); + VelocitySE2 v = m_jacobian.forward(c, jv); return new ModelR3(p, v); } // for testing only - public void setVelocity(GlobalVelocityR3 v) { + public void setVelocity(VelocitySE2 v) { Pose2d pose = getState().pose(); - GlobalAccelerationR3 a = new GlobalAccelerationR3(0, 0, 0); + AccelerationSE2 a = new AccelerationSE2(0, 0, 0); ControlR3 control = new ControlR3(pose, v, a); JointVelocities jv = m_jacobian.inverse(control.model()); @@ -490,87 +490,87 @@ public Command processorWithProfile() { public MoveAndHold homeToL1() { return m_transit.endless("homeToL1", - HolonomicPose2d.make(m_home, -1.5), - HolonomicPose2d.make(L2, -1.7)); + WaypointSE2.irrotational(m_home, -1.5, 1.2), + WaypointSE2.irrotational(L2, -1.7, 1.2)); } // NEVER CALL public Command l1ToHome() { return m_transit.terminal("l1ToHome", - HolonomicPose2d.make(L2, 1.3), - HolonomicPose2d.make(m_home, 1.5)); + WaypointSE2.irrotational(L2, 1.3, 1.2), + WaypointSE2.irrotational(m_home, 1.5, 1.2)); } public MoveAndHold homeToL2() { return m_transit.endless("homeToL2", - HolonomicPose2d.make(m_home, 1.5), - HolonomicPose2d.make(L2, 1.5)); + WaypointSE2.irrotational(m_home, 1.5, 1.2), + WaypointSE2.irrotational(L2, 1.5, 1.2)); } public Command l2ToHome() { return m_transit.terminal("l2ToHome", - HolonomicPose2d.make(L2, -1.5), - HolonomicPose2d.make(m_home, -1.5)); + WaypointSE2.irrotational(L2, -1.5, 1.2), + WaypointSE2.irrotational(m_home, -1.5, 1.2)); } public MoveAndHold homeToL3() { return m_transit.endless("homeToL3", - HolonomicPose2d.make(m_home, 0.8), - HolonomicPose2d.make(L3, 1.5)); + WaypointSE2.irrotational(m_home, 0.8, 1.2), + WaypointSE2.irrotational(L3, 1.5, 1.2)); } public Command l3ToHome() { return m_transit.terminal("l3ToHome", - HolonomicPose2d.make(L3, -1.5), - HolonomicPose2d.make(m_home, -2.3)); + WaypointSE2.irrotational(L3, -1.5, 1.2), + WaypointSE2.irrotational(m_home, -2.3, 1.2)); } public MoveAndHold homeToL4() { return m_transit.endless("homeToL4", - HolonomicPose2d.make(m_home, 0.1), - HolonomicPose2d.make(L4, 1.5)); + WaypointSE2.irrotational(m_home, 0.1, 1.2), + WaypointSE2.irrotational(L4, 1.5, 1.2)); } public MoveAndHold homeToL4Back() { return m_transit.endless("homeToL4", - HolonomicPose2d.make(m_home, 0.1), - HolonomicPose2d.make(L4_BACK, -1.5)); + WaypointSE2.irrotational(m_home, 0.1, 1.2), + WaypointSE2.irrotational(L4_BACK, -1.5, 1.2)); } public Command l4ToHome() { return m_transit.terminal("l4ToHome", - HolonomicPose2d.make(L4, -1.5), - HolonomicPose2d.make(m_home, -3)); + WaypointSE2.irrotational(L4, -1.5, 1.2), + WaypointSE2.irrotational(m_home, -3, 1.2)); } public Command l4BackToHome() { return m_transit.terminal("l4ToHome", - HolonomicPose2d.make(L4_BACK, 1.5), - HolonomicPose2d.make(m_home, -3)); + WaypointSE2.irrotational(L4_BACK, 1.5, 1.2), + WaypointSE2.irrotational(m_home, -3, 1.2)); } public Command homeToAlgaeL2() { return m_transit.endless("homeToAlgaeL2", - HolonomicPose2d.make(m_home, 1.5), - HolonomicPose2d.make(ALGAE_L2, 1.5)); + WaypointSE2.irrotational(m_home, 1.5, 1.2), + WaypointSE2.irrotational(ALGAE_L2, 1.5, 1.2)); } public Command homeToAlgaeL3() { return m_transit.endless("homeToAlgaeL3", - HolonomicPose2d.make(m_home, 0), - HolonomicPose2d.make(ALGAE_L3, 1.5)); + WaypointSE2.irrotational(m_home, 0, 1.2), + WaypointSE2.irrotational(ALGAE_L3, 1.5, 1.2)); } public Command algaeL2ToHome() { return m_transit.terminal("homeToAlgaeL2", - HolonomicPose2d.make(ALGAE_L2, -1.0), - HolonomicPose2d.make(m_home, Math.PI)); + WaypointSE2.irrotational(ALGAE_L2, -1.0, 1.2), + WaypointSE2.irrotational(m_home, Math.PI, 1.2)); } public Command algaeL3ToHome() { return m_transit.terminal("homeToAlgaeL3", - HolonomicPose2d.make(ALGAE_L3, -1.0), - HolonomicPose2d.make(m_home, Math.PI)); + WaypointSE2.irrotational(ALGAE_L3, -1.0, 1.2), + WaypointSE2.irrotational(m_home, Math.PI, 1.2)); } /** @@ -598,14 +598,14 @@ public Command algaeReefExit(Supplier level) { */ public MoveAndHold homeToBarge() { return m_transit.endless("homeToBarge", - HolonomicPose2d.make(m_home, 0), - HolonomicPose2d.make(BARGE, -1)); + WaypointSE2.irrotational(m_home, 0, 1.2), + WaypointSE2.irrotational(BARGE, -1, 1.2)); } public MoveAndHold bargeToHome() { return m_transit.endless("bargeToHome", - HolonomicPose2d.make(BARGE, 2.5), - HolonomicPose2d.make(m_home, Math.PI)); + WaypointSE2.irrotational(BARGE, 2.5, 1.2), + WaypointSE2.irrotational(m_home, Math.PI, 1.2)); } @@ -672,8 +672,8 @@ private void logConfig(EAWConfig c, JointVelocities jv, JointAccelerations ja, J m_log_jointA.log(() -> ja); m_log_jointF.log(() -> jf); Pose2d p = m_kinematics.forward(c); - GlobalVelocityR3 v = m_jacobian.forward(c, jv); - GlobalAccelerationR3 a = m_jacobian.forwardA(c, jv, ja); + VelocitySE2 v = m_jacobian.forward(c, jv); + AccelerationSE2 a = m_jacobian.forwardA(c, jv, ja); m_log_pose.log(() -> p); m_log_cartesianV.log(() -> v); m_log_cartesianA.log(() -> a); 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..24c6aab4b 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -4,16 +4,13 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; -import org.team100.lib.subsystems.prr.JointAccelerations; -import org.team100.lib.subsystems.prr.JointVelocities; import org.team100.lib.subsystems.r3.commands.GoToPosePosition; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.JointConstraint; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TorqueConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -22,7 +19,6 @@ /** Make a trajectory from the start to the end and follow it. */ public class MechTrajectories extends Command { - private static final boolean USE_JOINT_CONSTRAINT = false; private final LoggerFactory m_log; private final CalgamesMech m_subsystem; @@ -36,21 +32,12 @@ public MechTrajectories( m_log = parent.type(this); m_subsystem = mech; List c = new ArrayList<>(); - if (USE_JOINT_CONSTRAINT) { - // This is experimental, don't use it. - c.add(new JointConstraint( - k, - j, - new JointVelocities(10, 10, 10), - new JointAccelerations(10, 10, 10))); - } else { - // These are known to work, but suboptimal. - c.add(new ConstantConstraint(m_log, 10, 5)); - c.add(new YawRateConstraint(m_log, 10, 5)); - // This is new - c.add(new TorqueConstraint(20)); - } + // These are known to work, but suboptimal. + c.add(new ConstantConstraint(m_log, 10, 5)); + c.add(new YawRateConstraint(m_log, 10, 5)); + // This is new + c.add(new TorqueConstraint(20)); // ALERT! // The parameters here used to be double these values; @@ -62,22 +49,22 @@ public MechTrajectories( } /** A command that goes from the start to the end and then finishes. */ - public Command terminal(String name, HolonomicPose2d start, HolonomicPose2d end) { + public Command terminal(String name, WaypointSE2 start, WaypointSE2 end) { /** Use the start course and ignore the start pose for now */ MoveAndHold f = new GoToPosePosition( - m_log, m_subsystem, start.course(), end, m_planner); + m_log, m_subsystem, start.course().toRotation(), end, m_planner); return f .until(f::isDone) .withName(name); } /** A command that goes from the start to the end and then waits forever. */ - public MoveAndHold endless(String name, HolonomicPose2d start, HolonomicPose2d end) { + public MoveAndHold endless(String name, WaypointSE2 start, WaypointSE2 end) { /** Use the start course and ignore the start pose for now */ GoToPosePosition c = new GoToPosePosition( - m_log, m_subsystem, start.course(), end, m_planner); + m_log, m_subsystem, start.course().toRotation(), end, m_planner); c.setName(name); return c; 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/Swerve/Auto/GoToCoralStation.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index d758dd5de..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,8 @@ import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.CoralStation; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.trajectory.Trajectory100; @@ -51,9 +52,9 @@ public Trajectory100 apply(Pose2d currentPose) { Rotation2d newInitialSpline = FieldConstants.calculateDeltaSpline( courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust); - List waypoints = new ArrayList<>(); - waypoints.add(new HolonomicPose2d(currTranslation, currentPose.getRotation(), newInitialSpline)); - waypoints.add(new HolonomicPose2d(goal.getTranslation(), goal.getRotation(), courseToGoal)); + List waypoints = new ArrayList<>(); + waypoints.add(new WaypointSE2(currentPose, DirectionSE2.irrotational(newInitialSpline), 1)); + waypoints.add(new WaypointSE2(goal, DirectionSE2.irrotational(courseToGoal), 1)); return m_planner.restToRest(waypoints); } 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/Binder.java b/comp/src/main/java/org/team100/frc2025/robot/Binder.java index bddda98f6..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,9 +7,9 @@ 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; import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.controller.r1.PIDFeedback; import org.team100.lib.controller.r3.ControllerFactoryR3; @@ -22,6 +22,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 +141,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 +189,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/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java index 746d3c550..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.HolonomicPose2d; +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; @@ -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; @@ -51,8 +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( - HolonomicPose2d.make(m_machinery.m_drive.getPose(), Math.PI), - HolonomicPose2d.make(3, 5, 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 2336ede87..96209e104 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java @@ -4,13 +4,15 @@ import java.util.List; import org.team100.lib.coherence.Takt; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +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; @@ -27,21 +29,21 @@ 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, - Rotation2d.kZero)); - waypoints.add(new HolonomicPose2d( - new Translation2d(1, 0), - Rotation2d.kZero, - Rotation2d.kZero)); + List waypoints = new ArrayList<>(); + waypoints.add(new WaypointSE2( + new Pose2d(new Translation2d(), Rotation2d.kZero), + new DirectionSE2(1, 0, 0), + 1)); + waypoints.add(new WaypointSE2( + new Pose2d(new Translation2d(1, 0), Rotation2d.kZero), + new DirectionSE2(1, 0, 0), + 1)); 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..41ef39a85 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -3,9 +3,9 @@ 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.HolonomicPose2d; +import org.team100.lib.geometry.AccelerationSE2; +import org.team100.lib.geometry.WaypointSE2; +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( - HolonomicPose2d.make(1, 0, 0, 0), - HolonomicPose2d.make(1.9, 0.5, 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); @@ -60,10 +63,10 @@ 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(); - 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/TrajectoryPlotter.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java deleted file mode 100644 index 8b9d38fff..000000000 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryPlotter.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.team100.frc2025.CalgamesArm; - -import java.awt.Dimension; - -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.xy.VectorSeriesCollection; -import org.team100.lib.trajectory.Trajectory100; - -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.convertRotated(t)); - XYPlot plot = new XYPlot( - dataSet, - new NumberAxis("X"), - new NumberAxis("Y"), - 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.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); - try { - Thread.sleep(50000); - } catch (InterruptedException e) { - } - } -} diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java deleted file mode 100644 index c09eb12be..000000000 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryTest.java +++ /dev/null @@ -1,135 +0,0 @@ -package org.team100.frc2025.CalgamesArm; - -import java.util.List; - -import org.team100.lib.geometry.HolonomicPose2d; -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; - -import edu.wpi.first.math.geometry.Pose2d; -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 { - LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); - - /** - * Yields a straight line. - * - * TrajectoryPlanner.restToRest() has several overloads: the one that takes - * two non-holonomic poses draws a straight line between them. - */ - // @Test - void testSimple() throws InterruptedException { - List c = List.of( - new ConstantConstraint(log, 1, 0.1), - new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner p = new TrajectoryPlanner(c); - 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); - } - - /** - * Yields a curve. - * - * A HolonomicPose2d 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 - * facing the other way (i.e. backwards) - */ - // @Test - void testCurved() throws InterruptedException { - 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 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))); - Trajectory100 t = p.restToRest(waypoints); - new TrajectoryPlotter(0.1).plot(t, "simple"); - Thread.sleep(5000); - } - - /** - * You can specify interior waypoints as well as start and end points. Note that - * specifying many such points will make the curve harder to calculate and - * harder to make smooth. - */ - // @Test - void testMultipleWaypoints() throws InterruptedException { - 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 HolonomicPose2d( - new Translation2d(1, 1), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d( - new Translation2d(5, 5), new Rotation2d(-2), new Rotation2d()), - new HolonomicPose2d( - new Translation2d(9, 9), new Rotation2d(-Math.PI / 2), new Rotation2d(Math.PI / 2))); - Trajectory100 t = p.restToRest(waypoints); - new TrajectoryPlotter(0.1).plot(t, "simple"); - Thread.sleep(5000); - } - - /** 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 HolonomicPose2d( - new Translation2d(1, 0.1), new Rotation2d(-Math.PI), new Rotation2d(Math.PI / 2)), - // place for gateway point? - new HolonomicPose2d( - new Translation2d(3, 7), new Rotation2d(Math.PI / 2), new Rotation2d()), - new HolonomicPose2d( - new Translation2d(6, 9), new Rotation2d(-((7 * Math.PI) / 36)), new Rotation2d(Math.PI / 2))); - @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 HolonomicPose2d( - new Translation2d(1, 0.1), new Rotation2d(-Math.PI), new Rotation2d(Math.PI / 2)), - // place for gateway point - new HolonomicPose2d( - new Translation2d(0.75, 3), new Rotation2d(-Math.PI), new Rotation2d(Math.PI / 2)), - // place for gateway point - new HolonomicPose2d( - new Translation2d(3, 7), new Rotation2d(Math.PI / 2), new Rotation2d()), - new HolonomicPose2d( - new Translation2d(6, 9), new Rotation2d(-((7 * Math.PI) / 36)), new Rotation2d(Math.PI / 2))); - @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 deleted file mode 100644 index 729c961ad..000000000 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryToVectorSeries.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.team100.frc2025.CalgamesArm; - -import org.jfree.data.xy.VectorSeries; -import org.team100.lib.geometry.HolonomicPose2d; -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; - - public TrajectoryToVectorSeries(double scale) { - m_scale = scale; - } - - /** Maps x to x, y to y */ - public VectorSeries convert(Trajectory100 t) { - VectorSeries s = new VectorSeries("trajectory"); - double duration = t.duration(); - for (double time = 0; time < duration; time += DT) { - TimedPose p = t.sample(time); - HolonomicPose2d pp = p.state().getPose(); - double x = pp.translation().getX(); - double y = pp.translation().getY(); - Rotation2d heading = pp.heading(); - double dx = m_scale * heading.getCos(); - double dy = m_scale * heading.getSin(); - s.add(x, y, dx, dy); - } - 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); - HolonomicPose2d 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/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/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..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 @@ -1,15 +1,15 @@ 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.ModelR3Logger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; 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..24055233d --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/AccelerationSE2.java @@ -0,0 +1,72 @@ +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 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..78bdf59f3 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. 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 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..f476fbfee --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionR2.java @@ -0,0 +1,46 @@ +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. + * + * 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; + } + + public static DirectionR2 fromRotation(Rotation2d r) { + return new DirectionR2(r.getCos(), r.getSin()); + } + + @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..49fe97389 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/DirectionSE2.java @@ -0,0 +1,99 @@ +package org.team100.lib.geometry; + +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 + * 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. + * + * This 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; + + 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; + } + + 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); + } + + /** 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); + } + + /** 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 + public boolean equals(Object obj) { + 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 + 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/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..b0e217c11 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -25,14 +25,49 @@ * Lots of utility functions. */ public class GeometryUtil { - private static final boolean DEBUG = false; + static final boolean DEBUG = false; 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; + } + /** 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; @@ -55,7 +90,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 +106,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) { @@ -139,29 +174,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) { - // Common case of dy == 0 - if (a.dy == 0.0) - return Math.abs(a.dx); - return Math.hypot(a.dx, a.dy); - } - - 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) @@ -172,20 +184,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(); - } - - 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) { @@ -199,16 +197,27 @@ 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); } - 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)); + /** 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), + MathUtil.interpolate(a.y, b.y, 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, + double x) { + return new WaypointSE2( + interpolate(a.pose(), b.pose(), x), + interpolate(a.course(), b.course(), x), + MathUtil.interpolate(a.scale(), b.scale(), x)); } /** @@ -250,27 +259,6 @@ public static Rotation3d interpolate(Rotation3d a, Rotation3d b, double x) { MathUtil.interpolate(a.getZ(), b.getZ(), x)); } - 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))); - } - - 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()); } @@ -291,7 +279,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(); @@ -300,7 +288,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(); @@ -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/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 deleted file mode 100644 index 0dddbbf8e..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose2d.java +++ /dev/null @@ -1,53 +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. - * - * @param translation - * @param heading where the front of the robot is facing - * @param course the direction the robot is moving - */ -public record HolonomicPose2d( - Translation2d translation, - Rotation2d heading, - Rotation2d course) { - - public Pose2d pose() { - return new Pose2d(translation, heading); - } - - public static HolonomicPose2d make(Pose2d p, Rotation2d course) { - return new HolonomicPose2d(p.getTranslation(), p.getRotation(), course); - } - - public static HolonomicPose2d make(Pose2d p, double course) { - return new HolonomicPose2d( - p.getTranslation(), - p.getRotation(), - new Rotation2d(course)); - } - - 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)); - } - - /** 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)); - } - - /** - * For tank drive, heading and course are the same. This is like the WPI - * trajectory. - */ - public static HolonomicPose2d tank(Pose2d p) { - return new HolonomicPose2d( - p.getTranslation(), p.getRotation(), p.getRotation()); - } -} 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 35a0eb532..000000000 --- a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.team100.lib.geometry; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; - -/** - * For constructing 3d splines, paths, and trajectories. - * - * @param translation - * @param heading where we're facing, can include roll - * @param course where we're going, ignores roll. - */ -public record HolonomicPose3d( - Translation3d translation, - Rotation3d heading, - Rotation3d course) { - -} 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..2a71d889b --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/Metrics.java @@ -0,0 +1,118 @@ +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.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; + +/** + * Various distance metrics and norms. + */ +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()); + } + + /** + * 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 projectedDistance(Pose2d a, Pose2d b) { + return translationalNorm(a.log(b)); + } + + /** + * 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); + } + + /** The magnitude of the translational velocity. */ + public static double translationalNorm(ChassisSpeeds a) { + return Math.hypot(a.vxMetersPerSecond, a.vyMetersPerSecond); + } + + ///////////////////////////////////////////////////////////////// + /// + /// DANGER ZONE + /// + /// Don't use anything below here unless you really know what you're doing + + /* + * 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. + */ + public static double l2Norm(Twist2d a) { + return Math.sqrt(a.dx * a.dx + a.dy * a.dy + a.dtheta * a.dtheta); + } + + /** + * L2 norm of all components of the twist. + * + * 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 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 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 + * + * 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 + */ + 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); + } +} 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..e5bea460d 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java +++ b/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java @@ -3,49 +3,33 @@ import org.team100.lib.util.Math100; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -/** - * HolonomicPose2d with: - * - * * 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 { - /** Position, heading and course. */ - private final HolonomicPose2d m_pose; + 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; - /** Change in curvature per meter, rad/m^2 */ - private final double m_dCurvatureDsRad_M2; /** - * @param pose location and heading of the robot - * @param course motion direction, radians - * @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. + * @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( - HolonomicPose2d pose, - double headingRate, - double curvatureRad_M, - double dCurvatureDsRad_M2) { - m_pose = pose; - m_headingRate = headingRate; + WaypointSE2 waypoint, + double headingRateRad_M, + double curvatureRad_M) { + m_waypoint = waypoint; + m_headingRateRad_M = headingRateRad_M; m_curvatureRad_M = curvatureRad_M; - m_dCurvatureDsRad_M2 = dCurvatureDsRad_M2; } - public HolonomicPose2d getPose() { - return m_pose; - } - - public Rotation2d getCourse() { - return m_pose.course(); + public WaypointSE2 getPose() { + return m_waypoint; } /** @@ -54,55 +38,72 @@ public Rotation2d getCourse() { * 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; } - /** 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) { + /** + * Linear interpolation of each component separately. + * + * Not a constant-twist arc. + */ + 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), - Math100.interpolate(m_dCurvatureDsRad_M2, other.m_dCurvatureDsRad_M2, x)); + GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x), + MathUtil.interpolate(m_headingRateRad_M, other.m_headingRateRad_M, 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()); + /** + * R2 (xy) planar distance only (IGNORES ROTATION) so that planar + * 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()); } - public boolean equals(final Object other) { + public boolean equals(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_waypoint.equals(p2dwc.m_waypoint)) { + if (DEBUG) + System.out.println("wrong waypoint"); + return false; + } + if (!Math100.epsilonEquals(m_headingRateRad_M, p2dwc.m_headingRateRad_M)) { + 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; + } + 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", - m_pose.translation().getX(), - m_pose.translation().getY(), - m_pose.heading().getRadians(), - m_pose.course(), - m_headingRate, - m_curvatureRad_M, - m_dCurvatureDsRad_M2); + "x %5.3f, y %5.3f, theta %5.3f, course %s, dtheta %5.3f, curvature %5.3f", + m_waypoint.pose().getTranslation().getX(), + m_waypoint.pose().getTranslation().getY(), + m_waypoint.pose().getRotation().getRadians(), + m_waypoint.course(), + m_headingRateRad_M, + m_curvatureRad_M); } } \ No newline at end of file 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/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/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/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/geometry/WaypointSE2.java b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java new file mode 100644 index 000000000..1e3c2a81f --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/WaypointSE2.java @@ -0,0 +1,85 @@ +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; + +/** + * 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 + * 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; + + /** Course without rotation, with unit scale. */ + public static WaypointSE2 irrotational(Pose2d p, double course, double scale) { + return new WaypointSE2(p, DirectionSE2.irrotational(course), 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/localization/AprilTagRobotLocalizer.java b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java index 7f91792ec..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; @@ -433,9 +432,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/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..b8cf0269a 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.WaypointSE2; 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; @@ -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 { @@ -515,9 +515,9 @@ public class Pose2dWithMotionLogger { public void log(Supplier vals) { if (!allow(m_level)) return; - HolonomicPose2d val = vals.get().getPose(); + WaypointSE2 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/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 b7684a7b8..874b8d4fa 100644 --- a/lib/src/main/java/org/team100/lib/state/ControlR3.java +++ b/lib/src/main/java/org/team100/lib/state/ControlR3.java @@ -1,9 +1,9 @@ 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; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -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() { @@ -124,13 +124,13 @@ 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(); + 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(); 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; @@ -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/state/ModelR3.java b/lib/src/main/java/org/team100/lib/state/ModelR3.java index 388d73add..7816cc8e3 100644 --- a/lib/src/main/java/org/team100/lib/state/ModelR3.java +++ b/lib/src/main/java/org/team100/lib/state/ModelR3.java @@ -1,9 +1,11 @@ package org.team100.lib.state; import org.team100.lib.geometry.GlobalVelocityR2; -import org.team100.lib.geometry.GlobalVelocityR3; +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; +import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -31,7 +33,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 +42,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 +101,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() { @@ -127,15 +129,18 @@ 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(); + public static ModelR3 fromTimedState(TimedState timedPose) { + Pose2dWithMotion state = timedPose.state(); + WaypointSE2 pose = state.getPose(); + Translation2d translation = pose.pose().getTranslation(); + double xx = translation.getX(); + double yx = translation.getY(); + double thetax = pose.pose().getRotation().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/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..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,8 +4,9 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; +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; import org.team100.lib.subsystems.r3.VelocitySubsystemR3; @@ -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(); - HolonomicPose2d startWaypoint = new HolonomicPose2d( - pose.getTranslation(), - pose.getRotation(), - startVelocity.angle().orElse(toGoal.getAngle())); - HolonomicPose2d endWaypoint = new HolonomicPose2d( - m_goal.getTranslation(), - m_goal.getRotation(), - m_endVelocity.angle().orElse(toGoal.getAngle())); + VelocitySE2 startVelocity = m_drive.getState().velocity(); + WaypointSE2 startWaypoint = new WaypointSE2( + pose, + DirectionSE2.irrotational(startVelocity.angle().orElse(toGoal.getAngle())), + 1); + WaypointSE2 endWaypoint = new WaypointSE2( + m_goal, + DirectionSE2.irrotational(m_endVelocity.angle().orElse(toGoal.getAngle())), + 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 d31249342..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,8 @@ import java.util.List; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.reference.r3.TrajectoryReferenceR3; import org.team100.lib.subsystems.r3.PositionSubsystemR3; @@ -20,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 WaypointSE2 m_goal; private final Rotation2d m_course; private final TrajectoryPlanner m_trajectoryPlanner; @@ -30,7 +31,7 @@ public GoToPosePosition( LoggerFactory parent, PositionSubsystemR3 subsystem, Rotation2d course, - HolonomicPose2d goal, + WaypointSE2 goal, TrajectoryPlanner trajectoryPlanner) { m_log = parent.type(this); m_subsystem = subsystem; @@ -42,8 +43,9 @@ public GoToPosePosition( @Override public void initialize() { - HolonomicPose2d m_currentPose = HolonomicPose2d.make( - m_subsystem.getState().pose(), m_course); + WaypointSE2 m_currentPose = new WaypointSE2( + m_subsystem.getState().pose(), + 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/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..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 @@ -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; @@ -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); @@ -287,7 +290,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 +303,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/FixedTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java index eab775a33..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,13 +59,13 @@ 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(); - 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/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/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java index 6d28540b5..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 @@ -4,13 +4,13 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.HolonomicPose2d; +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; 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; @@ -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))); + WaypointSE2.tank(m_drive.getPose()), + WaypointSE2.tank(m_goal))); m_viz.setViz(m_trajectory); } @@ -61,13 +61,13 @@ 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(); - 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/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/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md index db8bb6bf6..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,12 +11,12 @@ 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. -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. \ No newline at end of file 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..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,13 +3,17 @@ import java.util.ArrayList; import java.util.List; -import org.team100.lib.trajectory.timing.TimedPose; +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. */ public class Trajectory100 { - private final List m_points; + private final List m_points; private final double m_duration; public Trajectory100() { @@ -18,21 +22,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,15 +46,15 @@ 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); - double betweenPoints = ceil.getTimeS() - floor.getTimeS(); - if (Math.abs(betweenPoints) <= 1e-12) { + final TimedState floor = getPoint(i - 1); + 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()); @@ -70,7 +73,7 @@ public int length() { return m_points.size(); } - public TimedPose getLastPoint() { + public TimedState getLastPoint() { return m_points.get(length() - 1); } @@ -78,11 +81,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); } @@ -97,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/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 2d3936c74..8d1253c0d 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -3,8 +3,9 @@ import java.util.List; import java.util.function.Function; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +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; @@ -27,6 +28,7 @@ * 4. assign timestamps to each step */ public class TrajectoryPlanner { + 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 @@ -101,24 +103,20 @@ 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)); } 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) { @@ -132,34 +130,33 @@ 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 HolonomicPose2d( - startTranslation, - startState.rotation(), - startingAngle), - new HolonomicPose2d( - endTranslation, - endState.rotation(), - courseToGoal)), + new WaypointSE2( + startState.pose(), + DirectionSE2.irrotational(startingAngle), + e1), + new WaypointSE2( + endState.pose(), + DirectionSE2.irrotational(courseToGoal), + 1.2)), startVelocity.norm(), - endVelocity.norm(), - magicNumbers); + endVelocity.norm()); } catch (TrajectoryGenerationException e) { System.out.println("WARNING: Trajectory Generation Exception"); return new Trajectory100(); } } - 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) { @@ -171,30 +168,30 @@ 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 HolonomicPose2d( - startTranslation, - startState.rotation(), - startCourse), - new HolonomicPose2d( - endTranslation, - endState.rotation(), - endCourse)), + new WaypointSE2( + startState.pose(), + DirectionSE2.irrotational(startCourse), + e1), + new WaypointSE2( + endState.pose(), + DirectionSE2.irrotational(endCourse), + 1.2)), splineEntranceVelocity, - splineExitVelocity, - magicNumbers); + splineExitVelocity); } catch (TrajectoryGenerationException e) { System.out.println("WARNING: Trajectory Generation Exception"); return new Trajectory100(); } } - 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); } /** @@ -207,10 +204,11 @@ public Trajectory100 restToRest(Pose2d start, Pose2d end) { Rotation2d courseToGoal = endTranslation.minus(startTranslation).getAngle(); try { - return restToRest( - List.of( - new HolonomicPose2d(startTranslation, start.getRotation(), courseToGoal), - new HolonomicPose2d(endTranslation, end.getRotation(), courseToGoal))); + // 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; } @@ -220,22 +218,28 @@ 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 { // Create a path from splines. Path100 path = PathFactory.pathFromWaypoints( waypoints, + m_trajectoryStep, 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. @@ -245,32 +249,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/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java index 00cdbaeb9..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 @@ -3,17 +3,30 @@ import java.util.ArrayList; import java.util.List; +import org.team100.lib.geometry.Metrics; 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 { + 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 + // scheduler can't see + // TODO: make this a constructor parameter. + private static final double INTERPOLATION_LIMIT = 0.3; private final List m_points; - /** in meters */ + /** + * Translational distance, just the xy plane, not the Twist arc + * or anything else, just xy distance. + */ private final double[] m_distances; public Path100(final List states) { @@ -26,8 +39,10 @@ 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); + double dist = Metrics.translationalDistance(p0.getPose().pose(), p1.getPose().pose()); + m_distances[i] = m_distances[i - 1] + dist; } } @@ -39,7 +54,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); @@ -52,40 +67,52 @@ 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. + * Walks through all the points to find the bracketing points, and then + * interpolates between them. * * @param distance in meters, always a non-negative number. */ 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; + 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) + 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(); @@ -97,4 +124,36 @@ public String toString() { } return builder.toString(); } + + //////////////////////////////////////////////////////////////////// + /// + /// DANGER ZONE + /// + /// Don't use anything here unless you know what you're doing. + /// + + /** + * Samples the entire path evenly by distance. Since the spline parameterizer + * now contains a pathwise distance limit, you shouldn't need this anymore. + */ + 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 (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 (DEBUG) + System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state); + samples[i] = state; + } + return samples; + } } 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..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 @@ -4,41 +4,21 @@ import java.util.List; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +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; -import org.team100.lib.trajectory.path.spline.SplineUtil; 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.Transform2d; import edu.wpi.first.math.geometry.Twist2d; public class PathFactory { + private static final boolean DEBUG = false; public static Path100 pathFromWaypoints( - List waypoints, - double maxDx, - double maxDy, - double maxDTheta, - final List mN) { - 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))); - } - // does not force C1, theta responds too much - // SplineUtil.forceC1(splines); - SplineUtil.optimizeSpline(splines); - return new Path100(PathFactory.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); - } - - public static Path100 pathFromWaypoints( - List waypoints, + List waypoints, + double maxNorm, double maxDx, double maxDy, double maxDTheta) { @@ -46,36 +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))); } - // 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) { - Translation2d p0 = waypoints.get(i - 1).getTranslation(); - 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))); - } - // 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)); + return new Path100(parameterizeSplines(splines, maxNorm, maxDx, maxDy, maxDTheta)); } /** @@ -94,6 +45,7 @@ public static Path100 withoutControlPoints( */ static List parameterizeSpline( HolonomicSpline s, + double maxNorm, double maxDx, double maxDy, double maxDTheta, @@ -103,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) { @@ -119,46 +72,66 @@ 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); - List samples = parameterizeSpline(s, maxDx, maxDy, maxDTheta, 0.0, 1.0); + if (DEBUG) + System.out.printf("SPLINE:\n%d\n%s\n", i, s); + List samples = parameterizeSpline(s, maxNorm, maxDx, maxDy, maxDTheta, 0.0, 1.0); samples.remove(0); rv.addAll(samples); } return rv; } + /** + * 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 s, + HolonomicSpline spline, + double maxNorm, // max distance between points List rv, - double t0, - double t1, + double t0, // [0,1] not time + double t1, // [0,1] not time double maxDx, double maxDy, double maxDTheta) { - Pose2d p0 = s.getPose2d(t0); - Pose2d phalf = s.getPose2d(t0 + (t1 - t0) * .5); - 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); + Pose2d p0 = spline.getPose2d(t0); + double thalf = (t0 + t1) / 2; + Pose2d phalf = spline.getPose2d(thalf); + Pose2d p1 = spline.getPose2d(t1); - 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); + + // 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 + || Math.abs(error.getTranslation().getY()) > maxDy + || Math.abs(error.getRotation().getRadians()) > maxDTheta + || Metrics.translationalNorm(twist_full) > maxNorm + || Metrics.l2Norm(twist_full) > maxNorm + || Metrics.l2Norm(p2t) > maxNorm) { + // add a point in between - 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); + getSegmentArc(spline, maxNorm, rv, t0, thalf, maxDx, maxDy, maxDTheta); + getSegmentArc(spline, maxNorm, rv, thalf, t1, maxDx, maxDy, maxDTheta); } else { - rv.add(s.getPose2dWithMotion(t1)); + // midpoint is close enough, this looks good + 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 5cab58027..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 @@ -1,11 +1,9 @@ package org.team100.lib.trajectory.path.spline; -import java.util.Optional; - -import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.Metrics; import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.util.Math100; +import org.team100.lib.geometry.WaypointSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,397 +21,216 @@ * 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; - // 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; - 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; - - /** - * 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(HolonomicPose2d p0, HolonomicPose2d p1) { - this(p0, p1, 1.2, 1.2); - } + private final Rotation2d m_heading0; /** * Specify the magic number you want: this scales the derivatives at the * 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. + * 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. * - * You'll probably want to call SplineUtil.forceC1() and - * SplineUtil.optimizeSpline() after creating these segments. + * 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. * - * Magic numbers also scale theta endpoints, so that the translation and - * rotation splines track together. + * To avoid confusion, the parameter should always be called "s". * - * @param p0 starting pose - * @param p1 ending pose - * @param mN0 starting magic number - * @param mN1 ending magic number + * @param p0 starting pose + * @param p1 ending pose */ - public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, double mN1) { - double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); - double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); + public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) { + // 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; + double scale1 = p1.scale() * 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()); - - 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; - // second derivatives are zero at the ends + + // Endpoints: + 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.pose().getRotation(); + double delta = p1.pose().getRotation().minus(p0.pose().getRotation()).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 = p1.course().y * scale1; + double dtheta0 = p0.course().theta * scale0; + double dtheta1 = p1.course().theta * scale1; + + // Second derivatives are zero: 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; - double dy1 = course1.getY() * scale1; - // second derivatives are zero at the ends 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_r0 = 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_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. */ - private HolonomicSpline( - SplineR1 x, - SplineR1 y, - SplineR1 theta, - Rotation2d r0) { - m_x = x; - m_y = y; - m_theta = theta; - m_r0 = r0; - } - - public Pose2dWithMotion getPose2dWithMotion(double p) { + /** + * TODO: eliminate the waypoint here, for sure eliminate the scale. + * + * @param s [0,1] + */ + public Pose2dWithMotion getPose2dWithMotion(double s) { return new Pose2dWithMotion( - new HolonomicPose2d( - getPoint(p), - getHeading(p), - getCourse(p).orElseThrow()), - getDHeadingDs(p), - getCurvature(p), - getDCurvatureDs(p)); + new WaypointSE2(getPose2d(s), getCourse(s), 1), // <<< TODO: remove the "1" + getDHeadingDs(s), + getCurvature(s)); } /** - * 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. + * 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 Optional 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)); + 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(getPoint(p), getHeading(p)); + public Pose2d getPose2d(double s) { + return new Pose2d(new Translation2d(x(s), y(s)), getHeading(s)); } //////////////////////////////////////////////////////////////////////// - protected Rotation2d getHeading(double t) { - return m_r0.rotateBy(Rotation2d.fromRadians(m_theta.getPosition(t))); - } - - protected double getDHeading(double t) { - return m_theta.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_theta.getPosition(0), - m_theta.getPosition(1), - dtheta0, - dtheta1, - m_theta.getAcceleration(0), - m_theta.getAcceleration(1)), - m_r0); - } - - /** - * 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_theta, - m_r0); + 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); - } - - /** - * 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); - 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. * - * @param t ranges from 0 to 1 - * @return the point on the spline for that t value + * TODO: elsewhere this is combined with R2 pathwise velocity, so this is wrong. */ - protected Translation2d getPoint(double t) { - return new Translation2d(x(t), y(t)); - } - - double x(double t) { - return m_x.getPosition(t); - } - - double y(double t) { - return m_y.getPosition(t); - } - - double theta(double t) { - return getHeading(t).getRadians(); + public double getDHeadingDs(double s) { + return getDHeading(s) / getVelocity(s); } - double dx(double t) { - return m_x.getVelocity(t); + /** x at s */ + public double x(double s) { + return m_x.getPosition(s); } - double dy(double t) { - return m_y.getVelocity(t); + /** y at s */ + double y(double s) { + return m_y.getPosition(s); } - double dtheta(double t) { - return m_theta.getVelocity(t); + /** heading at s */ + private Rotation2d getHeading(double s) { + double headingFromZero = m_heading.getPosition(s); + return m_heading0.rotateBy(Rotation2d.fromRadians(headingFromZero)); } - double ddx(double t) { - return m_x.getAcceleration(t); + /** dx/ds */ + public double dx(double s) { + return m_x.getVelocity(s); } - double ddy(double t) { - return m_y.getAcceleration(t); + /** dy/ds */ + double dy(double s) { + return m_y.getVelocity(s); } - double ddtheta(double t) { - return m_theta.getAcceleration(t); + /** dheading/ds */ + double dtheta(double s) { + return m_heading.getVelocity(s); } - double dddx(double t) { - return m_x.getJerk(t); + /** d^2x/ds^2 */ + public double ddx(double s) { + return m_x.getAcceleration(s); } - double dddy(double t) { - return m_y.getJerk(t); + /** d^2y/ds^2 */ + double ddy(double s) { + return m_y.getAcceleration(s); } - double dddtheta(double t) { - return m_theta.getJerk(t); + /** 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. */ - protected double getVelocity(double t) { - return Math.hypot(dx(t), dy(t)); + private double getVelocity(double 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); } /** * 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. :-) */ - protected double getCurvature(double t) { - double dx = dx(t); - double dy = dy(t); - double ddx = ddx(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)); + 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) { + // this isn't really zero + return 0; } - return maxC; + return (dx * ddy - ddx * dy) / Math.pow(d, 1.5); } - - /** 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/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..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 @@ -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.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; import edu.wpi.first.math.geometry.Rotation3d; @@ -25,30 +29,35 @@ 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; - 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) { - double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); - double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); + public HolonomicSpline3d(Pose3dWithDirection p0, Pose3dWithDirection p1, double mN0, double mN1) { + double scale0 = mN0 * p0.translation().getDistance(p1.translation()); + double scale1 = mN1 * p0.translation().getDistance(p1.translation()); - Translation3d course0 = new Translation3d(1, 0, 0).rotateBy(p0.course()); - Translation3d course1 = new Translation3d(1, 0, 0).rotateBy(p1.course()); + DirectionSE3 course0 = p0.course(); + DirectionSE3 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/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/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/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java index 9ab7e4f07..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,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 = false; 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; } /** @@ -38,11 +48,35 @@ public CapsizeAccelerationConstraint( * If the curvature is zero, this will return infinity. */ @Override - public NonNegativeDouble getMaxVelocity(final Pose2dWithMotion state) { - double mMaxCentripetalAccel = m_limits.getMaxCapsizeAccelM_S2() * m_scale.getAsDouble(); - double radius = 1 / state.getCurvature(); + public double maxV(final Pose2dWithMotion state) { + double radius = 1 / Math.abs(state.getCurvatureRad_M()); // abs is used here to make sure sqrt is happy. - return new NonNegativeDouble(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) { + if (DEBUG) + System.out.println("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) { + if (DEBUG) + System.out.println("too fast for the curvature, slowing down is ok"); + return m_maxDecel * m_scale.getAsDouble(); + } + double decel = -Math.sqrt(alongsq); + if (DEBUG) + System.out.printf("decel %f\n", decel); + return decel; } /** @@ -56,19 +90,13 @@ 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(); - - 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); + private double alongSq(Pose2dWithMotion state, double velocity) { + 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/ConstantConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java index 94989ef4d..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 @@ -21,13 +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) { - 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 deleted file mode 100644 index ba53cc3c0..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstrainedState.java +++ /dev/null @@ -1,95 +0,0 @@ -package org.team100.lib.trajectory.timing; - -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 { - // 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 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) { - NonNegativeDouble constraintVel = constraint.getMaxVelocity(m_state); - double value = constraintVel.getValue(); - setVelocityM_S(Math.min(getVelocityM_S(), value)); - } - } - - /** - * Clamp constraint state accelerations to the 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); - } - - } - - public Pose2dWithMotion getState() { - return m_state; - } - - public double getDistanceM() { - return m_distanceM; - } - - 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; - } -} \ No newline at end of file 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..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,21 +33,26 @@ public DiamondConstraint(LoggerFactory parent, double maxVX, double maxVY, doubl } @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - Rotation2d course = state.getPose().course(); - Rotation2d heading = state.getPose().heading(); + public double maxV(Pose2dWithMotion state) { + Rotation2d course = state.getPose().course().toRotation(); + 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 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) { - return new MinMaxAcceleration(-m_maxAccel.getAsDouble(), m_maxAccel.getAsDouble()); + public double maxAccel(Pose2dWithMotion state, double velocityM_S) { + // TODO: this should also have a diamond shape + 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/JointConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java deleted file mode 100644 index ae448a422..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/JointConstraint.java +++ /dev/null @@ -1,123 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import org.team100.lib.geometry.GlobalAccelerationR3; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; -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) { - HolonomicPose2d pose = state.getPose(); - // Velocity if translation speed were 1.0 m/s. - GlobalVelocityR3 v = new GlobalVelocityR3( - state.getCourse().getCos(), - state.getCourse().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()); - - GlobalVelocityR3 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(); - - 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 - GlobalVelocityR3 v = new GlobalVelocityR3(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); - 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); - - GlobalAccelerationR3 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/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 7f440dd9b..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 @@ -6,25 +6,32 @@ 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 * schedule. */ public class ScheduleGenerator { + + 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 */ + + /** 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; - /** 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, @@ -32,15 +39,8 @@ 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)); - samples.add(state); - } + // Pose2dWithMotion[] samples = path.resample(step); + Pose2dWithMotion[] samples = path.resample(); return timeParameterizeTrajectory(samples, start_vel, end_vel); } catch (TimingException e) { e.printStackTrace(); @@ -50,262 +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. */ - private Trajectory100 timeParameterizeTrajectory( - List samples, + public Trajectory100 timeParameterizeTrajectory( + Pose2dWithMotion[] samples, double start_vel, - double end_vel) throws TimingException { - List constrainedStates = forwardPass(samples, start_vel); - Pose2dWithMotion lastState = samples.get(samples.size() - 1); - backwardsPass(lastState, end_vel, constrainedStates); - return integrate(constrainedStates); + 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); } /** - * 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). + * Creates a list of timed states. */ - private List forwardPass(List samples, double start_vel) { - 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 (Pose2dWithMotion sample : samples) { - double dsM = sample.distanceM(predecessor.getState()); - ConstrainedState constrainedState = new ConstrainedState( - sample, dsM + predecessor.getDistanceM()); - constrainedStates.add(constrainedState); - forwardWork(predecessor, constrainedState); - predecessor = constrainedState; + 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 constrainedStates; + return timedStates; } - private void forwardWork(ConstrainedState s0, ConstrainedState s1) { - // constant-twist path length between states - double dsM = s1.getState().distanceM(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. - while (true) { - // first try the previous state accel to get the new state velocity - double v1 = v1(s0.getVelocityM_S(), s0.getMaxAccel(), dsM); - s1.setVelocityM_S(v1); - - // also use max accels for the new state accels - s1.setMinAccel(-HIGH_ACCEL); - s1.setMaxAccel(HIGH_ACCEL); - - // reduce velocity according to constraints - s1.clampVelocity(m_constraints); - - // reduce accel according to constraints - s1.clampAccel(m_constraints); - - // motionless - if (Math.abs(dsM) < EPSILON) { - return; - } - - double accel = accel(s0.getVelocityM_S(), s1.getVelocityM_S(), dsM); - 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 - s0.setMaxAccel(s1.getMaxAccel()); - continue; - } - if (accel > s0.getMinAccel() + EPSILON) { - // set the previous state accel to whatever the constrained velocity implies - s0.setMaxAccel(accel); - } - return; + /** + * 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; } /** - * Backwards pass + * 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 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(lastState, endState.getDistanceM()); - successor.setVelocityM_S(end_velocity); - successor.setMinAccel(-HIGH_ACCEL); - successor.setMaxAccel(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; + 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; } - /** 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(); - if (ds > 0) { - // must be negative if we're walking backwards. - throw new IllegalStateException(); + /** + * 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; + } - 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.getVelocityM_S(), s1.getMinAccel(), ds); - - if (s0.getVelocityM_S() <= 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.clampAccel(m_constraints); - - // motionless - if (Math.abs(ds) < EPSILON) { - return; + /** + * 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; } - - // implied accel using the constrained v0 - double accel = accel(s1.getVelocityM_S(), s0.getVelocityM_S(), ds); - if (accel < s0.getMinAccel() - EPSILON) { - // accel is too low which implies that s1 accel is too low, try again - s1.setMinAccel(s0.getMinAccel()); - continue; + // 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); } - // set final accel to the implied value - s1.setMinAccel(accel); - return; } } /** - * Integrate the constrained states forward in time to obtain the TimedStates. + * Adjusts velocities[i] for decel constraint based on the state at i+1. * - * last state accel is always zero, which might be wrong. + * 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 static Trajectory100 integrate(List states) throws TimingException { - List poses = new ArrayList<>(states.size()); - double time = 0.0; // time along path - double distance = 0.0; // distance along path - double v0 = 0.0; - 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 dt = 0.0; - if (i > 0) { - double prevAccel = accel(v0, v1, ds); - poses.get(i - 1).set_acceleration(prevAccel); - dt = dt(v0, v1, ds, prevAccel); + 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; } - time += dt; - if (Double.isNaN(time) || Double.isInfinite(time)) { - throw new TimingException(); + 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); } - poses.add(new TimedPose(state.getState(), time, v1, 0)); - v0 = v1; - distance = state.getDistanceM(); } - return new Trajectory100(poses); } /** - * If accelerating, find the time to go from v0 to v1. Otherwise find the time - * to go distance ds at speed v0. + * Computes the length of each arc and accumulates. */ - private static double dt( - double v0, - double v1, - double ds, - double accel) throws TimingException { - if (Math.abs(accel) > EPSILON) { - return (v1 - v0) / accel; + 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]; } - if (Math.abs(v0) > EPSILON) { - return ds / v0; + return distances; + } + + /** + * 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)); } - throw new TimingException(); + return minVelocity; } /** - * 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 + * Returns the lowest (i.e. closest to zero) acceleration constraint from the + * list of constraints. Always positive or zero. */ - 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); + 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; } /** - * 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 + * Returns the highest (i.e. closest to zero) deceleration constraint from the + * list of constraints. Always negative or zero. */ - static double accel(double v0, double v1, 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 - */ - return (v1 * v1 - v0 * v0) / (2.0 * ds); + 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; } - public static class TimingException extends Exception { + private static double dt( + double v0, + double v1, + 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) { + // If moving, find the time to go distance dq at speed v0. + return arcLength / v0; + } + return 0; } } \ 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 257963f76..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 @@ -20,6 +20,7 @@ * omega limit calculation is correct. */ public class SwerveDriveDynamicsConstraint implements TimingConstraint { + private static final boolean DEBUG = false; private final SwerveKinodynamics m_limits; private final Mutable vScale; private final Mutable aScale; @@ -41,12 +42,12 @@ 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.getCourse(); - - Rotation2d course_local = course.minus(state.getPose().heading()); + Rotation2d course = state.getPose().course().toRotation(); + Rotation2d heading = state.getPose().pose().getRotation(); + Rotation2d course_local = course.minus(heading); double vx = course_local.getCos(); double vy = course_local.getSin(); // rad/m @@ -63,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() { @@ -78,15 +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); - return new MinMaxAcceleration( - minAccel, - maxAccel); + // 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 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/TimedPose.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java deleted file mode 100644 index 6102dcede..000000000 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedPose.java +++ /dev/null @@ -1,103 +0,0 @@ -package org.team100.lib.trajectory.timing; - -import org.team100.lib.geometry.Pose2dWithMotion; - -import edu.wpi.first.math.MathUtil; - -/** - * Represents a state within a 2d holonomic trajectory, i.e. with heading - * independent from course. - * - * The timing fields are set by the ScheduleGenerator. - */ -public class TimedPose { - private final Pose2dWithMotion m_state; - /** Time we achieve this state. */ - private final double m_timeS; - /** ds/dt */ - private final double m_velocityM_S; - /** d^2s/dt^2 */ - private double m_accelM_S_S; - - public TimedPose( - Pose2dWithMotion state, - double t, - double velocity, - double acceleration) { - m_state = state; - m_timeS = t; - m_velocityM_S = velocity; - m_accelM_S_S = acceleration; - } - - public Pose2dWithMotion state() { - return m_state; - } - - public double getTimeS() { - return m_timeS; - } - - 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; - } - - /** this means acceleration along the path, not centripetal acceleration. */ - public double acceleration() { - return m_accelM_S_S; - } - - @Override - public String toString() { - return String.format("state %s, time %5.3f, vel %5.3f, acc %5.3f", - m_state, - m_timeS, - m_velocityM_S, - 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); - - double interpolant = new_s / m_state.distanceM(other.m_state); - if (Double.isNaN(interpolant)) { - interpolant = 1.0; - } - - return new TimedPose( - m_state.interpolate(other.m_state, interpolant), - new_t, - new_v, - m_accelM_S_S); - } - - public double distance(TimedPose other) { - return m_state.distanceM(other.m_state); - } - - @Override - public boolean equals(final Object other) { - if (!(other instanceof TimedPose)) { - return false; - } - TimedPose ts = (TimedPose) other; - boolean stateEqual = m_state.equals(ts.m_state); - if (!stateEqual) { - return false; - } - return Math.abs(m_timeS - ts.m_timeS) <= 1e-12; - } -} 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 new file mode 100644 index 000000000..fbbab2cd7 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java @@ -0,0 +1,115 @@ +package org.team100.lib.trajectory.timing; + +import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.util.Math100; + +/** + * Represents a state within a 2d holonomic trajectory, i.e. with heading + * independent from course. + * + * The timing fields are set by the ScheduleGenerator. + */ +public class TimedState { + private static final boolean DEBUG = false; + private final Pose2dWithMotion m_state; + /** Time we achieve this state. */ + private final double m_timeS; + /** Instantaneous pathwise velocity, m/s. */ + private final double m_velocityM_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, + double t, + double velocity, + double acceleration) { + m_state = state; + m_timeS = t; + m_velocityM_S = velocity; + m_accelM_S_S = acceleration; + } + + 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; + } + + /** Instantaneous pathwise (not centripetal) acceleration, m/s^2. */ + public double acceleration() { + return m_accelM_S_S; + } + + @Override + public String toString() { + return String.format("state %s, time %5.3f, vel %5.3f, acc %5.3f", + m_state, + m_timeS, + m_velocityM_S, + m_accelM_S_S); + } + + /** + * Linear interpolation by time. + * + * Velocity of this state is the initial velocity. + * Acceleration of this state is constant through the whole arc. + */ + 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; + + 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 TimedState( + m_state.interpolate(other.m_state, interpolant), + tLerp, + vLerp, + m_accelM_S_S); + } + + /** Translation only, ignores rotation */ + public double distanceCartesian(TimedState other) { + return m_state.distanceCartesian(other.m_state); + } + + @Override + public boolean equals(final Object other) { + if (!(other instanceof TimedState)) { + if (DEBUG) + System.out.println("wrong type"); + return false; + } + TimedState ts = (TimedState) other; + if (!m_state.equals(ts.m_state)) { + if (DEBUG) + System.out.println("wrong state"); + return false; + } + 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/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 a75bcdc31..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 @@ -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; @@ -45,24 +46,34 @@ 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) { - Rotation2d course = state.getCourse(); + 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 Translation2d u = new Translation2d(1.0, course); - Translation2d r = state.getPose().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) { 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); + 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 e47aafb73..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) { - final Translation2d translation = state.getPose().translation(); + 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/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index fecc12e75..47641cc09 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) { @@ -49,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; @@ -92,4 +95,98 @@ 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, dx. + * + * a = (v1^2 - v0^2) / 2dx + * + * note dx can be negative, which implies negative time. + * + * @param v0 initial velocity + * @param v1 final velocity + * @param dx distance + */ + 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 = 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 + */ + return (v1 * v1 - v0 * v0) / (2.0 * dx); + } + + /** + * Return final velocity, v1, given initial velocity, v0, and acceleration over + * distance dx. + * + * v1 = sqrt(v0^2 + 2adx) + * + * note a can be negative. + * + * note dx can be negative, which implies backwards time + * + * @param v0 initial velocity + * @param a acceleration + * @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 v0(double v1, 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 + * v0 = sqrt(v1^2 - 2*a*dx) + */ + return Math.sqrt(v1 * v1 - 2.0 * a * dx); + } + } 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..b73070566 100644 --- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java +++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java @@ -2,12 +2,12 @@ import java.util.List; -import org.team100.lib.geometry.HolonomicPose2d; +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; 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,11 +31,11 @@ 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()) { - HolonomicPose2d pose = p.state().getPose(); - arr[ndx + 0] = pose.translation().getX(); - arr[ndx + 1] = pose.translation().getY(); - arr[ndx + 2] = pose.heading().getDegrees(); + 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(); + arr[ndx + 2] = pose.pose().getRotation().getDegrees(); ndx += 3; } return arr; 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..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 @@ -5,7 +5,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,7 +22,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 +45,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 +68,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..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 @@ -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.HolonomicPose2d; +import org.team100.lib.geometry.DeltaSE2; +import org.team100.lib.geometry.WaypointSE2; 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; @@ -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; @@ -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), @@ -178,11 +178,13 @@ void testErrZero() { 0.01, 0.02); ModelR3 measurement = new ModelR3(); ModelR3 currentReference = ModelR3 - .fromTimedPose(new TimedPose( + .fromTimedState(new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 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); @@ -194,9 +196,11 @@ void testErrXAhead() { 0.01, 0.02); 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)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + .fromTimedState(new TimedState(new Pose2dWithMotion( + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 0, 0)); + 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); @@ -208,9 +212,11 @@ void testErrXBehind() { 0.01, 0.02); 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)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + .fromTimedState(new TimedState(new Pose2dWithMotion( + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 0, 0)); + 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); @@ -223,9 +229,11 @@ 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( - HolonomicPose2d.make(0, 0, 1, 0), 0, 0, 0), 0, 0, 0)); - GlobalDeltaR3 err = controller.positionError(measurement, currentReference); + .fromTimedState(new TimedState(new Pose2dWithMotion( + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(1)), 0, 1.2), + 0, 0), 0, 0, 0)); + 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); @@ -241,18 +249,17 @@ void testErrorAhead() { // setpoint is also at the origin Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), - 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; // constant speed 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); + 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); assertEquals(0, positionError.getRadians(), DELTA); @@ -267,16 +274,16 @@ 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, - 0, // no curvature - 0); // no change in curvature + WaypointSE2.irrotational( + new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), 0, 1.2), + 0, 0); double t = 0; // moving double velocity = 1; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalDeltaR3 positionError = controller.positionError(measurement, currentReference); + 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); assertEquals(0, positionError.getRadians(), DELTA); @@ -289,20 +296,18 @@ 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( - HolonomicPose2d.make(0, 0, 0, 0), 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; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalVelocityR3 error = controller.velocityError(measurement, currentReference); + 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); assertEquals(0, error.y(), DELTA); @@ -317,22 +322,21 @@ 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( - HolonomicPose2d.make(0, 0, 0, 0), - 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; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalVelocityR3 error = controller.velocityError(measurement, currentReference); + 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); assertEquals(-1, error.y(), DELTA); @@ -346,18 +350,18 @@ 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, - 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; // 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); - GlobalVelocityR3 speeds = controller.feedforward(nextReference); + ControlR3 nextReference = ControlR3.fromTimedState(setpoint); + VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); assertEquals(0, speeds.theta(), DELTA); @@ -370,18 +374,18 @@ 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, - 0, // no curvature - 0); // no change in curvature + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2), + 0, 0); double t = 0; // moving 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); - GlobalVelocityR3 speeds = controller.feedforward(nextReference); + ControlR3 nextReference = ControlR3.fromTimedState(setpoint); + VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); assertEquals(0, speeds.theta(), DELTA); @@ -394,17 +398,16 @@ 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), - 1, - 1, // driving and turning - 0); // no change in curvature + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 1, 1); double t = 0; // moving double velocity = 1; // constant speed double acceleration = 0; - ControlR3 nextReference = ControlR3.fromTimedPose(new TimedPose(state, t, velocity, acceleration)); - GlobalVelocityR3 speeds = controller.feedforward(nextReference); + 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); assertEquals(0, speeds.y(), DELTA); @@ -418,17 +421,15 @@ 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); + 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; - // 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; @@ -436,11 +437,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); + 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 assertEquals(0, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -454,17 +455,15 @@ 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); + 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; - // 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; @@ -472,11 +471,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); + 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 assertEquals(0, speeds.x(), DELTA); assertEquals(-1, speeds.y(), DELTA); @@ -490,17 +489,15 @@ 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); + 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; - // 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; @@ -508,11 +505,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); + 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 // but needs negative rotation // setpoint should be negative theta @@ -529,17 +526,15 @@ 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); + 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; - // 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; @@ -547,10 +542,10 @@ void testFeedbackSideways() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(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); + new VelocitySE2(1, 0, 0)); + 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 assertEquals(0, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -564,17 +559,15 @@ 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); + 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; - // 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; @@ -582,10 +575,10 @@ void testFeedbackSidewaysPlusY() { double acceleration = 0; ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(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); + new VelocitySE2(1, 0, 0)); + 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 assertEquals(0, speeds.x(), DELTA); assertEquals(-1, speeds.y(), DELTA); @@ -599,17 +592,15 @@ 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); + 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; - // 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; @@ -618,11 +609,11 @@ void testFullFeedbackAhead() { // motion is on setpoint ModelR3 measurement = new ModelR3( currentState, - new GlobalVelocityR3(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); + new VelocitySE2(1, 0, 0)); + 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); // we're exactly on the setpoint so zero feedback assertEquals(0, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -637,17 +628,15 @@ 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); + 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; - // 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; @@ -656,11 +645,11 @@ void testFullFeedbackSideways() { // measurement is too slow ModelR3 measurement = new ModelR3( currentPose, - new GlobalVelocityR3(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); + new VelocitySE2(0.5, 0, 0)); + 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); // speed up assertEquals(0.5, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -677,32 +666,30 @@ 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); + 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; - // 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; // 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. - 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 +697,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..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 @@ -9,8 +9,9 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.DirectionSE2; +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; @@ -129,21 +130,31 @@ void testTrajectoryDone() { @Test void testFieldRelativeTrajectory() { - List waypoints = new ArrayList<>(); - waypoints.add(new HolonomicPose2d( - new Translation2d(), Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(0))); - waypoints.add(new HolonomicPose2d( - new Translation2d(100, 4), Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(0))); - waypoints.add(new HolonomicPose2d( - new Translation2d(196, 13), Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0))); + List waypoints = new ArrayList<>(); + waypoints.add(new WaypointSE2( + new Pose2d( + new Translation2d(), + Rotation2d.fromDegrees(180)), + new DirectionSE2(1, 0, 0), 1)); + waypoints.add(new WaypointSE2( + new Pose2d( + new Translation2d(100, 4), + Rotation2d.fromDegrees(180)), + new DirectionSE2(1, 0, 0), 1)); + waypoints.add(new WaypointSE2( + new Pose2d( + new Translation2d(196, 13), + Rotation2d.fromDegrees(0)), + new DirectionSE2(1, 0, 0), 1)); 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, @@ -166,7 +177,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/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/util/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java similarity index 64% 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 939bb8527..327c3bed7 100644 --- a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java @@ -1,11 +1,11 @@ -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.GeometryUtil; +import org.team100.lib.trajectory.path.spline.HolonomicSpline; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Quaternion; @@ -17,8 +17,130 @@ 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.811, splineHR, DELTA); + Pose2d p0 = spline.getPose2d(0.49); + Pose2d p1 = spline.getPose2d(0.51); + double discreteHR = GeometryUtil.headingRatio(p0, p1); + assertEquals(0.811, 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() { { @@ -86,58 +208,6 @@ void testSlog() { assertEquals(1.571, twist.dtheta, DELTA); } - @Test - void testDistance2() { - assertEquals(1, GeometryUtil.distanceM(new Translation2d(1, 0), new Translation2d(0, 0)), DELTA); - } - - @Test - void testDistance3() { - 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); - - } - @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 new file mode 100644 index 000000000..7eb14297c --- /dev/null +++ b/lib/src/test/java/org/team100/lib/geometry/MetricsTest.java @@ -0,0 +1,71 @@ +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; + +public class MetricsTest { + private static final double DELTA = 0.001; + + @Test + void testDistance() { + // same pose => 0 + assertEquals(0, + Metrics.projectedDistance( + new Pose2d(1, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), + DELTA); + // 1d distance + assertEquals(1, + Metrics.projectedDistance( + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), + DELTA); + // 2d distance + assertEquals(1.414, + 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.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.projectedDistance( + new Pose2d(0, 1, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), + DELTA); + // order doesn't matter + assertEquals(1.571, + 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.projectedDistance( + 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); + // 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 2bc60e880..f398e4637 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,33 @@ 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); + assertEquals(0, Metrics.translationalNorm(between), EPSILON); + assertEquals(1, Metrics.doubleGeodesicDistance(start, end), EPSILON); + assertEquals(0, Metrics.translationalDistance(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.0, between.dtheta, EPSILON); + assertEquals(1.043, Metrics.translationalNorm(between), 0.001); + assertEquals(1.414, Metrics.doubleGeodesicDistance(start, end), 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/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/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/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/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..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,7 @@ 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; import edu.wpi.first.math.VecBuilder; @@ -77,7 +77,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 +92,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..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,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); }; TernarySearch s = new TernarySearch(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); }; TernarySearch s = new TernarySearch(f, 1e-3, 100); 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/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 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..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 @@ -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; @@ -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; @@ -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); @@ -258,10 +258,10 @@ 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(); - 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..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 @@ -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; @@ -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; @@ -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); @@ -235,10 +235,10 @@ 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(); - 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/r3/commands/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java index ff91daee8..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 @@ -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.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.logging.LoggerFactory; @@ -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,9 +55,11 @@ void testSimple() throws IOException { drive, (start, end) -> new Trajectory100( List.of( - new TimedPose( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 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..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.HolonomicPose2d; +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,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; /** @@ -46,8 +47,9 @@ 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))); + 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 e5cd84cf4..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 @@ -3,11 +3,11 @@ 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.HolonomicPose2d; +import org.team100.lib.geometry.WaypointSE2; 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; @@ -20,17 +20,19 @@ 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); } @Test - void testTimedPose() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -41,11 +43,13 @@ void testTimedPose() { } @Test - void testTimedPose2() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState2() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -56,11 +60,13 @@ void testTimedPose2() { } @Test - void testTimedPose3() { - ControlR3 s = ControlR3.fromTimedPose( - new TimedPose( + void testTimedState3() { + ControlR3 s = ControlR3.fromTimedState( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -72,12 +78,13 @@ 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( - HolonomicPose2d.make(0, 0, 0, 0), - 0, 1, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 1), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -91,7 +98,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 +109,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..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 @@ -4,11 +4,11 @@ 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.HolonomicPose2d; +import org.team100.lib.geometry.WaypointSE2; 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; @@ -21,17 +21,19 @@ 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); } @Test - void testTimedPose() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 0, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -42,11 +44,13 @@ void testTimedPose() { } @Test - void testTimedPose2() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState2() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 0, 1)); assertEquals(0, s.x().x(), DELTA); assertEquals(0, s.x().v(), DELTA); @@ -57,11 +61,13 @@ void testTimedPose2() { } @Test - void testTimedPose3() { - ModelR3 s = ModelR3.fromTimedPose( - new TimedPose( + void testTimedState3() { + ModelR3 s = ModelR3.fromTimedState( + new TimedState( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -73,11 +79,13 @@ 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( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 1), 0, 1, 0)); assertEquals(0, s.x().x(), DELTA); assertEquals(1, s.x().v(), DELTA); @@ -91,7 +99,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 +110,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/targeting/TargetsTest.java b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java index e410480ed..96940a0c8 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(); @@ -81,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), @@ -89,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(); @@ -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/ParameterizationTest.java b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java new file mode 100644 index 000000000..86953c109 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java @@ -0,0 +1,159 @@ +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 boolean DEBUG = false; + 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); + } + + /** + * 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 + */ + @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.1, 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); + + // this is wrong somehow + if (DEBUG) + System.out.printf("TRAJECTORY\n%s\n", trajectory); + + 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 new file mode 100644 index 000000000..b9d0391e0 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java @@ -0,0 +1,57 @@ +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; + +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; + } + + 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 new file mode 100644 index 000000000..1f1c0c4bd --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java @@ -0,0 +1,92 @@ +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.trajectory.path.spline.HolonomicSpline; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class SplineToVectorSeries { + + private static final double DS = 0.05; + /** Length of the vector indicating heading */ + private final double m_scale; + + public SplineToVectorSeries(double scale) { + m_scale = scale; + } + + /** + * Show little arrows. + * + * @return (x, y, dx, dy) + */ + public VectorSeries convert(String name, List splines) { + VectorSeries series = new VectorSeries(name); + for (HolonomicSpline spline : splines) { + 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(); + series.add(x, y, dx, dy); + } + + } + 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/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index a2065ca53..9c07bd26f 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -5,14 +5,20 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; 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.testing.Timeless; -import org.team100.lib.trajectory.timing.TimedPose; +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.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; @@ -21,7 +27,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()); @@ -36,26 +42,26 @@ void testPreviewAndAdvance() { Translation2d goalTranslation = end.getTranslation(); 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)); + List waypoints = List.of( + 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); Trajectory100 trajectory = planner.restToRest(waypoints); - TimedPose sample = trajectory.sample(0); - assertEquals(0, sample.state().getPose().translation().getX(), DELTA); + TimedState sample = trajectory.sample(0); + 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 @@ -68,22 +74,24 @@ void testSample() { Translation2d goalTranslation = end.getTranslation(); 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)); + List waypoints = List.of( + 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); Trajectory100 trajectory = planner.restToRest(waypoints); + if (DEBUG) + trajectory.dump(); - assertEquals(1.417, trajectory.duration(), DELTA); - TimedPose sample = trajectory.sample(0); - assertEquals(0, sample.state().getPose().translation().getX(), 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().translation().getX(), DELTA); + assertEquals(0.828, 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); } /** @@ -91,100 +99,133 @@ void testSample() { */ @Test void testSampleThoroughly() { - - List waypoints = List.of( - new HolonomicPose2d(new Translation2d(), Rotation2d.kZero, Rotation2d.kZero), - new HolonomicPose2d(new Translation2d(1, 0), Rotation2d.kZero, Rotation2d.kZero)); + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + new Translation2d(), + Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1)); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 trajectory = planner.restToRest(waypoints); + 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.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(1.415, trajectory.duration(), 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.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); + 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); + } + + /** + * 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 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)); + 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); - // 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); + long start = System.nanoTime(); + for (int rep = 0; rep < reps; ++rep) { + for (int t = 0; t < times; ++t) { + spline.getPose2dWithMotion(0.1 * t); + } + } + long end = System.nanoTime(); + long duration = end - start; + if (DEBUG) + 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.1, 0.02, 0.2, 0.1); + assertEquals(22.734, 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)); - /** Does the index help? No. */ - // There's no need to run this all the time - // @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)); + ///////////// + // INTERPOLATE TRAJECTORY POINTS (335 ns) SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + ScheduleGenerator generator = new ScheduleGenerator(constraints); - Trajectory100 trajectory = planner.restToRest(waypoints); + Trajectory100 trajectory = generator.timeParameterizeTrajectory(path, 0.1, 0, 0); + TrajectoryPlotter.plot(trajectory, 1); - assertEquals(1851, trajectory.length()); - int reps = 500000; - int times = 10; - long start = System.nanoTime(); + assertEquals(313, trajectory.length()); + + start = System.nanoTime(); for (int rep = 0; rep < reps; ++rep) { for (int t = 0; t < times; ++t) { trajectory.sample(0.1 * t); } } - long end = System.nanoTime(); - long duration = end - start; + end = System.nanoTime(); + duration = end - start; if (DEBUG) - System.out.printf("duration (ns) total (ms) %.0f per sample (ns) %.2f\n", + 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 9d8d9a5f9..1b07fdd98 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -7,8 +7,9 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GlobalVelocityR3; -import org.team100.lib.geometry.HolonomicPose2d; +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.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -19,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; @@ -33,44 +34,59 @@ 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())); + 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)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); Trajectory100 t = planner.restToRest(waypoints); - assertEquals(12, t.length()); - TimedPose p = t.getPoint(6); - assertEquals(0.6, p.state().getPose().translation().getX(), DELTA); + assertEquals(17, t.length()); + TimedState tp = t.getPoint(0); + // start at zero velocity + assertEquals(0, tp.velocityM_S(), DELTA); + TimedState p = t.getPoint(8); + assertEquals(0.5, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); } @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)); - SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest(logger); - + 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)); - 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().translation().getX(), DELTA); + Trajectory100 t = planner.restToRest(waypoints); + assertEquals(17, t.length()); + TimedState tp = t.getPoint(0); + assertEquals(0, tp.velocityM_S(), DELTA); + TimedState p = t.getPoint(8); + assertEquals(0.5, p.state().getPose().pose().getTranslation().getX(), DELTA); assertEquals(0, p.state().getHeadingRateRad_M(), DELTA); - } /** @@ -80,9 +96,17 @@ 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))); + 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, 1), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1.2)); List constraints = new ArrayList<>(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); long startTimeNs = System.nanoTime(); @@ -99,20 +123,18 @@ 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()); - TimedPose p = t.getPoint(6); - assertEquals(0.585, p.state().getPose().translation().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); } - - @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); @@ -127,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, @@ -140,32 +162,66 @@ 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); } + /** + * 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 testBackingUp2() { - SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); - List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); + void test2d() { + 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 GlobalVelocityR3(-1, 0, 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(1.176, traj.duration(), DELTA); + if (DEBUG) + traj.dump(); + assertEquals(2.741, traj.duration(), DELTA); } + /** + * 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 test2d() { - SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); - List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); + void test2d2() { + List waypoints = List.of( + 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), + new CapsizeAccelerationConstraint(logger, 0.5, 1)); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - ModelR3 start = new ModelR3(Pose2d.kZero, new GlobalVelocityR3(0, 1, 0)); - Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); - Trajectory100 traj = planner.movingToRest(start, end); - assertEquals(2.958, traj.duration(), DELTA); + + Trajectory100 traj = planner.generateTrajectory(waypoints, 1, 1); + if (DEBUG) + traj.dump(); + assertEquals(4.603, 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 new file mode 100644 index 000000000..37c3ed391 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java @@ -0,0 +1,176 @@ +package org.team100.lib.trajectory; + +import java.awt.Dimension; +import java.awt.Frame; +import java.util.List; +import java.util.function.Supplier; + +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.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; + +public class TrajectoryPlotter { + public static final boolean SHOW = false; + private static final int SIZE = 500; + + 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) { + VectorSeries converted = convert(name, t); + XYDataset dataSet = TrajectoryPlotter.collect(converted); + if (SHOW) + actuallyPlot(name, () -> new VectorRenderer(), dataSet); + } + + 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); + XYDataset dataSet = TrajectoryPlotter.collect(converted); + if (SHOW) + actuallyPlot(name, () -> new VectorRenderer(), dataSet); + } + + public VectorSeries convert(String name, List s) { + return splineConverter.convert(name, s); + } + + public static XYDataset collect(VectorSeries... converted) { + VectorSeriesCollection dataSet = new VectorSeriesCollection(); + for (VectorSeries c : converted) { + dataSet.addSeries(c); + } + return dataSet; + } + + public static XYDataset collect(XYSeries... series) { + XYSeriesCollection dataSet = new XYSeriesCollection(); + for (XYSeries c : series) { + dataSet.addSeries(c); + } + return dataSet; + } + + public static Range xRange(XYDataset dataset) { + double max = Double.NEGATIVE_INFINITY; + double min = Double.POSITIVE_INFINITY; + 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) + min = x - 0.1; + } + return new Range(min, max); + } + + public static Range yRange(XYDataset dataset) { + double max = Double.NEGATIVE_INFINITY; + double min = Double.POSITIVE_INFINITY; + 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); + } + + /** + * 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) { + if (!SHOW) + return; + + // "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); + } + + frame.pack(); + frame.setVisible(true); + frame.setDefaultCloseOperation(WindowConstants.DISPOSE_ON_CLOSE); + } + + public static void plot( + Trajectory100 t, + double arrows) { + TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); + VectorSeries series = plotter.convert("trajectory", t); + XYDataset dataSet = TrajectoryPlotter.collect(series); + if (SHOW) + actuallyPlot("compare", () -> new VectorRenderer(), dataSet); + } + + public static void plot( + List splines, + double arrows) { + TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); + VectorSeries series = plotter.convert("before", splines); + XYDataset dataSet = TrajectoryPlotter.collect(series); + if (SHOW) + actuallyPlot("compare", () -> new VectorRenderer(), dataSet); + } + + public static void plot( + Path100 path, + double arrows) { + TrajectoryPlotter plotter = new TrajectoryPlotter(arrows); + VectorSeries series = plotter.convert("path", path); + XYDataset dataSet = TrajectoryPlotter.collect(series); + if (SHOW) + 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 new file mode 100644 index 000000000..22d16f5e3 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -0,0 +1,198 @@ +package org.team100.lib.trajectory; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.List; + +import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.Metrics; +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.TimedState; +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; + +public class TrajectoryTest { + private static final boolean DEBUG = false; + LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); + + /** + * Yields a straight line. + * + * TrajectoryPlanner.restToRest() has several overloads: the one that takes + * two non-holonomic poses draws a straight line between them. + */ + @Test + void testSimple() throws InterruptedException { + List c = List.of( + new ConstantConstraint(log, 1, 0.1), + new YawRateConstraint(log, 1, 1)); + TrajectoryPlanner p = new TrajectoryPlanner(c); + Trajectory100 t = p.restToRest( + new Pose2d(0, 0, new Rotation2d()), + new Pose2d(10, 1, new Rotation2d())); + 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 + void testCircle() { + // see HolonomicSplineTest.testCircle(); + // this is to see how to create the dtheta and curvature + // without the spline. + double scale = 1.3; + 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 = 1.3; + 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(); + 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) { + 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(); + // 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 = 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, + intrinsicCa, p0.velocityM_S(), + dcourse, dcourse1); + p0 = p1; + } + + TrajectoryPlotter.plot(trajectory, 0.25); + } + + /** + * Yields a curve. + * + * 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 + * facing the other way (i.e. backwards) + */ + @Test + void testCurved() throws InterruptedException { + 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(1, 1), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(9, 9), + new Rotation2d(-Math.PI / 2)), + new DirectionSE2(0, 1, 0), 1)); + Trajectory100 t = p.restToRest(waypoints); + new TrajectoryPlotter(0.5).plot("curved", t); + } + + /** + * You can specify interior waypoints as well as start and end points. Note that + * specifying many such points will make the curve harder to calculate and + * harder to make smooth. + */ + @Test + void testMultipleWaypoints() throws InterruptedException { + 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(1, 1), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(5, 5), + new Rotation2d(-2)), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(9, 9), + new Rotation2d(-Math.PI / 2)), + new DirectionSE2(0, 1, 0), 1)); + Trajectory100 t = p.restToRest(waypoints); + 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 new file mode 100644 index 000000000..f01e0d705 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java @@ -0,0 +1,81 @@ +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.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 */ + private final double m_scale; + + public TrajectoryToVectorSeries(double scale) { + m_scale = scale; + } + + /** Maps x to x, y to y */ + 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) { + TimedState p = t.sample(time); + WaypointSE2 pp = p.state().getPose(); + 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); + if (DEBUG) + System.out.printf("%f\n", time); + } + 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) { + TimedState 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) { + TimedState 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/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 9d365edc8..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 @@ -6,22 +6,17 @@ 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.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; 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), @@ -31,13 +26,21 @@ class Path100Test { private static final List WAYPOINTS = Arrays.asList( new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(24, 0, Math.toRadians(30), 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(24, 0, new Rotation2d(Math.toRadians(30))), 0, 1.2), + 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(36, 12, Math.toRadians(60), 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(36, 12, new Rotation2d(Math.toRadians(60))), 0, 1.2), + 0, 0), new Pose2dWithMotion( - HolonomicPose2d.make(60, 12, Math.toRadians(90), 0), 0, 0, 0)); + WaypointSE2.irrotational( + new Pose2d(60, 12, new Rotation2d(Math.toRadians(90))), 0, 1.2), + 0, 0)); @Test void testEmpty() { @@ -45,63 +48,10 @@ 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); } - @Test - void testSimple() { - // spline is in the x direction, no curvature. - HolonomicSpline spline = new HolonomicSpline( - new HolonomicPose2d( - new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d( - new Translation2d(1, 0), new Rotation2d(), new Rotation2d())) { - - @Override - public Translation2d getPoint(double t) { - return new Translation2d(t, 0); - } - - @Override - public Rotation2d getHeading(double t) { - return Rotation2d.kZero; - } - - @Override - public Optional getCourse(double t) { - return Optional.of(Rotation2d.kZero); - } - - @Override - public double getDHeading(double t) { - return 0; - } - - @Override - public double getCurvature(double t) { - return 0; - } - - @Override - public double getDCurvature(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); @@ -118,98 +68,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()); } - /** - * 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( - HolonomicPose2d.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), - 0.1, 0, 0), - new Pose2dWithMotion( - HolonomicPose2d.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), - 0.1, 0, 0), - new Pose2dWithMotion( - HolonomicPose2d.make(60.0, 24.0, Math.toRadians(180), 0), - 0.1, 0, 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.getCourse().getRadians()); - } - } - - assertEquals(0.0, path.getMinDistance(), DELTA); - // paths no longer use constant-twist arcs so this is just the lengths - assertEquals(84, 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); - - // course is +x - assertEquals(0, sample0.getCourse().getDegrees()); - - // heading is 0 - assertEquals(0, sample0.getPose().heading().getDegrees()); - - Pose2dWithMotion sample12 = path.sample(12.0); - assertEquals(12, 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); - - 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(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(60, sample6.getPose().heading().getDegrees(), DELTA); - - Pose2dWithMotion sample72 = path.sample(72.0); - assertEquals(48, 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); - - Pose2dWithMotion sample8 = path.sample(84); - assertEquals(60, 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); - - } } 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..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 @@ -7,20 +7,13 @@ 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.WaypointSE2; 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.TrajectoryPlotter; 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; @@ -30,133 +23,148 @@ 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() { - 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)); - Path100 path = PathFactory.pathFromWaypoints( - waypoints, - 0.0127, - 0.0127, - Math.toRadians(1.0)); - assertEquals(10, path.length()); - } /** 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(), 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))); - Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1); - - assertEquals(9, path.length()); + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + 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, 1), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1)); + Path100 path = PathFactory.pathFromWaypoints(waypoints, 0.1, 0.01, 0.01, 0.1); + + assertEquals(54, 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); + p = path.getPoint(8); + assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); assertEquals(0, p.getHeadingRateRad_M(), DELTA); } @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())); + List waypoints = List.of( + 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)); Path100 path = PathFactory.pathFromWaypoints( - waypoints, 0.01, 0.01, 0.1); - assertEquals(2, path.length()); + waypoints, 0.1, 0.01, 0.01, 0.1); + assertEquals(17, 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); + p = path.getPoint(8); + 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 once again does not work. */ + @Test + void testSpin() { + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), + new DirectionSE2(0, 0, 1), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kCCW_90deg), + new DirectionSE2(0, 0, 1), 1)); + assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints,0.1, 0.01, 0.01, 0.1)); + } + + /** Hard corners once again do not work. */ @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))); - assertThrows(IllegalArgumentException.class, - () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d()), + 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), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1)); + assertThrows(IllegalArgumentException.class, () -> PathFactory.pathFromWaypoints(waypoints,0.1, 0.01, 0.01, 0.1)); } @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())); - assertThrows(IllegalArgumentException.class, - () -> PathFactory.pathFromWaypoints(waypoints, 0.01, 0.01, 0.1)); + // note none of these directions include rotation; it's all in between. + List waypoints = List.of( + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(2, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 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); } @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)); + WaypointSE2 p1 = new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1.2); + WaypointSE2 p2 = new WaypointSE2( + new Pose2d( + new Translation2d(15, 10), + Rotation2d.kZero), + 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); @@ -170,23 +178,32 @@ 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); - assertEquals(20.416, arclength, 0.001); + WaypointSE2 pose = cur_pose.getPose(); + 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.428, arclength, 0.001); } @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), - 1.0, 1.0); + new WaypointSE2( + new Pose2d( + new Translation2d(0, -1), + Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + 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().translation().getX(), p.getPose().translation().getY()); + System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), + p.getPose().pose().getTranslation().getY()); } } @@ -197,9 +214,17 @@ 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))); + 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, 1), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1.2)); long startTimeNs = System.nanoTime(); Path100 t = new Path100(new ArrayList<>()); final int iterations = 100; @@ -208,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); @@ -218,9 +244,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(5, t.length()); - Pose2dWithMotion p = t.getPoint(1); - assertEquals(0.417, p.getPose().translation().getX(), DELTA); + assertEquals(33, t.length()); + Pose2dWithMotion p = t.getPoint(4); + 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/HolonomicSpline3dTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java index 1451a60cb..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,9 +3,11 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -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; @@ -15,16 +17,26 @@ 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 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); 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..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,14 +1,15 @@ 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.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.Metrics; 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,9 +17,11 @@ 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.ConstantConstraint; import org.team100.lib.trajectory.timing.ScheduleGenerator; import org.team100.lib.trajectory.timing.TimingConstraint; @@ -31,78 +34,211 @@ 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); + 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())); - Translation2d t = s.getPoint(0); + 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)); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); + + 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); } @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())); - Translation2d t = s.getPoint(0); + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(2, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1)); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); + + 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); } @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 HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), - new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(1), new Rotation2d())); + 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(1)), + new DirectionSE2(1, 0, 0), 1.2)); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); - 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()); - } - } + // now that the magic numbers apply to the rotational scaling, the heading rate + // is constant. + Translation2d t = s.getPose2d(0).getTranslation(); + assertEquals(0, t.getX(), DELTA); + t = s.getPose2d(1).getTranslation(); + assertEquals(1, t.getX(), DELTA); + + Pose2dWithMotion p = s.getPose2dWithMotion(0); + 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().pose().getTranslation().getX(), DELTA); + assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); + // high rotation rate in the middle + assertEquals(0.915, p.getHeadingRateRad_M(), DELTA); + + p = s.getPose2dWithMotion(1); + 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); + + } + + @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 WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d()), + new DirectionSE2(1, 0, 1), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 1), 1)); + + 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. - 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(1, p.getHeadingRateRad_M(), DELTA); + assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA); + assertEquals(0.707, 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); + assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA); + assertEquals(0.707, 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.getHeadingRateRad_M(), DELTA); + assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA); + assertEquals(1, p.getPose().pose().getRotation().getRadians(), DELTA); + assertEquals(0.707, p.getHeadingRateRad_M(), DELTA); } @@ -110,212 +246,272 @@ 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())); - 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()); - } - } + new WaypointSE2( + new Pose2d( + new Translation2d(), + new Rotation2d(2.5)), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(-2.5)), + new DirectionSE2(1, 0, 0), 1)); + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("rotation", List.of(s)); } + /** Turning in place splines work. */ @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. - // - // the spline code currently does not behave correctly, it wants the theta - // speed to be zero at each of the spline endpoints below. - // - // magic number of 1 makes something about 1.5% from a circle, close enough. - double magicNumber = 1.0; + void spin() { + double scale = 0.9; 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), - 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), - 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), - 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), - magicNumber, magicNumber); + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), + new DirectionSE2(0, 0, 1), scale), + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kCCW_90deg), + new DirectionSE2(0, 0, 1), scale)); + List splines = new ArrayList<>(); splines.add(s0); - splines.add(s1); - 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()); - // } - // } - - // optimization does not help, the individual splines are already "optimal." - - SplineUtil.forceC1(splines); - - SplineUtil.optimizeSpline(splines); - - // spline joints are C1 smooth (i.e. same slope) - assertTrue(SplineUtil.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 < 4; ++i) { - HolonomicSpline s = splines.get(i); - 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. + 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() { + double scale = 1.3; + 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); + checkCircle(splines, 0.008, 0.006); + TrajectoryPlotter.plot(splines, 0.1); + + } + + @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 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.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 \n", + s, p0.getHeadingRateRad_M(), p0.getCurvatureRad_M(), + dheadingDx2, curveDx2); + p0 = p1; + } + } + + private void checkCircle(List splines, double rangeError, double azimuthError) { + double actualRangeError = 0; + double actualAzimuthError = 0; + 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)); + + // 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); + actualAzimuthError = Math.max(actualAzimuthError, Math.abs(error.getRadians())); + + double k = spline.getCurvature(s); + + double dh = spline.getDHeadingDs(s); if (DEBUG) - System.out.printf("%.1f, %.2f, %.2f, %.2f\n", - i + j, p.getX(), p.getY(), p.getRotation().getRadians()); + System.out.printf("%f, %f, %f, %f, %f\n", s, p.getX(), p.getY(), k, dh); } } + 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)); + } @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. - double magicNumber = 1.0; + void testLine() { // 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), - magicNumber, magicNumber); + new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 0), 1)); // 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), - magicNumber, magicNumber); + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + new Rotation2d(1)), + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(2, 0), + Rotation2d.k180deg), + new DirectionSE2(1, 0, 0), 1)); List splines = new ArrayList<>(); splines.add(s0); splines.add(s1); + TrajectoryPlotter.plot(splines, 0.1); + } - // 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); - - // spline joints are C1 smooth (i.e. same slope) - assertTrue(SplineUtil.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()); - } - } + /** + * 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 scale = 0.7; + WaypointSE2 p0 = new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d(-1, 0)), + 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), scale); + WaypointSE2 p2 = new WaypointSE2( + new Pose2d( + new Translation2d(1, 1), + new Rotation2d(0, 1)), + new DirectionSE2(0, 1, 0), scale); + + if (DEBUG) + System.out.println("s01"); + HolonomicSpline s01 = new HolonomicSpline(p0, p1); + if (DEBUG) + System.out.println("s12"); + HolonomicSpline s12 = new HolonomicSpline(p1, p2); + List splines = new ArrayList<>(); + splines.add(s01); + splines.add(s12); + 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. - double magicNumber = 1.0; + // because path generation never looks across spline boundaries, + // it is possible to make sharp corners at the "knots." + // 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), - magicNumber, magicNumber); + 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)); // 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), - magicNumber, magicNumber); + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), + new DirectionSE2(0, 1, 0), 1), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 1), + Rotation2d.kZero), + new DirectionSE2(0, 1, 0), 1)); List splines = new ArrayList<>(); 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); for (HolonomicSpline s : splines) { if (DEBUG) System.out.printf("spline %s\n", s); } - // spline joints are C1 smooth (i.e. same slope) - assertTrue(SplineUtil.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()); - } - } - - 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 = new ArrayList<>(); + List constraints = List.of(new ConstantConstraint(logger, 1, 1)); ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); - Trajectory100 trajectory = scheduleGenerator.timeParameterizeTrajectory(path, + Trajectory100 traj = scheduleGenerator.timeParameterizeTrajectory(path, 0.05, 0, 0); if (DEBUG) - System.out.printf("trajectory %s\n", trajectory); - + traj.dump(); + TrajectoryPlotter.plot(traj, 0.1); } @Test @@ -323,9 +519,16 @@ 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), - 1.2, 1.2); + new WaypointSE2( + new Pose2d( + new Translation2d(0, -1), + Rotation2d.kZero), + new DirectionSE2(1, 0, 0), 1.2), + new WaypointSE2( + new Pose2d( + new Translation2d(1, 0), + Rotation2d.kZero), + 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)); @@ -333,18 +536,23 @@ void testEntryVelocity() { } List splines = List.of(s0); - List motion = PathFactory.parameterizeSplines(splines, 0.05, 0.05, 0.05); + + TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); + plotter.plot("splines", splines); + + 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().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()); } } @@ -360,6 +568,9 @@ 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); } 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..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 @@ -1,61 +1,105 @@ 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.HolonomicPose2d; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +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() { - HolonomicPose2d a = new HolonomicPose2d( - new Translation2d(0, 100), new Rotation2d(), Rotation2d.fromDegrees(270)); - HolonomicPose2d b = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); - HolonomicPose2d c = new HolonomicPose2d( - new Translation2d(100, 100), new Rotation2d(), Rotation2d.fromDegrees(90)); + void test0() { + WaypointSE2 a = new WaypointSE2( + new Pose2d( + new Translation2d(0, 100), + new Rotation2d()), + new DirectionSE2(0, -1, 0), 1); + WaypointSE2 b = new WaypointSE2( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 c = new WaypointSE2( + new Pose2d( + new Translation2d(100, 100), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); splines.add(new HolonomicSpline(b, c)); - assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); - - HolonomicPose2d d = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(90)); - HolonomicPose2d e = new HolonomicPose2d( - new Translation2d(0, 50), new Rotation2d(), Rotation2d.fromDegrees(0)); - HolonomicPose2d f = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(), Rotation2d.fromDegrees(-90)); - HolonomicPose2d g = new HolonomicPose2d( - new Translation2d(100, 0), new Rotation2d(), Rotation2d.fromDegrees(-180)); - - List splines1 = new ArrayList<>(); - splines1.add(new HolonomicSpline(d, e)); - splines1.add(new HolonomicSpline(e, f)); - splines1.add(new HolonomicSpline(f, g)); - - assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); - - HolonomicPose2d h = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); - HolonomicPose2d i = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); - HolonomicPose2d j = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(), Rotation2d.fromDegrees(45)); - HolonomicPose2d k = new HolonomicPose2d( - new Translation2d(150, 0), new Rotation2d(), Rotation2d.fromDegrees(270)); - HolonomicPose2d l = new HolonomicPose2d( - new Translation2d(150, -50), new Rotation2d(), Rotation2d.fromDegrees(270)); + TrajectoryPlotter.plot(splines, 5); + + } + + @Test + void test1() { + + WaypointSE2 d = new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1); + WaypointSE2 e = new WaypointSE2( + new Pose2d( + new Translation2d(0, 50), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 f = new WaypointSE2( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d()), + new DirectionSE2(0, -1, 0), 1); + WaypointSE2 g = new WaypointSE2( + new Pose2d( + new Translation2d(100, 0), + new Rotation2d()), + new DirectionSE2(-1, 0, 0), 1); + + 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); + + } + + @Test + void test2() { + WaypointSE2 h = new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 i = new WaypointSE2( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 j = new WaypointSE2( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d()), + new DirectionSE2(1, 1, 0), 1); + WaypointSE2 k = new WaypointSE2( + new Pose2d( + new Translation2d(150, 0), + new Rotation2d()), + new DirectionSE2(0, -1, 0), 1); + WaypointSE2 l = new WaypointSE2( + new Pose2d( + new Translation2d(150, -50), + new Rotation2d()), + new DirectionSE2(0, -1, 0), 1); List splines2 = new ArrayList<>(); splines2.add(new HolonomicSpline(h, i)); @@ -63,63 +107,104 @@ 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); + } @Test - void testHolonomic() { - HolonomicPose2d a = new HolonomicPose2d( - new Translation2d(0, 100), new Rotation2d(), Rotation2d.fromDegrees(270)); - HolonomicPose2d b = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(0)); - HolonomicPose2d c = new HolonomicPose2d( - new Translation2d(100, 100), new Rotation2d(Math.PI), Rotation2d.fromDegrees(90)); + void test3() { + WaypointSE2 a = new WaypointSE2( + new Pose2d( + new Translation2d(0, 100), + new Rotation2d()), + new DirectionSE2(0, -1, 0), 1); + WaypointSE2 b = new WaypointSE2( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d(Math.PI / 2)), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 c = new WaypointSE2( + new Pose2d( + new Translation2d(100, 100), + new Rotation2d(Math.PI)), + new DirectionSE2(0, 1, 0), 1); List splines = new ArrayList<>(); splines.add(new HolonomicSpline(a, b)); splines.add(new HolonomicSpline(b, c)); - assertTrue(SplineUtil.optimizeSpline(splines) < 0.014); - - HolonomicPose2d d = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(90)); - HolonomicPose2d e = new HolonomicPose2d( - new Translation2d(0, 50), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(0)); - HolonomicPose2d f = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(Math.PI), Rotation2d.fromDegrees(-90)); - HolonomicPose2d g = new HolonomicPose2d( - new Translation2d(100, 0), new Rotation2d(), Rotation2d.fromDegrees(-180)); - - List splines1 = new ArrayList<>(); - splines1.add(new HolonomicSpline(d, e)); - splines1.add(new HolonomicSpline(e, f)); - splines1.add(new HolonomicSpline(f, g)); - - assertEquals(0.54, SplineUtil.optimizeSpline(splines1), 0.01); - - HolonomicPose2d h = new HolonomicPose2d( - new Translation2d(0, 0), new Rotation2d(), Rotation2d.fromDegrees(0)); - HolonomicPose2d i = new HolonomicPose2d( - new Translation2d(50, 0), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(0)); - HolonomicPose2d j = new HolonomicPose2d( - new Translation2d(100, 50), new Rotation2d(Math.PI), Rotation2d.fromDegrees(45)); - HolonomicPose2d k = new HolonomicPose2d( - new Translation2d(150, 0), new Rotation2d(), Rotation2d.fromDegrees(270)); - HolonomicPose2d l = new HolonomicPose2d( - new Translation2d(150, -50), new Rotation2d(Math.PI / 2), Rotation2d.fromDegrees(270)); + TrajectoryPlotter.plot(splines, 5); - 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)); + } + + @Test + void test4() { + + WaypointSE2 d = new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1); + WaypointSE2 e = new WaypointSE2( + new Pose2d( + new Translation2d(0, 50), + new Rotation2d(Math.PI / 2)), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 f = new WaypointSE2( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d(Math.PI)), + new DirectionSE2(0, -1, 0), 1); + WaypointSE2 g = new WaypointSE2( + new Pose2d( + new Translation2d(100, 0), + new Rotation2d()), + new DirectionSE2(-1, 0, 0), 1); - 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); + 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); } + @Test + void test5() { + + WaypointSE2 h = new WaypointSE2( + new Pose2d( + new Translation2d(0, 0), + new Rotation2d()), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 i = new WaypointSE2( + new Pose2d( + new Translation2d(50, 0), + new Rotation2d(Math.PI / 2)), + new DirectionSE2(1, 0, 0), 1); + WaypointSE2 j = new WaypointSE2( + new Pose2d( + new Translation2d(100, 50), + new Rotation2d(Math.PI)), + new DirectionSE2(1, 1, 0), 1); + WaypointSE2 k = new WaypointSE2( + new Pose2d( + new Translation2d(150, 0), + new Rotation2d()), + new DirectionSE2(0, -1, 0), 1); + WaypointSE2 l = new WaypointSE2( + new Pose2d( + new Translation2d(150, -50), + new Rotation2d(Math.PI / 2)), + new DirectionSE2(0, -1, 0), 1); + + 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)); + + TrajectoryPlotter.plot(splines, 5); + + } } 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..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 @@ -15,6 +15,16 @@ public class SplineR1Test { private static final boolean DEBUG = false; + /** + * 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 void testSample() { @@ -23,13 +33,19 @@ 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) { + 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); 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/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java index e6eb35a34..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 @@ -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.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -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,11 +29,13 @@ void testSimple() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + WaypointSE2.irrotational( + 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 @@ -43,11 +48,13 @@ void testSimpleMoving() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + WaypointSE2.irrotational( + 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 @@ -60,11 +67,13 @@ void testSimpleOverspeed() { SwerveKinodynamicsFactory.forTest(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 1, 0); + WaypointSE2.irrotational( + 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 @@ -76,10 +85,12 @@ void testSimple2() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.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); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 1); + assertEquals(-4.083, c.maxDecel(p, 0), DELTA); + assertEquals(4.083, c.maxAccel(p, 0), DELTA); + assertEquals(2.021, c.maxV(p), DELTA); } @Test @@ -91,10 +102,12 @@ void testStraightLine() { SwerveKinodynamicsFactory.forTest2(logger), CENTRIPETAL_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.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); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0); + 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 00398eb5a..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 @@ -3,13 +3,16 @@ 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.WaypointSE2; 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.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,17 +21,21 @@ 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); - assertEquals(2, c.getMaxVelocity(state).getValue(), DELTA); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0); + assertEquals(2, c.maxV(state), DELTA); } @Test void testAccel() { ConstantConstraint c = new ConstantConstraint(logger, 2, 3); Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); - assertEquals(-3, c.getMinMaxAcceleration(state, 1).getMinAccel(), DELTA); - assertEquals(3, c.getMinMaxAcceleration(state, 1).getMaxAccel(), DELTA); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0); + 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 365bdba2d..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 @@ -3,13 +3,16 @@ 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.WaypointSE2; 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.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,34 +22,46 @@ 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); + WaypointSE2.irrotational( + 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( - HolonomicPose2d.make(0, 0, 0, Math.PI / 2), 0, 0, 0); + 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( - HolonomicPose2d.make(0, 0, 0, Math.PI / 4), 0, 0, 0); + 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 void testVelocity() { DiamondConstraint c = new DiamondConstraint(logger, 2, 3, 4); Pose2dWithMotion state = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0); + WaypointSE2.irrotational( + 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( - HolonomicPose2d.make(0, 0, 0, Math.PI / 2), 0, 0, 0); + 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( - HolonomicPose2d.make(0, 0, 0, Math.PI / 4), 0, 0, 0); + 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/JointConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java deleted file mode 100644 index 6e18e2cf0..000000000 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/JointConstraintTest.java +++ /dev/null @@ -1,91 +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.HolonomicPose2d; -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; - -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( - HolonomicPose2d.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); - } - - @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( - HolonomicPose2d.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); - } - - @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( - HolonomicPose2d.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); - } - - @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( - HolonomicPose2d.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); - } - - @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( - HolonomicPose2d.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 56b207d34..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; @@ -11,9 +10,10 @@ 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; +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; @@ -22,8 +22,8 @@ import org.team100.lib.trajectory.Trajectory100; 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; import edu.wpi.first.math.geometry.Translation2d; @@ -34,10 +34,14 @@ 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(WaypointSE2.irrotational( + 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), + new Pose2dWithMotion(WaypointSE2.irrotational( + 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)); public static final List HEADINGS = List.of( GeometryUtil.fromDegrees(0), @@ -76,16 +80,17 @@ 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.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(), @@ -98,28 +103,35 @@ public void checkTrajectory( } /** - * Turning in place does not work. + * Turning in place does not work, but it also doesn't fail. */ @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))); - - // Triangle profile. - assertThrows(IllegalArgumentException.class, - () -> buildAndCheckTrajectory( - path, 1.0, new ArrayList(), 0.0, 0.0, 20.0, 5.0)); - - // Trapezoidal profile. - assertThrows(IllegalArgumentException.class, - () -> buildAndCheckTrajectory( - path, 1.0, new ArrayList(), 0.0, 0.0, 10.0, 5.0)); - - // Trapezoidal profile with start and end velocities. - assertThrows(IllegalArgumentException.class, - () -> buildAndCheckTrajectory( - path, 1.0, new ArrayList(), 5.0, 2.0, 10.0, 5.0)); + new Pose2dWithMotion( + new WaypointSE2( + new Pose2d(0, 0, new Rotation2d(0)), + new DirectionSE2(0, 0, 1), 1), + 1, 0), + new Pose2dWithMotion( + new WaypointSE2( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + new DirectionSE2(0, 0, 1), 1), + 1, 0))); + + assertEquals(0, path.getMaxDistance(), DELTA); + assertEquals(2, path.length()); + if (DEBUG) + System.out.printf("PATH:\n%s\n", path); + + List constraints = new ArrayList(); + ScheduleGenerator u = new ScheduleGenerator(constraints); + Trajectory100 traj = u.timeParameterizeTrajectory( + path, + 1.0, + 0.0, + 0.0); + assertEquals(0, traj.duration(), DELTA); } /** @@ -136,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()); } /** @@ -166,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 @@ -186,18 +198,22 @@ void testConditionalVelocityConstraint() { class ConditionalTimingConstraint implements TimingConstraint { @Override - public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { - if (state.getPose().translation().getX() >= 24.0) { - return new NonNegativeDouble(5.0); + public double maxV(Pose2dWithMotion state) { + if (state.getPose().pose().getTranslation().getX() >= 24.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; } } @@ -213,14 +229,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; } } @@ -230,33 +251,6 @@ public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithMotion state, 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. * @@ -264,9 +258,17 @@ 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))); + 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, 1), + new Rotation2d()), + new DirectionSE2(0, 1, 0), 1.2)); long startTimeNs = System.nanoTime(); final int iterations = 100; final double SPLINE_SAMPLE_TOLERANCE_M = 0.05; @@ -275,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); @@ -293,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()); - TimedPose p = t.getPoint(6); - assertEquals(0.575, p.state().getPose().translation().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/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java index 8cd74a668..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 @@ -3,14 +3,16 @@ 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.WaypointSE2; 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.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; class SwerveDriveDynamicsConstraintTest { private static final double DELTA = 0.001; @@ -22,21 +24,25 @@ void testVelocity() { SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1); // motionless - double m = c.getMaxVelocity(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0)).getValue(); + double m = c.maxV(new Pose2dWithMotion( + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0)); assertEquals(5, m, DELTA); // moving in +x, no curvature, no rotation - m = c.getMaxVelocity(new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), - 0, 0, 0)).getValue(); + m = c.maxV(new Pose2dWithMotion( + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0)); // 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), - 5, 0, 0)).getValue(); + m = c.maxV(new Pose2dWithMotion( + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 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); @@ -46,11 +52,10 @@ 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), - 11.313708, 0, 0); - m = c.getMaxVelocity( - state) - .getValue(); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 11.313708, 0); + 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 @@ -63,9 +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( - HolonomicPose2d.make(0, 0, 0, 0), 0, 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/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java index 6d14b5b5a..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 @@ -3,36 +3,43 @@ 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.WaypointSE2; 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; @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( - HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 0, 0), 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( - HolonomicPose2d.make(0.5, 0, 0, 0), 0, 0, 0), + WaypointSE2.irrotational( + new Pose2d(0.5, 0, new Rotation2d(0)), 0, 1.2), + 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 + 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); - assertEquals(0.125, intermediate_state.state().getPose().translation().getX(), EPSILON); + // close to the start state by distance + assertEquals(0.125, intermediate_state.state().getPose().pose().getTranslation().getX(), EPSILON); } } 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..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 @@ -3,9 +3,12 @@ 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.WaypointSE2; 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,10 +17,12 @@ 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); + WaypointSE2.irrotational( + 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 @@ -25,10 +30,12 @@ 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); + WaypointSE2.irrotational( + 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 @@ -36,10 +43,12 @@ 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); + WaypointSE2.irrotational( + 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 @@ -47,10 +56,12 @@ 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); + WaypointSE2.irrotational( + 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 @@ -58,10 +69,12 @@ 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); + WaypointSE2.irrotational( + 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 @@ -69,9 +82,11 @@ 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); + WaypointSE2.irrotational( + 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/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 3c2c7a6cf..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 @@ -6,8 +6,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.HolonomicPose2d; 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,7 +16,9 @@ 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; /** @@ -30,9 +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( - new Pose2dWithMotion(HolonomicPose2d.make(0, 0, 0, 0), 0, 0, 0), - new Pose2dWithMotion(HolonomicPose2d.make(5, 0, 0, 0), 0, 0, 0)); + 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) }; // No rotation. public static final List HEADINGS = List.of( @@ -42,58 +48,63 @@ 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. + * + * TODO: this tickles a bug in the schedule generator dt(), fix it. + */ @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(Arrays.asList(WAYPOINTS)), 0.5, 0, 0); + // Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); print(traj); } } 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..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 @@ -3,9 +3,11 @@ 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.WaypointSE2; 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,12 +19,13 @@ void testOutside() { VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint( new Translation2d(), new Translation2d(1, 1), 1); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(-1, -1, 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); - assertEquals(Double.POSITIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(Double.POSITIVE_INFINITY, c.getMaxVelocity(p).getValue(), DELTA); + 0); + 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 @@ -31,12 +34,13 @@ 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), + 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); - assertEquals(Double.POSITIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(1, c.getMaxVelocity(p).getValue(), DELTA); + 0); + 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 dd198b645..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 @@ -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.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -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,12 +28,12 @@ void testNormal() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest(logger), YAW_RATE_SCALE); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 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); - assertEquals(8.485, c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); - assertEquals(2.828, c.getMaxVelocity(p).getValue(), DELTA); + 0); + assertEquals(-8.485, c.maxDecel(p, 0), DELTA); + assertEquals(8.485, c.maxAccel(p, 0), DELTA); + assertEquals(2.828, c.maxV(p), DELTA); } @Test @@ -39,9 +42,11 @@ 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 - 0, 0); - assertEquals(5.656, c.getMaxVelocity(p).getValue(), DELTA); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 1, // spatial, so rad/m + 0); + assertEquals(5.656, c.maxV(p), DELTA); } @Test @@ -52,13 +57,14 @@ void testAccel() { YAW_RATE_SCALE); // driving and spinning Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), 1, - 0, 0); + WaypointSE2.irrotational( + new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), + 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 @@ -68,13 +74,13 @@ void testAccel2() { YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forRealisticTest(logger), scale); Pose2dWithMotion p = new Pose2dWithMotion( - HolonomicPose2d.make(0, 0, 0, 0), + 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); + assertEquals(-16.971, c.maxDecel(p, 0), DELTA); assertEquals(16.971, - c.getMinMaxAcceleration(p, 0).getMaxAccel(), DELTA); + c.maxAccel(p, 0), 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 c6bada2fd..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; @@ -38,14 +44,6 @@ void testSolveQuadraticZero() { assertEquals(0, roots.size()); } - - - - - - - - @Test void testGetMinDistance() { double measurement = 4; @@ -54,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)); + } + } 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"); }