Motion Control For Vision Targets

I’m trying to decide what is the best controller to drive the robot to coordinates from my camera.
I have my current robot pose coordinates (x,y,theta) and the vision target coordinates(x,y,theta), and I want to drive my robot to the vision target autonomously.

I would love to use motion profiling to drive the robot accurately but since my set point coordinates are constantly changing I’m not sure which motion profiling tool is the best since new target coordinates are constantly being sent from the vision system which means I have to create new trajectories on the fly.
keep in mind the autonomous movement using vision is supposed to run also during teleop.

Pure Pursuit does a pretty good job of solving the problem you’re describing.

You can read some background here (http://www8.cs.umu.se/kurser/TDBD17/VT06/utdelat/Assignment%20Papers/Path%20Tracking%20for%20a%20Miniature%20Robot.pdf). Also search Chief Delphi for Pure Pursuit, as several teams have been posting about it, including code.

For what it’s worth, here is our implementation, with a lot of comments explaining our reasoning: https://github.com/Team488/SeriouslyCommonLib/blob/master/src/main/java/xbot/common/subsystems/drive/PurePursuitCommand.java

1 Like

I have tried to do something similar with a motion profiling library developed by my team. You can find it here: https://github.com/Arctos6135/RobotPathfinder/. I haven’t gotten around to test it though. Here’s some code for the command:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.commands;

import java.util.concurrent.CancellationException;
import java.util.concurrent.ExecutionException;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.Future;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.OI;
import frc.robot.Robot;
import frc.robot.RobotMap;
import frc.robot.subsystems.Vision.VisionException;
import robot.pathfinder.core.TrajectoryParams;
import robot.pathfinder.core.Waypoint;
import robot.pathfinder.core.WaypointEx;
import robot.pathfinder.core.path.PathType;
import robot.pathfinder.core.trajectory.TankDriveTrajectory;

public class AdvancedVisionAlign extends Command {

    private boolean error = false;

    private final long RESPONSE_TIMEOUT = 200; // Milliseconds
    private final double RECALCULATION_INTERVAL = 0.5; // Seconds

    private TrajectoryParams params;
    private TankDriveTrajectory trajectory;
    
    private FollowTrajectory followerCommand;

    private double lastTime;

    public AdvancedVisionAlign() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(Robot.drivetrain);
        requires(Robot.vision);
    }

    // Called just before this Command runs the first time
    @Override
    protected void initialize() {
        // First check if vision is ready
        if(!Robot.vision.ready()) {
            SmartDashboard.putString("Last Error", "Error: Vision is offline");
            OI.errorRumbleDriverMinor.execute();
            error = true;
            return;
        }
        // If vision is not enabled, attempt to enable it
        if(!Robot.vision.getVisionEnabled()) {
            try {
                Robot.vision.setVisionEnabled(true, true, 100);
                // Short delay to wait for camera to switch
                Thread.sleep(100);
            }
            catch(VisionException | InterruptedException e) {
                SmartDashboard.putString("Last Error", "Error: " + e.getMessage());
                OI.errorRumbleDriverMinor.execute();
                error = true;
                return;
            }
        }

        // Get the parameters we need
        double visionAngleOffset, visionXOffset, visionYOffset;
        try {
            long start = System.currentTimeMillis();
            // Give the Jetson some time to figure it out
            while(Double.isNaN(visionAngleOffset = Robot.vision.getTargetAngleOffset())) {
                try {
                    // If we exceeded the time limit, signal an error
                    if(System.currentTimeMillis() - start >= RESPONSE_TIMEOUT) {
                        SmartDashboard.putString("Last Error", "Error: Could not find vision target");
                        OI.errorRumbleDriverMinor.execute();
                        error = true;
                        return;
                    }

                    // Sleep for 20ms
                    Thread.sleep(20);
                }
                catch(InterruptedException e) {
                    SmartDashboard.putString("Last Error", "Error: That wasn't supposed to happen");
                    OI.errorRumbleDriverMinor.execute();
                    error = true;
                    return;
                }
            }
            visionXOffset = Robot.vision.getTargetXOffset();
            visionYOffset = Robot.vision.getTargetYOffset();
        }
        catch(VisionException e) {
            SmartDashboard.putString("Last Error", "Error: Vision went offline unexpectedly");
            OI.errorRumbleDriverMajor.execute();
            error = true;
            return;
        }

        params = new TrajectoryParams();
        params.isTank = true;
        params.pathType = PathType.QUINTIC_HERMITE;
        params.segmentCount = 100;
        // Set the waypoints
        params.waypoints = new Waypoint[] {
            new Waypoint(0, 0, Math.PI / 2),
            // The second waypoint has coordinates relative to the first waypoint, which is just the robot's current position
            new Waypoint(visionXOffset, visionYOffset, -visionAngleOffset + Math.PI / 2),
        };
        // Set alpha to be 3/4 of the diagonal distance
        params.alpha = Math.sqrt(visionXOffset * visionXOffset + visionYOffset * visionYOffset) * 0.75;

        trajectory = new TankDriveTrajectory(RobotMap.specs, params);
        followerCommand = new FollowTrajectory(trajectory);
        // We can't call start() on the command as it also requires drivetrain, which would cause this command to be interrupted. 
        // Thus we just call the raw methods and not hand control to WPILib.
        followerCommand.initialize();

        lastTime = timeSinceInitialized();
    }

    // For concurrent trajectory generation
    private final ExecutorService executorService = Executors.newSingleThreadExecutor();
    private Future<TankDriveTrajectory> trajGenFuture;

    // Called repeatedly when this Command is scheduled to run
    @Override
    protected void execute() {
        followerCommand.execute();
        // Check the result of the asynchronous computation
        if(trajGenFuture != null && trajGenFuture.isDone()) {
            // Simply replace the follower command
            try {
                followerCommand = new FollowTrajectory(trajGenFuture.get());
                // Don't forget this
                followerCommand.initialize();
            }
            catch(CancellationException e) {
                // Well, it seems like that the computation was so slow that it got cancelled. 
            }
            catch(InterruptedException | ExecutionException e) {
                // This should not happen.
                SmartDashboard.putString("Last Error", "Error: Unexpected exception in asynchronous trajectory recalculation");
            }
        }
        if(timeSinceInitialized() - lastTime >= RECALCULATION_INTERVAL) {
            // Recalculate the trajectory, if the target is still in sight
            double angleOffset, xOffset, yOffset;
            try {
                angleOffset = Robot.vision.getTargetAngleOffset();
                xOffset = Robot.vision.getTargetXOffset();
                yOffset = Robot.vision.getTargetYOffset();
            }
            catch(VisionException e) {
                // Report the error, but don't cause the current command to finish
                SmartDashboard.putString("Last Error", "Error: Vision went offline unexpectedly");
                lastTime = timeSinceInitialized();
                return;
            }
            // Make sure it's not NaN
            if(!Double.isNaN(angleOffset)) {
                // Terminate the previous trajectory generation thread if it is still not done
                if(trajGenFuture != null && !trajGenFuture.isDone()) {
                    trajGenFuture.cancel(true);
                }
                // Do the trajectory generation in a separate thread
                trajGenFuture = executorService.submit(() -> {
                    // Set the waypoints
                    params.waypoints = new Waypoint[] {
                        // The first waypoint has the velocity equal to our current velocity
                        new WaypointEx(0, 0, Math.PI / 2, (Robot.drivetrain.getLeftSpeed() + Robot.drivetrain.getRightSpeed()) / 2),
                        // The second waypoint has coordinates relative to the first waypoint, which is just the robot's current position
                        new Waypoint(xOffset, yOffset, -angleOffset + Math.PI / 2),
                    };
                    // Set alpha to be 3/4 of the diagonal distance
                    params.alpha = Math.sqrt(xOffset * xOffset + yOffset * yOffset) * 0.75;
                    return new TankDriveTrajectory(RobotMap.specs, params);
                });
                // Only update the last timestamp if the vision processing was successful
                lastTime = timeSinceInitialized();
            }
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    @Override
    protected boolean isFinished() {
        if(error) {
            return true;
        }
        return followerCommand.isFinished();
    }

    // Called once after isFinished returns true
    @Override
    protected void end() {
        followerCommand.end();
        if(Robot.vision.ready() && Robot.vision.getVisionEnabled()) {
            try {
                Robot.vision.setVisionEnabled(false);
            }
            catch(VisionException e) {
                SmartDashboard.putString("Last Error", "Failed to disable vision!");
                OI.errorRumbleDriverMinor.execute();
            }
        }
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    @Override
    protected void interrupted() {
        followerCommand.interrupted();
        if(Robot.vision.ready() && Robot.vision.getVisionEnabled()) {
            try {
                Robot.vision.setVisionEnabled(false);
            }
            catch(VisionException e) {
                SmartDashboard.putString("Last Error", "Failed to disable vision!");
                OI.errorRumbleDriverMinor.execute();
            }
        }
    }
}

FollowTrajectory.java:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.commands;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import frc.robot.subsystems.Drivetrain;
import robot.pathfinder.core.trajectory.TankDriveTrajectory;
import robot.pathfinder.follower.Follower.DirectionSource;
import robot.pathfinder.follower.Follower.DistanceSource;
import robot.pathfinder.follower.Follower.Motor;
import robot.pathfinder.follower.Follower.TimestampSource;
import robot.pathfinder.follower.TankFollower;

public class FollowTrajectory extends Command {

    public static final Motor L_MOTOR = Robot.drivetrain::setLeftMotor;
    public static final Motor R_MOTOR = Robot.drivetrain::setRightMotor;
    public static final DirectionSource GYRO = Robot.drivetrain::getHeading;
    public static final DistanceSource L_DISTANCE_SOURCE = Robot.drivetrain::getLeftDistance;
    public static final DistanceSource R_DISTANCE_SOURCE = Robot.drivetrain::getRightDistance;
    public static final TimestampSource TIMESTAMP_SOURCE = Timer::getFPGATimestamp;

    public static double kP_l = 0, kD_l = 0, kV_l = 0, kA_l = 0, kDP_l = 0;
    public static double kP_h = 0, kD_h = 0, kV_h = 0, kA_h = 0, kDP_h = 0;

    private final TankDriveTrajectory trajectory;
    private TankFollower follower;

    public FollowTrajectory(TankDriveTrajectory trajectory) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(Robot.drivetrain);
        this.trajectory = trajectory;
    }

    // Called just before this Command runs the first time
    // Note we made this method public! This is so that Commands that wrap around this one have an easier time.
    @Override
    public void initialize() {
        if(Robot.drivetrain.getGear() == Drivetrain.Gear.HIGH) {
            follower = new TankFollower(trajectory, L_MOTOR, R_MOTOR, L_DISTANCE_SOURCE, R_DISTANCE_SOURCE, TIMESTAMP_SOURCE, 
                    GYRO, kV_h, kA_h, kP_h, kD_h, kDP_h);
        }
        else {
            follower = new TankFollower(trajectory, L_MOTOR, R_MOTOR, L_DISTANCE_SOURCE, R_DISTANCE_SOURCE, TIMESTAMP_SOURCE, 
                    GYRO, kV_l, kA_l, kP_l, kD_l, kDP_l);
        }

        follower.initialize();
    }

    // Called repeatedly when this Command is scheduled to run
    @Override
    public void execute() {
        follower.run();

        if(Robot.isInDebugMode) {
            SmartDashboard.putNumber("Follower Left Output", follower.lastLeftOutput());
            SmartDashboard.putNumber("Follower Right Output", follower.lastRightOutput());

            SmartDashboard.putNumber("Follower Left Velocity", follower.lastLeftVelocity());
            SmartDashboard.putNumber("Follower Right Velocity", follower.lastRightVelocity());

            SmartDashboard.putNumber("Follower Left Acceleration", follower.lastLeftAcceleration());
            SmartDashboard.putNumber("Follower Right Acceleration", follower.lastRightAcceleration());

            SmartDashboard.putNumber("Follower Left Error", follower.lastLeftError());
            SmartDashboard.putNumber("Follower Right Error", follower.lastRightError());
            
            SmartDashboard.putNumber("Follower Left Error Derivative", follower.lastLeftDerivative());
            SmartDashboard.putNumber("Follower Right Error Derivative", follower.lastRightDerivative());

            SmartDashboard.putNumber("Follower Directional Error", follower.lastDirectionalError());
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    @Override
    public boolean isFinished() {
        return !follower.isRunning();
    }

    // Called once after isFinished returns true
    @Override
    public void end() {
        follower.stop();
        Robot.drivetrain.setMotors(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    @Override
    public void interrupted() {
        follower.stop();
        Robot.drivetrain.setMotors(0, 0);
    }
}
1 Like