Class SwerveDriveKinematicsConstraint

java.lang.Object
edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint
All Implemented Interfaces:
TrajectoryConstraint

A class that enforces constraints on the swerve drive kinematics. This can be used to ensure that the trajectory is constructed so that the commanded velocities for all 4 wheels of the drivetrain stay below a certain limit.
  • Constructor Details

    • SwerveDriveKinematicsConstraint

      public SwerveDriveKinematicsConstraint(SwerveDriveKinematics kinematics, double maxSpeed)
      Constructs a swerve drive kinematics constraint.
      Parameters:
      kinematics - Swerve drive kinematics.
      maxSpeed - The max speed that a side of the robot can travel at in m/s.
  • Method Details

    • getMaxVelocity

      public double getMaxVelocity(Pose2d pose, double curvature, double velocity)
      Returns the max velocity given the current pose and curvature.
      Specified by:
      getMaxVelocity in interface TrajectoryConstraint
      Parameters:
      pose - The pose at the current point in the trajectory.
      curvature - The curvature at the current point in the trajectory in rad/m.
      velocity - The velocity at the current point in the trajectory before constraints are applied in m/s.
      Returns:
      The absolute maximum velocity.
    • getMinMaxAcceleration

      public TrajectoryConstraint.MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity)
      Returns the minimum and maximum allowable acceleration for the trajectory given pose, curvature, and speed.
      Specified by:
      getMinMaxAcceleration in interface TrajectoryConstraint
      Parameters:
      pose - The pose at the current point in the trajectory.
      curvature - The curvature at the current point in the trajectory in rad/m.
      velocity - The speed at the current point in the trajectory in m/s.
      Returns:
      The min and max acceleration bounds.