Trajectories are slow to generate

[SharkDjokovic]

I'm trying to run a RR command in an OpMode by initializing it in init, and it keeps stalling in init and then says "Stuck in Stop." This only happens when I add multiple splines.

When I add the trajectories in start, the init process works fine, and then it stalls in a position when I click start, waits 15 seconds, and then says "Stuck in Stop." Everything works fine when I remove the trajectories or some of the splines.

OpMode:

package org.firstinspires.ftc.teamcode.Drive.OpModes.TestAutonomous;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.arcrobotics.ftclib.command.CommandScheduler;
import com.arcrobotics.ftclib.command.InstantCommand;
import com.arcrobotics.ftclib.command.ParallelCommandGroup;
import com.arcrobotics.ftclib.command.SequentialCommandGroup;
import com.arcrobotics.ftclib.command.WaitCommand;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;

import org.firstinspires.ftc.teamcode.Drive.DriveConstants;
import org.firstinspires.ftc.teamcode.Drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.TrajectorySequences.TrajectorySequence;
import org.firstinspires.ftc.teamcode.TrajectorySequences.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.Utility.CommandBase.Commands.DriveCommand;
import org.firstinspires.ftc.teamcode.Utility.CommandBase.Commands.RestCommand;
import org.firstinspires.ftc.teamcode.Utility.CommandBase.Commands.SpecialOuttakeCommand;
import org.firstinspires.ftc.teamcode.Utility.CommandBase.Commands.TapeDropCommand;
import org.firstinspires.ftc.teamcode.Utility.Hardware.RobotHardware;
import org.firstinspires.ftc.teamcode.Utility.Vision.RedRightProcessor;
import org.firstinspires.ftc.vision.VisionPortal;
import org.opencv.core.Scalar;

@Autonomous
@Config

public class NEWRedRight extends OpMode {
    private VisionPortal visionPortal;
    private RedRightProcessor colorMassDetectionProcessor;

    private RobotHardware robot;
    private ElapsedTime time_since_start;
    private double loop;

    public TrajectorySequence movement1Left;

    @Override
    public void init() {
        CommandScheduler.getInstance().reset();
        robot = new RobotHardware(hardwareMap);

        CommandScheduler.getInstance().registerSubsystem(robot.armSystem);
        CommandScheduler.getInstance().registerSubsystem(robot.claw);
        CommandScheduler.getInstance().registerSubsystem(robot.angleOfArm);
        CommandScheduler.getInstance().registerSubsystem(robot.driveSubsystem);

        telemetry.addData("Not Ready: ", "Not able to proceed to camera detection... Restart robot now.");
        telemetry.update();

        robot.claw.grabBoth();

        Scalar lower = new Scalar(0, 110, 65);
        Scalar upper = new Scalar(8, 200, 150);
        double minArea = 100;

        colorMassDetectionProcessor = new RedRightProcessor(
                lower,
                upper,
                () -> minArea,
                () -> 213, //left third of frame
                () -> 426, //right third of frame
                400
        );
        visionPortal = new VisionPortal.Builder()
                .setCamera(hardwareMap.get(WebcamName.class, "Webcam"))
                .addProcessor(colorMassDetectionProcessor)
                .build();

        movement1Left = robot.driveSubsystem.trajectorySequenceBuilder(new Pose2d(16.45, -64.16, Math.toRadians(90.00)))
                .splineTo(
                        new Vector2d(9.32, -35.43), Math.toRadians(117.35),
                        SampleMecanumDrive.getVelocityConstraint(25, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )
                .splineToConstantHeading(
                        new Vector2d(20.81, -41.88), Math.toRadians(90.00),
                        SampleMecanumDrive.getVelocityConstraint(25, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )
                .splineTo(
                        new Vector2d(47.97, -34.91), Math.toRadians(0.00),
                        SampleMecanumDrive.getVelocityConstraint(25, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )
                .splineTo(
                        new Vector2d(40.14, -35.09), Math.toRadians(0.00),
                        SampleMecanumDrive.getVelocityConstraint(25, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )
                .splineToConstantHeading(
                        new Vector2d(55.81, -60.33), Math.toRadians(0.00),
                        SampleMecanumDrive.getVelocityConstraint(25, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )
                .build();

    }

    @Override
    public void init_loop() {
        telemetry.addData("Successful: ", "Ready for RedRight (Backdrop Side)");
        telemetry.addData("Ready to Run: ", "2 pixel autonomous. All subsystems initialized.");
        telemetry.addData("Currently Recorded Position", colorMassDetectionProcessor.getRecordedPropPosition());
        telemetry.addData("Camera State", visionPortal.getCameraState());
        telemetry.addData("Currently Detected Mass Center", "x: " + colorMassDetectionProcessor.getLargestContourX() + ", y: " + colorMassDetectionProcessor.getLargestContourY());
        telemetry.addData("Currently Detected Mass Area", colorMassDetectionProcessor.getLargestContourArea());
        CommandScheduler.getInstance().run();
        robot.armSystem.loop();
    }

    @Override
    public void start() {
        time_since_start = new ElapsedTime();
        if (visionPortal.getCameraState() == VisionPortal.CameraState.STREAMING) {
            visionPortal.stopLiveView();
            visionPortal.stopStreaming();
        }

        RedRightProcessor.PropPositions recordedPropPosition = colorMassDetectionProcessor.getRecordedPropPosition();
        robot.driveSubsystem.setPoseEstimate(new Pose2d(16.45, -64.16, Math.toRadians(90.00)));

        switch (recordedPropPosition) {
            case LEFT:
            case UNFOUND:
                CommandScheduler.getInstance().schedule(
                        new SequentialCommandGroup(
                            new DriveCommand(robot.driveSubsystem, movement1Left)
                        )
                );
                break;
            case RIGHT:
                //write code
                break;
            case MIDDLE:
                //write code
                break;
        }
    }

    @Override
    public void loop() {
        CommandScheduler.getInstance().run();
        robot.armSystem.loop();
        robot.slidesSubsystem.loop();
        robot.driveSubsystem.update();

        double time = System.currentTimeMillis();
        telemetry.addData("Time Elapsed: ", time_since_start);
        telemetry.addData("Current Loop Time: ", time - loop);

        robot.currentUpdate(telemetry);
        robot.pidArmUpdateTelemetry(telemetry);

        loop = time;
        telemetry.update();
    }

    @Override
    public void stop() {
        colorMassDetectionProcessor.close();
        visionPortal.close();
        telemetry.addLine("Closed Camera.");
        telemetry.update();
        CommandScheduler.getInstance().reset();
    }
}

DriveSubsystem:

package org.firstinspires.ftc.teamcode.Utility.CommandBase.Subsystems;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.localization.Localizer;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.arcrobotics.ftclib.command.SubsystemBase;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;

import org.firstinspires.ftc.teamcode.Drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.TrajectorySequences.TrajectorySequence;
import org.firstinspires.ftc.teamcode.TrajectorySequences.TrajectorySequenceBuilder;

import java.util.List;

public class DriveSubsystem extends SubsystemBase {

    private final SampleMecanumDrive drive;
    private final boolean fieldCentric;

    public DriveSubsystem(SampleMecanumDrive drive, boolean isFieldCentric) {
        this.drive = drive;
        fieldCentric = isFieldCentric;
    }

    public void setMode(DcMotor.RunMode mode) {
        drive.setMode(mode);
    }

    public void setPIDFCoefficients(DcMotor.RunMode mode, PIDFCoefficients coefficients) {
        drive.setPIDFCoefficients(mode, coefficients);
    }

    public void setPoseEstimate(Pose2d pose) {
        drive.setPoseEstimate(pose);
    }

    public void update() {
        drive.update();
    }

    public void updatePoseEstimate() {
        drive.updatePoseEstimate();
    }

    public void drive(double leftY, double leftX, double rightX) {
        Pose2d poseEstimate = getPoseEstimate();

        Vector2d input = new Vector2d(-leftY, -leftX).rotated(
                fieldCentric ? -poseEstimate.getHeading() : 0
        );

        drive.setWeightedDrivePower(
                new Pose2d(
                        input.getX(),
                        input.getY(),
                        -rightX
                )
        );
    }

    public void setDrivePower(Pose2d drivePower) {
        drive.setDrivePower(drivePower);
    }

    public Pose2d getPoseEstimate() {
        return drive.getPoseEstimate();
    }

    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
        return drive.trajectoryBuilder(startPose);
    }

    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
        return drive.trajectoryBuilder(startPose, reversed);
    }

    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
        return drive.trajectoryBuilder(startPose, startHeading);
    }

    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
        return drive.trajectorySequenceBuilder(startPose);
    }

    public void followTrajectory(Trajectory trajectory) {
        drive.followTrajectoryAsync(trajectory);
    }

    public void followTrajectorySequence(TrajectorySequence trajectory) {
        drive.followTrajectorySequenceAsync(trajectory);
    }

    public void followTrajectorySequenceNotAsync(TrajectorySequence trajectory) {
        drive.followTrajectorySequence(trajectory);
    }

    public boolean isBusy() {
        return drive.isBusy();
    }

    public void turn(double radians) {
        drive.turnAsync(radians);
    }

    public List < Double > getWheelVelocities() {
        return drive.getWheelVelocities();
    }

    public void stop() {
        drive(0, 0, 0);
    }

    public Pose2d getPoseVelocity() {
        return drive.getPoseVelocity();
    }

    public Localizer getLocalizer() {
        return drive.getLocalizer();
    }

}

[rbrott]

Maybe the splines are just taking too long to generate? Ideally you could time that section and print out how long it takes. The easiest knob to make generating splines faster is reducing the resolution (or at some point consider migrating to RR 1.0 which generates trajectories more quickly).

[SharkDjokovic]

What do you suggest reducing the resolution to? Will this still be accurate? What does the resolution change?

[rbrott]

You can change the resolution to something like 2-4 inches. The resolution affects the sampling of constraints for motion profile generation. The higher the number is, the coarser the profile is and the less strictly constraints will be followed.

[SharkDjokovic]

You mentioned constraints, are these constraints like velocity or specific to accuracy? Will decreasing the resolution cause motions to be less accurate?

[SharkDjokovic]

Also, if I switch to rr1.0, my arm subsystem uses RR0.5.6's MotionProfile class. Would I need to copy in the Kotlin function for that? Also -- I'm looking at this Action system. If I need to run a PID loop or something similar, should I still create an Action and just run that Action within the loop?

[rbrott]

You mentioned constraints, are these constraints like velocity or specific to accuracy? Will decreasing the resolution cause motions to be less accurate?

Don't worry too much about accuracy here.

Also, if I switch to rr1.0, my arm subsystem uses RR0.5.6's MotionProfile class. Would I need to copy in the Kotlin function for that?

You can use https://rr.brott.dev/docs/v1-0-0-beta6/core/kdoc/core/com.acmerobotics.roadrunner/constant-profile.html or copy the old code. Whatever floats your boat.

Also -- I'm looking at this Action system. If I need to run a PID loop or something similar, should I still create an Action and just run that Action within the loop?

You can write your own action or use whatever FSM/asynchronous system you have already.

[SharkDjokovic]

Alright sounds good. I'm looking at the FTCLib sample command written in https://rr.brott.dev/docs/v1-0/guides/ftclib-commands/ Assuming I use this for my drive command like so:

                Action movement1Left = drive.actionBuilder(drive.pose)
                        .splineToConstantHeading(new Vector2d(26.03, 42.57), Math.toRadians(270.00))
                        .splineToConstantHeading(new Vector2d(26.03, 53.19), Math.toRadians(270.00))
                        .splineTo(new Vector2d(48.67, 36.65), Math.toRadians(0.00))
                        .build();

                CommandScheduler.getInstance().schedule(
                        new SequentialCommandGroup(
                                new ActionCommand(movement1Left) //error on this line since I'm not specifying any requirements
                        )
                );

Requirements can't be left empty but I don't believe I have any. What is generally used in the requirements section? Can I just set this as null? Or Collections.emptySet()?

[rbrott]

Each command should declare any subsystems it controls as requirements. This backs the scheduler’s resource management mechanism, ensuring that no more than one command requires a given subsystem at the same time. This prevents situations such as two different pieces of code attempting to set the same motor controller to different output values.

(from https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html#getrequirements)

If you don't care about the guarantees provided by requirements, you can give it an empty set.

[SharkDjokovic]

This has persisted as an issue for us for RR0.5.6. Changing the resolution to any number has not changed anything, unfortunately. We ran a Profiler to check CPU/Memory. The image is attached.

CPU spiked at 53% and 253mb Memory. Is there a fix for this?

I'm wondering if this is a FTCLib/RR issue. I tried directly creating trajectory sequences in init without using the subsystem, and it still lagged out until an eventual "stuck in stop."

[rbrott]

We ran a Profiler to check CPU/Memory. The image is attached.

This image unfortunately is not very helpful. I need more granular information about what the CPU is executing. See this section on the timeline view. I can be most helpful if you export a trace and attach it here for me to take a look. Make sure you capture the trajectory generation process in the trace. I would start the trace in Android Studio, initialize the op mode with the trajectories, and let it collect data for 15s before stopping.

I'm wondering if this is a FTCLib/RR issue.

Seems unlikely given that you can comment out a few trajectories and have everything run fine. (As an aside, one of the trajectories you commented out is likely the problem. Consider trying to reproduce with one of them in a smaller op mode with less going on.)

[SharkDjokovic]

So I'm trying to export the trace now, but it's not an option. I right-clicked on the CPU part. Although it's an "option," it doesn't let me select it. I've also tried going through the sessions pane. It only gives me an option to delete the trace there.

Since the control hub is Android 7 (I think?), it doesn't allow me to record the trace...

[rbrott]

Maybe you need to follow these instructions: https://developer.android.com/studio/profile#advanced-profiling

You can also (1) make a small example without dependencies on your code and have me profile (like what happened in https://github.com/acmerobotics/road-runner/issues/97#issue-2082585152) or (2) print timings in your opmode of each segment individually to find the problem segment.