TurnAsync issues

[Team14423]

What might account for TurnAsync underturning on small margins? At first, I thought it was our angle math and norming, but we have that right now. If we do TurnAsync(5), we get 1 degree of turn. If we do TurnAsync(9), we get 5 degrees. We originally thought it was our state machine and not properly checking drive.isBusy, but we put in a waitTimer and wound up seeing nope - it just goes to the heading short of the target.

FWIW, it appears that we need to retune - our splining, etc. has lots of errors while it gets to position, and it oscillates on difficult linetoheading. Is the turn code based on the same profiles, such that we're just getting large errors? If not, then it's odd that the pre and post turn headings just don't match the turn degrees.

[rbrott]

At first, I thought it was our angle math and norming, but we have that right now.

I assume that means you're passing radians to turnAsync()/turn().

What might account for TurnAsync underturning on small margins?

Small angles test the limits of the feedforward model. The effects of static friction are more apparent, and slight inaccuracies in kStatic can disproportionately affect the final turning error. If this is the main hangup, I would call the feedforward good enough and use feedback to close the gap.

FWIW, it appears that we need to retune - our splining, etc. has lots of errors while it gets to position, and it oscillates on difficult linetoheading. Is the turn code based on the same profiles, such that we're just getting large errors? If not, then it's odd that the pre and post turn headings just don't match the turn degrees.

Oh well in that case it's hard to diagnose the problem precisely. I recommend turning off feedback and reducing max vel/accel to better assess the quality of the tuning. The turn code isn't based on exactly the same profiles, but its linked via the drive parameters to general trajectory/path execution.

[Team14423]

Thank you. Comments below.

At first, I thought it was our angle math and norming, but we have that right now.

I assume that means you're passing radians to turnAsync()/turn().

**Yes - we log in degrees but calculate in radians.

What might account for TurnAsync underturning on small margins?

Small angles test the limits of the feedforward model. The effects of static friction are more apparent, and slight inaccuracies in kStatic can disproportionately affect the final turning error. If this is the main hangup, I would call the feedforward good enough and use feedback to close the gap.

**It would not surprise me if kStatic was off. I was looking at the issue below about spiked velocity readings and thinking our profile looks nothing like that. We can get it close, but nowhere near exact. Not sure how we use feedback to close the loop. Also seems the opposite from the suggestion below, so it sounds like we need to get our PID right. We ran the auto tuner, but the results have never been great for us. May be because our Max Vel was too high, as you suggest.

FWIW, it appears that we need to retune - our splining, etc. has lots of errors while it gets to position, and it oscillates on difficult linetoheading. Is the turn code based on the same profiles, such that we're just getting large errors? If not, then it's odd that the pre and post turn headings just don't match the turn degrees.

Oh well in that case it's hard to diagnose the problem precisely. I recommend turning off feedback and reducing max vel/accel to better assess the quality of the tuning. The turn code isn't based on exactly the same profiles, but its linked via the drive parameters to general trajectory/path execution.

**How do we turn off feedback? Max vel is 35, which feels pretty low for how fast our motors are. Then again, it feels like we are spinning out of control when we strafe (which we can't do in a straight line in teleop - it sort of arcs) so the speed isn't helping much. As it is, we have our vx/vy weights down to .4 for teleop/localization test.

[rbrott]

We ran the auto tuner, but the results have never been great for us.

Yes, the automatic tuners are far from perfect. I would take their outputs as a starting point for manual tuning.

Also seems the opposite from the suggestion below, so it sounds like we need to get our PID right.

Yes. Feedback is necessary to obtain the last bit of accuracy and robustness, but it often obscures and impedes debugging when applied prematurely.

How do we turn off feedback?

If you're using PID, set the coefficients/gains to zero.

[Team14423]

Thanks. We will retune, and if we have trouble, I'll pick this back up.

[Team14423]

OK, back again. The team spent a couple hours tuning yesterday, and (perhaps because of our motor design choices) it was difficult. The automaticfeedforward tuner drives the robot forward about 10 inches, and then yields numbers that are way too high. We got pretty close with a lot of manual adjustment, but then the StraightTest landed way (30") short. So, we increased Kv and Ka until we got reasonably close. We then did the back and forth test, and it worked reasonably well, except our x error was always about 2 inches off at the end, no matter what we did with PID (we set both as high as 20, but simply could not close the loop on those last 2 inches). PID follower test was OK, except the robot tended to drift to the side as it drove and then correct, and also was a bit short before turning.

The good news is that with an increase in Kstatic, the turning part seems to work a bit better (though our first angle is off).

In practice, we still see relatively high errors. I attach two videos: https://photos.app.goo.gl/WbsQEt6o82eyL6nW8

Two observations from these (other than the obvious errors):

1. The first path shows a hook at the end (and the robot does that). But the path has no such hook. It just says splineto a point and heading. So why the overshoot?
2. The robot snaps back to the final position on the Dashboard, but in reality it is not at that final position. Does the trajectory.end() we send in to the next one affect what we see there? For the first drive to the boxes, that heading is usually off by 20-30 degrees.

Any thoughts appreciated, as we are scratching our heads over this. We saw Noah's tuning video and the profile coming out of automatic was almost exactly perfect, and ours was....not even close.

[rbrott]

The first path shows a hook at the end (and the robot does that). But the path has no such hook. It just says splineto a point and heading. So why the overshoot?

Ah there's probably some subtle path mis-specification (I believe some quickstart versions had such an error in the past). Post the code, and I can help remove the hook.

The robot snaps back to the final position on the Dashboard, but in reality it is not at that final position. Does the trajectory.end() we send in to the next one affect what we see there? For the first drive to the boxes, that heading is usually off by 20-30 degrees.

The position should not "snap" unless you call setPoseEstimate() (which you should only call at the beginning to specify the starting position). Passing trajectory.end() as the start of the next trajectory is perfectly fine; it shouldn't affect the pose estimate at all.

Any thoughts appreciated, as we are scratching our heads over this. We saw Noah's tuning video and the profile coming out of automatic was almost exactly perfect, and ours was....not even close.

Yes... he does make it seem easy.

[Team14423]

We aren't calling setPoseEstimate().

Here is the initial trajectory. Angletogoal just calculates the difference between the x,y and the goal (offset by 5 inches). It is uses Vector2d angle functions and is pretty accurate (here it returns 350 degrees or so). isRed is 1. I'm leaving the commented code in - could that be the problem in the way the build happens? Power Target is 363 at that point (long story).

trajectory1 = robot.drive.trajectoryBuilder(startPose) .splineToLinearHeading(new Pose2d(-7, isRed -12, robot.shooter.angleToGoal(-7, -12, robot.shooter.redPowerShot1)), -15) //.splineToLinearHeading(new Pose2d(-7, isRed -12, Math.toRadians(PowerTarget)), 0) .build();

Here is the full code if you are a glutton for punishment. AutonCode. Note that the Turn State afterward is not in the video I sent. We added that after because our end heading from the spline trajectory was always off - so we are hoping to Turn to the proper angle.

[rbrott]

Ok I think I roughly reproduced your trajectory locally. I believe you just need to convert -15 to radians.

[Team14423]

Ahh! Thank you. We messed around with the tangent to get a bigger curve, but the simulator seemed to work fine with this number. We may use it (in radians) or get rid of it, but good to know that was the issue.

Any thoughts on the large errors we are seeing? The odometry wheels look pretty good (we even posted video of them running to reddit for fun), but it's obviously struggling some moves. It may just be that strafing is really, really hard for our robot - we are unable to do it at all in teleop. A right strafe moves us to the right but the robot arcs. We suspect a weight balance issue, but I don't know if we'll be able to correct it.

[rbrott]

Any thoughts on the large errors we are seeing?

The errors you can see in the dashboard are from flawed execution. You can try adjusting feedback, constraints, or follower timeout/admissible error if you only care about the final accuracy.

The errors that don't register due to wheel slip or other localization problems are separate.

[Team14423]

Thanks. We will keep at it with the tuning, and also record the dashboard and the robot to match errors. I would think localization errors won't show, but wheel slip will if our odometry wheels are working. A couple final questions:

1. Where do we set timeout/tolerance? I must have missed that in the docs. I'll take another look. You are correct that final accuracy is the most important thing for us.
2. Is there a way to constrain wheel slippage on strafe? Do we just crank VY_Weight down to .1 or something?

[Team14423]

Nevermind the first question - I found it

[rbrott]

Is there a way to constrain wheel slippage on strafe? Do we just crank VY_Weight down to .1 or something?

The usual assumption with strafe slipping is that it reduces strafe velocity by a fixed multiplicative factor. You can adjust for this by tuning the lateral multiplier.

If strafing is very poor, you may just want to restrict the robot to tank paths. You can also write a custom constraint that limits strafe velocity.

The vx/vy weights don't really do what you think. They exist to map joystick inputs to wheel powers without clipping and with more flexibility than typical normalization/scaling.

[Team14423]

OK, thank you. Our lateral multiplier is pretty high (like 2), because we were not going far enough in strafe test, but maybe we need to dial that back and rely on feedback.

[Team14423]

OK, back for more, circling back to where we started, in a way. Thank you for your continuing feedbaclk. The team added weight to the corners (turned out they were spoiled last year and just happened to have even weight distribution so they thought it would happen all the time). Surprise surprise, it now strafes much better (we also lowered lateral_multiplier from 2 down to 1.

But our turning is still off. At first, it was off as many as 20 degrees in 90 - the odometry was correct - the robot just wasn't getting there. So, we retuned again, upping Kv, Ka, Kstatic, and even PID. It's mostly there on positional - but the turn/turnasync is still off. We run a simple turn test and are off 3 degrees in 90, 6 degrees in 180. Kv got the error down a little bit. We upped Kstatic and/or Ka, and it started jerking in the middle, though made the error smaller a bit.

We also played with track_width (that is the purpose of Turn Test, after all), and 4 inches in either direction did not limit the error. This leads me to believe it's a tolerance/Kstatic thing. We cranked P up to like 30, no good. I then traced the code, [nevermind]. But it does look like it uses maxjerk and overshoot, both of which might help us.

So, that's a long intro into:

1. What can we do to reduce our error? a. Is it possible or do we just live with it? b. Is it a Kstatic thing to go the last mile as discussed above? c. nevermind about maxjerk d. Should we use overshoot? (that's what the kids did when writing their own code in prior years)

Updates:

1. Digging more into the code, I see the PID now in sample mechanum drive (I also found trajectory admissibleerror...), though the constructor doesn't seem to send FF coefficients in. It also ends based on time, not error - is that an issue? It looks like not because setdrivesignal handles it.

2. I note that dashboard drawing is right after.movement but in the same iteration. Just FYI, we went to manual bulk reads, so that may be stale. Consider putting a dashboard update in idle state?

Any input appreciated.

[rbrott]

1. What can we do to reduce our error? a. Is it possible or do we just live with it?

It's probably possible.

b. Is it a Kstatic thing to go the last mile as discussed above?

Hard to say without a graph/data. Track width and feedback should pretty directly affect the error.

d. Should we use overshoot? (that's what the kids did when writing their own code in prior years)

I'm not exactly sure what you mean by "using overshoot."

1. Digging more into the code, I see the PID now in sample mechanum drive (I also found trajectory admissibleerror...), though the constructor doesn't seem to send FF coefficients in. It also ends based on time, not error - is that an issue? It looks like not because setdrivesignal handles it.

The follower should end based on time, unless the error becomes admissible prematurely.

[Team14423]

**Thanks. Your responses are so nicely formatted - I'll have to figure out the markdown.

b. Is it a Kstatic thing to go the last mile as discussed above?

Hard to say without a graph/data. Track width and feedback should pretty directly affect the error.

**I would have thought so. It's possible we need some I, but since turnasync is based on duration I wonder if it's timing out. Maybe we'll try to capture a graph. I'm also thinking we should zero everything out and run a turn to like 3 degrees and up Kstatic until the robot moves.

d. Should we use overshoot? (that's what the kids did when writing their own code in prior years)

I'm not exactly sure what you mean by "using overshoot." **TurnAsync uses simplemotionprofilegenerator, which takes overshoot in the constructor. That appears to take a parameter overshoot, that adds a segment to go past the target and move back - as far as I can tell according to the comments.

    Digging more into the code, I see the PID now in sample mechanum drive (I also found trajectory admissibleerror...), though the constructor doesn't seem to send FF coefficients in. It also ends based on time, not error - is that an issue? It looks like not because setdrivesignal handles it.

The follower should end based on time, unless the error becomes admissible prematurely.

**I don't see any admissible error setting in the turncontroller PIDController, nor in the simpleMotionProfile. The PID Controller is only setting a correction to the Profile's target velocity Omega:

double correction = turnController.update(currentPose.getHeading());
 setDriveSignal(new DriveSignal(new Pose2d(
                        0, 0, targetOmega + correction
                ), new Pose2d(
                        0, 0, targetAlpha
                )));

The end condition in the .TURN state is if (t >= turnProfile.duration()) so my impression of what's happening is that we are timing out before we get there, but that there is no position based error detection (and hence no admissible error). That said, I don't fully understand how turnProfile.duration is being calculated, so if there is an admissible error in there we can look into that.

[rbrott]

**TurnAsync uses simplemotionprofilegenerator, which takes overshoot in the constructor. That appears to take a parameter overshoot, that adds a segment to go past the target and move back - as far as I can tell according to the comments.

OK I wanted to make sure that's what you meant. That setting shouldn't have an effect in this case. It exists to handle corner profile generation cases where a "monotonic" profile is impossible without violating constraints. All turn profiles are from rest to rest which guarantees that they're "monotonic feasible."

I don't see any admissible error setting in the turncontroller PIDController, nor in the simpleMotionProfile.

Ah yes that's correct. Admissible error is only built into trajectory followers. The "user-space" turn implementation is pretty bare bones.

[Team14423]

Ah yes that's correct. Admissible error is only built into trajectory followers. The "user-space" turn implementation is pretty bare bones.<

OK, thank. It sounds like we either need to figure out our feedforward settings by really upping Kstatic or write our own error-based turn code so that PID can close the loop.

[Team14423]

More feedforward tuning didn't help. We did the test of setting Kstatic by trying to set it high enough to move the robot 3 degrees, then tuning the rest. That worked, and we retuned. Kv and Ka were about the same Ka a bit small). We also increase Max Angle_vell and accel. The robot still just turns short and doesn't move short distances. It's off more for large distances. We think the issue is lack of feedback and the time based turn code. Feedforward is just not getting us there.

So, we tried a simple PIDF, trying to compare the current getX() in the TURN state with the current heading, and stopping if it is less than 1 degree. The robot never moves. It turns out, the way that motionstate is grabbed in each cycle grabs small segments (as near as we can tell), so getX is only .02 degrees (we are using radians in the code). It then resets the target position to the small number. Is that how motionprofile.get() is supposed to work? Calculate it in little slices? It would explain why our velocities are so low.

We will try to implement where we just set the target once, and then use the PID controller to calculate speed based on how far we are from the target. I guess my question is whether there's anything built in for that. The HolonomicPIDVAFollower seems like it should, but obviously there's no turn trajectory, so I'm wondering if there's a PID based follower we can use with the basic simple motion profile, or whether we are going to have to write something on our own.

UPDATED: It looks like maybe I can use the MotionState segments to generate the speed profiles, but that I can use the feedback control to continue to completion. It also looks like I can create a special "turning" PID Coefficient set that ups I a bit more.

[rbrott]

Is that how motionprofile.get() is supposed to work? Calculate it in little slices?

Yes, that's intended. The get() method is usually first called shortly after the turn begins with a small time. This small time usually corresponds with an angle close to the starting one. I recommend doing your error check after the profile has elapsed (like the follower admissible error system).

We will try to implement where we just set the target once, and then use the PID controller to calculate speed based on how far we are from the target.

Sure, you can re-plan your trajectory in realtime. There is built-in support insofar as one can generate profiles interpolating between arbitrary motion states.

The HolonomicPIDVAFollower seems like it should

Not really. The user still needs to manage profile regeneration.

It looks like maybe I can use the MotionState segments to generate the speed profiles

Correct.

but that I can use the feedback control to continue to completion

You don't really need feedback at the follower level if you're constantly replanning.

It also looks like I can create a special "turning" PID Coefficient set that ups I a bit more.

Yes, that's a separate approach that requires minimal changes.

[Team14423]

You don't really need feedback at the follower level if you're constantly replanning.

So, the current (untested) untested code is an amalgam. The turn PIDController will set correction based on full distance from target but the base velocity and acceleration will be calculated by the profile. That way, the PIDController error can keep the full error and stop when we get to the desired error (without respect to the smaller segments).

I recommend doing your error check after the profile has elapsed (like the follower admissible error system).

I suppose this is an alternative, but more complex. I'd run the regular turn, and then when t>duration but we are short, we'd have to keep driving. But then how do we do that? Set a new controller target in the PID and then set the motor power to the PID only? We'd have no profile velocity, obviously. I'll look at the admissible error code in the follower to see how that works. Thanks for your continued input - this is driving us bonkers.

[Team14423]

OK, so looking at the follower code, if I just add the admissible error check as an or to the timeout, it will keep running even if the profile duration is over? I had thought that if t>duration then the motion state returned in motionprofile.get(t) would not work.

[rbrott]

motionProfile.get(t) snaps to the endpoints for low/high values t.

[Team14423]

motionProfile.get(t) snaps to the endpoints for low/high values t.

OK, then that sounds doable. Could just change if t>duration to if t>duration&&abs(error)<admissible and it should keep driving until it gets there. We can tune special PID coefficients (including Ki) to make sure that the robot keeps moving if Kstatic isn't getting the job done.

[Team14423]

Here is the modified TURN state - can't test it until tomorrow. Moved the test to the top, as we use manual bulk reads, so currentpose won't update in the same cycle. Hoping that MotionState.end() does what I think it does so we don't have to store another variable.

                double t = clock.seconds() - turnStart;

                MotionState targetState = turnProfile.get(t);

                turnController.setTargetPosition(targetState.getX());

                MotionState endState = turnProfile.end();
                double error = endState.getX() - currentPose.getHeading();
                //&& error <Math.toRadians(1) is new here
                if (t >= turnProfile.duration() && Math.abs(error) <Math.toRadians(1)) {
                    mode = Mode.IDLE;
                    setDriveSignal(new DriveSignal());
                } else {
                    double correction = turnController.update(currentPose.getHeading());
                    double targetOmega = targetState.getV();
                    double targetAlpha = targetState.getA();
                    setDriveSignal(new DriveSignal(new Pose2d(
                            0, 0, targetOmega + correction
                    ), new Pose2d(
                            0, 0, targetAlpha
                    )));

                    Pose2d newPose = lastPoseOnTurn.copy(lastPoseOnTurn.getX(), lastPoseOnTurn.getY(), targetState.getX());

                    fieldOverlay.setStroke("#4CAF50");
                    DashboardUtil.drawRobot(fieldOverlay, newPose);

[Team14423]

OK, FWIW, here is the final TURN state in Sample Mechanum Drive for posterity. Big changes from our draft are a) a time timeout and b) setting a base Velocity after the end of duration (the MotionProfile endstate returns V=0 and A=-4, which didn't work at all). We also created our own Turn PID for the turn controller that could be tweaked separately from HEADING_PID - more Ki .We will likely modify further to allow Turn and TurnAsync to set an admissible error.

        case TURN: {
            double t = clock.seconds() - turnStart;

            MotionState targetState = turnProfile.get(t);

            turnController.setTargetPosition(targetState.getX());

            MotionState endState = turnProfile.end();

            double error = Angle.normDelta(endState.getX() - currentPose.getHeading());
            System.out.println("TURN_Error " + Math.toDegrees(error));
            System.out.println("TURN_Heading " + Math.toDegrees(currentPose.getHeading()));
            System.out.println("TURN_Final Target " + Math.toDegrees(endState.getX()));
            System.out.println("TURN_Int Target " + Math.toDegrees(targetState.getX()));
            System.out.println("TURN_time " + t);
            System.out.println("TURN_duration " + turnProfile.duration());

            //&& error <Math.toRadians(1) is new here
            if (t >= turnProfile.duration() && (Math.abs(error) <Math.toRadians(2)||t>turnProfile.duration()+.5))
            {
                mode = Mode.IDLE;
                setDriveSignal(new DriveSignal());
            } else {
                double correction = turnController.update(currentPose.getHeading());
                double targetOmega;
                double targetAlpha;
                if(t<=turnProfile.duration())
                {
                    targetOmega = targetState.getV();
                    targetAlpha = targetState.getA();
                }
                else
                {
                    targetOmega = Math.signum(correction)*1;
                    targetAlpha = 0;
                }
                System.out.println("TURN_V " + targetOmega);
                System.out.println("TURN_A " + targetAlpha);
                System.out.println("TURN_Correction " + correction);
                System.out.println("TURN_PID error " + Math.toDegrees(turnController.getLastError()));
                System.out.println("TURN_PID target " + Math.toDegrees(turnController.getTargetPosition()));

                setDriveSignal(new DriveSignal(new Pose2d(
                        0, 0, targetOmega + correction
                ), new Pose2d(
                        0, 0, targetAlpha
                )));

            }
                Pose2d newPose = lastPoseOnTurn.copy(lastPoseOnTurn.getX(), lastPoseOnTurn.getY(), targetState.getX());

                fieldOverlay.setStroke("#4CAF50");
                DashboardUtil.drawRobot(fieldOverlay, newPose);

            break;

[rbrott]

the MotionProfile endstate returns V=0 and A=-4

Ah yeah that doesn't really make sense. I'll make a note to fix profile extension in a future release.

[Team14423]

Truth be told, we have played with V (I think we add a minimum value now - similar to Kstatic). I think the PID correction wasn't enough to keep it moving small distances in a reasonable amount of time.

[rbrott]

Little late but profile/trajectory extension is fixed in https://github.com/acmerobotics/road-runner/commit/e9b2d927d43ec99e4931979f598c63f7c8547eba.

[Team14423]

Thanks. We are behind, so will update this before we get going.