Trajectory Generation in Autonomous: Path Following Issues

eljoaquinino · February 21, 2020, 1:27am

This is our team’s first year using path planning for autonomous, and we’re experiencing a puzzling consistency issue. Our most recent code creates a simple S-curve trajectory, and the robot follows the path pretty well the first time we run the code; however, when we re-enable the robot, it drives straight forward for a couple of meters and then performs the S-curve. We verified the gyro, encoder, and pose readings are reset in autoInit(). We can’t think of any other reason why it would be adding this additional movement. Any ideas would be much appreciated!

Edit: To clarify, the path runs properly every time we re-deploy the code and enable the robot, but not in subsequent enable/disable cycles.

LocPham · February 21, 2020, 1:08am

Try to redeploy it ?

eljoaquinino · February 21, 2020, 1:20am

It runs correctly every time we redeploy it. Sorry, should have clarified.

UnofficialForth · February 21, 2020, 3:18am

Is it possible you can post your code so we can take a look at it?

Off the top of my head, I would guess there is something not “Resetting” after your auto ends. We had to reset a few variables in our code in AutoDisable(). Kind of a shot in the dark, but might help.

eljoaquinino · February 21, 2020, 5:12am

Don’t have it with me right now but we’ll send it first thing tomorrow morning, sorry. And yeah, that’s what we suspect but we just have no idea what. We’ll try and have it do some weird stuff before the trajectory but after enabling to see if it’s something we missed that we have to reset or if it’s something that the ramseteCommand is failing to resetting. Thank you!

chenyx · February 21, 2020, 5:31am

The pose you dump into trajectory generator is relative to the field. Therefore, when you run the trajectory, you should have your robot’s pose the same as the starting pose of the trajectory.

If the trajectory you want to run is relative to the robot, you can either reset your odometry or use the transform convenience methods of the Trajectory class to fit the starting pose of the trajectory onto your robot’s current pose.

mray190 · February 21, 2020, 5:36pm

The key for us (we had this EXACT same issue) was to set the Pose of your odometry module back to your origin in DisabledInit.

We were originally just resetting encoders, but without setting the Pose, our error would be significantly off and the feedback portion of your trajectory command would add unwanted speed to your wheels in the second or third autonomous run

sargeanthost · February 21, 2020, 5:49am

How do you reset the pose?

mray190 · February 21, 2020, 5:36pm

See:
https://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.html#resetPosition(edu.wpi.first.wpilibj.geometry.Pose2d,edu.wpi.first.wpilibj.geometry.Rotation2d)

Example usage from our code:

github.com

ViperbotsValor6800/FRC-6800-2020-code/blob/dev/Competition/src/main/cpp/subsystems/Drivetrain.cpp#L96


    //rightDrive.SetInverted(!rightDrive.GetInverted());
}


void Drivetrain::ResetEncoders() {
    m_leftEncoder.SetPosition(0);
    m_rightEncoder.SetPosition(0);
}


void Drivetrain::ResetOdometry(frc::Pose2d pose) {
    ResetEncoders();
    m_odometry.ResetPosition(pose, frc::Rotation2d(units::degree_t(GetHeading())));
}
void Drivetrain::ResetIMU() {
    imu.Reset();
}


// rev::CANEncoder& GetLeftEncoder() {
//     return m_leftEncoder;
// }


// rev::CANEncoder& GetRightEncoder() {

eljoaquinino · February 21, 2020, 6:19am

We’ll try exactly that and let you know how it works. We figured this wasn’t the issue since we printed the pose coordinates and angles onto the dashboard and saw them resetting, but from the sound of what you described, it might just do - we’re seeing that same unwanted speed. Thank you!

eljoaquinino · February 21, 2020, 6:22am

That’s what we’ll try tomorrow based on @mray190’s code. As I replied to him, we saw the pose coordinates reset on the dashboard, but that’s probably what it is since it seems to be the consensus of the replies thus far. Thank you!

bsimmons · February 21, 2020, 7:12am

We found this out the hard way when our robot would just floor it down the other end of our field, obviously not following our path. Once we updated the starting pose of the path to match the current pose of the robot (or vice versa), it worked flawlessly.

eljoaquinino · February 21, 2020, 5:35pm

@mray190 @bsimmons @chenyx It worked! Now any paths we make in the trajectoryGenerator work consistently. Since it was going too fast and kept losing control, we wanted to use PathWeaver paths to include velocity vectors. However, when we test paths we create in Path Weaver, we see the robot not follow the path (for example, instead of doing a U shape it just rushes forward). Any ideas? The path imported correctly and everything.

chenyx · February 21, 2020, 6:13pm

I am not very familiar with path weaver, but can you check the starting pose of your trajectory and the robot pose when you start following it? They should be pretty much the same

eljoaquinino · February 21, 2020, 6:25pm

Yeah, they’re the same. Think we might have it working, need some more testing.

bsimmons · February 21, 2020, 10:33pm

Does it run forward and vaguely follow a U shape?

Also, it’s my understanding that the control points in pathweaver don’t control velocity, the control the curvature of the path around that point.
If you want to slow the robot down, go into the pathweaver project settings and change your max velocity, that’s what we did.

system · February 20, 2021, 10:33pm

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.