Seeking Examples for Applying Trajectory Constraints to WPI Trajectory

[dormouss]

Hello! We’re using a Swerve Drive this year, and in conjunction, we’re using the WPI-provided Trajectory methods to generate our Trajectories for autonomous.

The trajectory generation code we’re using is like this:

private static TrajectoryConfig trajectoryConfiguration = new TrajectoryConfig(Constants.SwerveDriveModuleConstants.k_MaxSpeed, 1)
                                                                .setKinematics(Constants.SwerveDriveModuleConstants.kinematics);

private static Pose2d start = new Pose2d(1.016, 2.286, new Rotation2d(0));

private static  Translation2d point1 = new  Translation2d(2.032, 2.54);
private static  Translation2d point2 = new  Translation2d(2.35, 3.81);
    <snip>
private static  Translation2d point13 = new  Translation2d(7.11, 1.91);

private static Pose2d end = new Pose2d(8.4, 1.8, new Rotation2d(0));

return TrajectoryGenerator.generateTrajectory(
        start, List.of(point1, point2, <snip>, point13), end,
        trajectoryConfiguration);

This provides us a basic Trajectory and we’re using it to some success on our Swerve drive. However, we’ve encountered some issues that make me wonder if we need to apply some contraints at various points in the Trajectory. I see on the WPI docs (Trajectory Constraints — FIRST Robotics Competition documentation) that contraints can be applied to the Trajectory, including SwerveDriveKinematicsConstraint. The documentation says, “For example, a custom constraint can keep the velocity of the trajectory under a certain threshold in a certain region or slow down the robot near turns for stability purposes.” (emphasis mine).

The documentation says we can apply a constraint to a region, but I don’t understand how to apply to these constraints to that region. The Trajectory generator uses types of Pose2d and a List<> of type Translation2d, and a configuration object of type TrajectoryConfig. There’s no obvious type-match to the Constraints type. I’ve tried to append a SwerveDriveKinematicsConstraint to a Translation2d point (e.g., new Translation2d(2.032, 2.54).SwerveDriveKinematicsConstraint(Constants.SwerveDriveModuleConstants.kinematics, 2.0) but this is a syntax error.

I’m looking for examples of how to apply the Contraints to our Trajectory.

Thanks!

[Peter_Johnson]

The functions to add constraints are functions on TrajectoryConfig, for example TrajectoryConfig.addConstraint().

You apply them to regions using RectangularRegionConstraint.

So you could do something like:

trajectoryConfiguration.addConstraint(
  new RectangularRegionConstraint(
    new Translation2d(0, 0),   // bottomLeftPoint
    new Translation2d(2, 2),   // topRightPoint
    new SwerveDriveKinematicsConstraint(
      Constants.SwerveDriveModuleConstants.kinematics, 2.0)));