Swerve Drive Kinematics

La clase SwerveDriveKinematics es una herramienta útil que convierte entre un objeto ChassisSpeeds a varios objetos SwerveModuleState , los cuales contienen velocidades y ángulos para cada módulo swerve de un robot.

Nota

Swerve drive kinematics uses a common coordinate system. You may wish to reference the Coordinate System section for details.

Clase swerve module state

La clase SwerveModuleState contiene información de la velocidad y ángulo de un modo singular de swerve drive. El constructor para una SwerveModuleState toma dos argumentos, la velocidad de la llanta en el módulo, y el ángulo del módulo.

Nota

In Java / Python, the velocity of the wheel must be in meters per second. In C++, the units library can be used to provide the velocity using any linear velocity unit.

Nota

Un ángulo de 0 corresponde a los módulos orientados hacía adelante.

Construyendo los objetos cinemáticos.

La clase SwerveDriveKinematics acepta un número variable de argumentos del constructor, con cada argumento siendo la ubicación de un módulo swerve relativo al centro del robot (así como Translation2d. El número de argumentos del constructor corresponde al número de módulos swerve.

Nota

Un swerve drive tiene que tener 2 módulos o más.

Nota

En C++, la clase se planifica en función del número de módulos. Por lo tanto, al construir un objeto SwerveDriveKinematics como un miembro variable de una clase, el número de módulos debe ser pasado como un argumento de plantilla. Por ejemplo, para un swerve drive común con cuatro módulos, el objeto cinemático debe ser construido como: frc::SwerveDriveKinematics<4>m_kinematics{...}.

Las ubicaciones de los módulos deben ser relativas con el centro del robot. Los valores positivos x representan el movimiento hacía la parte delantera del robot, mientras que los calores positivos representan el movimiento del robot hacía la izquierda.

JAVA

// Locations for the swerve drive modules relative to the robot center.
Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
  m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);

C++

// Locations for the swerve drive modules relative to the robot center.
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};

// Creating my kinematics object using the module locations.
frc::SwerveDriveKinematics<4> m_kinematics{
  m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
  m_backRightLocation};

PYTHON

# Python requires using the right class for the number of modules you have

from wpimath.geometry import Translation2d
from wpimath.kinematics import SwerveDrive4Kinematics

# Locations for the swerve drive modules relative to the robot center.
frontLeftLocation = Translation2d(0.381, 0.381)
frontRightLocation = Translation2d(0.381, -0.381)
backLeftLocation = Translation2d(-0.381, 0.381)
backRightLocation = Translation2d(-0.381, -0.381)

# Creating my kinematics object using the module locations
self.kinematics = SwerveDrive4Kinematics(
  frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation
)

Convertir las velocidades del chasis a estados de los módulos

The toSwerveModuleStates(ChassisSpeeds speeds) (Java / Python) / ToSwerveModuleStates(ChassisSpeeds speeds) (C++) method should be used to convert a ChassisSpeeds object to a an array of SwerveModuleState objects. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual module states.

Los elementos en la matriz que se devuelven por este método son el mismo orden en el cual se construyó el objeto cinemático. Por ejemplo, si el objeto cinemático se construyó con la ubicación del módulo frontal izquierdo, la ubicación del módulo frontal derecho, la ubicación del módulo posterior izquierdo y la ubicación del módulo posterior derecho en ese orden, los elementos de la matriz serían el estado del módulo frontal izquierdo, el estado del módulo frontal derecho, el estado del módulo posterior izquierdo y el estado del módulo posterior derecho en ese orden.

JAVA

// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to module states
SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds);

// Front left module state
SwerveModuleState frontLeft = moduleStates[0];

// Front right module state
SwerveModuleState frontRight = moduleStates[1];

// Back left module state
SwerveModuleState backLeft = moduleStates[2];

// Back right module state
SwerveModuleState backRight = moduleStates[3];

C++

// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
frc::ChassisSpeeds speeds{1_mps, 3_mps, 1.5_rad_per_s};

// Convert to module states. Here, we can use C++17's structured
// bindings feature to automatically split up the array into its
// individual SwerveModuleState components.
auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(speeds);

PYTHON

from wpimath.kinematics import ChassisSpeeds

# Example chassis speeds: 1 meter per second forward, 3 meters
# per second to the left, and rotation at 1.5 radians per second
# counterclockwise.
speeds = ChassisSpeeds(1.0, 3.0, 1.5)

# Convert to module states
frontLeft, frontRight, backLeft, backRight = self.kinematics.toSwerveModuleStates(speeds)

Optimización del ángulo del módulo

La clase SwerveModuleState contiene un método estático optimize() (Java) / Optimize() (C++) que se utiliza para «optimizar» la velocidad y el punto de ajuste del ángulo de un determinado SwerveModuleState para minimizar el cambio de rumbo. Por ejemplo, si el punto de ajuste angular de un cierto módulo de cinemática inversa es de 90 grados, pero su ángulo actual es de -89 grados, este método negará automáticamente la velocidad del punto de ajuste del módulo y hará que el punto de ajuste angular sea de -90 grados para reducir la distancia. el módulo tiene que viajar.

Este método toma dos parámetros: el estado deseado (generalmente del método toSwerveModuleStates) y el ángulo actual. Devolverá el nuevo estado optimizado que puede usar como punto de ajuste en su circuito de control de retroalimentación.

JAVA

var frontLeftOptimized = SwerveModuleState.optimize(frontLeft,
   new Rotation2d(m_turningEncoder.getDistance()));

C++

auto flOptimized = frc::SwerveModuleState::Optimize(fl,
   units::radian_t(m_turningEncoder.GetDistance()));

PYTHON

from wpimath.kinematics import SwerveModuleState
from wpimath.geometry import Rotation2d

frontLeftOptimized = SwerveModuleState.optimize(frontLeft,
   Rotation2d(self.m_turningEncoder.getDistance()))

Field-oriented drive

Recuerde que un objeto ChassisSpeeds puede crearse a partir de un conjunto de velocidades deseadas orientadas al campo. Esta característica puede ser utilizada para obtener estados de módulo a partir de un conjunto de velocidades deseadas orientadas al campo.

JAVA

// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise. The current
// robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0));

// Now use this in our kinematics
SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds);

C++

// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise. The current
// robot angle is 45 degrees.
frc::ChassisSpeeds speeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
  2_mps, 2_mps, units::radians_per_second_t(std::numbers::pi / 2.0), Rotation2d(45_deg));

// Now use this in our kinematics
auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(speeds);

PYTHON

from wpimath.kinematics import ChassisSpeeds
import math
from wpimath.geometry import Rotation2d

# The desired field relative speed here is 2 meters per second
# toward the opponent's alliance station wall, and 2 meters per
# second toward the left field boundary. The desired rotation
# is a quarter of a rotation per second counterclockwise. The current
# robot angle is 45 degrees.
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, math.pi / 2.0, Rotation2d.fromDegrees(45.0))

# Now use this in our kinematics
self.moduleStates = self.kinematics.toSwerveModuleStates(speeds)

Usando centros de rotación personalizados

En ocasiones, girar alrededor de una esquina específica puede ser deseable para ciertas maniobras evasivas. Este tipo de comportamiento también es apoyado por las clases de WPILib. El mismo método ToSwerveModuleStates() acepta un segundo parámetro para el centro de rotación (así como``Translation2d``). Justo como la ubicación de las llantas, la Translation2d representa que el centro de rotación debe ser relativo al centro del robot.

Nota

Debido a que todos los robots son un marco rígido, las velocidades vx y vy del objeto ChassisSpeeds aplican a la totalidad del robot. Sin embargo, el omega del objeto ChassisSpeeds se medirá desde el centro de rotación.

Por ejemplo, uno puede establecer el centro de rotación en un cierto módulo y si el objeto ChassisSpeeds tiene vx y vy de cero y un omega que no sea cero, el robot parecerá rotar alrededor de ese particular módulo swerve.

Convirtiendo estados de módulo a velocidades del chasis

One can also use the kinematics object to convert an array of SwerveModuleState objects to a singular ChassisSpeeds object. The toChassisSpeeds(SwerveModuleState... states) (Java / Python) / ToChassisSpeeds(SwerveModuleState... states) (C++) method can be used to achieve this.

JAVA

// Example module states
var frontLeftState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
var frontRightState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
var backLeftState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
var backRightState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(
  frontLeftState, frontRightState, backLeftState, backRightState);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;

C++

// Example module States
frc::SwerveModuleState frontLeftState{23.43_mps, Rotation2d(-140.19_deg)};
frc::SwerveModuleState frontRightState{23.43_mps, Rotation2d(-39.81_deg)};
frc::SwerveModuleState backLeftState{54.08_mps, Rotation2d(-109.44_deg)};
frc::SwerveModuleState backRightState{54.08_mps, Rotation2d(-70.56_deg)};

// Convert to chassis speeds. Here, we can use C++17's structured bindings
// feature to automatically break up the ChassisSpeeds struct into its
// three components.
auto [forward, sideways, angular] = kinematics.ToChassisSpeeds(
  frontLeftState, frontRightState, backLeftState, backRightState);

PYTHON

from wpimath.kinematics import SwerveModuleState
from wpimath.geometry import Rotation2d

# Example module states
frontLeftState = SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19))
frontRightState = SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81))
backLeftState = SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44))
backRightState = SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))

# Convert to chassis speeds
chassisSpeeds = self.kinematics.toChassisSpeeds(
  frontLeftState, frontRightState, backLeftState, backRightState)

# Getting individual speeds
forward = chassisSpeeds.vx
sideways = chassisSpeeds.vy
angular = chassisSpeeds.omega

Module state visualization with AdvantageScope

By recording a set of swerve module states using NetworkTables or WPILib data logs, AdvantageScope can be used to visualize the state of a swerve drive. The code below shows how a set of SwerveModuleState objects can be published to NetworkTables.

JAVA

public class Example {
  private final StructArrayPublisher<SwerveModuleState> publisher;

  public Example() {
    // Start publishing an array of module states with the "/SwerveStates" key
    publisher = NetworkTableInstance.getDefault()
      .getStructArrayTopic("/SwerveStates", SwerveModuleState.struct).publish();
  }

  public void periodic() {
    // Periodically send a set of module states
    publisher.set(new SwerveModuleState[] {
      frontLeftState,
      frontRightState,
      backLeftState,
      backRightState
    });
  }
}

C++

class Example {
  nt::StructArrayPublisher<frc::SwerveModuleState> publisher

 public:
  Example() {
    // Start publishing an array of module states with the "/SwerveStates" key
    publisher = nt::NetworkTableInstance::GetDefault()
      .GetStructArrayTopic<frc::SwerveModuleState>("/SwerveStates").Publish();
  }

  void Periodic() {
    // Periodically send a set of module states
    swervePublisher.Set(
      std::vector{
        frontLeftState,
        frontRightState,
        backLeftState,
        backRightState
      }
    );
  }
};

PYTHON

import ntcore
from wpimath.kinematics import SwerveModuleState

# get the default instance of NetworkTables
nt = ntcore.NetworkTableInstance.getDefault()

# Start publishing an array of module states with the "/SwerveStates" key
topic = nt.getStructArrayTopic("/SwerveStates", SwerveModuleState)
self.pub = topic.publish()

def periodic(self):
  # Periodically send a set of module states
  self.pub.set([frontLeftState,frontRightState,backLeftState,backRightState])

See the documentation for the swerve tab for more details on visualizing this data using AdvantageScope.

Screenshot of an AdvantageScope window displaying a swerve visualization.