La cinématique de l’entraînement de type « à embardée » (Swerve)
N.D.T. (le mot « Swerve » sera utilisé tout le long de cette page, afin d’alléger le texte). La classe SwerveDriveKinematics est un outil utile qui convertit entre un objet ChassisSpeeds et plusieurs objets SwerveModuleState, qui contient les vitesses et les angles pour chaque module Swerve d’un robot avec entraînement de ce type.
Note
Swerve drive kinematics uses a common coordinate system. You may wish to reference the Coordinate System section for details.
La classe d’état du module swerve
La classe SwerveModuleState contient des informations sur la vitesse et l’angle d’un seul module Swerve. Le constructeur d’un SwerveModuleState prend deux arguments, la vitesse de la roue sur le module et l’angle du module.
Note
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.
Note
Un angle de 0 correspond aux modules tournés vers l’avant.
Construction de l’objet cinématique
La classe SwerveDriveKinematics accepte un nombre variable d’arguments constructeur, chaque argument étant l’emplacement d’un module swerve par rapport au centre du robot (sous la forme d’un Translation2d. Le nombre d’arguments constructeur correspond au nombre de modules Swerve.
Note
Un robot doit avoir au moins 2 modules Swerve.
Note
En C ++, la classe est basée sur le nombre de modules. Par conséquent, lors de la construction d’un objet SwerveDriveKinematics en tant que variable membre d’une classe, le nombre de modules doit être transmis en tant qu’argument. Par exemple, pour un entraînement Swerve typique avec quatre modules, l’objet cinématique doit être construit comme suit: frc::SwerveDriveKinematics<4> m_kinematics{...}.
Les emplacements des modules doivent être relatifs au centre du robot. Les valeurs x positives représentent le déplacement vers l’avant du robot tandis que les valeurs y positives représentent le déplacement vers la gauche du robot.
// 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
);
// 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 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
)
Conversion des vitesses de châssis en états de module
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.
Les éléments du tableau renvoyé par cette méthode sont du même ordre dans lequel l’objet cinématique a été construit. Par exemple, si l’objet cinématique a été construit avec l’emplacement du module avant-gauche, l’emplacement du module avant-droit, l’emplacement du module arrière-gauche et l’emplacement du module arrière-droit dans cet ordre, les éléments du tableau seront: état du module avant-gauche, état du module avant-droit, état du module arrière-gauche et état du module arrière-droit (en respectant cet ordre).
// 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];
// 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);
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)
Module d’optimisation de l’angle
La classe SwerveModuleState contient une méthode statique optimize() (Java) / Optimize() (C++) qui est utilisée pour « optimiser » les points de consigne de vitesse et d’angle pour un SwerveModuleState donné afin de minimiser le changement d’orientation angulaire. Par exemple, si le point de consigne angulaire d’un certain module de cinématique inverse est de 90 degrés, mais que votre angle actuel est de -89 degrés, cette méthode annulera automatiquement la vitesse du point de consigne du module et fournira le point de consigne angulaire -90 degrés pour réduire la distance parcourue par le module.
Cette méthode prend deux paramètres : l’état désiré (généralement par l’intermédiaire de la méthode toSwerveModuleStates) et l’angle actuel. Il retournera le nouvel état optimisé que vous pouvez utiliser comme point de consigne dans votre boucle de contrôle de rétroaction.
var frontLeftOptimized = SwerveModuleState.optimize(frontLeft,
new Rotation2d(m_turningEncoder.getDistance()));
auto flOptimized = frc::SwerveModuleState::Optimize(fl,
units::radian_t(m_turningEncoder.GetDistance()));
from wpimath.kinematics import SwerveModuleState
from wpimath.geometry import Rotation2d
frontLeftOptimized = SwerveModuleState.optimize(frontLeft,
Rotation2d(self.m_turningEncoder.getDistance()))
Entraînement orienté terrain
Recall peut être créé à partir d’un ensemble de vitesses orientées en fonction du terrain de jeu. Cette fonction peut être utilisée également pour obtenir les vitesses de roue à partir du même ensemble (de vitesses orientées en fonction du terrain de jeu).
// 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);
// 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);
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)
Utilisation de centres de rotation personnalisés
Parfois, la rotation autour d’un point spécifique peut être souhaitable pour certaines manœuvres d’évitement. Ce type de comportement est également pris en charge par les classes WPILib. La même méthode ToSwerveModuleStates() accepte un deuxième paramètre pour le centre de rotation (comme un Translation2d). Tout comme les emplacements des roues, le Translation2d représentant le centre de rotation doit être relatif au centre du robot.
Note
Étant donné que tous les robots ont un châssis rigide, les vitesses vx et vy fournies par l’objet ChassisSpeeds s’appliqueront toujours à l’intégralité du robot. Cependant, omega de l’objet ChassisSpeeds sera mesuré à partir du centre de rotation.
Par exemple, on peut définir le centre de rotation sur un certain module et si l’objet `ChassisSpeeds fourni a un vx et un vy de zéro et un omega différent de zéro, le robot tournerar autour de ce module Swerve spécifique.
Conversion des états des modules en vitesses de châssis
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.
// 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;
// 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);
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.
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
});
}
}
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
}
);
}
};
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.
