Pathfinder returning infinity?

Technical Programming
1

I was using the Pathfinder library to generate and run a profile, and I decided to print out the left and right values that Pathfinder calculates. The robot looks good, but on the console, it prints that the left and right power are infinity? I isolated that the problem does not come from the turning calculations, but instead with the encoderFollower’s calculate method. Then I checked my encoder to see if it was the thing that was acting up. It was a little off probably because it wasn’t tuned, but not enough such that it would have such a dramatic effect. I also tried to use the DistanceFollower and I got the same results. The profile runs ok, but the values are infinity.
Here is the code in question:

    public DrivetrainMotionProfileJaciEncoderCommand(PathGenerator path) {
    	leftTraj = path.modifier.getLeftTrajectory(); 
    	rightTraj = path.modifier.getRightTrajectory();
    	maxVelocity = path.maxVelocity;
    }

    protected void initialize() {
    	Robot.drivetrain.resetEncoders(); 
    	Robot.drivetrain.resetGyro();
    	leftFollower = new EncoderFollower(leftTraj);
    	rightFollower = new EncoderFollower(rightTraj);
    	leftFollower.reset();
    	rightFollower.reset();
    	//Wheel diameter in feet
    leftFollower.configureEncoder(Robot.drivetrain.leftBottomMotor.getSelectedSensorPosition(0), RobotMap.DRIVETRAIN_ENCODER_TICKS_PER_REVOLUTION, RobotMap.DRIVETRAIN_WHEEL_DIAMETER / 12);
    	rightFollower.configureEncoder(Robot.drivetrain.rightBottomMotor.getSelectedSensorPosition(0), RobotMap.DRIVETRAIN_ENCODER_TICKS_PER_REVOLUTION, RobotMap.DRIVETRAIN_WHEEL_DIAMETER / 12);
    	leftFollower.configurePIDVA(SmartDashboard.getNumber("Motion Profile P", 0.006), SmartDashboard.getNumber("Motion Profile I", 0), SmartDashboard.getNumber("Motion Profile D", 0.03), 1 / maxVelocity, 0);
    	rightFollower.configurePIDVA(SmartDashboard.getNumber("Motion Profile P", 0.006), SmartDashboard.getNumber("Motion Profile I", 0), SmartDashboard.getNumber("Motion Profile D", 0.03), 1 / maxVelocity, 0);
    }

    protected void execute() {
    	double leftOutput = leftFollower.calculate(Robot.drivetrain.leftBottomMotor.getSelectedSensorPosition(0));
    	double rightOutput = rightFollower.calculate(Robot.drivetrain.rightBottomMotor.getSelectedSensorPosition(0));
    	double gyroHeading = Robot.drivetrain.getGyroAngle();
    	double desiredHeading = Pathfinder.r2d(leftFollower.getHeading());
    	double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
    	double turn = 0.8 * (-1.0 / 80.0) * angleDifference;
    	Robot.drivetrain.tankDrive(leftOutput + turn, rightOutput - turn);
    	System.out.println("Left Power: " + (leftOutput + turn) + "Right Power: " + (rightOutput - turn));
    }

    protected boolean isFinished() {
    	if(leftFollower.isFinished() && rightFollower.isFinished()){
    		System.out.println("Path has finished");
    		return true; 
    	}else {
    		return false; 
    	}
    }

PathGenerator is a class I made that create the path usingPathfinder when its constructed.
Waypoints:

Waypoint] points = new Waypoint] {
			new Waypoint(0, 3.25, 0),
			new Waypoint(10, 3.25, 0),
			new Waypoint(24, 3.25, 0),
			new Waypoint(27, 6, Pathfinder.d2r(90))
	};

Plus my maxVelocity is 7.813 ft/sec and my max acceleration is 6.104 ft/sec^2.
What causes the infinity? And is it something I should be worrying about?

2

Also, this might be important too. The PID values are P-0.006 I-0.0 D-0.03.

3

Try this: https://www.chiefdelphi.com/forums/showpost.php?p=1771927&postcount=60

The reason why you are probably seeing infinity when you turn is that if you’re calculating the angle difference incorrectly, it will apply the turn power opposite of what you want, which will in turn, induce more error and Pathfinder will calculate a larger turn from the resultant encoder reading which ends up spiraling out of control (Your robot will just turn in circles when you start turning).

4

I tried to add the heading to the desired heading but I still got infinity. There is definitely something wrong with the calculate method of the encoder follower.

Sent from my SM-J327T1 using Tapatalk

5

These are the distance follower calculations. If you zero Kp, Kd and Ka you will be running on feed forward (Kv) only and the result should always be <1

public double calculate(double distance_covered) {
	if (segment &lt; trajectory.length()) {
		Trajectory.Segment seg = trajectory.get(segment);
		double error = seg.position - distance_covered;
		double calculated_value = kp * error + // Proportional
				kd * ((error - last_error) / seg.dt) + // Derivative
				(kv * seg.velocity + ka * seg.acceleration); // V and A Terms
		last_error = error;
		heading = seg.heading;
		segment++;

		return calculated_value;
	} else
		return 0;
}
6

I found out the problem. It was that I used the values that I got in Vannaka’s MP generator to generate a profile in Pathfinder. I figured out how to copy the ccvs onto the roboRIO and run it from there. Although it worked the time it took for it to finish was way longer that I expected. I used the velocity view on the generator. Does anyone know how accurate this is. What should I do to make the profile complete faster?

Sent from my SM-J327T1 using Tapatalk

7

Look at the csv files for left and right wheels using Excel. The number of rows multiplied by the time increment you have chosen should be the target time to run the profile. You can adjust that using the velocity, accel and jerk settings to get your (realistic) target time.

Then you need to tune the profile using the p, a, d and turn gains.

Try logging command and actual left and right wheel distances, angles and speeds to a csv file and plot the results using Excel to see how well you are tuned and check the results of changes.

8

Well, I’ve run into a few more problems. First of all, when tunning the PIDs I found that using 0 works the best. I know that PID values should be low since this is motion profiling, but still is PID values of 0 normal. Anything higher offshoots the path. Another thing is with the velocity ratio. When I use 1 / max velocity like it says on the Pathfinder guide the turns in my paths happen much later than expected. However, if I give it another value in place of max velocity in that expression, it works much better. In fact, I was able to sort of tune it to a point where the path looks almost like the one I expect. Is this normal? What am I doing wrong?

9

What value are you using for your period? Typically, the most common value I’ve found is 0.05, but there have been some examples that use 0.005. When originally implementing Pathfinder for my own team, I used 0.005, which caused us to use heavily over-inflated values before Pathfinder would behave ‘properly’. For example we stuck 400 yards/second as our velocity before our robot would follow the path at close to full power (Using ‘real’ values had the robot follow the path, but at < 50% power). Switching to a period of 0.05 seconds solved this problem.

I think this problem occurs due to the period being far much faster than WPILib’s scheduler. The scheduler runs at loop once every ~20ms (Note that this is not a guarantee rate at can fluctuate slightly, but most people can ignore this), so being too fast throws it off somehow. If you really want to, you can use a Notifier guarantee that the path will be read at the specific period you set it to (You can see our implementation here). Even if you don’t decide to run at a faster rate, using a notifier helps the robot run a path far more consistently than if you didn’t since the notifier guarantees that the read and execution of the path occurs at your set interval rather than being beholden to random variance.

10

You should be able to get a path running reasonably in the Command you are using with a .02 period using Timed Robot.

What kind of drive is on the robot? What wheels and wheelbase width and how did you arrive at it? What top speed? Are you being consistent in your units? (Distance Follower makes that part easier.)
Your code uses gyroHeading. Is that 0 - 360 or +/-180

With 0 for PID values you are running on feedforward only.

Robot Casserole has a Simple CSV Logger on GitHub.

11

I’ve been using 0.01 as by period. Besides that I’m pretty sure I’ve been consistent with by values. I had a max velocity of 12 ft/sec and a max acceleration of of 7 ft/sec/sec. I’ll try reduce the period then implement the notifier.

Sent from my SM-J327T1 using Tapatalk

12

The period must be the same as the frequency with which you process the trajectory. So if you are running commands at 20 ms, your period must be .02.

13

So basically I could run the trajectory at 10 ms so long as I set the notifier to run at 0.01 sec. What is the benefit of having a shorter period. I would assume that everything is more accurate because there are more points, but more points mean more calculations, so wouldn’t that make it slow too?

Sent from my SM-J327T1 using Tapatalk

14

Smaller periods allow you to be more accurate, but doesn’t necessarily make it slower to calculate. All of the heavy calculation is done when you generate the path and store it to a .csv file. The calculate portion of reading a trajectory takes the ideal values of where it should be/how fast it should be going and then adjusts the output based on your encoder as feedback to take into account variables from the real world.

The upper limit of how fast you could go would probably end up being how fast your encoder can update/how fast you can read that value.

The only other downside I can see of being ‘too accurate’ would be smaller variations in following your path from the real world would cause your robot to react more aggressively to maintain that path.

15

Another note about making paths with smaller periods: Although it may not take a lot of time to process on the robot, it will take a lot of time processing off the robot. Most people wont really feel this effect, but if you really want to be competitive, decreasing the time it takes to make the paths does make a difference at a competition for tuning your paths rapidly. For example, at Champs you get 10 minutes on the practice field before you have to requeue, which can take anywhere from 30 minutes to over an hour depending on how many teams are at the practice fields. Being able to make effective use of that time means you need to minimize time spend doing code redeploys and waiting around for code changes is wasteful if you don’t need to. This is something I realized this year in using pathfinder, where our team was able to rapidly iterate our autos by running them up to 15 times in 10 minutes, whereas most other teams were only doing a handful in that same time period. It’s also something I’m really excited about improving for my team in 2019 by moving more things outside of the base robot code to minimize redeploys.

16

What techniques did your team use to cut down on path generation time?? Are you estimating the paths with arcs, like 254?

Thanks

17

Most of our work this year was guess-and-check, since this was our first year working with any type of implementation of motion profiling. We first estimated the paths that we wanted to run by defining points on the field through measurements and then our first pass was to throw them into Vannaka’s Motion Profile Generator as a sanity check to ensure the paths were valid.

The real speed improvements came from writing our own separate PathfinderGen Java project that would generate the paths. Once you have defined your paths and the parameters you can use to reliably follow it, its just a matter of doing small adjustments, which was a lot easier to do in a bare-bones Java project. An improvement I’d like to make to this is for it to automatically ignore generating unchanged paths so that we don’t have to manually comment/uncomment the paths we’re not working/working on, as doing a full pass on every single path takes a long time.

We then used WinSCP with it’s auto sync feature to sync the generated paths after PathfinderGen creates them to avoid manually copying the files over. One thing about this is that WinSCP can get a little buggy once you loose connection to your robot and may not actually sync properly, so manually resetting the connection every time you connect to the robot is a must.

Overall, this process is fast enough where we’ve basically regenerated a modified path and uploaded it to the robot by the time our robot has been manually reset. My plan is to move more parameters off the robot, like elevator height and shooting angles to further reduce the amount of times we have to redeploy the actual robot code.

18

I’ve tried to use the notifier in our code. Here is our implementation:


public DrivetrainMotionProfileJaciEncoderCommand(String nameOfPath, double dt, double maxVelocity) {
    	requires(Robot.drivetrain);
    	leftCSV = new File("/home/lvuser/Paths/" + nameOfPath + "_left_Jaci.csv");
    	rightCSV = new File("/home/lvuser/Paths/" + nameOfPath + "_right_Jaci.csv");
    	leftTraj = Pathfinder.readFromCSV(leftCSV);
        rightTraj = Pathfinder.readFromCSV(rightCSV);
        System.out.println("CSV has been locked and loaded");
    	this.maxVelocity = maxVelocity; 
    	this.dt = dt; 
    	profileProcessor = new Notifier(new RunProfile());
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

 protected void initialize() {
    	leftFollower = new EncoderFollower(leftTraj);
    	rightFollower = new EncoderFollower(rightTraj);
    	leftFollower.reset();
    	rightFollower.reset();
    	//Wheel diameter in feet
    	leftFollower.configureEncoder(Robot.drivetrain.leftBottomMotor.getSensorCollection().getQuadraturePosition(), RobotMap.DRIVETRAIN_ENCODER_TICKS_PER_REVOLUTION, RobotMap.DRIVETRAIN_WHEEL_DIAMETER / 12);
    	rightFollower.configureEncoder(Robot.drivetrain.rightBottomMotor.getSensorCollection().getQuadraturePosition(), RobotMap.DRIVETRAIN_ENCODER_TICKS_PER_REVOLUTION, RobotMap.DRIVETRAIN_WHEEL_DIAMETER / 12);
    	leftFollower.configurePIDVA(SmartDashboard.getNumber("kp", 0.0), SmartDashboard.getNumber("ki", 0), SmartDashboard.getNumber("kd", 0.0), 1 / maxVelocity, SmartDashboard.getNumber("ka", 0));
    	rightFollower.configurePIDVA(SmartDashboard.getNumber("kp", 0.0), SmartDashboard.getNumber("ki", 0), SmartDashboard.getNumber("kd", 0.0), 1 / maxVelocity, SmartDashboard.getNumber("ka", 0));
    	profileProcessor = new Notifier(new RunProfile());
    	profileProcessor.startPeriodic(dt);
    }

protected boolean isFinished() {
    	if(leftFollower.isFinished() && rightFollower.isFinished()){
    		System.out.println("Path has finished");
    		return true; 
    	}else {
    		return false; 
    	}
    }

protected void end() {
    	profileProcessor.stop();
    	Robot.drivetrain.stop();
    }

class RunProfile implements java.lang.Runnable {
    	int segmentNumber = 0; 
		@Override
		public void run() {
			double leftOutput = leftFollower.calculate(Robot.drivetrain.leftBottomMotor.getSensorCollection().getQuadraturePosition());
	    	double rightOutput = rightFollower.calculate(Robot.drivetrain.rightBottomMotor.getSensorCollection().getQuadraturePosition());
	    	double gyroHeading = Robot.drivetrain.getGyroAngle();
	    	double desiredHeading = Pathfinder.r2d(leftFollower.getHeading());
	    	//Pathfinder is counter-clockwise while gyro is clockwise so gyro heading is added
	    	double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading + gyroHeading);
	    	//kg is the turn gain and is the p for the angle loop
	    	double turn = SmartDashboard.getNumber("kg", 0.08) * (-1.0 / 80.0) * angleDifference;
	    	Robot.drivetrain.tankDrive(leftOutput + turn, rightOutput - turn);
	    	System.out.println("Left Power: " + (leftOutput + turn) + " Right Power: " + (rightOutput - turn));
	    	segmentNumber++; 
		}
    }

We made dt 0.05 by the way.
However, when we tried to use this it ran for a while and then the path finished. We then had it print out the segment number and the segment and found out that the notifier ran too quickly. It got to the last point in like 1 second. I know I’m not using the wrong units, so what could be wrong with our notifier?
I also should mention that we also have another notifier in Robot running at the same rate, which logs data. That one works fine, as far as logging is concerned. We commented that out to ensure that it wasn’t the problem, but it was the same results.

19

Can you define how the notifier “ran too quickly”? Are you going through all your segments in the given path? Are you stopping in the middle of your path? (unlikely, given that you return true when both sides finish). Does your robot execute the path at the correct scale? For example, if you tell a robot to move in an S-curve 3 ft. forward and 3 ft. left from its initial position, is it actually going roughly 3 ft. or does it look like a scaled down version (like 2 ft. forward and 2 ft. left). If so your encoder counts per revolution may be incorrect. (Our team used a 360 counts per rev encoder for our wheels, but it’s read through the Talon SRX using the quad encoder settings, which multiplies the value 4x so a full revolution ends up being 1440 instead of 360).

I don’t see anything obviously wrong in your code. I would double check if the dt you’re passing is actually 0.05 by printing it out. Secondly, I would double check the dt value in your .csv paths to make sure that they are 0.05.

20

How many segments are in your profile? This should be the value in leftTraj.length.

Does segmentNumber agree with leftTraj.length after running?

You can use Timer.getFPGATimestamp() to determine your Notifier step time to find out what it is exactly.

What would be your expected profile time?

Your step time should be the same as leftFollower.getSegment().dt

You could also set your Notifier time step by reading that value.