Creating/running trajectories

Overview

Blacksmith provides a relatively simple yet versatile way to create and run TrajectorySequences. There are three main parts:

• Creating trajectories
• Running the initial trajectory
• Running subsequent trajectories

Or, if you want, you can just 'build a TrajectorySequence' and use that how you'd like.

Creating trajectories

Anvil.forgeTrajectory()

Anvil.forgeTrajectory is the entry point into creating Anvil trajectories.

Params

drive: SampleMecanumDrive - The SampleMecanumDrive that will run the trajectories

startPose: Pose2d - The starting position of the robot

danger

Even though Anvil.forgeTrajectory() is supposed to take a SampleMecanumDrive instance as it's first argument, it's actually typed as 'Object', meaning you can pass in any object you want, which you aren't supposed to.

If that doesn't make much sense, just keep this in mind: Make sure you're passing in a SampleMecanumDrive instance, and not some other object by accident

Returns

Anvil - A new Anvil instance to build upon

Java

/* There are two ways to format your trajectory creation */

// First way is the builder pattern
public Anvil myFirstAnvilTrajectory(Pose2d startPose) {
    return Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
        .forward(...) // You can easily chain methods this way
        .turn(...)    // to form a clean builder syntax
        .back(...);
}

// Alternatively, you can use a lambda
public Anvil mySecondAnvilTrajectory(Pose2d startPose) {
    return Anvil.forgeTrajectory(sampleMecanumDrive, startPose, (instance) -> {
        instance          // This way can allow you to break up the methods with
            .forward(...) // other logic or computations, like below
            .turn(...);   // (You can still chain methods like this though)

        double x = someComputationWithAReallyLongNameForNoReason();

        instance.back(x);
    });
}

Kotlin

/* There are two ways to format your trajectory creation */

// First way is the builder pattern
fun myFirstAnvilTrajectory(startPose: Pose2d) =
    Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
        .forward(...) // You can easily chain methods this way
        .turn(...)    // to form a clean builder syntax
        .back(...)

// Alternatively, you can use a lambda w/ receiver
fun mySecondAnvilTrajectory(startPose: Pose2d) =
    Anvil.forgeTrajectory(sampleMecanumDrive, startPose, (anvil) -> {
        forward(...) // This way can allow you to break up the methods with
        turn(...)    // other logic or computations, like below
                     // (You can still chain methods like this though)
        val x = someComputationWithAReallyLongNameForNoReason()

        anvil.back(x)
    })

Running the initial trajectory

Running the first trajectory to kickstart your auto is a bit different than running the rest of the trajectories in your auto.

You first need to start with actually creating your Anvil instance, as shown before. We can just call the method with a start pose to create it, like so:

Anvil myFirstCreatedAnvilTrajectory = myFirstAnvilTrajectory(myStartPose);

This is 'created', but not 'built', in that it can still be modified and tweaked (i.e., you can still do something like:)

myFirstCreatedAnvilTrajectory
    .turn(720) /* Set up GlobalUnits so you can use degrees or whatever unit you want */
    .whateverElse(...);

Now that we have it, in our 'main' method (go if you're using BlackOp, runOpMode if you're using LinearOpMode, etc.) we can do this, to tell Anvil what the initial trajectory is:

@Override
public void `go/runOpMode`() {
    Anvil myFirstCreatedAnvilTrajectory = // ...

    Anvil // (Doesn't actually start the auto yet, just tells Anvil what to start it with)
        .startAutoWith(myFirstCreatedAnvilTrajectory)
}

If you want to use the Scheduler API for your auto, you can do the following, which tells Anvil to start the auto the second the Scheduler starts looping.

// ...
Anvil
    .startAutoWith(myFirstCreatedAnvilTrajectory)
    .onSchedulerLaunch()
// ...

If you don't want to use the scheduler, you can do the following whenever to start the auto. Note that it takes a parameter called 'runAsync', which is a boolean that tells Anvil if you want to run this trajectory asynchronously or not

AnvilRunner runner = Anvil.startAutoWith(myFirstCreatedAnvilTrajectory)

// ...

runner.start(true/false)

caution

Using .onSchedulerLaunch() implicitly runs the trajectories asynchronously.

Trajectories running asynchronously must be updated in a loop with sampleMecanumDrive.update()

Putting it all together

Java w/ Scheduler

public class MyAutoWithScheduler extends BlackOp {
    /* Declare your components here (I recommend @CreateOnGo) */

    @Override
    public void go() {
        Anvil myFirstCreatedAnvilTrajectory = myFirstAnvilTrajectory(myStartPose);

        Anvil
            .startAutoWith(myFirstCreatedAnvilTrajectory)
            .onSchedulerLaunch();

        // ...

        Scheduler.launchOnStart(this, () -> {
            sampleMecanumDrive.update()
            // Other stuff to call each loop
        });
    }
}

Kotlin w/ Scheduler

class MyAutoWithScheduler : BlackOp() {
    /* Declare your components here (I recommend createOnGo) */

    override fun go() {
        val myFirstCreatedAnvilTrajectory = myFirstAnvilTrajectory(myStartPose);

        Anvil
            .startAutoWith(myFirstCreatedAnvilTrajectory)
            .onSchedulerLaunch()

        // ...

        Scheduler.launchOnStart(opmode = this) {
            sampleMecanumDrive.update()
            // Other stuff to call each loop
        }
    }
}

Java w/out Scheduler

public class MyAutoWithScheduler extends LinearOpMode {
    /* Declare your components here */

    @Override
    public void runOpMode() throws InterruptedException {
        /* Initialize your components here */

        Anvil myFirstCreatedAnvilTrajectory = myFirstAnvilTrajectory(myStartPose);

        AnvilRunner runner = Anvil
            .startAutoWith(myFirstCreatedAnvilTrajectory);

        // ...

        waitForStart();

        runner.start(true); // Starts the traj asynchronously
        while (opModeIsActive() && !isStopRequested()) {
            sampleMecanumDrive.update();
        }
    }
}

Kotlin w/out Scheduler

class MyAutoWithScheduler : LinearOpMode() {
    /* Declare your components here */

    override fun runOpMode() {
        /* Initialize your components here */

        Anvil myFirstCreatedAnvilTrajectory = myFirstAnvilTrajectory(myStartPose)

        val runner = Anvil
            .startAutoWith(myFirstCreatedAnvilTrajectory)

        // ...

        waitForStart()

        runner.start(true); // Starts the traj asynchronously
        while (opModeIsActive() && !isStopRequested) {
            sampleMecanumDrive.update()
        }
    }
}

Running subsequent trajectories

Say that you have your main trajectory, but after you finish your main trajectory, you want to park. But, because you have to read a signal cone with a camera to know where to park, there isn't time to build the trajectory during initialization.

Normally, building a trajectory takes a while, and can bog down your auto... but with Anvil, it's super easy to build it in a background thread... in fact, that's what it does by default!

Lets say you have a main trajectory like this:

public Anvil myMainTrajectory(Pose2d startPose) {
    return Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
        .forward(...) // You can easily chain methods this way
        .turn(...)    // to form a clean builder syntax
        .back(...);
}

and a parking trajectory like this:

public Anvil myParkTrajectory(Pose2d startPose) {
    return Anvil.forgeTrajectory(sampleMecanumDrive, startPose, (instance) -> {
        if (mySignalId == 1) {
            instance.forward(10);
        } else {
            instance.back(10);
        }
    });
}

How would we run myParkTrajectory right after myMainTrajectory? Easy:

public Anvil myMainTrajectory(Pose2d startPose) {
    return Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
        // ...
        .back(...)
        .thenRun(this::myParkTrajectory);
}

Yes, it's really that simple. We just pass in a lambda or a method reference to the next Anvil trajectory to run.

The trajectory is implicitly created in a background thread without any extra code, and automatically runs when the trajectory reaches .thenRun()

(.thenRun() is called in a temporalMarker behind the scenes)

Be aware

The method in question MUST take in a single Pose2d parameter and MUST return an Anvil instance.

The startPose passed to the function by Anvil is by default the pose of the parent trajectory at the place .thenRun() is called.

For example:

Anvil.forgeTrajectory(drive, new Pose2d()) // Starts at 0, 0, 0
    .forward(10) // Goes to 0, 10, 0

    // The startPose passed to 'otherTrajectory' will be 0, 10, 0
    .thenRun(this::otherTrajectory)

    .back(10) // Goes back to 0, 0, 0

If you want to use a different start pose, read on to the Run options section

But what if you want to, say, run the new trajectory synchronously, or conditionally, or something? Well Blacksmith promises versatility, and Anvil provides.

Run options

When calling .thenRun(), you can pass in an optional lambda which takes in a AnvilRunConfig object which you can then set to your liking:

// ...
.thenRun(this::nextTraj, (config) -> config
    // Defaults to 'true' if called without a param
    // Defaults to 'false' if not called at all
    .buildSynchronously(true/false)

    // Defaults to 'true' if called without a param
    // Defaults to 'false' if not called at all
    .runSynchronously(true/false)

    // Trajectory only runs if this returns true
    // Defaults to '() -> true' if not called at all
    .runOnlyIf(() -> true/false)

    // Sets the Pose2d to pass to the trajectory
    // Defaults to the position of the parent trajectory at the time of calling
    // .thenRun()
    .setStartPose(() -> Pose2d()));

info

For Kotlin users, this, of course, is a lambda with a receiver, so you don't need the dots

tip

If you're calling a lot of thenRuns with the same options, I recommend creating a global variable like so:

private AnvilRunConfig myConfig = (config) -> config
    .x()
    .y()
    .z();

and just passing it in like .thenRun(this::nextTraj, myConfig)

info

This is the default config if no custom config is passed in:

config
    .buildSynchronously(false) // Builds in background thread
    .runSynchronously(false) // Runs async
    .onlyRunIf(() -> true) // Always runs
    .setStartPose(() -> getCurrentEndPose()); // Start pose is parent's current pose

info

Just to make it clear, you don't have to call all 4 methods. You can just call 1 method if you'd like.

Putting it all together

Java w/out Run config

public class MyAuto extends MyBaseAuto {
    @Override
    public Anvil myMainTrajectory(Pose2d startPose) {
        return Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
            .forward(...)
            .turn(...)
            .back(...)
            .thenRun(this::myParkTrajectory);
    }

    public Anvil myParkTrajectory(Pose2d startPose) {
        return Anvil.forgeTrajectory(sampleMecanumDrive, startPose, (instance) -> {
            if (mySignalId == 1) {
                instance.forward(10);
            } else {
                instance.back(10);
            }
        });
    }
}

Kotlin w/out Run config

class MyAuto : MyBaseAuto() {
    override fun myMainTrajectory(Pose2d startPose) =
        Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
            .forward(...)
            .turn(...)
            .back(...)
            .thenRun(this::myParkTrajectory)

    fun myParkTrajectory(Pose2d startPose) =
        Anvil.forgeTrajectory(sampleMecanumDrive, startPose) {
            if (mySignalId == 1) {
                forward(10)
            } else {
                back(10)
            }
        }
}

Java w/ Run config

public class MyAuto extends MyBaseAuto {
    @Override
    public Anvil myMainTrajectory(Pose2d startPose) {
        return Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
            .forward(...)
            .turn(...)
            .back(...)                            // Runs the parkTraj synchronously
            .thenRun(this::myParkTrajectory, (config) -> config.runSynchronously());
    }

    public Anvil myParkTrajectory(Pose2d startPose) {
        return Anvil.forgeTrajectory(sampleMecanumDrive, startPose, (instance) -> {
            if (mySignalId == 1) {
                instance.forward(10);
            } else {
                instance.back(10);
            }
        });
    }
}

Kotlin w/ Run config

class MyAuto : MyBaseAuto() {
    override fun myMainTrajectory(Pose2d startPose) =
        Anvil.forgeTrajectory(sampleMecanumDrive, startPose)
            .forward(...)
            .turn(...)
            .back(...)           // Runs the parkTraj synchronously
            .thenRun(this::myParkTrajectory) { runSynchronously() }

    fun myParkTrajectory(Pose2d startPose) =
        Anvil.forgeTrajectory(sampleMecanumDrive, startPose) {
            if (mySignalId == 1) {
                forward(10)
            } else {
                back(10)
            }
        }
}

Just building a TrajectorySequence

If you just wanna build a TrajectorySequence without worrying about any other Anvil nonsense, that's easy as well.

Just

Java

// (Referencing the first section's method)
Anvil myFirstTrajectoryCreated = myFirstAnvilTrajectory(new Pose2d())

TrajectorySequence ts = myFirstTrajectoryCreated.<TrajectorySequence>build()

Kotlin

// (Referencing the first section's method)
val myFirstTrajectoryCreated = myFirstAnvilTrajectory(Pose2d())

val ts = myFirstTrajectoryCreated.build<TrajectorySequence>()

You notice how you have to pass in the <TrajectorySequence>? Yeah, that's mandatory, don't forget it. It tells the compiler exactly what class the TrajectorySequence actually is.

Make sure you're passing in the right class, or it will throw a ClassCastException at runtime

Getting the raw TrajectorySequenceBuilder

You can get the raw TrajectorySequenceBuilder like so:

Java

TrajectorySequenceBuilder builder = myAnvil.<TrajectorySequenceBuilder>getRawBuilder()

Kotlin

val builder = myAnvil.getRawBuilder<TrajectorySequenceBuilder>()

You notice how you have to pass in the <TrajectorySequenceBuilder>? Yeah, that's mandatory, don't forget it. It tells the compiler exactly what class the TrajectorySequenceBuilder actually is.

Make sure you're passing in the right class, or it will throw a ClassCastException at runtime