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 24c6aab4..19eca592 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -10,7 +10,9 @@ import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; import org.team100.lib.subsystems.r3.commands.GoToPosePosition; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TorqueConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -44,8 +46,9 @@ public MechTrajectories( // These finer grains make smoother paths and schedules but // take longer to compute, so if it takes too long, make these // numbers bigger! - m_planner = new TrajectoryPlanner(0.01, 0.1, 0.05, c); - // m_planner = new TrajectoryPlanner(0.02, 0.2, 0.1, c); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(0.05, 0.01, 0.01, 0.1); + m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); } /** A command that goes from the start to the end and then finishes. */ 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 index 8652e75c..de09926c 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java @@ -9,7 +9,10 @@ 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.path.PathFactory; +import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import edu.wpi.first.math.geometry.Pose2d; @@ -17,11 +20,11 @@ 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))); + public BigLoop(LoggerFactory log, SwerveKinodynamics kinodynamics) { + List constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); } @Override 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 5287f788..a01abf30 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 @@ -12,6 +12,9 @@ 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.path.PathFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; +import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import edu.wpi.first.math.geometry.Pose2d; @@ -34,8 +37,10 @@ public GoToCoralStation( double scale) { m_station = station; m_scale = scale; - m_planner = new TrajectoryPlanner( - new TimingConstraintFactory(kinodynamics).auto(log.type(this))); + List constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); } @Override diff --git a/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java b/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java index 7c65ed31..23655df0 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java @@ -1,5 +1,7 @@ package org.team100.frc2025.robot; +import java.util.List; + import org.team100.lib.config.AnnotatedCommand; import org.team100.lib.config.AutonChooser; import org.team100.lib.config.ElevatorUtil.ScoringLevel; @@ -11,6 +13,9 @@ import org.team100.lib.profile.r3.HolonomicProfileFactory; import org.team100.lib.profile.r3.ProfileR3; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.path.PathFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; +import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import edu.wpi.first.wpilibj2.command.Command; @@ -34,8 +39,11 @@ public AllAutons(Machinery machinery) { 5); final FullStateControllerR3 controller = ControllerFactoryR3 .auto2025LooseTolerance(autoLog); - final TrajectoryPlanner planner = new TrajectoryPlanner( - new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(autoLog)); + List constraints = new TimingConstraintFactory(machinery.m_swerveKinodynamics) + .medium(autoLog); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + final TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); // WARNING! The glass widget will override the default, so check it! // Run the auto in pre-match testing! 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 96209e10..37b74937 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java @@ -10,6 +10,9 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.path.PathFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; +import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import edu.wpi.first.math.geometry.Pose2d; @@ -38,8 +41,10 @@ public static void init(Machinery machinery) { 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)); + List constraints = new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); planner.restToRest(waypoints); // Exercise the drive motors. 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 41ef39a8..74c40c8e 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -17,7 +17,9 @@ import org.team100.lib.subsystems.prr.JointVelocities; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -47,7 +49,9 @@ void homeToL4() { List c = List.of( new ConstantConstraint(log, 1, 1), new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner m_planner = new TrajectoryPlanner(c); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 t = m_planner.restToRest(List.of( WaypointSE2.irrotational( 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 450f9197..5886932a 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java @@ -7,6 +7,7 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; +import org.team100.lib.geometry.Metrics; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; @@ -368,7 +369,7 @@ void estimateRobotPose( continue; } - final double distanceM = distance(m_prevPose, pose); + final double distanceM = Metrics.translationalDistance(m_prevPose, pose); if (distanceM > VISION_CHANGE_TOLERANCE_M) { // The new estimate is too far from the previous one: it's probably garbage. m_prevPose = pose; @@ -426,13 +427,4 @@ static double[] visionMeasurementStdDevs(double distanceM) { Double.MAX_VALUE }; } - /////////////////////////////////////// - - /** Distance between pose translations. */ - 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 a.getTranslation().getDistance(b.getTranslation()); - } - } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectory.java deleted file mode 100644 index 07977a8e..00000000 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectory.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.team100.lib.subsystems.r3.commands; - -import java.util.function.BiFunction; -import java.util.function.Supplier; - -import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; -import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.visualization.TrajectoryVisualization; - -import edu.wpi.first.math.geometry.Pose2d; - -/** - * Drive from the current state to a field-relative goal. - * - * The goal is supplied at initialization time. - * - * The trajectory is supplied by a function that takes both the current pose and - * the goal. You could use something like `TrajectoryPlanner.movingToRest()` for - * this function. - */ -public class DriveToPoseWithTrajectory extends MoveAndHold { - private final LoggerFactory m_log; - private final Supplier m_goal; - private final VelocitySubsystemR3 m_drive; - private final BiFunction m_trajectories; - private final ControllerR3 m_controller; - private final TrajectoryVisualization m_viz; - - private Trajectory100 m_trajectory; - - private VelocityReferenceControllerR3 m_referenceController; - - /** - * @param trajectories function that takes a start and end pose and returns a - * trajectory between them. - */ - public DriveToPoseWithTrajectory( - LoggerFactory parent, - Supplier goal, - VelocitySubsystemR3 drive, - BiFunction trajectories, - ControllerR3 controller, - TrajectoryVisualization viz) { - m_log = parent.type(this); - m_goal = goal; - m_drive = drive; - m_trajectories = trajectories; - m_controller = controller; - m_viz = viz; - addRequirements(m_drive); - } - - @Override - public void initialize() { - m_trajectory = m_trajectories.apply(m_drive.getState(), m_goal.get()); - if (m_trajectory.isEmpty()) { - m_trajectory = null; - return; - } - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(m_log, m_trajectory); - m_referenceController = new VelocityReferenceControllerR3( - m_log, m_drive, m_controller, reference); - m_viz.setViz(m_trajectory); - } - - @Override - public void execute() { - if (m_trajectory == null) - return; - m_referenceController.execute(); - } - - @Override - public boolean isDone() { - return m_trajectory == null - || m_referenceController == null - || m_referenceController.isDone(); - } - - @Override - public double toGo() { - return (m_referenceController == null) ? 0 : m_referenceController.toGo(); - } - - @Override - public void end(boolean interrupted) { - m_drive.stop(); - m_viz.clear(); - m_referenceController = null; - } -} 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 deleted file mode 100644 index 8408167f..00000000 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryAndExitVelocity.java +++ /dev/null @@ -1,108 +0,0 @@ -package org.team100.lib.subsystems.r3.commands; - -import java.util.List; - -import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; -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; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; -import org.team100.lib.trajectory.Trajectory100; -import org.team100.lib.trajectory.TrajectoryPlanner; -import org.team100.lib.visualization.TrajectoryVisualization; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Drive to a specified pose and exit velocity, using a trajectory constructed - * at initialization time. - */ -public class DriveToPoseWithTrajectoryAndExitVelocity extends MoveAndHold { - private final LoggerFactory m_log; - private final Pose2d m_goal; - private final VelocitySE2 m_endVelocity; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; - private final TrajectoryVisualization m_viz; - private final TrajectoryPlanner m_planner; - - private VelocityReferenceControllerR3 m_referenceController; - - public DriveToPoseWithTrajectoryAndExitVelocity( - LoggerFactory parent, - Pose2d goal, - VelocitySE2 endVelocity, - VelocitySubsystemR3 drive, - ControllerR3 controller, - TrajectoryPlanner planner, - TrajectoryVisualization viz) { - m_log = parent.type(this); - m_goal = goal; - m_endVelocity = endVelocity; - m_drive = drive; - m_controller = controller; - m_planner = planner; - m_viz = viz; - addRequirements(m_drive); - } - - @Override - public void initialize() { - Pose2d pose = m_drive.getState().pose(); - Translation2d toGoal = m_goal.getTranslation().minus(pose.getTranslation()); - 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(), - m_endVelocity.norm()); - - if (trajectory.length() == 0) { - m_referenceController = null; - return; - } - - m_viz.setViz(trajectory); - - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(m_log, trajectory); - m_referenceController = new VelocityReferenceControllerR3( - m_log, m_drive, m_controller, reference); - } - - @Override - public void execute() { - if (m_referenceController != null) - m_referenceController.execute(); - - } - - @Override - public boolean isDone() { - if (m_referenceController == null) - return true; - return m_referenceController.isDone(); - } - - @Override - public double toGo() { - return (m_referenceController == null) ? 0 : m_referenceController.toGo(); - } - - @Override - public void end(boolean interrupted) { - m_drive.stop(); - m_viz.clear(); - } -} 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 4335fea4..f21d5e3a 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 @@ -9,8 +9,11 @@ import org.team100.lib.subsystems.tank.TankDrive; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimedState; +import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.controller.LTVUnicycleController; @@ -38,8 +41,10 @@ public ToPoseWithTrajectory( m_goal = goal; m_drive = drive; m_viz = viz; - m_planner = new TrajectoryPlanner( - List.of(new ConstantConstraint(log, 1, 1))); + List constraints = List.of(new ConstantConstraint(log, 1, 1)); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); m_controller = new LTVUnicycleController(0.020); addRequirements(drive); } 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 3d72d278..0cfc98de 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/README.md @@ -15,7 +15,7 @@ The process of constructing a trajectory has three stages: 3. Construct a list of points interpolated along the secant lines, such that the points aren't too far apart. -4. Using a list of kinodynamic constraints (see `lib.trajectory.timing`), assign a time for each point. The resulting list of `TimedState`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 `TrajectoryFactory`, producing `Trajectory100`. To use a trajectory, you `sample()` it, with time (in seconds) as the parameter. The resulting `TimedState` is interpolated between from the list above. 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 8d1253c0..c568d0cd 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -1,242 +1,59 @@ package org.team100.lib.trajectory; import java.util.List; -import java.util.function.Function; -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; -import org.team100.lib.trajectory.timing.ScheduleGenerator; -import org.team100.lib.trajectory.timing.TimingConstraint; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGenerationException; +import org.team100.lib.trajectory.timing.TrajectoryFactory; /** - * Creates a trajectory in four steps: + * Creates a trajectory in three steps: * * 1. create a spline * 2. create points along the spline so that the secants between the points are - * within the spline sample tolerance - * 3. walk down the secant lines using the step distance - * 4. assign timestamps to each step + * within the spline sample tolerance, and the points are close enough together + * 3. assign timestamps to each step + * + * This used to support moving start and end states but we never used it, so + * it's gone. */ 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 - * "twist" segment with rotation is a curve. If the scallops are too big, make - * this number smaller. If the trajectories are too slow to generate, make this - * number bigger. - */ - private static final double SPLINE_SAMPLE_TOLERANCE_M = 0.02; - /** - * Maximum theta error. - */ - private static final double SPLINE_SAMPLE_TOLERANCE_RAD = 0.2; - /** - * Size of steps along the path. Make this number smaller for tight curves to - * look better. Make it bigger to make trajectories (a little) faster to - * generate. - */ - private static final double TRAJECTORY_STEP_M = 0.1; - /* - * If we try to start a trajectory while respecting initial velocity, but the - * initial velocity is less than 0.01 m/s, just treat it as rest-to-rest. - */ - private static final double VELOCITY_EPSILON = 1e-2; - - private final double m_splineTolerance; - private final double m_splineRotationTolerance; - private final double m_trajectoryStep; - private final ScheduleGenerator m_scheduleGenerator; - - public TrajectoryPlanner(List constraints) { - this(SPLINE_SAMPLE_TOLERANCE_M, SPLINE_SAMPLE_TOLERANCE_RAD, TRAJECTORY_STEP_M, constraints); - } - - public TrajectoryPlanner( - double splineTolerance, - double splineRotationTolerance, - double trajectoryStep, - List constraints) { - m_splineTolerance = splineTolerance; - m_splineRotationTolerance = splineRotationTolerance; - m_trajectoryStep = trajectoryStep; - m_scheduleGenerator = new ScheduleGenerator(constraints); - - } + private final PathFactory m_pathFactory; + private final TrajectoryFactory m_trajectoryFactory; - /** A square counterclockwise starting with +x. */ - public List square(Pose2d p0) { - Pose2d p1 = p0.plus(new Transform2d(1, 0, Rotation2d.kZero)); - Pose2d p2 = p0.plus(new Transform2d(1, 1, Rotation2d.kZero)); - Pose2d p3 = p0.plus(new Transform2d(0, 1, Rotation2d.kZero)); - return List.of( - restToRest(p0, p1), - restToRest(p1, p2), - restToRest(p2, p3), - restToRest(p3, p0)); - } - - /** Make a square that gets a reset starting point at each corner. */ - public List> permissiveSquare() { - return List.of( - x -> restToRest(x, x.plus(new Transform2d(1, 0, Rotation2d.kZero))), - x -> restToRest(x, x.plus(new Transform2d(0, 1, Rotation2d.kZero))), - x -> restToRest(x, x.plus(new Transform2d(-1, 0, Rotation2d.kZero))), - x -> restToRest(x, x.plus(new Transform2d(0, -1, Rotation2d.kZero)))); - } - - /** From current to x+1 */ - public Trajectory100 line(Pose2d initial) { - return restToRest( - initial, - initial.plus(new Transform2d(1, 0, Rotation2d.kZero))); + public TrajectoryPlanner(PathFactory pathFactory, TrajectoryFactory trajectoryFactory) { + m_pathFactory = pathFactory; + m_trajectoryFactory = trajectoryFactory; } + /** + * Makes a trajectory through the supplied waypoints, starting and ending + * motionless. + */ public Trajectory100 restToRest(List waypoints) { return generateTrajectory(waypoints, 0.0, 0.0); } - public Trajectory100 movingToRest(ModelR3 startState, Pose2d end) { - return movingToMoving(startState, new ModelR3(end)); - } - - public Trajectory100 movingToMoving(ModelR3 startState, ModelR3 endState) { - Translation2d startTranslation = startState.translation(); - VelocitySE2 startVelocity = startState.velocity(); - - Translation2d endTranslation = endState.translation(); - VelocitySE2 endVelocity = endState.velocity(); - - // should work with out this. - if (startVelocity.norm() < VELOCITY_EPSILON && endVelocity.norm() < VELOCITY_EPSILON) { - return restToRest(startState.pose(), endState.pose()); - } - - Translation2d full = endTranslation.minus(startTranslation); - Rotation2d courseToGoal = full.getAngle(); - Rotation2d startingAngle = startVelocity.angle().orElse(courseToGoal); - - // 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(); - - try { - return generateTrajectory( - List.of( - new WaypointSE2( - startState.pose(), - DirectionSE2.irrotational(startingAngle), - e1), - new WaypointSE2( - endState.pose(), - DirectionSE2.irrotational(courseToGoal), - 1.2)), - startVelocity.norm(), - 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) { - Translation2d startTranslation = startState.translation(); - VelocitySE2 startVelocity = startState.velocity(); - - Translation2d endTranslation = endState.translation(); - VelocitySE2 endVelocity = endState.velocity(); - - // should work with out this. - if (startVelocity.norm() < VELOCITY_EPSILON && endVelocity.norm() < VELOCITY_EPSILON) { - return restToRest(startState.pose(), endState.pose()); - } - - Translation2d full = endTranslation.minus(startTranslation); - - // 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(); - - try { - return generateTrajectory( - List.of( - new WaypointSE2( - startState.pose(), - DirectionSE2.irrotational(startCourse), - e1), - new WaypointSE2( - endState.pose(), - DirectionSE2.irrotational(endCourse), - 1.2)), - splineEntranceVelocity, - 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); - } - - /** - * Produces straight lines from start to end. - */ - public Trajectory100 restToRest(Pose2d start, Pose2d end) { - Translation2d startTranslation = start.getTranslation(); - Translation2d endTranslation = end.getTranslation(); - - Rotation2d courseToGoal = endTranslation.minus(startTranslation).getAngle(); - - try { - // 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; - } - } + ///////////////////////////////////////////////////////////////////////////////////// + /// + /// /** - * The shape of the spline accommodates the start and end velocities. + * Makes a trajectory through the supplied waypoints, with start and end + * velocities. */ - public Trajectory100 generateTrajectory( - List waypoints, - double start_vel, - double end_vel) { + Trajectory100 generateTrajectory( + 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); + Path100 path = m_pathFactory.fromWaypoints(waypoints); if (DEBUG) System.out.printf("PATH\n%s\n", path); // Generate the timed trajectory. - Trajectory100 result = m_scheduleGenerator.timeParameterizeTrajectory( - path, - m_trajectoryStep, - start_vel, - end_vel); + Trajectory100 result = m_trajectoryFactory.fromPath(path, start_vel, end_vel); if (DEBUG) System.out.printf("TRAJECTORY\n%s\n", result); return result; diff --git a/lib/src/main/java/org/team100/lib/trajectory/examples/TrajectoryExamples.java b/lib/src/main/java/org/team100/lib/trajectory/examples/TrajectoryExamples.java new file mode 100644 index 00000000..ff1715a7 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/examples/TrajectoryExamples.java @@ -0,0 +1,69 @@ +package org.team100.lib.trajectory.examples; + +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.targeting.TargetUtil; +import org.team100.lib.trajectory.Trajectory100; +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.Transform2d; + +/** Examples that are mostly only useful for testing. */ +public class TrajectoryExamples { + + private final TrajectoryPlanner p; + + public TrajectoryExamples(TrajectoryPlanner p) { + this.p = p; + } + + /** A square counterclockwise starting with +x. */ + public List square(Pose2d p0) { + Pose2d p1 = p0.plus(new Transform2d(1, 0, Rotation2d.kZero)); + Pose2d p2 = p0.plus(new Transform2d(1, 1, Rotation2d.kZero)); + Pose2d p3 = p0.plus(new Transform2d(0, 1, Rotation2d.kZero)); + return List.of( + restToRest(p0, p1), + restToRest(p1, p2), + restToRest(p2, p3), + restToRest(p3, p0)); + } + + /** Make a square that gets a reset starting point at each corner. */ + public List> permissiveSquare() { + return List.of( + x -> restToRest(x, x.plus(new Transform2d(1, 0, Rotation2d.kZero))), + x -> restToRest(x, x.plus(new Transform2d(0, 1, Rotation2d.kZero))), + x -> restToRest(x, x.plus(new Transform2d(-1, 0, Rotation2d.kZero))), + x -> restToRest(x, x.plus(new Transform2d(0, -1, Rotation2d.kZero)))); + } + + /** From current to x+1 */ + public Trajectory100 line(Pose2d initial) { + return restToRest( + initial, + initial.plus(new Transform2d(1, 0, Rotation2d.kZero))); + } + + /** + * Produces straight lines from start to end, without rotation. + * + * This is only useful for testing: in reality we always want rotation and curves. + */ + public Trajectory100 restToRest(Pose2d start, Pose2d end) { + Rotation2d courseToGoal = TargetUtil.absoluteBearing(start.getTranslation(), end.getTranslation()); + // direction towards goal without rotating + DirectionSE2 direction = DirectionSE2.irrotational(courseToGoal); + return p.restToRest(List.of( + new WaypointSE2(start, direction, 1), + new WaypointSE2(end, direction, 1))); + } + + + +} 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 f16dc9fa..acc0f7d9 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 @@ -5,7 +5,6 @@ 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; @@ -30,14 +29,15 @@ public class Path100 { private final double[] m_distances; public Path100(final List states) { - m_points = new ArrayList<>(states.size()); - m_distances = new double[states.size()]; + int n = states.size(); + m_points = new ArrayList<>(n); + m_distances = new double[n]; if (states.isEmpty()) { return; } m_distances[0] = 0.0; m_points.add(states.get(0)); - for (int i = 1; i < states.size(); ++i) { + for (int i = 1; i < n; ++i) { m_points.add(states.get(i)); Pose2dWithMotion p0 = getPoint(i - 1); Pose2dWithMotion p1 = getPoint(i); @@ -71,9 +71,11 @@ public double getMaxDistance() { * Walks through all the points to find the bracketing points, and then * interpolates between them. * + * Beware, can return null if the path is empty. + * * @param distance in meters, always a non-negative number. */ - public Pose2dWithMotion sample(double distance) throws ScheduleGenerator.TimingException { + public Pose2dWithMotion sample(double distance) { if (distance >= getMaxDistance()) { // off the end return getPoint(length() - 1); @@ -105,11 +107,11 @@ public Pose2dWithMotion sample(double distance) throws ScheduleGenerator.TimingE return lerp; } } - throw new ScheduleGenerator.TimingException(); + return null; } /** Just returns the list of points with no further sampling. */ - public Pose2dWithMotion[] resample() throws ScheduleGenerator.TimingException { + public Pose2dWithMotion[] resample() { return m_points.toArray(Pose2dWithMotion[]::new); } @@ -136,7 +138,7 @@ public String toString() { * 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 { + public Pose2dWithMotion[] resample(double step) { double maxDistance = getMaxDistance(); if (maxDistance == 0) throw new IllegalArgumentException("max distance must be greater than zero"); @@ -150,6 +152,7 @@ Pose2dWithMotion[] resample(double step) throws ScheduleGenerator.TimingExceptio // the values here are just interpolated from the original values. double d = Math.min(i * step, maxDistance); Pose2dWithMotion state = sample(d); + if (state == null) continue; if (DEBUG) System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state); samples[i] = state; 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 9dce4431..2549b87c 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 @@ -15,18 +15,70 @@ public class PathFactory { 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 + * "twist" segment with rotation is a curve. If the scallops are too big, make + * this number smaller. If the trajectories are too slow to generate, make this + * number bigger. + */ + private static final double SPLINE_SAMPLE_TOLERANCE_M = 0.02; + /** + * Maximum theta error. + */ + private static final double SPLINE_SAMPLE_TOLERANCE_RAD = 0.2; + /** + * Size of steps along the path. Make this number smaller for tight curves to + * look better. Make it bigger to make trajectories (a little) faster to + * generate. + */ + private static final double TRAJECTORY_STEP_M = 0.1; + + private final double maxNorm; + private final double maxDx; + private final double maxDy; + private final double maxDTheta; + + public PathFactory() { + this(TRAJECTORY_STEP_M, + SPLINE_SAMPLE_TOLERANCE_M, + SPLINE_SAMPLE_TOLERANCE_M, + SPLINE_SAMPLE_TOLERANCE_RAD); + } - public static Path100 pathFromWaypoints( - List waypoints, + public PathFactory( double maxNorm, double maxDx, double maxDy, double maxDTheta) { + this.maxNorm = maxNorm; + this.maxDx = maxDx; + this.maxDy = maxDy; + this.maxDTheta = maxDTheta; + } + + /** + * A path that passes through the waypoints and control directions. + */ + public Path100 fromWaypoints(List waypoints) { + List splines = splinesFromWaypoints(waypoints); + return fromSplines(splines); + } + + ///////////////////////////////////////////////////////////////////////////////////// + /// + /// + /// + + /** + * Make a list of splines with the waypoints as knots. + */ + private List splinesFromWaypoints(List waypoints) { 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))); } - return new Path100(parameterizeSplines(splines, maxNorm, maxDx, maxDy, maxDTheta)); + return splines; } /** @@ -36,49 +88,38 @@ public static Path100 pathFromWaypoints( * the specified tolerance (dx, dy, dtheta) of the actual spline. * * The trajectory scheduler consumes these points, interpolating between them - * with straight lines. It might be better to sample the spline directly. - * - * @param s the spline to parametrize - * @param t0 starting percentage of spline to parametrize - * @param t1 ending percentage of spline to parametrize - * @return list of Pose2dWithCurvature that approximates the original spline + * with straight lines. */ - static List parameterizeSpline( - HolonomicSpline s, - double maxNorm, - double maxDx, - double maxDy, - double maxDTheta, - double t0, - double t1) { - List rv = new ArrayList<>(); - rv.add(s.getPose2dWithMotion(0.0)); - double dt = (t1 - t0); - for (double t = 0; t < t1; t += dt) { - PathFactory.getSegmentArc(s, maxNorm, rv, t, t + dt, maxDx, maxDy, maxDTheta); - } - return rv; + List samplesFromSpline(HolonomicSpline spline) { + List result = new ArrayList<>(); + result.add(spline.getPose2dWithMotion(0.0)); + getSegmentArc(spline, result, 0, 1); + return result; } - public static List parameterizeSplines( - List splines, - double maxNorm, - double maxDx, - double maxDy, - double maxDTheta) { - List rv = new ArrayList<>(); + public Path100 fromSplines(List splines) { + return new Path100(samplesFromSplines(splines)); + } + + /** + * For testing only. Do not call this directly + */ + public List samplesFromSplines(List splines) { + List result = new ArrayList<>(); if (splines.isEmpty()) - return rv; - rv.add(splines.get(0).getPose2dWithMotion(0.0)); + return result; + result.add(splines.get(0).getPose2dWithMotion(0.0)); for (int i = 0; i < splines.size(); i++) { HolonomicSpline s = splines.get(i); if (DEBUG) System.out.printf("SPLINE:\n%d\n%s\n", i, s); - List samples = parameterizeSpline(s, maxNorm, maxDx, maxDy, maxDTheta, 0.0, 1.0); + List samples = samplesFromSpline(s); + // the sample at the end of the previous spline is the same as the one for the + // beginning of the next, so don't include it twice. samples.remove(0); - rv.addAll(samples); + result.addAll(samples); } - return rv; + return result; } /** @@ -88,19 +129,15 @@ public static List parameterizeSplines( * * Note if the path is s-shaped, then bisection can find the middle :-) */ - private static void getSegmentArc( + private void getSegmentArc( HolonomicSpline spline, - double maxNorm, // max distance between points List rv, - double t0, // [0,1] not time - double t1, // [0,1] not time - double maxDx, - double maxDy, - double maxDTheta) { - Pose2d p0 = spline.getPose2d(t0); - double thalf = (t0 + t1) / 2; - Pose2d phalf = spline.getPose2d(thalf); - Pose2d p1 = spline.getPose2d(t1); + double s0, + double s1) { + Pose2d p0 = spline.getPose2d(s0); + double shalf = (s0 + s1) / 2; + Pose2d phalf = spline.getPose2d(shalf); + Pose2d p1 = spline.getPose2d(s1); // twist from p0 to p1 Twist2d twist_full = p0.log(p1); @@ -112,8 +149,8 @@ private static void getSegmentArc( Transform2d error = phalf_predicted.minus(phalf); // also prohibit large changes in direction between points - Pose2dWithMotion p20 = spline.getPose2dWithMotion(t0); - Pose2dWithMotion p21 = spline.getPose2dWithMotion(t1); + Pose2dWithMotion p20 = spline.getPose2dWithMotion(s0); + Pose2dWithMotion p21 = spline.getPose2dWithMotion(s1); Twist2d p2t = p20.getPose().course().minus(p21.getPose().course()); // note the extra conditions to avoid points too far apart. @@ -126,12 +163,11 @@ private static void getSegmentArc( || Metrics.l2Norm(twist_full) > maxNorm || Metrics.l2Norm(p2t) > maxNorm) { // add a point in between - - getSegmentArc(spline, maxNorm, rv, t0, thalf, maxDx, maxDy, maxDTheta); - getSegmentArc(spline, maxNorm, rv, thalf, t1, maxDx, maxDy, maxDTheta); + getSegmentArc(spline, rv, s0, shalf); + getSegmentArc(spline, rv, shalf, s1); } else { - // midpoint is close enough, this looks good - rv.add(spline.getPose2dWithMotion(t1)); + // midpoint is close enough, so add the endpoint + rv.add(spline.getPose2dWithMotion(s1)); } } } 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 ec28340b..629fd54b 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 @@ -4,7 +4,6 @@ import org.team100.lib.geometry.DirectionR3; import org.team100.lib.geometry.DirectionSE3; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose3dWithDirection; import org.team100.lib.util.Math100; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java index fbbab2cd..269870ff 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java @@ -7,7 +7,7 @@ * Represents a state within a 2d holonomic trajectory, i.e. with heading * independent from course. * - * The timing fields are set by the ScheduleGenerator. + * The timing fields are set by the TrajectoryFactory. */ public class TimedState { private static final boolean DEBUG = false; diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java similarity index 76% rename from lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java rename to lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java index 2b2eabdc..3b61bceb 100755 --- a/lib/src/main/java/org/team100/lib/trajectory/timing/ScheduleGenerator.java +++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java @@ -12,11 +12,7 @@ * Given a path, produces a trajectory, which includes the path and adds a * schedule. */ -public class ScheduleGenerator { - - public static class TimingException extends Exception { - } - +public class TrajectoryFactory { public static final boolean DEBUG = false; private static final double EPSILON = 1e-6; @@ -26,50 +22,50 @@ public static class TimingException extends Exception { private final List m_constraints; - public ScheduleGenerator(List constraints) { + public TrajectoryFactory(List constraints) { m_constraints = constraints; } /** - * Samples the path evenly by distance, then assigns a time to each sample. + * Samples the path, then assigns a time to each sample. */ - public Trajectory100 timeParameterizeTrajectory( - Path100 path, - double step, - double start_vel, - double end_vel) { - try { - // Pose2dWithMotion[] samples = path.resample(step); - Pose2dWithMotion[] samples = path.resample(); - return timeParameterizeTrajectory(samples, start_vel, end_vel); - } catch (TimingException e) { - e.printStackTrace(); - System.out.println("WARNING: Timing exception"); - return new Trajectory100(); - } + public Trajectory100 fromPath(Path100 path, double start_vel, double end_vel) { + Pose2dWithMotion[] samples = getSamples(path); + return fromSamples(samples, start_vel, end_vel); + } + + ///////////////////////////////////////////////////////////////////////////////////// + /// + /// + + /** + * Return an array of poses from the path. + */ + private Pose2dWithMotion[] getSamples(Path100 path) { + return path.resample(); } /** - * Input is some set of samples (could be evenly sampled or not). + * Input is a list of samples (could be evenly sampled or not). * * Output is these same samples with time. */ - public Trajectory100 timeParameterizeTrajectory( + private Trajectory100 fromSamples( Pose2dWithMotion[] samples, double start_vel, 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); + double[] distances = distances(samples); + double[] velocities = velocities(samples, start_vel, end_vel, distances); + double[] accels = accels(distances, velocities); + double[] runningTime = runningTime(distances, velocities, accels); + List timedStates = timedStates(samples, velocities, accels, runningTime); return new Trajectory100(timedStates); } /** * Creates a list of timed states. */ - private List getTimedStates( + private List timedStates( Pose2dWithMotion[] samples, double[] velocities, double[] accels, double[] runningTime) { int n = samples.length; List timedStates = new ArrayList<>(n); @@ -82,7 +78,7 @@ private List getTimedStates( /** * Computes duration of each arc and accumulate. Assigns a time to each point. */ - private double[] getRunningTime(double[] distances, double[] velocities, double[] accels) { + private double[] runningTime(double[] distances, double[] velocities, double[] accels) { int n = distances.length; double[] runningTime = new double[n]; for (int i = 1; i < n; ++i) { @@ -102,7 +98,7 @@ private double[] getRunningTime(double[] distances, double[] velocities, double[ * The very last accel is always zero, but it's never used since it describes * samples off the end of the trajectory. */ - private double[] getAccels(double[] distances, double[] velocities) { + private double[] accels(double[] distances, double[] velocities) { int n = distances.length; double[] accels = new double[n]; for (int i = 0; i < n - 1; ++i) { @@ -116,7 +112,7 @@ private double[] getAccels(double[] distances, double[] velocities) { * Assigns a velocity to each sample, using velocity, accel, and decel * constraints. */ - private double[] getVelocities( + private double[] velocities( Pose2dWithMotion[] samples, double start_vel, double end_vel, double[] distances) { double velocities[] = new double[samples.length]; forward(samples, start_vel, distances, velocities); @@ -129,9 +125,8 @@ private double[] getVelocities( } /** - * Computes velocities[i+1] using velocity and acceleration constraints using - * the - * state at i. + * Computes velocities[i+1] using velocity and acceleration constraints + * referencing the state at i. */ private void forward( Pose2dWithMotion[] samples, double start_vel, double[] distances, double[] velocities) { @@ -145,12 +140,12 @@ private void forward( break; } // velocity constraint depends only on state - double maxV = velocityConstraint(samples[i + 1]); + double maxVelocity = maxVelocity(samples[i + 1]); // start with the maximum velocity - velocities[i + 1] = maxV; + velocities[i + 1] = maxVelocity; // 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]); + double maxAccel = maxAccel(samples[i], velocities[i]); if (impliedAccel > maxAccel + EPSILON) { velocities[i + 1] = Math100.v1(velocities[i], maxAccel, arclength); } @@ -158,10 +153,12 @@ private void forward( } /** - * Adjusts velocities[i] for decel constraint based on the state at i+1. + * Adjusts velocities[i] for decel constraint referencing the state at i+1. * * This isn't strictly correct since the decel constraint should operate at i, - * but walking backwards through the path, only i+1 is available. + * but walking backwards through the path, only i+1 is available, and the + * samples should be enough close together, and the velocity should change + * smoothly smooth enough so it shouldn't matter much in practice. */ private void backward( Pose2dWithMotion[] samples, double end_vel, double[] distances, double[] velocities) { @@ -175,7 +172,7 @@ private void backward( } 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]); + double maxDecel = maxDecel(samples[i + 1], velocities[i + 1]); if (impliedAccel < maxDecel - EPSILON) { velocities[i] = Math100.v0(velocities[i + 1], maxDecel, arclength); } @@ -185,7 +182,7 @@ private void backward( /** * Computes the length of each arc and accumulates. */ - private double[] getDistances(Pose2dWithMotion[] samples) { + private double[] distances(Pose2dWithMotion[] samples) { int n = samples.length; double distances[] = new double[n]; for (int i = 1; i < n; ++i) { @@ -199,7 +196,7 @@ private double[] getDistances(Pose2dWithMotion[] samples) { * Returns the lowest (i.e. closest to zero) velocity constraint from the list * of constraints. Always positive or zero. */ - private double velocityConstraint(Pose2dWithMotion sample) { + private double maxVelocity(Pose2dWithMotion sample) { double minVelocity = HIGH_V; for (TimingConstraint constraint : m_constraints) { minVelocity = Math.min(minVelocity, constraint.maxV(sample)); @@ -211,7 +208,7 @@ private double velocityConstraint(Pose2dWithMotion sample) { * Returns the lowest (i.e. closest to zero) acceleration constraint from the * list of constraints. Always positive or zero. */ - private double accelConstraint(Pose2dWithMotion sample, double velocity) { + private double maxAccel(Pose2dWithMotion sample, double velocity) { double minAccel = HIGH_ACCEL; for (TimingConstraint constraint : m_constraints) { minAccel = Math.min(minAccel, constraint.maxAccel(sample, velocity)); @@ -223,7 +220,7 @@ private double accelConstraint(Pose2dWithMotion sample, double velocity) { * Returns the highest (i.e. closest to zero) deceleration constraint from the * list of constraints. Always negative or zero. */ - private double decelConstraint(Pose2dWithMotion sample, double velocity) { + private double maxDecel(Pose2dWithMotion sample, double velocity) { double maxDecel = -HIGH_ACCEL; for (TimingConstraint constraint : m_constraints) { maxDecel = Math.max(maxDecel, constraint.maxDecel(sample, velocity)); 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 aaf386b0..7e3a4f35 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,8 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; @@ -24,9 +24,10 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.examples.TrajectoryExamples; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; -import org.team100.lib.trajectory.timing.ScheduleGenerator; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; @@ -44,9 +45,12 @@ public class ReferenceControllerR3Test implements Timeless { void testTrajectoryStart() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); // stepTime(); - Trajectory100 t = planner.restToRest( + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless @@ -97,10 +101,12 @@ void testTrajectoryStart() { void testTrajectoryDone() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); stepTime(); - - Trajectory100 t = planner.restToRest( + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless @@ -152,15 +158,12 @@ void testFieldRelativeTrajectory() { double stepSize = 2; - Path100 path = PathFactory.pathFromWaypoints(waypoints, stepSize, 2, 0.25, 0.1); + PathFactory pathFactory = new PathFactory(stepSize, 2, 0.25, 0.1); + Path100 path = pathFactory.fromWaypoints(waypoints); assertFalse(path.isEmpty()); - ScheduleGenerator u = new ScheduleGenerator(Arrays.asList()); - Trajectory100 trajectory = u.timeParameterizeTrajectory( - path, - stepSize, - start_vel, - end_vel); + TrajectoryFactory u = new TrajectoryFactory(Arrays.asList()); + Trajectory100 trajectory = u.fromPath(path, start_vel, end_vel); if (DEBUG) System.out.printf("TRAJECTORY:\n%s\n", trajectory); diff --git a/lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java b/lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java index c61b80a6..2798b0cb 100644 --- a/lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java +++ b/lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java @@ -16,8 +16,11 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -28,12 +31,15 @@ public class TrajectoryReferenceTest implements Timeless { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); @Test void testSimple() { Cache.clear(); - Trajectory100 t = planner.restToRest( + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); TrajectoryReferenceR3 r = new TrajectoryReferenceR3(logger, t); 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 831f42b8..25cdb68d 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 @@ -14,8 +14,12 @@ import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimedState; +import org.team100.lib.trajectory.timing.TimingConstraint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -251,10 +255,14 @@ void testTrajectory() { final ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); AnalyticalJacobian j = new AnalyticalJacobian(k); - TrajectoryPlanner planner = new TrajectoryPlanner(List.of(new ConstantConstraint(logger, 1, 1))); + List constraints = List.of(new ConstantConstraint(logger, 1, 1)); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Pose2d start = new Pose2d(1, -1, Rotation2d.kZero); Pose2d end = new Pose2d(2, 1, Rotation2d.k180deg); - Trajectory100 t = planner.restToRest(start, end); + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest(start, end); double d = t.duration(); double dt = d / 20; for (double time = 0; time < d; time += dt) { 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 c9a03760..83889227 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 @@ -15,8 +15,12 @@ import org.team100.lib.state.ModelR3; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimedState; +import org.team100.lib.trajectory.timing.TimingConstraint; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -228,10 +232,14 @@ void testTrajectory() { final ElevatorArmWristKinematics k = new ElevatorArmWristKinematics(2, 1); Jacobian j = new Jacobian(k); - TrajectoryPlanner planner = new TrajectoryPlanner(List.of(new ConstantConstraint(logger, 1, 1))); + List constraints = List.of(new ConstantConstraint(logger, 1, 1)); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Pose2d start = new Pose2d(1, -1, Rotation2d.kZero); Pose2d end = new Pose2d(2, 1, Rotation2d.k180deg); - Trajectory100 t = planner.restToRest(start, end); + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest(start, end); double d = t.duration(); double dt = d / 20; for (double time = 0; time < d; time += dt) { 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 deleted file mode 100644 index bd915db5..00000000 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithTrajectoryTest.java +++ /dev/null @@ -1,99 +0,0 @@ -package org.team100.lib.subsystems.r3.commands; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import java.io.IOException; -import java.util.List; - -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.WaypointSE2; -import org.team100.lib.geometry.Pose2dWithMotion; -import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; -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.Fixture; -import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; -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.Trajectory100; -import org.team100.lib.trajectory.TrajectoryPlanner; -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; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -class DriveToPoseWithTrajectoryTest implements Timeless { - - private static final double DELTA = 0.001; - private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); - private static final TrajectoryVisualization viz = new TrajectoryVisualization(logger); - - SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); - List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - - @Test - void testSimple() throws IOException { - Fixture fixture = new Fixture(); - - Pose2d goal = Pose2d.kZero; - SwerveDriveSubsystem drive = fixture.drive; - - ControllerR3 controller = ControllerFactoryR3.test(logger); - DriveToPoseWithTrajectory command = new DriveToPoseWithTrajectory( - logger, () -> goal, - drive, - (start, end) -> new Trajectory100( - List.of( - new TimedState( - new Pose2dWithMotion( - WaypointSE2.irrotational( - new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), - 0, 0), - 0, 0, 0))), - controller, - viz); - command.initialize(); - assertEquals(0, fixture.drive.getPose().getX(), DELTA); - command.execute(); - command.end(false); - } - - /** Demonstrate how to use DriveToWaypoint to go to apriltags. */ - @Test - void testAprilTag() throws IOException { - Fixture fixture = new Fixture(); - SwerveDriveSubsystem drive = fixture.drive; - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); - - Transform2d transform = new Transform2d( - new Translation2d(-1, -1), - Rotation2d.kZero); - - Pose2d goal = layout.getTagPose(Alliance.Blue, 1).get().toPose2d().plus(transform); - assertEquals(15.300, goal.getX(), DELTA); - assertEquals(0.876, goal.getY(), DELTA); - assertEquals(-0.942, goal.getRotation().getRadians(), DELTA); - - ControllerR3 m_controller = ControllerFactoryR3.test(logger); - DriveToPoseWithTrajectory command = new DriveToPoseWithTrajectory( - logger, () -> goal, drive, - (start, end) -> planner.movingToRest(start, end), - m_controller, viz); - command.initialize(); - assertEquals(0, fixture.drive.getPose().getX(), DELTA); - command.execute(); - command.end(false); - } - -} 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 21071290..fb40fff4 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 @@ -14,8 +14,10 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; import org.team100.lib.trajectory.timing.TimingConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.YawRateConstraint; import org.team100.lib.visualization.TrajectoryVisualization; @@ -29,6 +31,7 @@ * https://docs.google.com/spreadsheets/d/1tt7Fq-gkR7aoY6kH2WFVxj4y__SMXiKE0scPG3eHSAk/edit?gid=0#gid=0 */ public class DriveWithTrajectoryFunctionTest implements Timeless { + private static final boolean DEBUG = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -38,7 +41,9 @@ public class DriveWithTrajectoryFunctionTest implements Timeless { List constraints = List.of( new ConstantConstraint(log, 2, 2), new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); /** * This is the key to using DriveWithTrajectoryFunction: a function that takes a diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java index 64655637..1583b6eb 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java @@ -20,6 +20,9 @@ import org.team100.lib.subsystems.swerve.Fixture; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +import org.team100.lib.trajectory.path.PathFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import org.team100.lib.visualization.TrajectoryVisualization; @@ -41,7 +44,10 @@ void nolog() { void testSimple() throws IOException { Fixture fixture = new Fixture(); List constraints = new TimingConstraintFactory(fixture.swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + TrajectoryExamples ex = new TrajectoryExamples(planner); // this initial step is required since the timebase is different? stepTime(); Experiments.instance.testOverride(Experiment.UseSetpointGenerator, true); @@ -50,7 +56,7 @@ void testSimple() throws IOException { logger, fixture.drive, control, - x -> List.of(planner.line(x)), + x -> List.of(ex.line(x)), viz); c.initialize(); assertEquals(0, fixture.drive.getPose().getX(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java index 80c90535..fd7bb6b1 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java @@ -34,6 +34,9 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +import org.team100.lib.trajectory.path.PathFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; import org.team100.lib.visualization.TrajectoryVisualization; @@ -52,8 +55,11 @@ public class DriveWithTrajectoryTest implements Timeless { void testTrajectoryStart() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - Trajectory100 t = planner.restToRest( + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless @@ -106,8 +112,11 @@ void testTrajectoryStart() { void testTrajectoryDone() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - Trajectory100 t = planner.restToRest( + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless @@ -146,8 +155,11 @@ void testRealDrive() throws IOException { SwerveModuleCollection collection = SwerveModuleCollection.get(logger, 10, 20, swerveKinodynamics); collection.reset(); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - Trajectory100 trajectory = planner.restToRest( + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 trajectory = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless diff --git a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java index 86953c10..b9b0d564 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java @@ -17,6 +17,7 @@ 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.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -113,8 +114,8 @@ void testPoses() { new Rotation2d(0)), new DirectionSE2(1, 0, 0), 1)); - List poses = PathFactory.parameterizeSplines( - List.of(spline), 0.1, 0.02, 0.2, 0.1); + PathFactory pathFactory = new PathFactory(0.1, 0.02, 0.2, 0.1); + List poses = pathFactory.samplesFromSplines(List.of(spline)); XYSeries sx = PathToVectorSeries.x("spline", poses); XYDataset dataSet = TrajectoryPlotter.collect(sx); @@ -126,7 +127,9 @@ void testTrajectory() { List c = List.of( new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner p = new TrajectoryPlanner(c); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner p = new TrajectoryPlanner(pathFactory, trajectoryFactory); List waypoints = List.of( new WaypointSE2( new Pose2d( diff --git a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java index b9d0391e..6e0cb7b4 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java +++ b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java @@ -6,7 +6,6 @@ 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; @@ -23,24 +22,20 @@ public PathToVectorSeries(double 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(); + 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); } return s; } diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 9c07bd26..be6d9e56 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -16,8 +16,7 @@ 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.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; @@ -47,7 +46,9 @@ void testPreviewAndAdvance() { new WaypointSE2(end, DirectionSE2.irrotational(angleToGoal), 1)); List constraints = new TimingConstraintFactory(limits).fast(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 trajectory = planner.restToRest(waypoints); @@ -79,7 +80,9 @@ void testSample() { new WaypointSE2(end, DirectionSE2.irrotational(angleToGoal), 1)); List constraints = new TimingConstraintFactory(limits).fast(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 trajectory = planner.restToRest(waypoints); if (DEBUG) @@ -113,7 +116,9 @@ void testSampleThoroughly() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 trajectory = planner.restToRest(waypoints); if (DEBUG) @@ -156,7 +161,7 @@ private void check(Trajectory100 trajectory, double t, double x) { * There's no need to run this all the time */ @Test - void testSamplePerformance() throws TimingException { + void testSamplePerformance() { 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), @@ -186,8 +191,8 @@ void testSamplePerformance() throws TimingException { // INTERPOLATE SPLINE POINTS (170 ns) - Path100 path = PathFactory.pathFromWaypoints( - waypoints, 0.1, 0.02, 0.2, 0.1); + PathFactory pathFactory = new PathFactory(0.1, 0.02, 0.2, 0.1); + Path100 path = pathFactory.fromWaypoints(waypoints); assertEquals(22.734, path.getMaxDistance(), 0.001); start = System.nanoTime(); @@ -207,9 +212,9 @@ void testSamplePerformance() throws TimingException { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(logger); List constraints = new TimingConstraintFactory(limits).fast(logger); - ScheduleGenerator generator = new ScheduleGenerator(constraints); + TrajectoryFactory generator = new TrajectoryFactory(constraints); - Trajectory100 trajectory = generator.timeParameterizeTrajectory(path, 0.1, 0, 0); + Trajectory100 trajectory = generator.fromPath(path, 0, 0); TrajectoryPlotter.plot(trajectory, 1); assertEquals(313, trajectory.length()); 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 1b07fdd9..28bfca0c 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -8,6 +8,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; @@ -17,8 +18,11 @@ import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +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.TrajectoryFactory; import org.team100.lib.trajectory.timing.SwerveDriveDynamicsConstraint; import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; @@ -48,7 +52,9 @@ void testLinear() { new Rotation2d()), new DirectionSE2(1, 0, 0), 1.2)); List constraints = new ArrayList<>(); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 t = planner.restToRest(waypoints); assertEquals(17, t.length()); TimedState tp = t.getPoint(0); @@ -79,7 +85,9 @@ void testLinearRealistic() { new SwerveDriveDynamicsConstraint(logger, limits, 1, 1), new YawRateConstraint(logger, limits, 0.2), new CapsizeAccelerationConstraint(logger, limits, 0.2)); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 t = planner.restToRest(waypoints); assertEquals(17, t.length()); TimedState tp = t.getPoint(0); @@ -108,7 +116,9 @@ void testPerformance() { new Rotation2d()), new DirectionSE2(0, 1, 0), 1.2)); List constraints = new ArrayList<>(); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); long startTimeNs = System.nanoTime(); Trajectory100 t = new Trajectory100(); // for profiling @@ -133,10 +143,13 @@ void testPerformance() { void testRestToRest() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); 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); + TrajectoryExamples ex = new TrajectoryExamples(planner); + Trajectory100 trajectory = ex.restToRest(start.pose(), end); assertEquals(1.565, trajectory.duration(), DELTA); /** progress along trajectory */ @@ -161,10 +174,33 @@ void testRestToRest() { void testMovingToRest() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.forRealisticTest(logger); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(logger); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); 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); + + VelocitySE2 startVelocity = start.velocity(); + + Translation2d full = end.getTranslation().minus(start.translation()); + Rotation2d courseToGoal = full.getAngle(); + Rotation2d startingAngle = startVelocity.angle().orElse(courseToGoal); + + // 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(); + Trajectory100 traj = planner.generateTrajectory( + List.of( + new WaypointSE2( + start.pose(), + DirectionSE2.irrotational(startingAngle), + e1), + new WaypointSE2( + end, + DirectionSE2.irrotational(courseToGoal), + 1.2)), + startVelocity.norm(), + 0); assertEquals(1.176, traj.duration(), DELTA); } @@ -183,10 +219,32 @@ void test2d() { List constraints = List.of( new ConstantConstraint(logger, 1, 1), new CapsizeAccelerationConstraint(logger, 1, 1)); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); 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); + VelocitySE2 startVelocity = start.velocity(); + + Translation2d full = end.getTranslation().minus(start.translation()); + Rotation2d courseToGoal = full.getAngle(); + Rotation2d startingAngle = startVelocity.angle().orElse(courseToGoal); + + // 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(); + Trajectory100 traj = planner.generateTrajectory( + List.of( + new WaypointSE2( + start.pose(), + DirectionSE2.irrotational(startingAngle), + e1), + new WaypointSE2( + end, + DirectionSE2.irrotational(courseToGoal), + 1.2)), + startVelocity.norm(), + 0); if (DEBUG) traj.dump(); assertEquals(2.741, traj.duration(), DELTA); @@ -216,7 +274,9 @@ void test2d2() { List constraints = List.of( new ConstantConstraint(logger, 1, 1), new CapsizeAccelerationConstraint(logger, 0.5, 1)); - TrajectoryPlanner planner = new TrajectoryPlanner(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); Trajectory100 traj = planner.generateTrajectory(waypoints, 1, 1); if (DEBUG) @@ -224,4 +284,52 @@ void test2d2() { assertEquals(4.603, traj.duration(), DELTA); } + /** + * accelerating at the start + * slowing before the "slow zone" in the middle + * speeding up again + * slowing to a stop at the end + */ + @Test + void testVariableConstraint() { + + class ConditionalTimingConstraint implements TimingConstraint { + @Override + public double maxV(Pose2dWithMotion state) { + double x = state.getPose().pose().getTranslation().getX(); + if (x < 1.5) { + return 2.0; + } + if (x < 2.5) { + return 1.0; + } + return 2.0; + + } + + @Override + public double maxAccel(Pose2dWithMotion state, double velocity) { + return 2; + } + + @Override + public double maxDecel(Pose2dWithMotion state, double velocity) { + return -1; + } + } + + List constraints = List.of( + new ConditionalTimingConstraint()); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + List waypoints = List.of( + new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3), + new WaypointSE2(new Pose2d(4, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3)); + Trajectory100 traj = planner.generateTrajectory(waypoints, 0, 0); + if (DEBUG) + traj.dump(); + + } + } diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java index 22d16f5e..8b9036d5 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java @@ -11,7 +11,10 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.trajectory.examples.TrajectoryExamples; +import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.YawRateConstraint; @@ -35,8 +38,11 @@ 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( + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner p = new TrajectoryPlanner(pathFactory, trajectoryFactory); + TrajectoryExamples ex = new TrajectoryExamples(p); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, new Rotation2d()), new Pose2d(10, 1, new Rotation2d())); new TrajectoryPlotter(0.5).plot("simple", t); @@ -45,9 +51,11 @@ void testSimple() throws InterruptedException { /** 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( + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(List.of(new ConstantConstraint(log, 1, 0.1))); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner p = new TrajectoryPlanner(pathFactory, trajectoryFactory); + TrajectoryExamples ex = new TrajectoryExamples(p); + Trajectory100 t = ex.restToRest( new Pose2d(0, 0, new Rotation2d()), new Pose2d(0, 0, new Rotation2d(1))); assertTrue(t.isEmpty()); @@ -77,8 +85,10 @@ void testCircle() { 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); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + Trajectory100 trajectory = planner.generateTrajectory(waypoints, 0, 0); TrajectoryPlotter.plot(trajectory, 0.25); } @@ -97,8 +107,10 @@ void testDheading() { 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); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); + Trajectory100 trajectory = planner.generateTrajectory(waypoints, 0, 0); double duration = trajectory.duration(); TimedState p0 = trajectory.sample(0); if (DEBUG) @@ -149,7 +161,9 @@ void testCurved() throws InterruptedException { List c = List.of( new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner p = new TrajectoryPlanner(c); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner p = new TrajectoryPlanner(pathFactory, trajectoryFactory); List waypoints = List.of( new WaypointSE2( new Pose2d( @@ -175,7 +189,9 @@ void testMultipleWaypoints() throws InterruptedException { List c = List.of( new ConstantConstraint(log, 2, 0.5), new YawRateConstraint(log, 1, 1)); - TrajectoryPlanner p = new TrajectoryPlanner(c); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c); + PathFactory pathFactory = new PathFactory(); + TrajectoryPlanner p = new TrajectoryPlanner(pathFactory, trajectoryFactory); List waypoints = List.of( new WaypointSE2( new Pose2d( 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 14e490e1..336fab9b 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 @@ -45,10 +45,8 @@ class Path100Test { @Test void testEmpty() { List splines = new ArrayList<>(); - double maxDx = 0.1; - double maxDy = 0.1; - double maxDTheta = 0.1; - Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.1, maxDx, maxDy, maxDTheta)); + PathFactory pathFactory = new PathFactory(0.1, 0.1, 0.1, 0.1); + Path100 path = pathFactory.fromSplines(splines); assertEquals(0, path.length(), 0.001); } diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java index 3e82241a..e4001be7 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java @@ -9,8 +9,8 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.GeometryUtil; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.TrajectoryPlotter; import org.team100.lib.trajectory.path.spline.HolonomicSpline; @@ -43,7 +43,8 @@ void testCorner() { 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); + PathFactory pathFactory = new PathFactory(0.1, 0.01, 0.01, 0.1); + Path100 path = pathFactory.fromWaypoints(waypoints); assertEquals(54, path.length()); Pose2dWithMotion p = path.getPoint(0); @@ -69,8 +70,8 @@ void testLinear() { new Translation2d(1, 0), new Rotation2d()), new DirectionSE2(1, 0, 0), 1)); - Path100 path = PathFactory.pathFromWaypoints( - waypoints, 0.1, 0.01, 0.01, 0.1); + PathFactory pathFactory = new PathFactory(0.1, 0.01, 0.01, 0.1); + Path100 path = pathFactory.fromWaypoints(waypoints); assertEquals(17, path.length()); Pose2dWithMotion p = path.getPoint(0); assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA); @@ -96,7 +97,9 @@ void testSpin() { 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)); + PathFactory pathFactory = new PathFactory(0.1, 0.01, 0.01, 0.1); + assertThrows(IllegalArgumentException.class, + () -> pathFactory.fromWaypoints(waypoints)); } /** Hard corners once again do not work. */ @@ -123,7 +126,9 @@ void testActualCorner() { 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)); + PathFactory pathFactory = new PathFactory(0.1, 0.01, 0.01, 0.1); + assertThrows(IllegalArgumentException.class, + () -> pathFactory.fromWaypoints(waypoints)); } @Test @@ -145,9 +150,10 @@ void testComposite() { 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); + PathFactory pathFactory = new PathFactory(0.1, 0.01, 0.01, 0.1); + Path100 trajectory = pathFactory.fromWaypoints(waypoints); + TrajectoryPlotter.plot(trajectory, 0.1); + assertEquals(59, trajectory.length(), 0.001); } @Test @@ -164,7 +170,8 @@ void test() { new DirectionSE2(1, 5, 0), 1.2); HolonomicSpline s = new HolonomicSpline(p1, p2); - List samples = PathFactory.parameterizeSpline(s, 0.1, 0.05, 0.05, 0.1, 0.0, 1.0); + PathFactory pathFactory = new PathFactory(0.1, 0.05, 0.05, 0.1); + List samples = pathFactory.samplesFromSpline(s); double arclength = 0; Pose2dWithMotion cur_pose = samples.get(0); @@ -199,7 +206,8 @@ void testDx() { Rotation2d.kZero), new DirectionSE2(0, 1, 0), 1)); List splines = List.of(s0); - List motion = PathFactory.parameterizeSplines(splines, 0.1, 0.001, 0.001, 0.001); + PathFactory pathFactory = new PathFactory(0.1, 0.001, 0.001, 0.001); + List motion = pathFactory.samplesFromSplines(splines); for (Pose2dWithMotion p : motion) { if (DEBUG) System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), @@ -231,12 +239,11 @@ void testPerformance() { final double SPLINE_SAMPLE_TOLERANCE_M = 0.05; final double SPLINE_SAMPLE_TOLERANCE_RAD = 0.2; for (int i = 0; i < iterations; ++i) { - t = PathFactory.pathFromWaypoints( - waypoints, + t = new PathFactory( 0.1, SPLINE_SAMPLE_TOLERANCE_M, SPLINE_SAMPLE_TOLERANCE_M, - SPLINE_SAMPLE_TOLERANCE_RAD); + SPLINE_SAMPLE_TOLERANCE_RAD).fromWaypoints(waypoints); } long endTimeNs = System.nanoTime(); double totalDurationMs = (endTimeNs - startTimeNs) / 1000000.0; 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 de4b5f0e..3177cd5a 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 @@ -22,7 +22,7 @@ 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.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import edu.wpi.first.math.geometry.Pose2d; @@ -502,13 +502,13 @@ void testMismatchedXYDerivatives() { System.out.printf("spline %s\n", s); } - Path100 path = new Path100(PathFactory.parameterizeSplines(splines, 0.1, 0.05, 0.05, 0.05)); + PathFactory pathFactory = new PathFactory(0.1, 0.05, 0.05, 0.05); + Path100 path = pathFactory.fromSplines(splines); if (DEBUG) System.out.printf("path %s\n", path); List constraints = List.of(new ConstantConstraint(logger, 1, 1)); - ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); - Trajectory100 traj = scheduleGenerator.timeParameterizeTrajectory(path, - 0.05, 0, 0); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); + Trajectory100 traj = trajectoryFactory.fromPath(path, 0, 0); if (DEBUG) traj.dump(); TrajectoryPlotter.plot(traj, 0.1); @@ -540,7 +540,8 @@ void testEntryVelocity() { TrajectoryPlotter plotter = new TrajectoryPlotter(0.1); plotter.plot("splines", splines); - List motion = PathFactory.parameterizeSplines(splines, 0.1, 0.05, 0.05, 0.05); + PathFactory pathFactory = new PathFactory(0.1, 0.05, 0.05, 0.05); + List motion = pathFactory.samplesFromSplines(splines); if (DEBUG) { for (Pose2dWithMotion p : motion) { System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(), @@ -563,11 +564,10 @@ void testEntryVelocity() { assertEquals(8.166666, limits.getMaxCapsizeAccelM_S2(), 1e-6); List constraints = List.of( new CapsizeAccelerationConstraint(logger, limits, 1.0)); - ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); + TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); // speed // a = v^2/r so v = sqrt(ar) = 2.858 - Trajectory100 trajectory = scheduleGenerator.timeParameterizeTrajectory(path, - 0.05, 2.858, 2.858); + Trajectory100 trajectory = trajectoryFactory.fromPath(path, 2.858, 2.858); plotter.plot("plot", trajectory); diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java similarity index 90% rename from lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java rename to lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java index 09d4fa6f..a157bf72 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java @@ -27,7 +27,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -public class ScheduleGeneratorTest { +public class TrajectoryFactoryTest { private static final boolean DEBUG = false; public static final double EPSILON = 1e-12; private static final double DELTA = 0.01; @@ -57,12 +57,8 @@ public Trajectory100 buildAndCheckTrajectory( double end_vel, double max_vel, double max_acc) { - ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 timed_traj = u.timeParameterizeTrajectory( - path, - step_size, - start_vel, - end_vel); + TrajectoryFactory u = new TrajectoryFactory(constraints); + Trajectory100 timed_traj = u.fromPath(path, start_vel, end_vel); checkTrajectory(timed_traj, constraints, start_vel, end_vel, max_vel, max_acc); return timed_traj; } @@ -125,12 +121,8 @@ void testJustTurningInPlace() { 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); + TrajectoryFactory u = new TrajectoryFactory(constraints); + Trajectory100 traj = u.fromPath(path, 0.0, 0.0); assertEquals(0, traj.duration(), DELTA); } @@ -271,24 +263,12 @@ void testPerformance() { new DirectionSE2(0, 1, 0), 1.2)); long startTimeNs = System.nanoTime(); final int iterations = 100; - final double SPLINE_SAMPLE_TOLERANCE_M = 0.05; - final double SPLINE_SAMPLE_TOLERANCE_RAD = 0.2; - final double TRAJECTORY_STEP_M = 0.1; - - Path100 path = PathFactory.pathFromWaypoints( - waypoints, - TRAJECTORY_STEP_M, - SPLINE_SAMPLE_TOLERANCE_M, - SPLINE_SAMPLE_TOLERANCE_M, - SPLINE_SAMPLE_TOLERANCE_RAD); + PathFactory pathFactory = new PathFactory(0.1, 0.05, 0.05, 0.2); + Path100 path = pathFactory.fromWaypoints(waypoints); Trajectory100 t = new Trajectory100(); - ScheduleGenerator m_scheduleGenerator = new ScheduleGenerator(new ArrayList<>()); + TrajectoryFactory m_trajectoryFactory = new TrajectoryFactory(new ArrayList<>()); for (int i = 0; i < iterations; ++i) { - t = m_scheduleGenerator.timeParameterizeTrajectory( - path, - TRAJECTORY_STEP_M, - 0, - 0); + t = m_trajectoryFactory.fromPath(path, 0, 0); } long endTimeNs = System.nanoTime(); double totalDurationMs = (endTimeNs - startTimeNs) / 1000000.0; diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java index 9878c7df..412928a2 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 @@ -5,7 +5,6 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; @@ -16,7 +15,7 @@ import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.path.Path100; -import org.team100.lib.trajectory.timing.ScheduleGenerator.TimingException; +import org.team100.lib.trajectory.path.PathFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -24,7 +23,6 @@ /** * Verify that trajectory schedule generation yields a realistic profile. * - * see * https://docs.google.com/spreadsheets/d/16UUCCz-qcPz_YZMnsJnVkVO1KGp5zHCOVo7EoJct2nA/edit?gid=0#gid=0 */ public class TrajectoryVelocityProfileTest implements Timeless { @@ -32,7 +30,7 @@ public class TrajectoryVelocityProfileTest implements Timeless { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); // A five-meter straight line. - public static final Pose2dWithMotion[] WAYPOINTS = new Pose2dWithMotion[] { + private 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( @@ -40,71 +38,66 @@ public class TrajectoryVelocityProfileTest implements Timeless { 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( - GeometryUtil.fromDegrees(0), - GeometryUtil.fromDegrees(0)); - - 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("%6.3f, %6.3f\n", t, traj.sample(t).velocityM_S()); - } - - } + private static List waypointList = Arrays.asList(WAYPOINTS).stream().map(p -> p.getPose()).toList(); + private static PathFactory pathFactory = new PathFactory(0.1, 0.1, 0.1, 0.1); + private static Path100 path = pathFactory.fromWaypoints(waypointList); /** - * This uses the default max accel which is ridiculously high. + * Default max accel and velocity makes a very fast triangle profile. */ @Test - void testNoConstraint() throws TimingException { + void testNoConstraint() { List constraints = new ArrayList(); - ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); - print(traj); + TrajectoryFactory u = new TrajectoryFactory(constraints); + Trajectory100 traj = u.fromPath(path, 0, 0); + if (DEBUG) + traj.dump(); } /** - * This produces a trapezoid. + * This produces a trapezoid with the correct cruise (3.5) and accel/decel the + * same (10) */ @Test - void testConstantConstraint() throws TimingException { + void testConstantConstraint() { // 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(WAYPOINTS, 0, 0); - print(traj); + TrajectoryFactory u = new TrajectoryFactory(constraints); + Trajectory100 traj = u.fromPath(path, 0, 0); + if (DEBUG) + traj.dump(); } /** - * This produces the desired current-limited exponential shape. + * This produces the desired current-limited exponential shape for acceleration, + * and faster decel at the end. * - * TODO: this tickles a bug in the schedule generator dt(), fix it. + * https://docs.google.com/spreadsheets/d/1sbB-zTBUjRRlWHaWXe-V1ZDhAZCFwItVVO1x3LmZ4B4/edit?gid=104506786#gid=104506786 */ @Test - void testSwerveConstraint() throws TimingException { + void testSwerveConstraint() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTrajectoryTimingTest(logger); List constraints = List.of(new SwerveDriveDynamicsConstraint(logger, limits, 1, 1)); - ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); - print(traj); + TrajectoryFactory u = new TrajectoryFactory(constraints); + Trajectory100 traj = u.fromPath(path, 0, 0); + if (DEBUG) + traj.dump(); } + /** + * Realistic, cruses at 3.5 (which is right) + * + * https://docs.google.com/spreadsheets/d/1sbB-zTBUjRRlWHaWXe-V1ZDhAZCFwItVVO1x3LmZ4B4/edit?gid=1802036642#gid=1802036642 + */ @Test - 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 + void testAuto() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTrajectoryTimingTest(logger); TimingConstraintFactory timing = new TimingConstraintFactory(limits); List constraints = timing.testAuto(logger); - ScheduleGenerator u = new ScheduleGenerator(constraints); - Trajectory100 traj = u.timeParameterizeTrajectory( - new Path100(Arrays.asList(WAYPOINTS)), 0.5, 0, 0); - // Trajectory100 traj = u.timeParameterizeTrajectory(WAYPOINTS, 0, 0); - print(traj); + TrajectoryFactory u = new TrajectoryFactory(constraints); + Trajectory100 traj = u.fromPath(path, 0, 0); + if (DEBUG) + traj.dump(); } }