Examples with AdvancedSplineTrajectoryBuilder me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder used on opensource projects

Example 1 with AdvancedSplineTrajectoryBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder in project Pathfinder2 by Wobblyyyy.

the class ExampleSpline method run.

public void run() {
    // before anything else, we have to get some stuff set up.
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    // alright! trajectory time! let's see what's up.
    // as you can see, this is mostly pretty self-explanatory.
    // you create an AdvancedSplineTrajectoryBuilder and use the
    // add methods provided by that class to construct a trajectory.
    // this method works well, but it's a bit verbose - there's a
    // solution to that problem you'll see in just a moment.
    Trajectory trajectory1 = new AdvancedSplineTrajectoryBuilder().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).add(new PointXYZ(0, 0, 0)).add(new PointXYZ(4, 6, 0)).add(new PointXYZ(6, 12, 0)).add(new PointXYZ(8, 24, 0)).build();
    Trajectory trajectory2 = new AdvancedSplineTrajectoryBuilder().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).add(new PointXYZ(8, 24, 0)).add(new PointXYZ(6, 36, 0)).add(new PointXYZ(4, 40, 0)).add(new PointXYZ(0, 42, 0)).build();
    // thankfully, there's an easier way to create trajectories just like
    // that - we can make use of a "SplineBuilderFactory"
    SplineBuilderFactory factory = new SplineBuilderFactory().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5));
    // set the default speed, step, tolerance, and angle tolerance
    // values for the factory. all of the spline builders produced by
    // the factory will have have these values by default.
    // now we can create new trajectories, without having to repeat
    // the same 4 lines for each of the trajectories.
    Trajectory trajectory3 = factory.builder().add(0, 60, Angle.fromDeg(0)).add(new PointXYZ(20, 60, 0)).add(new PointXYZ(30, 60, 0)).add(new PointXYZ(40, 70, 0)).build();
    Trajectory trajectory4 = factory.builder().add(new PointXYZ(40, 70, 0)).add(new PointXYZ(30, 60, 0)).add(new PointXYZ(20, 60, 0)).add(0, 60, Angle.fromDeg(0)).build();
    // time to actually make the robot move now! once again, most of
    // these methods are fairly self-explanatory. basically, follow
    // the first two trajectories, come to a complete stop, and then
    // follow the next two trajectories, but each of those trajectories
    // should have an individual timeout of 10 seconds.
    pathfinder.followTrajectories(trajectory1, trajectory2).andThen(pf -> {
        // any other code you want to be executed after the
        // trajectory is finished
        pf.setTranslation(new Translation(0, 0, 0));
    }).followTrajectory(trajectory3).tickUntil(10_000).followTrajectory(trajectory4).tickUntil(10_000);
}

Example 2 with AdvancedSplineTrajectoryBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder in project Pathfinder2 by Wobblyyyy.

the class TestAdvancedSplineTrajectory method testSplineTo.

private void testSplineTo(PointXYZ... points) {
    if (points.length < 1)
        throw new IllegalArgumentException();
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setStep(0.1).setTolerance(tolerance).setSpeed(speed).setAngleTolerance(angleTolerance);
    builder.setInterpolationMode(mode);
    for (PointXYZ point : points) builder.add(point);
    builder.setInterpolationMode(InterpolationMode.DEFAULT);
    Trajectory trajectory = builder.build();
    PointXYZ target = points[points.length - 1];
    testTrajectory(trajectory, target);
}

Example 3 with AdvancedSplineTrajectoryBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testAkimaSplineInterpolation.

@Test
public void testAkimaSplineInterpolation() {
    Trajectory trajectory = new AdvancedSplineTrajectoryBuilder().setStep(0.5).setTolerance(DEFAULT_TOLERANCE).setAngleTolerance(Angle.fromDeg(1)).setInterpolationMode(InterpolationMode.AKIMA).setSpeed(0.5).add(new PointXYZ(0, 0, 0)).add(new PointXYZ(4, 10, 0)).add(new PointXYZ(8, 12, 0)).add(new PointXYZ(10, 13, 0)).add(new PointXYZ(12, 14, 0)).add(new PointXYZ(14, 15, 0)).build();
    pathfinder.followTrajectory(trajectory);
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(14, 15, 0));
}

Example 4 with AdvancedSplineTrajectoryBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testCubicSplineInterpolation.

@Test
public void testCubicSplineInterpolation() {
    Trajectory trajectory = new AdvancedSplineTrajectoryBuilder().setStep(0.1).setTolerance(DEFAULT_TOLERANCE).setAngleTolerance(Angle.fromDeg(1)).setInterpolationMode(InterpolationMode.CUBIC).setSpeed(0.5).add(new PointXYZ(0, 0, 0)).add(new PointXYZ(4, 10, 0)).add(new PointXYZ(8, 12, 0)).add(new PointXYZ(10, 13, 0)).add(new PointXYZ(12, 14, 0)).add(new PointXYZ(14, 15, 0)).build();
    pathfinder.followTrajectory(trajectory);
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(14, 15, 0));
}

Example 5 with AdvancedSplineTrajectoryBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method splineTo.

/**
 * Create a spline trajectory to a certain target point, and then follow
 * that aforementioned trajectory.
 *
 * <p>
 * If this method is called on a set of points with non-monotonic Y
 * values, this will instead invoke
 * {@link #multiSplineTo(double, double, Angle, PointXYZ...)}, which
 * supports non-monotonic Y values.
 * </p>
 *
 * @param speed          the speed at which the robot should move. This
 *                       is a constant value.
 * @param tolerance      the tolerance used for determining whether the
 *                       robot is at the target point.
 * @param angleTolerance same thing as {@code tolerance}, but for the
 *                       robot's angle.
 * @param points         a set of control points for the spline. This
 *                       will automatically insert the robot's current
 *                       position into this array. This array must have
 *                       AT LEAST two points.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder splineTo(double speed, double tolerance, Angle angleTolerance, PointXYZ... points) {
    if (points.length < 2)
        throw new IllegalArgumentException("At least two control points are required to use the " + "splineTo method.");
    InvalidSpeedException.throwIfInvalid("Invalid speed value provided! Speed must be between 0 and 1.", speed);
    InvalidToleranceException.throwIfInvalid("Invalid tolerance! Tolerance must be a positive number.", tolerance);
    if (angleTolerance.deg() < 0)
        throw new InvalidToleranceException("Invalid angle tolerance! " + "Angle tolerance must be greater than 0 degrees.");
    NotNull.throwExceptionIfNull("One or more points provided to splineTo was null!", (Object[]) points);
    // non-monotonic Y values means we need to use a multi spline instead
    if (!Spline.areMonotonicY(points))
        return multiSplineTo(speed, tolerance, angleTolerance, points);
    // length is the total distance of the spline (NOT all control points)
    double length = PointXY.distance(points[0], points[points.length - 1]);
    // step should be relatively small - by default, it's 1/20th of
    // the spline's length.
    double step = length / Core.pathfinderStepDivisor;
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setSpeed(speed).setTolerance(tolerance).setAngleTolerance(angleTolerance).setStep(step);
    PointXYZ robotPosition = getPosition();
    // to the trajectory
    if (!robotPosition.equals(points[0]))
        builder.add(robotPosition);
    PointXYZ lastPoint = points[0];
    for (int i = 1; i < points.length; i++) {
        PointXYZ point = points[i];
        if (point == null)
            throw new NullPointException("Cannot use the splineTo method with a null " + "control point!");
        if (!point.equals(lastPoint)) {
            builder.add(point);
            lastPoint = point;
        } else {
            throw new SplineException("There were duplicate adjacent points in the set " + "of control points! This means there's the same " + "point, twice in a row. The points were: " + Arrays.toString(points));
        }
    }
    Trajectory trajectory = builder.build();
    return followTrajectory(trajectory);
}