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}