Wierd issues whilst using afterTime(0, a) and waitSeconds(). (v1.0)

[Xtendera]

Okay I will try to be as detailed as I can here.

I am making a autonomous program with RR v1.0. We are having some trouble understanding how our code is supposed to look.

Suppose the follow example code.

Pose2d beginPose = new Pose2d(40, 64, Math.toRadians(270));
Actions.runBlocking(drive.actionBuilder(beginPose).
                strafeTo(new Vector2d(40, 32)).
                afterTime(0, sliderAction.highBucket()).build())

Naturally, looking at this example, what the code does is pretty clear.

1. Strafes forward 32 units.
2. While doing the above task, it moves the slider up.

Assuming that the slider action works fine (it has been tested, it does), this is exactly what this snippet should do. In practice, it does not do this. It actually does:

1. Strafes forward 32 units.
2. After task 1 is completed, it moves the slider up.

Clearly, something is not working properly. After a bit of tinkering, switching the two instructions seems to fix the issue:

Actions.runBlocking(drive.actionBuilder(beginPose).
                afterTime(0, sliderAction.highBucket()).
                strafeTo(new Vector2d(40, 32)).build())

Already, this is quite odd. In the docs it is clear that afterTime is meant to be used after another instruction, not before. These results were verified with one of our sister teams who have also setup roadrunner.

Lets ramp it up a bit. Consider the following:

Actions.runBlocking(drive.actionBuilder(beginPose).
                afterTime(0, sliderAction.highBucket()).
                strafeTo(new Vector2d(40, 32)).
                waitSeconds(0.9).
                stopAndAdd(intakeAction.outake()).
                waitSeconds(0.9).
                stopAndAdd(sliderAction.reset()).
                waitSeconds(4).
                build())

For context, the intakeAction.outake() method immediately returns false, as in it turns on our intake system, then it acts like the action is done and continues to the next instruction. This means that the code should do the following, ignoring the jank solution to the first problem:

1. Strafes forward 32 units.
2. While doing the above task, it moves the slider up.
3. After the two tasks above are completed, it waits 0.9 seconds
4. After the wait, it turns on the intake motor.
5. Immediately after the number 4 task, it waits another 0.9 seconds.
6. Resets the slider down
7. Waits a couple seconds for motor power.

In reality, the first waitSeconds works perfectly. The second, however, does not. It ONLY works if the wait is 1 or greater. If it is a decimal value, it does not work. Yeah, I don't really know why either.

For our sister teams, they were having similar issues (although I am not sure if it is the exact same) with their roadrunner. As soon as afterTime() is swapped out for a stopAndAdd(), everything works perfectly.

I would like some insight into this issue(s), as I am not sure where to even start debugging it.

EDIT 1: If needed, I will post a video of our robot doing whats described above on Thursday, as that is our next work day meeting.

[rbrott]

In the docs it is clear that afterTime is meant to be used after another instruction, not before.

So the docs say "Schedules action a to execute in parallel starting dt seconds after the last trajectory segment, turn, or other action." I can see how you say it needs to be placed after another instruction, though it's logical enough to expect that placing it at the beginning of the builder will lead to the action being scheduled from the very start of the trajectory. Further, given the wording, I don't follow why you expect the code

Pose2d beginPose = new Pose2d(40, 64, Math.toRadians(270));
Actions.runBlocking(drive.actionBuilder(beginPose).
                strafeTo(new Vector2d(40, 32)).
                afterTime(0, sliderAction.highBucket()).build())

to result in the action running concurrently with the spline. Anyway, it seems like you understand now how the API works.

I would like some insight into this issue(s), as I am not sure where to even start debugging it.

One thing you can do is inspect the Action that comes out of the builder. Ideally you can play around with the builder without needing to deploy code to your robot.

To demonstrate what I mean, I coded up a similar builder:

    @Test
    fun testIssue115() {
        val base =
            TrajectoryActionBuilder(
                { TurnAction(it) },
                { TrajectoryAction(it) },
                TEST_TRAJECTORY_BUILDER_PARAMS,
                Pose2d(0.0, 0.0, 0.0),
                0.0,
                TurnConstraints(2.0, -1.0, 1.0),
                TranslationalVelConstraint(20.0),
                ProfileAccelConstraint(-10.0, 10.0),
            )

        val a = base
            .afterTime(0.0, LabelAction("A"))
            .strafeTo(Vector2d(40.0, 32.0))
            .waitSeconds(0.9)
            .stopAndAdd(LabelAction("B"))
            .waitSeconds(0.9)
            .stopAndAdd(LabelAction("C"))
            .waitSeconds(4.0)
            .build()
        println(a)
    }

Here's the output after a little reformatting:

ParallelAction(initialActions=[
    SequentialAction(initialActions=[
        Trajectory, 
        SleepAction(dt=0.9), 
        B, 
        SleepAction(dt=0.9), 
        C, 
        SleepAction(dt=4.0)
    ]), 
    SequentialAction(initialActions=[SleepAction(dt=0.0), A])
])

Looks correct to me, so I'm not sure what's going on with your robot. In the future, I'd love to extend the log file data collection to include the progress of actions and make debugging easier.

[Xtendera]

So the docs say "Schedules action a to execute in parallel starting dt seconds after the last trajectory segment, turn, or other action."

That makes sense now! I am still not sure why the second issue happens but I will try and figure it out around Christmas break.

Also it I feel like having an actually parallelAction-type function in the Trajectory builder would be super nice. Like something that works the way a ParallelAction works ("It will execute in parallel alongside the last instruction until both of them have completed") would be a great addition and would help our workflows a lot.

But either way thanks for the detailed response and I will attempt to figure out the second issue soon.

[rbrott]

Also it I feel like having an actually parallelAction-type function in the Trajectory builder would be super nice.

Do you have an example of the function used in the builder and what action you'd expect it to produce?

[Xtendera]

Do you have an example of the function used in the builder and what action you'd expect it to produce?

Actions.runBlocking(drive.actionBuilder(beginPose).
                strafeTo(new Vector2d(40, 32)).
                inParallel(sliderAction.whatever()).
                strafeTo(new Vector2d(40, 48))
                build())

This is what it would do:

1. Strafe to 40, 32 while doing sliderAction.
2. After both the strafe AND the sliderAction finishes, strafe to 40,48

The difference is that it would wait until the action and the strafe finished before moving on. This means that if the strafe finishes first, the robot will stand in place until the slider finishes. If the slider finishes first, it will keep strafing until it reaches the proper position.