Roadrunner 1.0 beta cannot turn or splineTo clockwise?

[pennygao]

I'm new to RoadRunner, and I gave a try to RoadRunner 1.0 beta (because sounds like this is the future). I downloaded Roadrunner quickstart (branch quickstart2) and tuned the drivetrain/localizer parameters using the opMode (with minor modifications) there. I'm able to finish all the tunings (with a few glitches) and finally get one spineTo (drive.pose to (20, 20, tangent = PI) working. But a splineTo clockwise from drive.pose to (20, -20, agent = - PI/2) does not work for me. The robot moved a along the trajectory for a very short time and enter a back and forth loop thus it no longer moves along.

The code looks like below, and this is the only thing the opMode does.

        Trajectory secondTrajectory = drive.trajectoryBuilder(drive.pose)
                .splineTo(new Vector2d(20, -20),  Math.toRadians(-90))
                .build();
        drive.followTrajectory(secondTrajectory).runBlocking();

I added some log for every loop and found the trajectory profile generated looks weird (while the drivetrain position reported by localizer looks correct). The profile duration is 153 seconds (while the splineTo that works for me finished with 5 seconds.) And the (x, y, rot) for txWorldTarget starts with non-zero values, like below:

txWorldTarget.x 10.26, txWorldTarget.y -2.37, txWorldTarget.rotation (degree) -30.05

(the complete log attached)

Meanwhile, I'm able to use drive.turn() to turn robot counterclockwise, but drive.turn(drive.pose, -Math.PI) is also not working, as it eventually calls constantProfile() -> profile(), and angle (== -PI) get passed as 1st parameter "length" in profile(), thus failed the check " require(length > 0.0)" in Profiles.kt.

Am I missing anything important?

Logs_for_analysis_bad.txt

[rbrott]

Hi, thanks for giving the new stuff a shot!

I'm able to finish all the tunings (with a few glitches)

I'd love to hear the details here if possible (and maybe even some regression data).

But a splineTo clockwise from drive.pose to (20, -20, agent = - PI/2) does not work for me.

Weird. I put in (excuse the Kotlin)

val traj = TrajectoryBuilder(
    Pose2d(0.0, 0.0, 0.0), Rotation2d.exp(0.0), 1e-6, 0.0,
    MecanumKinematics(18.0).WheelVelConstraint(40.0),
    ProfileAccelConstraint(-30.0, 50.0),
    1.0
)
    .splineTo(Vector2d(20.0, -20.0), -PI / 2)
    .build()

and got a path of and a profile of Seems like maybe your drive.pose isn't the origin like I assumed. The attached log is indeed suspicious.

Meanwhile, I'm able to use drive.turn() to turn robot counterclockwise, but drive.turn(drive.pose, -Math.PI) is also not working, as it eventually calls constantProfile() -> profile(), and angle (== -PI) get passed as 1st parameter "length" in profile(), thus failed the check " require(length > 0.0)" in Profiles.kt.

Yup, that's a bug 🙂 I just pushed a quick fix that hopefully works.

[pennygao]

Hi, thanks a lot for responding!

I tried java version of what you've done, and found the difference that triggered the problem. So below code works,

    TrajectoryBuilder trajBuilder = new TrajectoryBuilder(
            new Pose2d(0.0, 0.0, 0.0),
            Rotation2d.exp(0.0),
            1e-6,
            0.0,
            new MecanumKinematics(7.0, 1.0).new WheelVelConstraint((10.0)),
            new ProfileAccelConstraint(-10, 15),
            0.25
    );

But once I replaced the constraints to also include angular velocity constraint (which is what the quickstart MecanumDrive.java is doing), like below, the generated motion profiling becomes problematic (as the trajectory's duration is >50 second, and the robot moves back and force in the middle of the path). The path drawn on Dashboard is correct, though.

    TrajectoryBuilder trajBuilder = new TrajectoryBuilder(
            new Pose2d(0.0, 0.0, 0.0),
            Rotation2d.exp(0.0),
            1e-6,
            0.0,
            new MecanumKinematics(7.0, 1.0).new WheelVelConstraint((10.0)),
            new MinVelConstraint(Arrays.asList(
                    new MecanumKinematics(7.0, 1.0).new WheelVelConstraint((10.0)),
                    new AngularVelConstraint(Math.PI/4))),
            new ProfileAccelConstraint(-10, 15),
            0.25
    );

Btw you might noticed the track width is 7, which is a lot smaller than a usual robot. That is related to one of the glitches I had in tuning the parameters. The track width I got from AndularRampLogger analysis is 14.5 inch, but when I tried a turn action to turn the robot 180 degree, it actually moved slightly more than 360 degrees. Since the track width is used to convert angular velocity to wheel velocity (for a in place turn), maybe it is also a bug in the code, where track width was not divided by 2 in calculating the wheel velocity? (I believe I have tried with 14.0 with above code, no help.)

Other than that, I also noticed below code in DriveView.java (from the quickstart project) should be updated:

public void setDrivePowers(Twist2d powers) {
    if (md != null) {
        md.setDrivePowers(powers);
    }

    if (td != null) {
        td.setDrivePowers(powers);
    }

    throw new AssertionError();
}

=>

public void setDrivePowers(Twist2d powers) {
    if (md != null) {
        md.setDrivePowers(powers);
        return;
    }

    if (td != null) {
        td.setDrivePowers(powers);
        return;
    }

    throw new AssertionError();
}

[rbrott]

Turns out there was a missing abs() in AngularVelConstraint(). Do you need me to cut a release? The mitigation for now is to remove that constraint (the mecanum wheel velocity constraint will limit the angular velocity anyway).

[rbrott]

Oh and I pushed a (hopeful) fix for the track width. Use whatever value makes turns work well (probably 1/2 of what the tuner gave).