001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory.constraint;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.kinematics.ChassisSpeeds;
009import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
010
011/**
012 * A class that enforces constraints on the swerve drive kinematics. This can be used to ensure that
013 * the trajectory is constructed so that the commanded velocities for all 4 wheels of the drivetrain
014 * stay below a certain limit.
015 */
016public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
017  private final double m_maxSpeed;
018  private final SwerveDriveKinematics m_kinematics;
019
020  /**
021   * Constructs a swerve drive kinematics constraint.
022   *
023   * @param kinematics Swerve drive kinematics.
024   * @param maxSpeed The max speed that a side of the robot can travel at in m/s.
025   */
026  public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics, double maxSpeed) {
027    m_maxSpeed = maxSpeed;
028    m_kinematics = kinematics;
029  }
030
031  /**
032   * Returns the max velocity given the current pose and curvature.
033   *
034   * @param pose The pose at the current point in the trajectory.
035   * @param curvature The curvature at the current point in the trajectory in rad/m.
036   * @param velocity The velocity at the current point in the trajectory before constraints are
037   *     applied in m/s.
038   * @return The absolute maximum velocity.
039   */
040  @Override
041  public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
042    // Represents the velocity of the chassis in the x direction
043    var xdVelocity = velocity * pose.getRotation().getCos();
044
045    // Represents the velocity of the chassis in the y direction
046    var ydVelocity = velocity * pose.getRotation().getSin();
047
048    // Create an object to represent the current chassis speeds.
049    var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
050
051    // Get the wheel speeds and normalize them to within the max velocity.
052    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
053    SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeed);
054
055    // Convert normalized wheel speeds back to chassis speeds
056    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
057
058    // Return the new linear chassis speed.
059    return Math.hypot(normSpeeds.vx, normSpeeds.vy);
060  }
061
062  /**
063   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
064   * curvature, and speed.
065   *
066   * @param pose The pose at the current point in the trajectory.
067   * @param curvature The curvature at the current point in the trajectory in rad/m.
068   * @param velocity The speed at the current point in the trajectory in m/s.
069   * @return The min and max acceleration bounds.
070   */
071  @Override
072  public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
073    return new MinMax();
074  }
075}