Motion Profile in swerve autonomous mode

Our team is now working on swerve’s auto. And we have run into several problems in motion profiling. We use the basic coordinated point to drive a path and our main auto code is below:

edit: l have found the error, l have mistaken the start point of the second trajectory!

Besides, l am puzzled that whether the end point of one trajectory should be the same as the next trajectory’s start point.

What’s more , how can l visualize these point in the path , Use pathweaver or PathPlanner?

Any answer is appreciated!

Yes, you want the end of one to be the start of another. The start point of the trajectory lets the generator know where it can expect the robot to be at the start of that trajectory. Since your robot should be at the end point of the last trajectory, then they should be the same.

1 Like

Optionally, we used to zero the odometry and encoders at the end of the first trajectory. I wouldn’t suggest this as it led to some error stack-up if you string together a lot of paths.

I definitely wish that there was a mode which ignored the exact start point of a trajectory, but I guess:

  • this makes it hard to pre-compute the trajectory
  • you might be able to fake it by starting the next trajectory a few feet away from the expected “real” start point. I have not tested that idea.

Thanks, l have tested several path respectively. They work very well , but when l combine it together , it doesn’t work as l expect.

For example , l have two trajectories,

    Trajectory firstStraightLine =
        TrajectoryGenerator.generateTrajectory(
            new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)),
            List.of(new Translation2d(1, 0)),
            new Pose2d(2, 0, Rotation2d.fromDegrees(0.0)),
            Constants.AutoConstants.RTNfastConfig);

    Trajectory secondstrafeLine = 
        TrajectoryGenerator.generateTrajectory(            
            new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)),
            List.of(new Translation2d(0, 1)),
            new Pose2d(0, 2, Rotation2d.fromDegrees(0.0)),
            Constants.AutoConstants.RTNfastConfig);

The first will let the robot to drive a straight line, and the second will let the robot to move left. l have tested it seperately and it works well . But when l combine it together, l expect it to drive an “L” line. But the robot won’t move like this . Is there any problem with it?

Both of those paths start from (0,0). When you start the second path after the first, it’s assuming you’re starting from (0,0)-- when it’s actually at (2,0), it starts correcting by going towards x=0.

If you want an arbitrary trajectory start pose, you need to generate a new path or use the Trajectory.transformBy(Transform2d transform) method to shift the path.

In your case, you would do something like

path2 = path2.transformBy(
    new Transform2d(
        path2.getInitialPose(),
        path1.sample(path1.getTotalTimeSeconds()).poseMeters
    )
);

to ensure the second path starts at the same location the first one ends. This way, you could perform path2 after any other trajectory. Keep in mind this also rotates path2 to match the end of path1.

3 Likes

Thanks a lot. But do l need to transform the start pose once l create l new path? That’s isn’t very friendly.

Does it mean that the rotation of the end point of the path should be next path’s start point’s rotation?

The two poses should match exactly.

So , if l want to rotate the robot during the path , should l set different rotation to the robot?

If you want it to rotate in motion, you will set the points along the way to have a different rotation.
For example, if you were trying to make the robot go from 0,0 to 10, 0 with a rotation of 90 degrees clockwise. It would look something like this:
image
With the first way point being Pose2d(0, 0, Rotation2d.fromDegrees(0.0)) and the second way point being Pose2d(10, 0, Rotation2d.fromDegrees(90)))


Going to (10, 0) with a 90 degree rotation
If you were starting from a Pos2d(x, y, Rotation2d.fromDegrees(z)) where x, y, and z are variables, and you want to get to (10, 0) with rotation 90 degrees, then:
First waypoint: Pos2d(x, y, Rotation2d.fromDegrees(z))
Second waypoint: Pos2d(10, 0, Rotation2d.fromDegrees(z + 90))

Notice that since you are just trying to get (10, 0), you can just specify this.


Going Left 10 and Rotation 90 degrees
With the same start point as above, what if you are trying to go left 10 and rotate 90 degrees clockwise? The following would be your waypoints.
First waypoint: Pos2d(x, y, Rotation2d.fromDegrees(z))
Second waypoint: Pos2d(x + 10, y, Rotation2d.fromDegrees(z + 90))

Note that because you are trying to shift your current position 10 units and 90 degrees, you have to add to the original waypoint in order to calculate the new waypoint.

You could also write the second waypoint as:
Second waypoint: Pos2d(x, y, Rotation2d.fromDegrees(z)) + Pos2d(10, 0, Rotation2d.fromDegrees(90))

When l test my path and set the path in this way:

    Trajectory firstStraightLine =
        TrajectoryGenerator.generateTrajectory(
            new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)),
            List.of(new Translation2d(1, 0)),
            new Pose2d(2, 0, Rotation2d.fromDegrees(0.0)),
            Constants.AutoConstants.RTNfastConfig);

l increase the “x” and maintain “y” to let the robot move straight . l think that it should move right or left. What’s the reason for that?

Can x,y be any points .

Besides, just use points to draw a path seems to be too abstract. l don’t know the whole path and is there any tools that can visualize the path for swerve?

X points forward, Y points to the left, and headings are counterclockwise from the positive X axis.

Thanks a lot. But do l need to transform the start pose once l create l new path? That’s isn’t very friendly.

You don’t need to transform the path if you create it with poses that line up with the previous. It looked like you wanted “robot strafes left 2 meters” instead of “robot goes from (0,0) to (0,2)”, which requires you to transform robot-relative movement into field-relative positions. Changing the poses X in the second trajectory to 2 instead of 0 would also make it line up with the first path.

Each trajectory represents some motion from pose A to pose B on the field-- If you create a second trajectory that starts at the same pose the first one ended, you can run it right after the first. When the second trajectory does not start at the end pose of the first (like the two trajectories you posted earlier) the robot has to correct its position since it isn’t where it’s supposed to be.

Using a robot-relative path and transforming it to field-relative allows you to perform it after ANY other path, which looked like what you thought should be happening.

Besides, just use points to draw a path seems to be too abstract. l don’t know the whole path and is there any tools that can visualize the path for swerve?

Yes, using a visualizer makes creating paths much easier. There are multiple visualizers floating around, each with their own problems. PathWeaver is the official WPILib tool, but it exports paths to JSON which must then be loaded from code. This and the trajectory generation don’t accept independent heading values by default while following the path, which requires you to do additional work if that’s something you want(manipulate the robot’s heading separate of the path heading for non-holonomic drivetrains). Other teams have created their own tools/libraries or alternatives like PathPlanner. Keep in mind these tools have their own quirks and you’ll have to learn how to use them. Of course, you could also just use these tools to visualize your paths and then generate a trajectory with those poses manually.

2 Likes

Thanks a lot!
l have tried both methods , however , my robot won’t move a “L” line.(move forward the same distance as moving left).

Trajectory firstStraightLine =
        TrajectoryGenerator.generateTrajectory(
            new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)),
            List.of(new Translation2d(0.25,0),new Translation2d(0.5, 0)),
            new Pose2d(0.75, 0, Rotation2d.fromDegrees(90.0)),
            Constants.AutoConstants.RTNfastConfig);

    Trajectory secondstrafeLine = 
        TrajectoryGenerator.generateTrajectory(
            new Pose2d(0.75, 0, Rotation2d.fromDegrees(90.0)),
            List.of(new Translation2d(0.75, 0.25), new Translation2d(0.75, 0.5)),
            new Pose2d(0.75, 1, Rotation2d.fromDegrees(0.0)),
            Constants.AutoConstants.slowConfig);

With this code, my robot will move forward more and move left less, though l have set y value a little bit bigger in the second path. It seems that l can’t control my robot precisely using these waypoints. Is there anything wrong with my use?

The trajectory generation here assumes you’re using a non-holonomic drivetrain (differential), where changing heading is required to move in more than one dimension. The path that’s generated will follow this rule and curve to account for changes in rotation. Since there’s no way for a non-holonomic drivetrain to move sideways, the path waypoints you gave above will result in an odd curve that would allow the robot to finish the path at the 90 degree rotation(it would also be smoother with only the start and end pose in this case).

If you want to control a swerve drive to follow a trajectory with independent heading, you can make use of the holonomic drive controller to supply your own heading targets instead of the ones implicit to the generated path. To make the trajectory an “L” and not curved, you would avoid rotating each trajectory against the direction of motion (the first would always be 0 degrees and the second would always be 90 degrees). The two paths don’t have to have matching start and end rotations in this case since you only care about the x/y of the path. You can make this a bit nicer to write by creating some wrapper to accept independent headings during trajectory generation and then use those when following.

2 Likes

Thanks that , so the rotation in the path doesn’t mean anything in swerve? l use a swervecontrollercommand to let the robot follow the trajectory. However , l also have another question, when l generate a trajectory , is there something l need to characterize it with my robot?

so the rotation in the path doesn’t mean anything in swerve?

It’s more that a swerve drive doesn’t have to use the path’s heading in order to follow the path since it can move sideways. You can have the swerve face whatever direction you want and still move along the path, which a differential drive cannot do. If you follow the trajectory heading it will look similar to a differential drive.

is there something l need to characterize it with my robot?

You should characterize your drivetrain so you can accurately control its motion. To follow a generated trajectory, your robot will be receiving some desired chassis speed at any point that follows the path and accounts for disturbances. Your kinematics turns this chassis speed into individual module states(velocity, rotation) for the swerve drive. Characterizing your drivetrain ensures you have a feedforward model that accurately determines how much input(voltage) is required to achieve any desired module state. Basically, you should know what inputs need to be applied to move the robot the way you want it to, instead of only reacting when the robot is off the path and tries to compensate.

Sysid is the tool that replaces frc-characterization for 2022. You can characterize the rotation of each swerve module by simply turning it in place, but linear motion is more difficult as you need the modules to be locked at 0 degrees. With frc-characterization you can modify the project to attempt holding the modules rotation at 0, or place some kind of jig in the module that prevents rotation. Then, it’s similar to characterizing a differential drivetrain.

Thanks , l notice that there is something different in SwerveControllerCommand

In the last 3 line, it has a line of Rotation2d.fromDrgrees(0) . It seems that l can control the heading of swerve in this way.

How can l know the correct input ?

So, if l have finished the characterization , it seems that l will get a few of feedforward parameters . But , l don’t use the feedforward in my teleop or auto? So , is the characterization still necessary now?

How can l know the correct input ?

Characterization results in feedforward gains to use on the mechanism.

In the last 3 line, it has a line of Rotation2d.fromDrgrees(0) . It seems that l can control the heading of swerve in this way.

Yes, that is what I meant. You can see how controlling this independently from the path can be tedious, since you have to supply it yourself separate from the trajectory.

But , l don’t use the feedforward in my teleop or auto?

Feedforward should be used when you are commanding your swerve modules to some desired state. Your s_Swerve::SetModuleStates consumer should be utilizing feedforward for rotation and velocity. You can use this when following a trajectory with the chassis speeds supplied by the swerve controller, but you can also simply control your robot through chassis speeds in teleop-- field relative control would be something like left stick x/y → field x/y velocity, right stick x → robot heading velocity.

When controlling your mechanisms toward a target state, feedforward is generally more important than PID. You want both, but feedforward should do the majority of the effort.

So, l need two feedforward . Can l get these two feedforward from the sysid or l also need the frc-characterization tool?

But , if l add a feedforward , it seems that l also need to use the feedforward in the teleop period. Will it bring some negative effects.