转向驱动运动学
``SwerveDriveKinematics’’类是一个有用的工具,可在``ChassisSpeeds’’对象和几个``SwerveModuleState’’对象之间进行转换,该对象包转向驱动机器人的每个速度模块的速度和角度。
备注
Swerve drive kinematics uses a common coordinate system. You may wish to reference the Coordinate System section for details.
转向模块状态类
``SwerveModuleState’’类包含有关转向驱动单个模块的速度和角度的信息。 ``SwerveModuleState’’的构造函数接受两个参数,即车轮在模块上的速度和模块的角度。
备注
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.
备注
角度0对应于面向前方的模块。
构造运动学对象
``SwerveDriveKinematics’’类接受可变数量的构造函数参数,每个参数都是相对于机器人中心的转向模块的位置(作为``Translation2d’’。构造函数参数的数量与转向的数量相对应模块。
备注
转向驱动必须具有2个或更多模块。
备注
在C ++中,该类以模块数量为模板。因此,在将“ SwerveDriveKinematics”对象构造为类的成员变量时,必须将多个模块作为模板参数传入。例如,对于具有四个模块的典型转向驱动,运动学对象必须按以下方式构造:``frc :: SwerveDriveKinematics <4> m_kinematics {…}’’。
模块的位置必须相对于机器人的中心。正的x值表示朝着机器人的前端移动,而正的y值表示朝着机器人的左侧移动。
// 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
)
将底盘速度转换为模块状态
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.
该方法返回的数组中的元素与运动学对象的构造顺序相同。例如,如果运动学对象的构造顺序是前左模块位置、前右模块位置、后左模块位置和后右模块位置,那么数组中的元素将依次是前左模块状态、前右模块状态、后左模块状态和后右模块状态。
// 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)
模块角度优化
``SwerveModuleState’’类包含一个静态``optimize()``(Java)/``Optimize()``(C ++)方法,用于``优化’’给定``SwerveModuleState’’的速度和角度设定值``以最大程度减少航向的变化。例如,如果逆运动学中某个模块的角度设定点为90度,但是您当前的角度为-89度,则此方法将自动抵消模块设定点的速度并使角度设定点为-90度以减小距离模块必须行进。
该方法有两个参数:所需状态(通常从toSwerveModuleStates方法)和当前角度。它将返回新的优化状态,您可以将其用作反馈控制回路中的设定值。
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()))
定向驱动
:ref:`Recall<docs/software/kinematics-and-odometry/intro-and-chassis-speeds:从现场相关速度中创建ChassisSpeeds对象>`可以从一组所需的现场导向速度中创建``ChassisSpeeds``对象。这个功能可以用来从一组所需的定向驱动速度获得模块状态。
// 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)
使用自定义旋转中心
有时,对于某些规避操作,可能需要绕一个特定的角旋转。 WPILib也支持这种类型的行为。相同的ToSwerveSpeeds()方法接受旋转中心的第二个参数(作为Translation2d)。就像轮子的位置一样,代表旋转中心的``Translation2d’’应该相对于机器人中心。
备注
由于所有机器人都是一个刚性框架,因此,从 “底盘速度 “对象中提供的 “vx “和 “vy “速度仍然适用于整个机器人。然而,来自``底盘速度``对象的``omega``将从旋转中心测量。
例如,可以将旋转中心设置在某个轮子上,如果提供的``ChassisSpeeds``对象的``vx``和``vy``为零,而``omega``非零,机器人就会出现围绕该特定的转向模块旋转。
将模块状态转换为底盘速度
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.