Java Conditional Command

[notmattlythgoe]

Here is an idea I had recently for conditional wait commands. Thoughts?

With just a couple of static imports you could be writing CommandGroups that look like this.

package org.usfirst.frc.team2363.robot.commands;

import static org.usfirst.frc.team2363.robot.Robot.drivetrain;
import static org.usfirst.frc.team2363.robot.commands.ConditionalWaitCommand.Operation.*;
import static org.usfirst.frc.team2363.robot.commands.ConditionalWaitCommand.*;

import edu.wpi.first.wpilibj.command.CommandGroup;

public class DriveToLine extends CommandGroup {
    
    public  DriveToLine() {
    	addSequential(new DriveCommand(0.5));
        addSequential(waitUntil(drivetrain.getDistance(), GREATER_THAN, 50));
        addSequential(new DriveCommand(0));
    }
}

package org.usfirst.frc.team2363.robot.commands;

import edu.wpi.first.wpilibj.command.Command;

public class ConditionalWaitCommand extends Command {
	
	public enum Operation {
		LESS_THAN, LESS_THAN_EQUAL, EQUAL, GREATER_THAN_EQUAL, GREATER_THAN
	}
	
	public interface IProvidesValue {
		double getValue();
	}
	
	public static ConditionalWaitCommand waitUntil(IProvidesValue operand1, Operation operation, double operand2) {
		return new ConditionalWaitCommand(operand1, operation, operand2, true);
	}
	
	public static ConditionalWaitCommand waitWhile(IProvidesValue operand1, Operation operation, double operand2) {
		return new ConditionalWaitCommand(operand1, operation, operand2, false);
	}
	
	private IProvidesValue operand1;
	private Operation operation;
	private double operand2;
	private boolean isUntil;

    private ConditionalWaitCommand(IProvidesValue operand1, Operation operation, double operand2, boolean isUntil) {
        this.operand1 = operand1;
        this.operation = operation;
        this.operand2 = operand2;
        this.isUntil = isUntil;
    }
    
    @Override
	protected void initialize() { }

	@Override
	protected void execute() { }

    protected boolean isFinished() {
    	switch (operation) {
			case EQUAL:
				if (operand1.getValue() == operand2) {
					return isUntil;
				}
			case GREATER_THAN:
				if (operand1.getValue() > operand2) {
					return isUntil;
				}
			case GREATER_THAN_EQUAL:
				if (operand1.getValue() >= operand2) {
					return isUntil;
				}
			case LESS_THAN:
				if (operand1.getValue() < operand2) {
					return isUntil;
				}
			case LESS_THAN_EQUAL:
				if (operand1.getValue() <= operand2) {
					return isUntil;
				}
    	}
        return !isUntil;
    }

	@Override
	protected void end() { }

	@Override
	protected void interrupted() { }
}

package org.usfirst.frc.team2363.robot.subsystems;


import org.usfirst.frc.team2363.robot.commands.ConditionalWaitCommand.IProvidesValue;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Drivetrain extends Subsystem {
    
    private Distance distance;
    private Encoder encoder = new Encoder(1, 2);

    public void initDefaultCommand() { }
    
    public IProvidesValue getDistance() {
    	if (distance == null) {
    		distance = new Distance();
    	}
    	return distance;
    }
    
    private class Distance implements IProvidesValue {

		@Override
		public double getValue() {
			return encoder.getDistance();
		}
    }
}

[randallh]

@notmattlythgoe

...
public class DriveToLine extends CommandGroup {
    
    public  DriveToLine() {
    	addSequential(new DriveCommand(0.5));
        addSequential(waitUntil(drivetrain.getDistance(), GREATER_THAN, 50));
        addSequential(new DriveCommand(0));
    }
}
...

Be sure that the command that precedes the conditional command is a one-shot command that executes once and returns immediately. Otherwise, it will never complete and your conditional command will never get a chance to run.

If you’re using Java 8, you could simplify most of your code by using lambdas rather than hard-code the comparison operations. For example, your ConditionalWaitCommand can use the java.util.function.Predicate functional interface to define when it is completed:

public class ConditionalWaitCommand extends Command {
	
	public static ConditionalWaitCommand waitUntil( Predicate isComplete ) {
	}

	private final Predicate isComplete;

	private ConditionalWaitCommand(Predicate isComplete) {
		this.isComplete = isComplete;
	}

	@Override
	protected void initialize() { }

	@Override
	protected void execute() {
		return isComplete().test();
	}

	protected boolean isFinished() { }

	@Override
	protected void end() { }

	@Override
	protected void interrupted() { }
}

This makes this class very simple but even more flexible than your version. Your DriveToLine command group becomes:

public class DriveToLine extends CommandGroup {
    
    public  DriveToLine() {
    	addSequential(new DriveCommand(0.5));
        addSequential(waitUntil(drivetrain.getDistance() > 50));
        addSequential(new DriveCommand(0));
    }
}

But sometimes it is much easier just to create a generic and reusable command that is easily instantiated with custom lambdas using static methods. One example might be a very simple but generic Command subclass that takes an optional lambda to run once, a predicate to know when it is complete, and an optional function to run when complete. The static factory methods make this really easy to reuse without having to create a concrete subclass.

Here’s what that might look like to create a command instance that drives at 50% power while the distance is greater than 50, and once that condition has occurred then stop:

    Drivetrain driveTrain = ...
    Command myCommand = ConditionalCommand.runUntil(driveTrain.drive(0.5),
                                                    drivetrain.getDistance() > 50.0,
                                                    driveTrain.stop());

where

public class ConditionalCommand extends Command {
	
	public static ConditionalCommand waitUntil( Runnable initial, Predicate isComplete, Runnable uponComplete ) {
		return new ConditionalCommand(initial, isComplete, uponComplete);
	}

	public static ConditionalCommand waitUntil( Predicate isComplete, Runnable uponComplete ) {
		return new ConditionalCommand(initial, isComplete, uponComplete);
	}

	private final Runnable initial;
	private final Predicate isComplete;
	private final Runnable uponComplete;
	private boolean completed;

	private ConditionalCommand(Predicate isComplete) {
		this.isComplete = isComplete;
	}

	@Override
	protected void initialize() {
		if ( initial != null ) initial.run();
	}

	@Override
	protected void execute() {
		if ( isComplete().test() ) {
			if ( uponComplete != null ) uponComplete.run();
			completed = true;
		}
	}

	protected boolean isFinished() {
		return completed;
	}

	@Override
	protected void end() { }

	@Override
	protected void interrupted() { }
}

This is not the only pattern, either.

[Joe_Ross]

In this particular case of the example, I think it would be easier to build the logic into a single command.

However, the pattern is definitely useful. We had cases last year where we wanted to start a parallel command halfway through another command (for a different subsytem). We sequenced them with waits, but a way to generically peek at the progress of the first command would be much better.

[GeeTwo]

My Java is a bit rusty, but won’t this command in the DriveToLine instantiator:

        addSequential(waitUntil(drivetrain.getDistance(), GREATER_THAN, 50));

Get the distance from the drivetrain once during the instantiator, rather than sending a handle for the drivetrain.getDistance method to waitUntil()?

[notmattlythgoe]

@randallh

Be sure that the command that precedes the conditional command is a one-shot command that executes once and returns immediately. Otherwise, it will never complete and your conditional command will never get a chance to run.

If you’re using Java 8, you could simplify most of your code by using lambdas rather than hard-code the comparison operations. For example, your ConditionalWaitCommand can use the java.util.function.Predicate functional interface to define when it is completed:

public class ConditionalWaitCommand extends Command {
	
	public static ConditionalWaitCommand waitUntil( Predicate isComplete ) {
	}

	private final Predicate isComplete;

	private ConditionalWaitCommand(Predicate isComplete) {
		this.isComplete = isComplete;
	}

	@Override
	protected void initialize() { }

	@Override
	protected void execute() {
		return isComplete().test();
	}

	protected boolean isFinished() { }

	@Override
	protected void end() { }

	@Override
	protected void interrupted() { }
}

This makes this class very simple but even more flexible than your version. Your DriveToLine command group becomes:

public class DriveToLine extends CommandGroup {
    
    public  DriveToLine() {
    	addSequential(new DriveCommand(0.5));
        addSequential(waitUntil(drivetrain.getDistance() > 50));
        addSequential(new DriveCommand(0));
    }
}

But sometimes it is much easier just to create a generic and reusable command that is easily instantiated with custom lambdas using static methods. One example might be a very simple but generic Command subclass that takes an optional lambda to run once, a predicate to know when it is complete, and an optional function to run when complete. The static factory methods make this really easy to reuse without having to create a concrete subclass.

Here’s what that might look like to create a command instance that drives at 50% power while the distance is greater than 50, and once that condition has occurred then stop:

    Drivetrain driveTrain = ...
    Command myCommand = ConditionalCommand.runUntil(driveTrain.drive(0.5),
                                                    drivetrain.getDistance() > 50.0,
                                                    driveTrain.stop());

where

public class ConditionalCommand extends Command {
	
	public static ConditionalCommand waitUntil( Runnable initial, Predicate isComplete, Runnable uponComplete ) {
		return new ConditionalCommand(initial, isComplete, uponComplete);
	}

	public static ConditionalCommand waitUntil( Predicate isComplete, Runnable uponComplete ) {
		return new ConditionalCommand(initial, isComplete, uponComplete);
	}

	private final Runnable initial;
	private final Predicate isComplete;
	private final Runnable uponComplete;
	private boolean completed;

	private ConditionalCommand(Predicate isComplete) {
		this.isComplete = isComplete;
	}

	@Override
	protected void initialize() {
		if ( initial != null ) initial.run();
	}

	@Override
	protected void execute() {
		if ( isComplete().test() ) {
			if ( uponComplete != null ) uponComplete.run();
			completed = true;
		}
	}

	protected boolean isFinished() {
		return completed;
	}

	@Override
	protected void end() { }

	@Override
	protected void interrupted() { }
}

This is not the only pattern, either.

Yeah, I should have set the first command as parallel not sequential. I like the lambda ideas. Going to have to play around with them.

GeeTwo, it works because getDistance() returns an object not a value. So the getValue() call on the object at the time of the comparison will get the current value.

[GeeTwo]

@notmattlythgoe

GeeTwo, it works because getDistance() returns an object not a value. So the getValue() call on the object at the time of the comparison will get the current value.

OK, my poorly exercised OO brain gets it, but my better exercised procedural brain sees spooky action at a getDistance().

That suggests to me that you would want to have polymorphic forms of the command, including one which allows **both **operands to be objects. This could then be used to do things like (for example) returning the original drive direction by driving faster with the left wheel than the right, then equalizing after a

waitUntil(leftAxle.getDistance(), GREATER_THAN_EQUAL, rightAxle.getDistance())

completes. If you do this, you should allow either operand or both to be objects. For sanity’s sake, a constructor using two numeric values could be created, but it should throw an exception telling the user that [s]he is trying to wait forever, or not at all.

[notmattlythgoe]

@GeeTwo

OK, my poorly exercised OO brain gets it, but my better exercised procedural brain sees spooky action at a getDistance().

That suggests to me that you would want to have polymorphic forms of the command, including one which allows **both **operands to be objects. This could then be used to do things like (for example) returning the original drive direction by driving faster with the left wheel than the right, then equalizing after a

waitUntil(leftAxle.getDistance(), GREATER_THAN_EQUAL, rightAxle.getDistance())

completes. If you do this, you should allow either operand or both to be objects. For sanity’s sake, a constructor using two numeric values could be created, but it should throw an exception telling the user that [s]he is trying to wait forever, or not at all.

The command can definitely be expanded to accept different inputs. However I disagree with the constructor that throws an exception. Why give them enough rope to hang themselves when you can just not give them the rope at all?

[GeeTwo]

@notmattlythgoe

The command can definitely be expanded to accept different inputs. However I disagree with the constructor that throws an exception. Why give them enough rope to hang themselves when you can just not give them the rope at all?

Ease of debugging. An exception that tells you that one of the operands ought to be an object is more informative than a no such method error at compile. It can also be caught and allowed to run if that’s what the programmer really wants. Sort of like a "do you really want to delete this? dialog.

[notmattlythgoe]

@GeeTwo

Ease of debugging. An exception that tells you that one of the operands ought to be an object is more informative than a no such method error at compile. It can also be caught and allowed to run if that’s what the programmer really wants. Sort of like a "do you really want to delete this? dialog.

Its much easier to debug a syntax error because the method they are trying to access doesn’t exist than allow it to get to the point that it runs and throws an exception.

[GeeTwo]

@notmattlythgoe

Its much easier to debug a syntax error because the method they are trying to access doesn’t exist than allow it to get to the point that it runs and throws an exception.

Seeing the syntax error, the normal reaction (at least on my team) would be to check for a typo of a failed import, because these are the usual causes of our syntax errors after punctuation. It would take a longer time to figure out a syntax error than a “NeitherOperandIsObject” exception.

[notmattlythgoe]

@GeeTwo

Seeing the syntax error, the normal reaction (at least on my team) would be to check for a typo of a failed import, because these are the usual causes of our syntax errors after punctuation. It would take a longer time to figure out a syntax error than a “NeitherOperandIsObject” exception.

That’s when the syntax error notes are helpful. They’ll tell you that the method parameters don’t match.

The method waitUntil(IProvidesValue, Operation, IProvidesValue) in the type ConditionalWaitCommand is not applicable for the arguments (double, Operation, double)

That’s telling you that a method with that name exists and what it is expecting as the parameters. No need to run it and track down where an exception was thrown from.

[SamCarlberg]

@GeeTwo

My Java is a bit rusty, but won’t this command in the DriveToLine instantiator:

        addSequential(waitUntil(drivetrain.getDistance(), GREATER_THAN, 50));

Get the distance from the drivetrain once during the instantiator, rather than sending a handle for the drivetrain.getDistance method to waitUntil()?

Yep. It would be better to pass a method reference

        addSequential(waitUntil(driveTrain::getDistance, GREATER_THAN, 50));

and call that method during each check.

@GeeTwo

OK, my poorly exercised OO brain gets it, but my better exercised procedural brain sees spooky action at a getDistance().

That suggests to me that you would want to have polymorphic forms of the command, including one which allows **both **operands to be objects. This could then be used to do things like (for example) returning the original drive direction by driving faster with the left wheel than the right, then equalizing after a

waitUntil(leftAxle.getDistance(), GREATER_THAN_EQUAL, rightAxle.getDistance())

completes. If you do this, you should allow either operand or both to be objects. For sanity’s sake, a constructor using two numeric values could be created, but it should throw an exception telling the user that [s]he is trying to wait forever, or not at all.

This could be done with a few generic methods, i.e.

    public static <T, U> ConditionalCommand waitUntil(Supplier<T> left, BiPredicate<T, U> tester, Supplier<U> right)
    public static <T, U> ConditionalCommand waitUntil(Supplier<T> left, BiPredicate<T, U> tester, U right)

So you could just call

waitUntil(driveTrain::getDistance, GREATER_THAN, 50);

where

GREATER_THAN = (left, right) -> left > right

[notmattlythgoe]

[quote=SamCarlberg;1505595][/quote]

Sam,

Any chance of getting some of these types of concepts into the WPILib at some point?

I’m really liking the method reference strategy.

[SamCarlberg]

I would be surprised if it made it in this year with kickoff so close.

It would also need to look similar in C++ with whatever it uses for lambdas and method references

[notmattlythgoe]

@SamCarlberg

I would be surprised if it made it in this year with kickoff so close

Yeah, I doubted anything additional would make it in this year. I meant more for future years.

[SamCarlberg]

@notmattlythgoe

Yeah, I doubted anything additional would make it in this year. I meant more for future years.

Maybe. It doesn’t seem like a huge project but it might not be a priority for next year

[SamCarlberg]

@Joe Ross

In this particular case of the example, I think it would be easier to build the logic into a single command.

However, the pattern is definitely useful. We had cases last year where we wanted to start a parallel command halfway through another command (for a different subsytem). We sequenced them with waits, but a way to generically peek at the progress of the first command would be much better.

If you just want a rising/falling edge trigger, you could just poll the isFinished() method of the previous command, though that would break the CommandGroup paradigm a little

[notmattlythgoe]

@SamCarlberg

If you just want a rising/falling edge trigger, you could just poll the isFinished() method of the previous command, though that would break the CommandGroup paradigm a little

I’ve written one off wait for specific value commands that basically do the same thing the initial thread was started about. Things like having the robot move at a certain speed and when it reaches certain distances fire off certain commands. Think 7 disc auto in 2013. Never needed to stop, but as you reached the certain distances you could activate a certain chain of events.

What is the CommandGroup paradigm for doing down a different command path based on a decision from a previous step? Think hot goals in 2014.

[SamCarlberg]

@notmattlythgoe

I’ve written one off wait for specific value commands that basically do the same thing the initial thread was started about. Things like having the robot move at a certain speed and when it reaches certain distances fire off certain commands. Think 7 disc auto in 2013. Never needed to stop, but as you reached the certain distances you could activate a certain chain of events.

What is the CommandGroup paradigm for doing down a different command path based on a decision from a previous step? Think hot goals in 2014.

Sounds a lot like an event/listener model.

That’s not how CommandGroups were intended to be used, which is why it breaks the paradigm.

[randallh]

@notmattlythgoe

Yeah, I should have set the first command as parallel not sequential. I like the lambda ideas. Going to have to play around with them.

If you’ve not already seen Strongback, you may want to take a look since its command framework (which we think is improved over WPILib’s) uses lambdas throughout. In fact, it uses lambdas throughout the whole API.

See the announcement on CD, and the Using Strongback online book.

Best regards