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 org.wpilib.math.trajectory.constraint;
006
007import org.wpilib.math.geometry.Pose2d;
008import org.wpilib.math.kinematics.ChassisVelocities;
009import org.wpilib.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_maxVelocity;
018  private final SwerveDriveKinematics m_kinematics;
019
020  /**
021   * Constructs a swerve drive kinematics constraint.
022   *
023   * @param kinematics Swerve drive kinematics.
024   * @param maxVelocity The max velocity that a side of the robot can travel at in m/s.
025   */
026  public SwerveDriveKinematicsConstraint(
027      final SwerveDriveKinematics kinematics, double maxVelocity) {
028    m_maxVelocity = maxVelocity;
029    m_kinematics = kinematics;
030  }
031
032  /**
033   * Returns the max velocity given the current pose and curvature.
034   *
035   * @param pose The pose at the current point in the trajectory.
036   * @param curvature The curvature at the current point in the trajectory in rad/m.
037   * @param velocity The velocity at the current point in the trajectory before constraints are
038   *     applied in m/s.
039   * @return The absolute maximum velocity.
040   */
041  @Override
042  public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
043    // Represents the velocity of the chassis in the x direction
044    var xdVelocity = velocity * pose.getRotation().getCos();
045
046    // Represents the velocity of the chassis in the y direction
047    var ydVelocity = velocity * pose.getRotation().getSin();
048
049    // Create an object to represent the current chassis velocities.
050    var chassisVelocities = new ChassisVelocities(xdVelocity, ydVelocity, velocity * curvature);
051
052    // Get the wheel velocities and normalize them to within the max velocity.
053    var wheelVelocities =
054        SwerveDriveKinematics.desaturateWheelVelocities(
055            m_kinematics.toSwerveModuleVelocities(chassisVelocities), m_maxVelocity);
056
057    // Convert normalized wheel velocities back to chassis velocities
058    var normVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
059
060    // Return the new linear chassis velocity.
061    return Math.hypot(normVelocities.vx, normVelocities.vy);
062  }
063
064  /**
065   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
066   * curvature, and velocity.
067   *
068   * @param pose The pose at the current point in the trajectory.
069   * @param curvature The curvature at the current point in the trajectory in rad/m.
070   * @param velocity The velocity at the current point in the trajectory in m/s.
071   * @return The min and max acceleration bounds.
072   */
073  @Override
074  public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
075    return new MinMax();
076  }
077}