Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
wpimath/robotpy_pybind_build_info.bzl generated
View File

@@ -558,13 +558,13 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
],
),
struct(
class_name = "ChassisSpeeds",
yml_file = "semiwrap/ChassisSpeeds.yml",
class_name = "ChassisVelocities",
yml_file = "semiwrap/ChassisVelocities.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/ChassisSpeeds.hpp",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/ChassisVelocities.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::math::ChassisSpeeds", "wpi__math__ChassisSpeeds.hpp"),
("wpi::math::ChassisVelocities", "wpi__math__ChassisVelocities.hpp"),
],
),
struct(
@@ -618,13 +618,13 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
],
),
struct(
class_name = "DifferentialDriveWheelSpeeds",
yml_file = "semiwrap/DifferentialDriveWheelSpeeds.yml",
class_name = "DifferentialDriveWheelVelocities",
yml_file = "semiwrap/DifferentialDriveWheelVelocities.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::math::DifferentialDriveWheelSpeeds", "wpi__math__DifferentialDriveWheelSpeeds.hpp"),
("wpi::math::DifferentialDriveWheelVelocities", "wpi__math__DifferentialDriveWheelVelocities.hpp"),
],
),
struct(
@@ -695,13 +695,13 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
],
),
struct(
class_name = "MecanumDriveWheelSpeeds",
yml_file = "semiwrap/MecanumDriveWheelSpeeds.yml",
class_name = "MecanumDriveWheelVelocities",
yml_file = "semiwrap/MecanumDriveWheelVelocities.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::math::MecanumDriveWheelSpeeds", "wpi__math__MecanumDriveWheelSpeeds.hpp"),
("wpi::math::MecanumDriveWheelVelocities", "wpi__math__MecanumDriveWheelVelocities.hpp"),
],
),
struct(
@@ -804,13 +804,13 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
],
),
struct(
class_name = "SwerveModuleState",
yml_file = "semiwrap/SwerveModuleState.yml",
class_name = "SwerveModuleVelocity",
yml_file = "semiwrap/SwerveModuleVelocity.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/SwerveModuleState.hpp",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/SwerveModuleVelocity.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::math::SwerveModuleState", "wpi__math__SwerveModuleState.hpp"),
("wpi::math::SwerveModuleVelocity", "wpi__math__SwerveModuleVelocity.hpp"),
],
),
struct(

70
wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufChassisSpeeds.java → wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufChassisVelocities.java generated
View File

@@ -17,10 +17,10 @@ import us.hebi.quickbuf.ProtoSource;
import us.hebi.quickbuf.ProtoUtil;
/**
* Protobuf type {@code ProtobufChassisSpeeds}
* Protobuf type {@code ProtobufChassisVelocities}
*/
@SuppressWarnings("hiding")
public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpeeds> implements Cloneable {
public final class ProtobufChassisVelocities extends ProtoMessage<ProtobufChassisVelocities> implements Cloneable {
private static final long serialVersionUID = 0L;
/**
@@ -38,14 +38,14 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
*/
private double omega;
private ProtobufChassisSpeeds() {
private ProtobufChassisVelocities() {
}
/**
* @return a new empty instance of {@code ProtobufChassisSpeeds}
* @return a new empty instance of {@code ProtobufChassisVelocities}
*/
public static ProtobufChassisSpeeds newInstance() {
return new ProtobufChassisSpeeds();
public static ProtobufChassisVelocities newInstance() {
return new ProtobufChassisVelocities();
}
/**
@@ -60,7 +60,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
* <code>optional double vx = 1;</code>
* @return this
*/
public ProtobufChassisSpeeds clearVx() {
public ProtobufChassisVelocities clearVx() {
bitField0_ &= ~0x00000001;
vx = 0D;
return this;
@@ -79,7 +79,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
* @param value the vx to set
* @return this
*/
public ProtobufChassisSpeeds setVx(final double value) {
public ProtobufChassisVelocities setVx(final double value) {
bitField0_ |= 0x00000001;
vx = value;
return this;
@@ -97,7 +97,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
* <code>optional double vy = 2;</code>
* @return this
*/
public ProtobufChassisSpeeds clearVy() {
public ProtobufChassisVelocities clearVy() {
bitField0_ &= ~0x00000002;
vy = 0D;
return this;
@@ -116,7 +116,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
* @param value the vy to set
* @return this
*/
public ProtobufChassisSpeeds setVy(final double value) {
public ProtobufChassisVelocities setVy(final double value) {
bitField0_ |= 0x00000002;
vy = value;
return this;
@@ -134,7 +134,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
* <code>optional double omega = 3;</code>
* @return this
*/
public ProtobufChassisSpeeds clearOmega() {
public ProtobufChassisVelocities clearOmega() {
bitField0_ &= ~0x00000004;
omega = 0D;
return this;
@@ -153,14 +153,14 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
* @param value the omega to set
* @return this
*/
public ProtobufChassisSpeeds setOmega(final double value) {
public ProtobufChassisVelocities setOmega(final double value) {
bitField0_ |= 0x00000004;
omega = value;
return this;
}
@Override
public ProtobufChassisSpeeds copyFrom(final ProtobufChassisSpeeds other) {
public ProtobufChassisVelocities copyFrom(final ProtobufChassisVelocities other) {
cachedSize = other.cachedSize;
if ((bitField0_ | other.bitField0_) != 0) {
bitField0_ = other.bitField0_;
@@ -172,7 +172,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
}
@Override
public ProtobufChassisSpeeds mergeFrom(final ProtobufChassisSpeeds other) {
public ProtobufChassisVelocities mergeFrom(final ProtobufChassisVelocities other) {
if (other.isEmpty()) {
return this;
}
@@ -190,7 +190,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
}
@Override
public ProtobufChassisSpeeds clear() {
public ProtobufChassisVelocities clear() {
if (isEmpty()) {
return this;
}
@@ -203,7 +203,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
}
@Override
public ProtobufChassisSpeeds clearQuick() {
public ProtobufChassisVelocities clearQuick() {
if (isEmpty()) {
return this;
}
@@ -217,10 +217,10 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
if (o == this) {
return true;
}
if (!(o instanceof ProtobufChassisSpeeds)) {
if (!(o instanceof ProtobufChassisVelocities)) {
return false;
}
ProtobufChassisSpeeds other = (ProtobufChassisSpeeds) o;
ProtobufChassisVelocities other = (ProtobufChassisVelocities) o;
return bitField0_ == other.bitField0_
&& (!hasVx() || ProtoUtil.isEqual(vx, other.vx))
&& (!hasVy() || ProtoUtil.isEqual(vy, other.vy))
@@ -260,7 +260,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
@Override
@SuppressWarnings("fallthrough")
public ProtobufChassisSpeeds mergeFrom(final ProtoSource input) throws IOException {
public ProtobufChassisVelocities mergeFrom(final ProtoSource input) throws IOException {
// Enabled Fall-Through Optimization (QuickBuffers)
int tag = input.readTag();
while (true) {
@@ -322,7 +322,7 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
}
@Override
public ProtobufChassisSpeeds mergeFrom(final JsonSource input) throws IOException {
public ProtobufChassisVelocities mergeFrom(final JsonSource input) throws IOException {
if (!input.beginObject()) {
return this;
}
@@ -372,8 +372,8 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
}
@Override
public ProtobufChassisSpeeds clone() {
return new ProtobufChassisSpeeds().copyFrom(this);
public ProtobufChassisVelocities clone() {
return new ProtobufChassisVelocities().copyFrom(this);
}
@Override
@@ -381,39 +381,39 @@ public final class ProtobufChassisSpeeds extends ProtoMessage<ProtobufChassisSpe
return ((bitField0_) == 0);
}
public static ProtobufChassisSpeeds parseFrom(final byte[] data) throws
public static ProtobufChassisVelocities parseFrom(final byte[] data) throws
InvalidProtocolBufferException {
return ProtoMessage.mergeFrom(new ProtobufChassisSpeeds(), data).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufChassisVelocities(), data).checkInitialized();
}
public static ProtobufChassisSpeeds parseFrom(final ProtoSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufChassisSpeeds(), input).checkInitialized();
public static ProtobufChassisVelocities parseFrom(final ProtoSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufChassisVelocities(), input).checkInitialized();
}
public static ProtobufChassisSpeeds parseFrom(final JsonSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufChassisSpeeds(), input).checkInitialized();
public static ProtobufChassisVelocities parseFrom(final JsonSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufChassisVelocities(), input).checkInitialized();
}
/**
* @return factory for creating ProtobufChassisSpeeds messages
* @return factory for creating ProtobufChassisVelocities messages
*/
public static MessageFactory<ProtobufChassisSpeeds> getFactory() {
return ProtobufChassisSpeedsFactory.INSTANCE;
public static MessageFactory<ProtobufChassisVelocities> getFactory() {
return ProtobufChassisVelocitiesFactory.INSTANCE;
}
/**
* @return this type's descriptor.
*/
public static Descriptors.Descriptor getDescriptor() {
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufChassisSpeeds_descriptor;
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufChassisVelocities_descriptor;
}
private enum ProtobufChassisSpeedsFactory implements MessageFactory<ProtobufChassisSpeeds> {
private enum ProtobufChassisVelocitiesFactory implements MessageFactory<ProtobufChassisVelocities> {
INSTANCE;
@Override
public ProtobufChassisSpeeds create() {
return ProtobufChassisSpeeds.newInstance();
public ProtobufChassisVelocities create() {
return ProtobufChassisVelocities.newInstance();
}
}

71
wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufDifferentialDriveWheelSpeeds.java → wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufDifferentialDriveWheelVelocities.java generated
View File

@@ -17,10 +17,10 @@ import us.hebi.quickbuf.ProtoSource;
import us.hebi.quickbuf.ProtoUtil;
/**
* Protobuf type {@code ProtobufDifferentialDriveWheelSpeeds}
* Protobuf type {@code ProtobufDifferentialDriveWheelVelocities}
*/
@SuppressWarnings("hiding")
public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<ProtobufDifferentialDriveWheelSpeeds> implements Cloneable {
public final class ProtobufDifferentialDriveWheelVelocities extends ProtoMessage<ProtobufDifferentialDriveWheelVelocities> implements Cloneable {
private static final long serialVersionUID = 0L;
/**
@@ -33,14 +33,14 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
*/
private double right;
private ProtobufDifferentialDriveWheelSpeeds() {
private ProtobufDifferentialDriveWheelVelocities() {
}
/**
* @return a new empty instance of {@code ProtobufDifferentialDriveWheelSpeeds}
* @return a new empty instance of {@code ProtobufDifferentialDriveWheelVelocities}
*/
public static ProtobufDifferentialDriveWheelSpeeds newInstance() {
return new ProtobufDifferentialDriveWheelSpeeds();
public static ProtobufDifferentialDriveWheelVelocities newInstance() {
return new ProtobufDifferentialDriveWheelVelocities();
}
/**
@@ -55,7 +55,7 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
* <code>optional double left = 1;</code>
* @return this
*/
public ProtobufDifferentialDriveWheelSpeeds clearLeft() {
public ProtobufDifferentialDriveWheelVelocities clearLeft() {
bitField0_ &= ~0x00000001;
left = 0D;
return this;
@@ -74,7 +74,7 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
* @param value the left to set
* @return this
*/
public ProtobufDifferentialDriveWheelSpeeds setLeft(final double value) {
public ProtobufDifferentialDriveWheelVelocities setLeft(final double value) {
bitField0_ |= 0x00000001;
left = value;
return this;
@@ -92,7 +92,7 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
* <code>optional double right = 2;</code>
* @return this
*/
public ProtobufDifferentialDriveWheelSpeeds clearRight() {
public ProtobufDifferentialDriveWheelVelocities clearRight() {
bitField0_ &= ~0x00000002;
right = 0D;
return this;
@@ -111,15 +111,15 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
* @param value the right to set
* @return this
*/
public ProtobufDifferentialDriveWheelSpeeds setRight(final double value) {
public ProtobufDifferentialDriveWheelVelocities setRight(final double value) {
bitField0_ |= 0x00000002;
right = value;
return this;
}
@Override
public ProtobufDifferentialDriveWheelSpeeds copyFrom(
final ProtobufDifferentialDriveWheelSpeeds other) {
public ProtobufDifferentialDriveWheelVelocities copyFrom(
final ProtobufDifferentialDriveWheelVelocities other) {
cachedSize = other.cachedSize;
if ((bitField0_ | other.bitField0_) != 0) {
bitField0_ = other.bitField0_;
@@ -130,8 +130,8 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
}
@Override
public ProtobufDifferentialDriveWheelSpeeds mergeFrom(
final ProtobufDifferentialDriveWheelSpeeds other) {
public ProtobufDifferentialDriveWheelVelocities mergeFrom(
final ProtobufDifferentialDriveWheelVelocities other) {
if (other.isEmpty()) {
return this;
}
@@ -146,7 +146,7 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
}
@Override
public ProtobufDifferentialDriveWheelSpeeds clear() {
public ProtobufDifferentialDriveWheelVelocities clear() {
if (isEmpty()) {
return this;
}
@@ -158,7 +158,7 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
}
@Override
public ProtobufDifferentialDriveWheelSpeeds clearQuick() {
public ProtobufDifferentialDriveWheelVelocities clearQuick() {
if (isEmpty()) {
return this;
}
@@ -172,10 +172,10 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
if (o == this) {
return true;
}
if (!(o instanceof ProtobufDifferentialDriveWheelSpeeds)) {
if (!(o instanceof ProtobufDifferentialDriveWheelVelocities)) {
return false;
}
ProtobufDifferentialDriveWheelSpeeds other = (ProtobufDifferentialDriveWheelSpeeds) o;
ProtobufDifferentialDriveWheelVelocities other = (ProtobufDifferentialDriveWheelVelocities) o;
return bitField0_ == other.bitField0_
&& (!hasLeft() || ProtoUtil.isEqual(left, other.left))
&& (!hasRight() || ProtoUtil.isEqual(right, other.right));
@@ -207,7 +207,7 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
@Override
@SuppressWarnings("fallthrough")
public ProtobufDifferentialDriveWheelSpeeds mergeFrom(final ProtoSource input) throws
public ProtobufDifferentialDriveWheelVelocities mergeFrom(final ProtoSource input) throws
IOException {
// Enabled Fall-Through Optimization (QuickBuffers)
int tag = input.readTag();
@@ -258,7 +258,8 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
}
@Override
public ProtobufDifferentialDriveWheelSpeeds mergeFrom(final JsonSource input) throws IOException {
public ProtobufDifferentialDriveWheelVelocities mergeFrom(final JsonSource input) throws
IOException {
if (!input.beginObject()) {
return this;
}
@@ -297,8 +298,8 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
}
@Override
public ProtobufDifferentialDriveWheelSpeeds clone() {
return new ProtobufDifferentialDriveWheelSpeeds().copyFrom(this);
public ProtobufDifferentialDriveWheelVelocities clone() {
return new ProtobufDifferentialDriveWheelVelocities().copyFrom(this);
}
@Override
@@ -306,41 +307,41 @@ public final class ProtobufDifferentialDriveWheelSpeeds extends ProtoMessage<Pro
return ((bitField0_) == 0);
}
public static ProtobufDifferentialDriveWheelSpeeds parseFrom(final byte[] data) throws
public static ProtobufDifferentialDriveWheelVelocities parseFrom(final byte[] data) throws
InvalidProtocolBufferException {
return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelSpeeds(), data).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelVelocities(), data).checkInitialized();
}
public static ProtobufDifferentialDriveWheelSpeeds parseFrom(final ProtoSource input) throws
public static ProtobufDifferentialDriveWheelVelocities parseFrom(final ProtoSource input) throws
IOException {
return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelSpeeds(), input).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelVelocities(), input).checkInitialized();
}
public static ProtobufDifferentialDriveWheelSpeeds parseFrom(final JsonSource input) throws
public static ProtobufDifferentialDriveWheelVelocities parseFrom(final JsonSource input) throws
IOException {
return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelSpeeds(), input).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelVelocities(), input).checkInitialized();
}
/**
* @return factory for creating ProtobufDifferentialDriveWheelSpeeds messages
* @return factory for creating ProtobufDifferentialDriveWheelVelocities messages
*/
public static MessageFactory<ProtobufDifferentialDriveWheelSpeeds> getFactory() {
return ProtobufDifferentialDriveWheelSpeedsFactory.INSTANCE;
public static MessageFactory<ProtobufDifferentialDriveWheelVelocities> getFactory() {
return ProtobufDifferentialDriveWheelVelocitiesFactory.INSTANCE;
}
/**
* @return this type's descriptor.
*/
public static Descriptors.Descriptor getDescriptor() {
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor;
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufDifferentialDriveWheelVelocities_descriptor;
}
private enum ProtobufDifferentialDriveWheelSpeedsFactory implements MessageFactory<ProtobufDifferentialDriveWheelSpeeds> {
private enum ProtobufDifferentialDriveWheelVelocitiesFactory implements MessageFactory<ProtobufDifferentialDriveWheelVelocities> {
INSTANCE;
@Override
public ProtobufDifferentialDriveWheelSpeeds create() {
return ProtobufDifferentialDriveWheelSpeeds.newInstance();
public ProtobufDifferentialDriveWheelVelocities create() {
return ProtobufDifferentialDriveWheelVelocities.newInstance();
}
}

173
wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufKinematicsDescriptor.java generated
View File

@@ -9,108 +9,109 @@ import us.hebi.quickbuf.ProtoUtil;
import us.hebi.quickbuf.RepeatedByte;
public final class ProtobufKinematicsDescriptor {
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4259,
"ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" +
"aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" +
"BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" +
"GAIgASgBUgJheRIUCgVhbHBoYRgDIAEoAVIFYWxwaGEiRQojUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2" +
"ZUtpbmVtYXRpY3MSHgoKdHJhY2t3aWR0aBgBIAEoAVIKdHJhY2t3aWR0aCJQCiRQcm90b2J1ZkRpZmZl" +
"cmVudGlhbERyaXZlV2hlZWxTcGVlZHMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" +
"cmlnaHQiVworUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsQWNjZWxlcmF0aW9ucxISCgRsZWZ0" +
"GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVudGlhbERy" +
"aXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIFcmlnaHQi" +
"pAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgLMiAud3Bp" +
"LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0GAIgASgL" +
"MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFyX2xlZnQY" +
"AyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/CgpyZWFyX3Jp" +
"Z2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0IqABCiJQ" +
"cm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250" +
"TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJl" +
"YXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJp" +
"dmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" +
"AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" +
"GAQgASgBUglyZWFyUmlnaHQipAEKJlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxBY2NlbGVyYXRpb25z",
"Eh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRS" +
"aWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJS" +
"aWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3Bp" +
"LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVs" +
"ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" +
"cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" +
"FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" +
"YXRpb24yZFIFYW5nbGUiewogUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb24SIgoMYWNjZWxl" +
"cmF0aW9uGAEgASgBUgxhY2NlbGVyYXRpb24SMwoFYW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9i" +
"dWZSb3RhdGlvbjJkUgVhbmdsZUI3ChVvcmcud3BpbGliLm1hdGgucHJvdG9CHFByb3RvYnVmS2luZW1h" +
"dGljc0Rlc2NyaXB0b3JQAUqAEwoGEgQAAFgBCggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoI" +
"CgEIEgMGAC4KCQoCCAESAwYALgoICgEIEgMIAD0KCQoCCAgSAwgAPQoICgEIEgMKACIKCQoCCAoSAwoA" +
"IgoKCgIEABIEDAAQAQoKCgMEAAESAwwIHQoLCgQEAAIAEgMNAhAKDAoFBAACAAUSAw0CCAoMCgUEAAIA" +
"ARIDDQkLCgwKBQQAAgADEgMNDg8KCwoEBAACARIDDgIQCgwKBQQAAgEFEgMOAggKDAoFBAACAQESAw4J" +
"CwoMCgUEAAIBAxIDDg4PCgsKBAQAAgISAw8CEwoMCgUEAAICBRIDDwIICgwKBQQAAgIBEgMPCQ4KDAoF" +
"BAACAgMSAw8REgoKCgIEARIEEgAWAQoKCgMEAQESAxIIJAoLCgQEAQIAEgMTAhAKDAoFBAECAAUSAxMC" +
"CAoMCgUEAQIAARIDEwkLCgwKBQQBAgADEgMTDg8KCwoEBAECARIDFAIQCgwKBQQBAgEFEgMUAggKDAoF" +
"BAECAQESAxQJCwoMCgUEAQIBAxIDFA4PCgsKBAQBAgISAxUCEwoMCgUEAQICBRIDFQIICgwKBQQBAgIB" +
"EgMVCQ4KDAoFBAECAgMSAxUREgoKCgIEAhIEGAAaAQoKCgMEAgESAxgIKwoLCgQEAgIAEgMZAhgKDAoF" +
"BAICAAUSAxkCCAoMCgUEAgIAARIDGQkTCgwKBQQCAgADEgMZFhcKCgoCBAMSBBwAHwEKCgoDBAMBEgMc",
"CCwKCwoEBAMCABIDHQISCgwKBQQDAgAFEgMdAggKDAoFBAMCAAESAx0JDQoMCgUEAwIAAxIDHRARCgsK" +
"BAQDAgESAx4CEwoMCgUEAwIBBRIDHgIICgwKBQQDAgEBEgMeCQ4KDAoFBAMCAQMSAx4REgoKCgIEBBIE" +
"IQAkAQoKCgMEBAESAyEIMwoLCgQEBAIAEgMiAhIKDAoFBAQCAAUSAyICCAoMCgUEBAIAARIDIgkNCgwK" +
"BQQEAgADEgMiEBEKCwoEBAQCARIDIwITCgwKBQQEAgEFEgMjAggKDAoFBAQCAQESAyMJDgoMCgUEBAIB" +
"AxIDIxESCgoKAgQFEgQmACkBCgoKAwQFARIDJggvCgsKBAQFAgASAycCEgoMCgUEBQIABRIDJwIICgwK" +
"BQQFAgABEgMnCQ0KDAoFBAUCAAMSAycQEQoLCgQEBQIBEgMoAhMKDAoFBAUCAQUSAygCCAoMCgUEBQIB" +
"ARIDKAkOCgwKBQQFAgEDEgMoERIKCgoCBAYSBCsAMAEKCgoDBAYBEgMrCCYKCwoEBAYCABIDLAInCgwK" +
"BQQGAgAGEgMsAhcKDAoFBAYCAAESAywYIgoMCgUEBgIAAxIDLCUmCgsKBAQGAgESAy0CKAoMCgUEBgIB" +
"BhIDLQIXCgwKBQQGAgEBEgMtGCMKDAoFBAYCAQMSAy0mJwoLCgQEBgICEgMuAiYKDAoFBAYCAgYSAy4C" +
"FwoMCgUEBgICARIDLhghCgwKBQQGAgIDEgMuJCUKCwoEBAYCAxIDLwInCgwKBQQGAgMGEgMvAhcKDAoF" +
"BAYCAwESAy8YIgoMCgUEBgIDAxIDLyUmCgoKAgQHEgQyADcBCgoKAwQHARIDMggqCgsKBAQHAgASAzMC" +
"GAoMCgUEBwIABRIDMwIICgwKBQQHAgABEgMzCRMKDAoFBAcCAAMSAzMWFwoLCgQEBwIBEgM0AhkKDAoF" +
"BAcCAQUSAzQCCAoMCgUEBwIBARIDNAkUCgwKBQQHAgEDEgM0FxgKCwoEBAcCAhIDNQIXCgwKBQQHAgIF" +
"EgM1AggKDAoFBAcCAgESAzUJEgoMCgUEBwICAxIDNRUWCgsKBAQHAgMSAzYCGAoMCgUEBwIDBRIDNgII" +
"CgwKBQQHAgMBEgM2CRMKDAoFBAcCAwMSAzYWFwoKCgIECBIEOQA+AQoKCgMECAESAzkIJwoLCgQECAIA" +
"EgM6AhgKDAoFBAgCAAUSAzoCCAoMCgUECAIAARIDOgkTCgwKBQQIAgADEgM6FhcKCwoEBAgCARIDOwIZ" +
"CgwKBQQIAgEFEgM7AggKDAoFBAgCAQESAzsJFAoMCgUECAIBAxIDOxcYCgsKBAQIAgISAzwCFwoMCgUE" +
"CAICBRIDPAIICgwKBQQIAgIBEgM8CRIKDAoFBAgCAgMSAzwVFgoLCgQECAIDEgM9AhgKDAoFBAgCAwUS" +
"Az0CCAoMCgUECAIDARIDPQkTCgwKBQQIAgMDEgM9FhcKCgoCBAkSBEAARQEKCgoDBAkBEgNACC4KCwoE" +
"BAkCABIDQQIYCgwKBQQJAgAFEgNBAggKDAoFBAkCAAESA0EJEwoMCgUECQIAAxIDQRYXCgsKBAQJAgES",
"A0ICGQoMCgUECQIBBRIDQgIICgwKBQQJAgEBEgNCCRQKDAoFBAkCAQMSA0IXGAoLCgQECQICEgNDAhcK" +
"DAoFBAkCAgUSA0MCCAoMCgUECQICARIDQwkSCgwKBQQJAgIDEgNDFRYKCwoEBAkCAxIDRAIYCgwKBQQJ" +
"AgMFEgNEAggKDAoFBAkCAwESA0QJEwoMCgUECQIDAxIDRBYXCgoKAgQKEgRHAEkBCgoKAwQKARIDRwgl" +
"CgsKBAQKAgASA0gCLQoMCgUECgIABBIDSAIKCgwKBQQKAgAGEgNICyAKDAoFBAoCAAESA0ghKAoMCgUE" +
"CgIAAxIDSCssCgoKAgQLEgRLAE4BCgoKAwQLARIDSwgkCgsKBAQLAgASA0wCFgoMCgUECwIABRIDTAII" +
"CgwKBQQLAgABEgNMCREKDAoFBAsCAAMSA0wUFQoLCgQECwIBEgNNAh8KDAoFBAsCAQYSA00CFAoMCgUE" +
"CwIBARIDTRUaCgwKBQQLAgEDEgNNHR4KCgoCBAwSBFAAUwEKCgoDBAwBEgNQCCEKCwoEBAwCABIDUQIT" +
"CgwKBQQMAgAFEgNRAggKDAoFBAwCAAESA1EJDgoMCgUEDAIAAxIDURESCgsKBAQMAgESA1ICHwoMCgUE" +
"DAIBBhIDUgIUCgwKBQQMAgEBEgNSFRoKDAoFBAwCAQMSA1IdHgoKCgIEDRIEVQBYAQoKCgMEDQESA1UI" +
"KAoLCgQEDQIAEgNWAhoKDAoFBA0CAAUSA1YCCAoMCgUEDQIAARIDVgkVCgwKBQQNAgADEgNWGBkKCwoE" +
"BA0CARIDVwIfCgwKBQQNAgEGEgNXAhQKDAoFBA0CAQESA1cVGgoMCgUEDQIBAxIDVx0eYgZwcm90bzM=");
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4280,
"ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iUQoZUHJvdG9idWZD" +
"aGFzc2lzVmVsb2NpdGllcxIOCgJ2eBgBIAEoAVICdngSDgoCdnkYAiABKAFSAnZ5EhQKBW9tZWdhGAMg" +
"ASgBUgVvbWVnYSJUChxQcm90b2J1ZkNoYXNzaXNBY2NlbGVyYXRpb25zEg4KAmF4GAEgASgBUgJheBIO" +
"CgJheRgCIAEoAVICYXkSFAoFYWxwaGEYAyABKAFSBWFscGhhIkUKI1Byb3RvYnVmRGlmZmVyZW50aWFs" +
"RHJpdmVLaW5lbWF0aWNzEh4KCnRyYWNrd2lkdGgYASABKAFSCnRyYWNrd2lkdGgiVAooUHJvdG9idWZE" +
"aWZmZXJlbnRpYWxEcml2ZVdoZWVsVmVsb2NpdGllcxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0" +
"GAIgASgBUgVyaWdodCJXCitQcm90b2J1ZkRpZmZlcmVudGlhbERyaXZlV2hlZWxBY2NlbGVyYXRpb25z" +
"EhIKBGxlZnQYASABKAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IlMKJ1Byb3RvYnVmRGlmZmVy" +
"ZW50aWFsRHJpdmVXaGVlbFBvc2l0aW9ucxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgB" +
"UgVyaWdodCKkAgoeUHJvdG9idWZNZWNhbnVtRHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASAB" +
"KAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmln" +
"aHQYAiABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJl" +
"YXJfbGVmdBgDIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8K" +
"CnJlYXJfcmlnaHQYBCABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmln" +
"aHQioAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoKZnJvbnRfbGVmdBgBIAEo" +
"AVIJZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgD" +
"IAEoAVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0IqEBCiNQcm90b2J1Zk1l" +
"Y2FudW1Ecml2ZVdoZWVsVmVsb2NpdGllcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoL" +
"ZnJvbnRfcmlnaHQYAiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBId" +
"CgpyZWFyX3JpZ2h0GAQgASgBUglyZWFyUmlnaHQipAEKJlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxB",
"Y2NlbGVyYXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgC" +
"IAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQY" +
"BCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdtb2R1bGVz" +
"GAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQcm90b2J1" +
"ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgC" +
"IAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlIm8KHFByb3RvYnVmU3dlcnZl" +
"TW9kdWxlVmVsb2NpdHkSGgoIdmVsb2NpdHkYASABKAFSCHZlbG9jaXR5EjMKBWFuZ2xlGAIgASgLMh0u" +
"d3BpLnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGUiewogUHJvdG9idWZTd2VydmVNb2R1bGVB" +
"Y2NlbGVyYXRpb24SIgoMYWNjZWxlcmF0aW9uGAEgASgBUgxhY2NlbGVyYXRpb24SMwoFYW5nbGUYAiAB" +
"KAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZUI3ChVvcmcud3BpbGliLm1hdGgu" +
"cHJvdG9CHFByb3RvYnVmS2luZW1hdGljc0Rlc2NyaXB0b3JQAUqAEwoGEgQAAFgBCggKAQwSAwAAEgoI" +
"CgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGAC4KCQoCCAESAwYALgoICgEIEgMIAD0KCQoCCAgSAwgA" +
"PQoICgEIEgMKACIKCQoCCAoSAwoAIgoKCgIEABIEDAAQAQoKCgMEAAESAwwIIQoLCgQEAAIAEgMNAhAK" +
"DAoFBAACAAUSAw0CCAoMCgUEAAIAARIDDQkLCgwKBQQAAgADEgMNDg8KCwoEBAACARIDDgIQCgwKBQQA" +
"AgEFEgMOAggKDAoFBAACAQESAw4JCwoMCgUEAAIBAxIDDg4PCgsKBAQAAgISAw8CEwoMCgUEAAICBRID" +
"DwIICgwKBQQAAgIBEgMPCQ4KDAoFBAACAgMSAw8REgoKCgIEARIEEgAWAQoKCgMEAQESAxIIJAoLCgQE" +
"AQIAEgMTAhAKDAoFBAECAAUSAxMCCAoMCgUEAQIAARIDEwkLCgwKBQQBAgADEgMTDg8KCwoEBAECARID" +
"FAIQCgwKBQQBAgEFEgMUAggKDAoFBAECAQESAxQJCwoMCgUEAQIBAxIDFA4PCgsKBAQBAgISAxUCEwoM" +
"CgUEAQICBRIDFQIICgwKBQQBAgIBEgMVCQ4KDAoFBAECAgMSAxUREgoKCgIEAhIEGAAaAQoKCgMEAgES" +
"AxgIKwoLCgQEAgIAEgMZAhgKDAoFBAICAAUSAxkCCAoMCgUEAgIAARIDGQkTCgwKBQQCAgADEgMZFhcK",
"CgoCBAMSBBwAHwEKCgoDBAMBEgMcCDAKCwoEBAMCABIDHQISCgwKBQQDAgAFEgMdAggKDAoFBAMCAAES" +
"Ax0JDQoMCgUEAwIAAxIDHRARCgsKBAQDAgESAx4CEwoMCgUEAwIBBRIDHgIICgwKBQQDAgEBEgMeCQ4K" +
"DAoFBAMCAQMSAx4REgoKCgIEBBIEIQAkAQoKCgMEBAESAyEIMwoLCgQEBAIAEgMiAhIKDAoFBAQCAAUS" +
"AyICCAoMCgUEBAIAARIDIgkNCgwKBQQEAgADEgMiEBEKCwoEBAQCARIDIwITCgwKBQQEAgEFEgMjAggK" +
"DAoFBAQCAQESAyMJDgoMCgUEBAIBAxIDIxESCgoKAgQFEgQmACkBCgoKAwQFARIDJggvCgsKBAQFAgAS" +
"AycCEgoMCgUEBQIABRIDJwIICgwKBQQFAgABEgMnCQ0KDAoFBAUCAAMSAycQEQoLCgQEBQIBEgMoAhMK" +
"DAoFBAUCAQUSAygCCAoMCgUEBQIBARIDKAkOCgwKBQQFAgEDEgMoERIKCgoCBAYSBCsAMAEKCgoDBAYB" +
"EgMrCCYKCwoEBAYCABIDLAInCgwKBQQGAgAGEgMsAhcKDAoFBAYCAAESAywYIgoMCgUEBgIAAxIDLCUm" +
"CgsKBAQGAgESAy0CKAoMCgUEBgIBBhIDLQIXCgwKBQQGAgEBEgMtGCMKDAoFBAYCAQMSAy0mJwoLCgQE" +
"BgICEgMuAiYKDAoFBAYCAgYSAy4CFwoMCgUEBgICARIDLhghCgwKBQQGAgIDEgMuJCUKCwoEBAYCAxID" +
"LwInCgwKBQQGAgMGEgMvAhcKDAoFBAYCAwESAy8YIgoMCgUEBgIDAxIDLyUmCgoKAgQHEgQyADcBCgoK" +
"AwQHARIDMggqCgsKBAQHAgASAzMCGAoMCgUEBwIABRIDMwIICgwKBQQHAgABEgMzCRMKDAoFBAcCAAMS" +
"AzMWFwoLCgQEBwIBEgM0AhkKDAoFBAcCAQUSAzQCCAoMCgUEBwIBARIDNAkUCgwKBQQHAgEDEgM0FxgK" +
"CwoEBAcCAhIDNQIXCgwKBQQHAgIFEgM1AggKDAoFBAcCAgESAzUJEgoMCgUEBwICAxIDNRUWCgsKBAQH" +
"AgMSAzYCGAoMCgUEBwIDBRIDNgIICgwKBQQHAgMBEgM2CRMKDAoFBAcCAwMSAzYWFwoKCgIECBIEOQA+" +
"AQoKCgMECAESAzkIKwoLCgQECAIAEgM6AhgKDAoFBAgCAAUSAzoCCAoMCgUECAIAARIDOgkTCgwKBQQI" +
"AgADEgM6FhcKCwoEBAgCARIDOwIZCgwKBQQIAgEFEgM7AggKDAoFBAgCAQESAzsJFAoMCgUECAIBAxID" +
"OxcYCgsKBAQIAgISAzwCFwoMCgUECAICBRIDPAIICgwKBQQIAgIBEgM8CRIKDAoFBAgCAgMSAzwVFgoL" +
"CgQECAIDEgM9AhgKDAoFBAgCAwUSAz0CCAoMCgUECAIDARIDPQkTCgwKBQQIAgMDEgM9FhcKCgoCBAkS" +
"BEAARQEKCgoDBAkBEgNACC4KCwoEBAkCABIDQQIYCgwKBQQJAgAFEgNBAggKDAoFBAkCAAESA0EJEwoM",
"CgUECQIAAxIDQRYXCgsKBAQJAgESA0ICGQoMCgUECQIBBRIDQgIICgwKBQQJAgEBEgNCCRQKDAoFBAkC" +
"AQMSA0IXGAoLCgQECQICEgNDAhcKDAoFBAkCAgUSA0MCCAoMCgUECQICARIDQwkSCgwKBQQJAgIDEgND" +
"FRYKCwoEBAkCAxIDRAIYCgwKBQQJAgMFEgNEAggKDAoFBAkCAwESA0QJEwoMCgUECQIDAxIDRBYXCgoK" +
"AgQKEgRHAEkBCgoKAwQKARIDRwglCgsKBAQKAgASA0gCLQoMCgUECgIABBIDSAIKCgwKBQQKAgAGEgNI" +
"CyAKDAoFBAoCAAESA0ghKAoMCgUECgIAAxIDSCssCgoKAgQLEgRLAE4BCgoKAwQLARIDSwgkCgsKBAQL" +
"AgASA0wCFgoMCgUECwIABRIDTAIICgwKBQQLAgABEgNMCREKDAoFBAsCAAMSA0wUFQoLCgQECwIBEgNN" +
"Ah8KDAoFBAsCAQYSA00CFAoMCgUECwIBARIDTRUaCgwKBQQLAgEDEgNNHR4KCgoCBAwSBFAAUwEKCgoD" +
"BAwBEgNQCCQKCwoEBAwCABIDUQIWCgwKBQQMAgAFEgNRAggKDAoFBAwCAAESA1EJEQoMCgUEDAIAAxID" +
"URQVCgsKBAQMAgESA1ICHwoMCgUEDAIBBhIDUgIUCgwKBQQMAgEBEgNSFRoKDAoFBAwCAQMSA1IdHgoK" +
"CgIEDRIEVQBYAQoKCgMEDQESA1UIKAoLCgQEDQIAEgNWAhoKDAoFBA0CAAUSA1YCCAoMCgUEDQIAARID" +
"VgkVCgwKBQQNAgADEgNWGBkKCwoEBA0CARIDVwIfCgwKBQQNAgEGEgNXAhQKDAoFBA0CAQESA1cVGgoM" +
"CgUEDQIBAxIDVx0eYgZwcm90bzM=");
static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, ProtobufGeometry2dDescriptor.getDescriptor());
static final Descriptors.Descriptor wpi_proto_ProtobufChassisSpeeds_descriptor = descriptor.internalContainedType(49, 77, "ProtobufChassisSpeeds", "wpi.proto.ProtobufChassisSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufChassisVelocities_descriptor = descriptor.internalContainedType(49, 81, "ProtobufChassisVelocities", "wpi.proto.ProtobufChassisVelocities");
static final Descriptors.Descriptor wpi_proto_ProtobufChassisAccelerations_descriptor = descriptor.internalContainedType(128, 84, "ProtobufChassisAccelerations", "wpi.proto.ProtobufChassisAccelerations");
static final Descriptors.Descriptor wpi_proto_ProtobufChassisAccelerations_descriptor = descriptor.internalContainedType(132, 84, "ProtobufChassisAccelerations", "wpi.proto.ProtobufChassisAccelerations");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(214, 69, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(218, 69, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(285, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVelocities_descriptor = descriptor.internalContainedType(289, 84, "ProtobufDifferentialDriveWheelVelocities", "wpi.proto.ProtobufDifferentialDriveWheelVelocities");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelAccelerations_descriptor = descriptor.internalContainedType(367, 87, "ProtobufDifferentialDriveWheelAccelerations", "wpi.proto.ProtobufDifferentialDriveWheelAccelerations");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelAccelerations_descriptor = descriptor.internalContainedType(375, 87, "ProtobufDifferentialDriveWheelAccelerations", "wpi.proto.ProtobufDifferentialDriveWheelAccelerations");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(456, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(464, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(542, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(550, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(837, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(845, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(1000, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelVelocities_descriptor = descriptor.internalContainedType(1008, 161, "ProtobufMecanumDriveWheelVelocities", "wpi.proto.ProtobufMecanumDriveWheelVelocities");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelAccelerations_descriptor = descriptor.internalContainedType(1160, 164, "ProtobufMecanumDriveWheelAccelerations", "wpi.proto.ProtobufMecanumDriveWheelAccelerations");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelAccelerations_descriptor = descriptor.internalContainedType(1172, 164, "ProtobufMecanumDriveWheelAccelerations", "wpi.proto.ProtobufMecanumDriveWheelAccelerations");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1326, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1338, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1419, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1431, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1532, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleVelocity_descriptor = descriptor.internalContainedType(1544, 111, "ProtobufSwerveModuleVelocity", "wpi.proto.ProtobufSwerveModuleVelocity");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAcceleration_descriptor = descriptor.internalContainedType(1636, 123, "ProtobufSwerveModuleAcceleration", "wpi.proto.ProtobufSwerveModuleAcceleration");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAcceleration_descriptor = descriptor.internalContainedType(1657, 123, "ProtobufSwerveModuleAcceleration", "wpi.proto.ProtobufSwerveModuleAcceleration");
/**
* @return this proto file's descriptor.

76
wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufMecanumDriveWheelSpeeds.java → wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufMecanumDriveWheelVelocities.java generated
View File

@@ -17,10 +17,10 @@ import us.hebi.quickbuf.ProtoSource;
import us.hebi.quickbuf.ProtoUtil;
/**
* Protobuf type {@code ProtobufMecanumDriveWheelSpeeds}
* Protobuf type {@code ProtobufMecanumDriveWheelVelocities}
*/
@SuppressWarnings("hiding")
public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<ProtobufMecanumDriveWheelSpeeds> implements Cloneable {
public final class ProtobufMecanumDriveWheelVelocities extends ProtoMessage<ProtobufMecanumDriveWheelVelocities> implements Cloneable {
private static final long serialVersionUID = 0L;
/**
@@ -43,14 +43,14 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
*/
private double rearRight;
private ProtobufMecanumDriveWheelSpeeds() {
private ProtobufMecanumDriveWheelVelocities() {
}
/**
* @return a new empty instance of {@code ProtobufMecanumDriveWheelSpeeds}
* @return a new empty instance of {@code ProtobufMecanumDriveWheelVelocities}
*/
public static ProtobufMecanumDriveWheelSpeeds newInstance() {
return new ProtobufMecanumDriveWheelSpeeds();
public static ProtobufMecanumDriveWheelVelocities newInstance() {
return new ProtobufMecanumDriveWheelVelocities();
}
/**
@@ -65,7 +65,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* <code>optional double front_left = 1;</code>
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds clearFrontLeft() {
public ProtobufMecanumDriveWheelVelocities clearFrontLeft() {
bitField0_ &= ~0x00000001;
frontLeft = 0D;
return this;
@@ -84,7 +84,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* @param value the frontLeft to set
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds setFrontLeft(final double value) {
public ProtobufMecanumDriveWheelVelocities setFrontLeft(final double value) {
bitField0_ |= 0x00000001;
frontLeft = value;
return this;
@@ -102,7 +102,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* <code>optional double front_right = 2;</code>
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds clearFrontRight() {
public ProtobufMecanumDriveWheelVelocities clearFrontRight() {
bitField0_ &= ~0x00000002;
frontRight = 0D;
return this;
@@ -121,7 +121,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* @param value the frontRight to set
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds setFrontRight(final double value) {
public ProtobufMecanumDriveWheelVelocities setFrontRight(final double value) {
bitField0_ |= 0x00000002;
frontRight = value;
return this;
@@ -139,7 +139,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* <code>optional double rear_left = 3;</code>
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds clearRearLeft() {
public ProtobufMecanumDriveWheelVelocities clearRearLeft() {
bitField0_ &= ~0x00000004;
rearLeft = 0D;
return this;
@@ -158,7 +158,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* @param value the rearLeft to set
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds setRearLeft(final double value) {
public ProtobufMecanumDriveWheelVelocities setRearLeft(final double value) {
bitField0_ |= 0x00000004;
rearLeft = value;
return this;
@@ -176,7 +176,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* <code>optional double rear_right = 4;</code>
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds clearRearRight() {
public ProtobufMecanumDriveWheelVelocities clearRearRight() {
bitField0_ &= ~0x00000008;
rearRight = 0D;
return this;
@@ -195,14 +195,15 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
* @param value the rearRight to set
* @return this
*/
public ProtobufMecanumDriveWheelSpeeds setRearRight(final double value) {
public ProtobufMecanumDriveWheelVelocities setRearRight(final double value) {
bitField0_ |= 0x00000008;
rearRight = value;
return this;
}
@Override
public ProtobufMecanumDriveWheelSpeeds copyFrom(final ProtobufMecanumDriveWheelSpeeds other) {
public ProtobufMecanumDriveWheelVelocities copyFrom(
final ProtobufMecanumDriveWheelVelocities other) {
cachedSize = other.cachedSize;
if ((bitField0_ | other.bitField0_) != 0) {
bitField0_ = other.bitField0_;
@@ -215,7 +216,8 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
}
@Override
public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtobufMecanumDriveWheelSpeeds other) {
public ProtobufMecanumDriveWheelVelocities mergeFrom(
final ProtobufMecanumDriveWheelVelocities other) {
if (other.isEmpty()) {
return this;
}
@@ -236,7 +238,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
}
@Override
public ProtobufMecanumDriveWheelSpeeds clear() {
public ProtobufMecanumDriveWheelVelocities clear() {
if (isEmpty()) {
return this;
}
@@ -250,7 +252,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
}
@Override
public ProtobufMecanumDriveWheelSpeeds clearQuick() {
public ProtobufMecanumDriveWheelVelocities clearQuick() {
if (isEmpty()) {
return this;
}
@@ -264,10 +266,10 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
if (o == this) {
return true;
}
if (!(o instanceof ProtobufMecanumDriveWheelSpeeds)) {
if (!(o instanceof ProtobufMecanumDriveWheelVelocities)) {
return false;
}
ProtobufMecanumDriveWheelSpeeds other = (ProtobufMecanumDriveWheelSpeeds) o;
ProtobufMecanumDriveWheelVelocities other = (ProtobufMecanumDriveWheelVelocities) o;
return bitField0_ == other.bitField0_
&& (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft))
&& (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight))
@@ -315,7 +317,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
@Override
@SuppressWarnings("fallthrough")
public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtoSource input) throws IOException {
public ProtobufMecanumDriveWheelVelocities mergeFrom(final ProtoSource input) throws IOException {
// Enabled Fall-Through Optimization (QuickBuffers)
int tag = input.readTag();
while (true) {
@@ -389,7 +391,7 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
}
@Override
public ProtobufMecanumDriveWheelSpeeds mergeFrom(final JsonSource input) throws IOException {
public ProtobufMecanumDriveWheelVelocities mergeFrom(final JsonSource input) throws IOException {
if (!input.beginObject()) {
return this;
}
@@ -454,8 +456,8 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
}
@Override
public ProtobufMecanumDriveWheelSpeeds clone() {
return new ProtobufMecanumDriveWheelSpeeds().copyFrom(this);
public ProtobufMecanumDriveWheelVelocities clone() {
return new ProtobufMecanumDriveWheelVelocities().copyFrom(this);
}
@Override
@@ -463,41 +465,41 @@ public final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage<Protobuf
return ((bitField0_) == 0);
}
public static ProtobufMecanumDriveWheelSpeeds parseFrom(final byte[] data) throws
public static ProtobufMecanumDriveWheelVelocities parseFrom(final byte[] data) throws
InvalidProtocolBufferException {
return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), data).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelVelocities(), data).checkInitialized();
}
public static ProtobufMecanumDriveWheelSpeeds parseFrom(final ProtoSource input) throws
public static ProtobufMecanumDriveWheelVelocities parseFrom(final ProtoSource input) throws
IOException {
return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelVelocities(), input).checkInitialized();
}
public static ProtobufMecanumDriveWheelSpeeds parseFrom(final JsonSource input) throws
public static ProtobufMecanumDriveWheelVelocities parseFrom(final JsonSource input) throws
IOException {
return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelVelocities(), input).checkInitialized();
}
/**
* @return factory for creating ProtobufMecanumDriveWheelSpeeds messages
* @return factory for creating ProtobufMecanumDriveWheelVelocities messages
*/
public static MessageFactory<ProtobufMecanumDriveWheelSpeeds> getFactory() {
return ProtobufMecanumDriveWheelSpeedsFactory.INSTANCE;
public static MessageFactory<ProtobufMecanumDriveWheelVelocities> getFactory() {
return ProtobufMecanumDriveWheelVelocitiesFactory.INSTANCE;
}
/**
* @return this type's descriptor.
*/
public static Descriptors.Descriptor getDescriptor() {
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor;
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufMecanumDriveWheelVelocities_descriptor;
}
private enum ProtobufMecanumDriveWheelSpeedsFactory implements MessageFactory<ProtobufMecanumDriveWheelSpeeds> {
private enum ProtobufMecanumDriveWheelVelocitiesFactory implements MessageFactory<ProtobufMecanumDriveWheelVelocities> {
INSTANCE;
@Override
public ProtobufMecanumDriveWheelSpeeds create() {
return ProtobufMecanumDriveWheelSpeeds.newInstance();
public ProtobufMecanumDriveWheelVelocities create() {
return ProtobufMecanumDriveWheelVelocities.newInstance();
}
}

120
wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufSwerveModuleState.java → wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufSwerveModuleVelocity.java generated
View File

@@ -17,66 +17,66 @@ import us.hebi.quickbuf.ProtoSource;
import us.hebi.quickbuf.ProtoUtil;
/**
* Protobuf type {@code ProtobufSwerveModuleState}
* Protobuf type {@code ProtobufSwerveModuleVelocity}
*/
@SuppressWarnings("hiding")
public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerveModuleState> implements Cloneable {
public final class ProtobufSwerveModuleVelocity extends ProtoMessage<ProtobufSwerveModuleVelocity> implements Cloneable {
private static final long serialVersionUID = 0L;
/**
* <code>optional double speed = 1;</code>
* <code>optional double velocity = 1;</code>
*/
private double speed;
private double velocity;
/**
* <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
*/
private final ProtobufRotation2d angle = ProtobufRotation2d.newInstance();
private ProtobufSwerveModuleState() {
private ProtobufSwerveModuleVelocity() {
}
/**
* @return a new empty instance of {@code ProtobufSwerveModuleState}
* @return a new empty instance of {@code ProtobufSwerveModuleVelocity}
*/
public static ProtobufSwerveModuleState newInstance() {
return new ProtobufSwerveModuleState();
public static ProtobufSwerveModuleVelocity newInstance() {
return new ProtobufSwerveModuleVelocity();
}
/**
* <code>optional double speed = 1;</code>
* @return whether the speed field is set
* <code>optional double velocity = 1;</code>
* @return whether the velocity field is set
*/
public boolean hasSpeed() {
public boolean hasVelocity() {
return (bitField0_ & 0x00000001) != 0;
}
/**
* <code>optional double speed = 1;</code>
* <code>optional double velocity = 1;</code>
* @return this
*/
public ProtobufSwerveModuleState clearSpeed() {
public ProtobufSwerveModuleVelocity clearVelocity() {
bitField0_ &= ~0x00000001;
speed = 0D;
velocity = 0D;
return this;
}
/**
* <code>optional double speed = 1;</code>
* @return the speed
* <code>optional double velocity = 1;</code>
* @return the velocity
*/
public double getSpeed() {
return speed;
public double getVelocity() {
return velocity;
}
/**
* <code>optional double speed = 1;</code>
* @param value the speed to set
* <code>optional double velocity = 1;</code>
* @param value the velocity to set
* @return this
*/
public ProtobufSwerveModuleState setSpeed(final double value) {
public ProtobufSwerveModuleVelocity setVelocity(final double value) {
bitField0_ |= 0x00000001;
speed = value;
velocity = value;
return this;
}
@@ -92,7 +92,7 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
* <code>optional .wpi.proto.ProtobufRotation2d angle = 2;</code>
* @return this
*/
public ProtobufSwerveModuleState clearAngle() {
public ProtobufSwerveModuleVelocity clearAngle() {
bitField0_ &= ~0x00000002;
angle.clear();
return this;
@@ -131,31 +131,31 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
* @param value the angle to set
* @return this
*/
public ProtobufSwerveModuleState setAngle(final ProtobufRotation2d value) {
public ProtobufSwerveModuleVelocity setAngle(final ProtobufRotation2d value) {
bitField0_ |= 0x00000002;
angle.copyFrom(value);
return this;
}
@Override
public ProtobufSwerveModuleState copyFrom(final ProtobufSwerveModuleState other) {
public ProtobufSwerveModuleVelocity copyFrom(final ProtobufSwerveModuleVelocity other) {
cachedSize = other.cachedSize;
if ((bitField0_ | other.bitField0_) != 0) {
bitField0_ = other.bitField0_;
speed = other.speed;
velocity = other.velocity;
angle.copyFrom(other.angle);
}
return this;
}
@Override
public ProtobufSwerveModuleState mergeFrom(final ProtobufSwerveModuleState other) {
public ProtobufSwerveModuleVelocity mergeFrom(final ProtobufSwerveModuleVelocity other) {
if (other.isEmpty()) {
return this;
}
cachedSize = -1;
if (other.hasSpeed()) {
setSpeed(other.speed);
if (other.hasVelocity()) {
setVelocity(other.velocity);
}
if (other.hasAngle()) {
getMutableAngle().mergeFrom(other.angle);
@@ -164,19 +164,19 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
}
@Override
public ProtobufSwerveModuleState clear() {
public ProtobufSwerveModuleVelocity clear() {
if (isEmpty()) {
return this;
}
cachedSize = -1;
bitField0_ = 0;
speed = 0D;
velocity = 0D;
angle.clear();
return this;
}
@Override
public ProtobufSwerveModuleState clearQuick() {
public ProtobufSwerveModuleVelocity clearQuick() {
if (isEmpty()) {
return this;
}
@@ -191,12 +191,12 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
if (o == this) {
return true;
}
if (!(o instanceof ProtobufSwerveModuleState)) {
if (!(o instanceof ProtobufSwerveModuleVelocity)) {
return false;
}
ProtobufSwerveModuleState other = (ProtobufSwerveModuleState) o;
ProtobufSwerveModuleVelocity other = (ProtobufSwerveModuleVelocity) o;
return bitField0_ == other.bitField0_
&& (!hasSpeed() || ProtoUtil.isEqual(speed, other.speed))
&& (!hasVelocity() || ProtoUtil.isEqual(velocity, other.velocity))
&& (!hasAngle() || angle.equals(other.angle));
}
@@ -204,7 +204,7 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
public void writeTo(final ProtoSink output) throws IOException {
if ((bitField0_ & 0x00000001) != 0) {
output.writeRawByte((byte) 9);
output.writeDoubleNoTag(speed);
output.writeDoubleNoTag(velocity);
}
if ((bitField0_ & 0x00000002) != 0) {
output.writeRawByte((byte) 18);
@@ -226,14 +226,14 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
@Override
@SuppressWarnings("fallthrough")
public ProtobufSwerveModuleState mergeFrom(final ProtoSource input) throws IOException {
public ProtobufSwerveModuleVelocity mergeFrom(final ProtoSource input) throws IOException {
// Enabled Fall-Through Optimization (QuickBuffers)
int tag = input.readTag();
while (true) {
switch (tag) {
case 9: {
// speed
speed = input.readDouble();
// velocity
velocity = input.readDouble();
bitField0_ |= 0x00000001;
tag = input.readTag();
if (tag != 18) {
@@ -267,7 +267,7 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
public void writeTo(final JsonSink output) throws IOException {
output.beginObject();
if ((bitField0_ & 0x00000001) != 0) {
output.writeDouble(FieldNames.speed, speed);
output.writeDouble(FieldNames.velocity, velocity);
}
if ((bitField0_ & 0x00000002) != 0) {
output.writeMessage(FieldNames.angle, angle);
@@ -276,16 +276,16 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
}
@Override
public ProtobufSwerveModuleState mergeFrom(final JsonSource input) throws IOException {
public ProtobufSwerveModuleVelocity mergeFrom(final JsonSource input) throws IOException {
if (!input.beginObject()) {
return this;
}
while (!input.isAtEnd()) {
switch (input.readFieldHash()) {
case 109641799: {
if (input.isAtField(FieldNames.speed)) {
case 2134260957: {
if (input.isAtField(FieldNames.velocity)) {
if (!input.trySkipNullValue()) {
speed = input.readDouble();
velocity = input.readDouble();
bitField0_ |= 0x00000001;
}
} else {
@@ -315,8 +315,8 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
}
@Override
public ProtobufSwerveModuleState clone() {
return new ProtobufSwerveModuleState().copyFrom(this);
public ProtobufSwerveModuleVelocity clone() {
return new ProtobufSwerveModuleVelocity().copyFrom(this);
}
@Override
@@ -324,39 +324,39 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
return ((bitField0_) == 0);
}
public static ProtobufSwerveModuleState parseFrom(final byte[] data) throws
public static ProtobufSwerveModuleVelocity parseFrom(final byte[] data) throws
InvalidProtocolBufferException {
return ProtoMessage.mergeFrom(new ProtobufSwerveModuleState(), data).checkInitialized();
return ProtoMessage.mergeFrom(new ProtobufSwerveModuleVelocity(), data).checkInitialized();
}
public static ProtobufSwerveModuleState parseFrom(final ProtoSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufSwerveModuleState(), input).checkInitialized();
public static ProtobufSwerveModuleVelocity parseFrom(final ProtoSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufSwerveModuleVelocity(), input).checkInitialized();
}
public static ProtobufSwerveModuleState parseFrom(final JsonSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufSwerveModuleState(), input).checkInitialized();
public static ProtobufSwerveModuleVelocity parseFrom(final JsonSource input) throws IOException {
return ProtoMessage.mergeFrom(new ProtobufSwerveModuleVelocity(), input).checkInitialized();
}
/**
* @return factory for creating ProtobufSwerveModuleState messages
* @return factory for creating ProtobufSwerveModuleVelocity messages
*/
public static MessageFactory<ProtobufSwerveModuleState> getFactory() {
return ProtobufSwerveModuleStateFactory.INSTANCE;
public static MessageFactory<ProtobufSwerveModuleVelocity> getFactory() {
return ProtobufSwerveModuleVelocityFactory.INSTANCE;
}
/**
* @return this type's descriptor.
*/
public static Descriptors.Descriptor getDescriptor() {
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufSwerveModuleState_descriptor;
return ProtobufKinematicsDescriptor.wpi_proto_ProtobufSwerveModuleVelocity_descriptor;
}
private enum ProtobufSwerveModuleStateFactory implements MessageFactory<ProtobufSwerveModuleState> {
private enum ProtobufSwerveModuleVelocityFactory implements MessageFactory<ProtobufSwerveModuleVelocity> {
INSTANCE;
@Override
public ProtobufSwerveModuleState create() {
return ProtobufSwerveModuleState.newInstance();
public ProtobufSwerveModuleVelocity create() {
return ProtobufSwerveModuleVelocity.newInstance();
}
}
@@ -364,7 +364,7 @@ public final class ProtobufSwerveModuleState extends ProtoMessage<ProtobufSwerve
* Contains name constants used for serializing JSON
*/
static class FieldNames {
static final FieldName speed = FieldName.forField("speed");
static final FieldName velocity = FieldName.forField("velocity");
static final FieldName angle = FieldName.forField("angle");
}

857
wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp generated
View File

@@ -16,434 +16,437 @@ static const uint8_t file_descriptor[] {
0x63,0x73,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x12,0x09,
0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x1a,
0x10,0x67,0x65,0x6f,0x6d,0x65,0x74,0x72,0x79,0x32,
0x64,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x22,0x4d,0x0a,
0x15,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x43,
0x68,0x61,0x73,0x73,0x69,0x73,0x53,0x70,0x65,0x65,
0x64,0x73,0x12,0x0e,0x0a,0x02,0x76,0x78,0x18,0x01,
0x20,0x01,0x28,0x01,0x52,0x02,0x76,0x78,0x12,0x0e,
0x0a,0x02,0x76,0x79,0x18,0x02,0x20,0x01,0x28,0x01,
0x52,0x02,0x76,0x79,0x12,0x14,0x0a,0x05,0x6f,0x6d,
0x65,0x67,0x61,0x18,0x03,0x20,0x01,0x28,0x01,0x52,
0x05,0x6f,0x6d,0x65,0x67,0x61,0x22,0x54,0x0a,0x1c,
0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x43,0x68,
0x61,0x73,0x73,0x69,0x73,0x41,0x63,0x63,0x65,0x6c,
0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x73,0x12,0x0e,
0x0a,0x02,0x61,0x78,0x18,0x01,0x20,0x01,0x28,0x01,
0x52,0x02,0x61,0x78,0x12,0x0e,0x0a,0x02,0x61,0x79,
0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x02,0x61,0x79,
0x12,0x14,0x0a,0x05,0x61,0x6c,0x70,0x68,0x61,0x18,
0x03,0x20,0x01,0x28,0x01,0x52,0x05,0x61,0x6c,0x70,
0x68,0x61,0x22,0x45,0x0a,0x23,0x50,0x72,0x6f,0x74,
0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66,0x65,0x72,
0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72,0x69,0x76,
0x65,0x4b,0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,0x63,
0x73,0x12,0x1e,0x0a,0x0a,0x74,0x72,0x61,0x63,0x6b,
0x77,0x69,0x64,0x74,0x68,0x18,0x01,0x20,0x01,0x28,
0x01,0x52,0x0a,0x74,0x72,0x61,0x63,0x6b,0x77,0x69,
0x64,0x74,0x68,0x22,0x50,0x0a,0x24,0x50,0x72,0x6f,
0x64,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x22,0x51,0x0a,
0x19,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x43,
0x68,0x61,0x73,0x73,0x69,0x73,0x56,0x65,0x6c,0x6f,
0x63,0x69,0x74,0x69,0x65,0x73,0x12,0x0e,0x0a,0x02,
0x76,0x78,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x02,
0x76,0x78,0x12,0x0e,0x0a,0x02,0x76,0x79,0x18,0x02,
0x20,0x01,0x28,0x01,0x52,0x02,0x76,0x79,0x12,0x14,
0x0a,0x05,0x6f,0x6d,0x65,0x67,0x61,0x18,0x03,0x20,
0x01,0x28,0x01,0x52,0x05,0x6f,0x6d,0x65,0x67,0x61,
0x22,0x54,0x0a,0x1c,0x50,0x72,0x6f,0x74,0x6f,0x62,
0x75,0x66,0x43,0x68,0x61,0x73,0x73,0x69,0x73,0x41,
0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,
0x6e,0x73,0x12,0x0e,0x0a,0x02,0x61,0x78,0x18,0x01,
0x20,0x01,0x28,0x01,0x52,0x02,0x61,0x78,0x12,0x0e,
0x0a,0x02,0x61,0x79,0x18,0x02,0x20,0x01,0x28,0x01,
0x52,0x02,0x61,0x79,0x12,0x14,0x0a,0x05,0x61,0x6c,
0x70,0x68,0x61,0x18,0x03,0x20,0x01,0x28,0x01,0x52,
0x05,0x61,0x6c,0x70,0x68,0x61,0x22,0x45,0x0a,0x23,
0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69,
0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,
0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d,
0x61,0x74,0x69,0x63,0x73,0x12,0x1e,0x0a,0x0a,0x74,
0x72,0x61,0x63,0x6b,0x77,0x69,0x64,0x74,0x68,0x18,
0x01,0x20,0x01,0x28,0x01,0x52,0x0a,0x74,0x72,0x61,
0x63,0x6b,0x77,0x69,0x64,0x74,0x68,0x22,0x54,0x0a,
0x28,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,
0x69,0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61,
0x6c,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,
0x6c,0x56,0x65,0x6c,0x6f,0x63,0x69,0x74,0x69,0x65,
0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66,0x74,0x18,
0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c,0x65,0x66,
0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67,0x68,0x74,
0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05,0x72,0x69,
0x67,0x68,0x74,0x22,0x57,0x0a,0x2b,0x50,0x72,0x6f,
0x74,0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66,0x65,
0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72,0x69,
0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x53,0x70,0x65,
0x65,0x64,0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66,
0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c,
0x65,0x66,0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67,
0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05,
0x72,0x69,0x67,0x68,0x74,0x22,0x57,0x0a,0x2b,0x50,
0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69,0x66,
0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,
0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x41,
0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,
0x6e,0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66,0x74,
0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c,0x65,
0x66,0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67,0x68,
0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05,0x72,
0x69,0x67,0x68,0x74,0x22,0x53,0x0a,0x27,0x50,0x72,
0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66,
0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72,
0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x50,0x6f,
0x73,0x69,0x74,0x69,0x6f,0x6e,0x73,0x12,0x12,0x0a,
0x04,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,
0x01,0x52,0x04,0x6c,0x65,0x66,0x74,0x12,0x14,0x0a,
0x05,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,
0x28,0x01,0x52,0x05,0x72,0x69,0x67,0x68,0x74,0x22,
0xa4,0x02,0x0a,0x1e,0x50,0x72,0x6f,0x74,0x6f,0x62,
0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,
0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d,0x61,
0x74,0x69,0x63,0x73,0x12,0x3f,0x0a,0x0a,0x66,0x72,
0x6f,0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,
0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,
0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,
0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,
0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x09,
0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12,
0x41,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72,
0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x0b,
0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,
0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,
0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,
0x6f,0x6e,0x32,0x64,0x52,0x0a,0x66,0x72,0x6f,0x6e,
0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x3d,0x0a,0x09,
0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,
0x03,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,
0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,
0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,
0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,
0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66,0x74,0x12,
0x3f,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f,0x72,0x69,
0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28,0x0b,0x32,
0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,
0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,
0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f,
0x6e,0x32,0x64,0x52,0x09,0x72,0x65,0x61,0x72,0x52,
0x69,0x67,0x68,0x74,0x22,0xa0,0x01,0x0a,0x22,0x50,
0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,
0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,
0x68,0x65,0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69,
0x6f,0x6e,0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,
0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,
0x01,0x28,0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,
0x4c,0x65,0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,
0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,
0x02,0x20,0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,
0x6e,0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,
0x09,0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,
0x18,0x03,0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65,
0x61,0x72,0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,
0x72,0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,
0x18,0x04,0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65,
0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22,0x9d,0x01,
0x0a,0x1f,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,
0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,
0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x53,0x70,0x65,
0x65,0x64,0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,
0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,
0x01,0x28,0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,
0x4c,0x65,0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,
0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,
0x02,0x20,0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,
0x6e,0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,
0x09,0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,
0x18,0x03,0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65,
0x61,0x72,0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,
0x72,0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,
0x18,0x04,0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65,
0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22,0xa4,0x01,
0x0a,0x26,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,
0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,
0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x41,0x63,0x63,
0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x73,
0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,
0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,
0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,
0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,
0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,
0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,
0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,
0x12,0x12,0x0a,0x04,0x6c,0x65,0x66,0x74,0x18,0x01,
0x20,0x01,0x28,0x01,0x52,0x04,0x6c,0x65,0x66,0x74,
0x12,0x14,0x0a,0x05,0x72,0x69,0x67,0x68,0x74,0x18,
0x02,0x20,0x01,0x28,0x01,0x52,0x05,0x72,0x69,0x67,
0x68,0x74,0x22,0x53,0x0a,0x27,0x50,0x72,0x6f,0x74,
0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66,0x65,0x72,
0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72,0x69,0x76,
0x65,0x57,0x68,0x65,0x65,0x6c,0x50,0x6f,0x73,0x69,
0x74,0x69,0x6f,0x6e,0x73,0x12,0x12,0x0a,0x04,0x6c,
0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,
0x04,0x6c,0x65,0x66,0x74,0x12,0x14,0x0a,0x05,0x72,
0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01,
0x52,0x05,0x72,0x69,0x67,0x68,0x74,0x22,0xa4,0x02,
0x0a,0x1e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,
0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,
0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,
0x63,0x73,0x12,0x3f,0x0a,0x0a,0x66,0x72,0x6f,0x6e,
0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,
0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,
0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,
0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,
0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x09,0x66,0x72,
0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12,0x41,0x0a,
0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,
0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x0b,0x32,0x20,
0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,
0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,
0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,
0x32,0x64,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,
0x69,0x67,0x68,0x74,0x12,0x3d,0x0a,0x09,0x72,0x65,
0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,
0x01,0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,
0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,
0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,
0x01,0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,
0x69,0x67,0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50,0x72,
0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,
0x76,0x65,0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,
0x65,0x6d,0x61,0x74,0x69,0x63,0x73,0x12,0x3a,0x0a,
0x07,0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x18,0x01,
0x20,0x03,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,
0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,
0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,
0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,
0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x08,0x72,
0x65,0x61,0x72,0x4c,0x65,0x66,0x74,0x12,0x3f,0x0a,
0x0a,0x72,0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,
0x74,0x18,0x04,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,
0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,
0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,
0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,
0x64,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67,
0x68,0x74,0x22,0xa0,0x01,0x0a,0x22,0x50,0x72,0x6f,
0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,
0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65,
0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,
0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,
0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,
0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,
0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,
0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,
0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,
0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,
0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,
0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,
0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,
0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,
0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,
0x52,0x69,0x67,0x68,0x74,0x22,0xa1,0x01,0x0a,0x23,
0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,
0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,
0x57,0x68,0x65,0x65,0x6c,0x56,0x65,0x6c,0x6f,0x63,
0x69,0x74,0x69,0x65,0x73,0x12,0x1d,0x0a,0x0a,0x66,
0x72,0x6f,0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,
0x01,0x20,0x01,0x28,0x01,0x52,0x09,0x66,0x72,0x6f,
0x6e,0x74,0x4c,0x65,0x66,0x74,0x12,0x1f,0x0a,0x0b,
0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,
0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x0a,0x66,
0x72,0x6f,0x6e,0x74,0x52,0x69,0x67,0x68,0x74,0x12,
0x1b,0x0a,0x09,0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,
0x66,0x74,0x18,0x03,0x20,0x01,0x28,0x01,0x52,0x08,
0x72,0x65,0x61,0x72,0x4c,0x65,0x66,0x74,0x12,0x1d,
0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f,0x72,0x69,0x67,
0x68,0x74,0x18,0x04,0x20,0x01,0x28,0x01,0x52,0x09,
0x72,0x65,0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22,
0xa4,0x01,0x0a,0x26,0x50,0x72,0x6f,0x74,0x6f,0x62,
0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,
0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x41,
0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,
0x6e,0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,
0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,
0x28,0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,
0x65,0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,
0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x02,
0x20,0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,
0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,
0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,
0x03,0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65,0x61,
0x72,0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,
0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,
0x04,0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65,0x61,
0x72,0x52,0x69,0x67,0x68,0x74,0x22,0x5b,0x0a,0x1d,
0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,
0x65,0x72,0x76,0x65,0x44,0x72,0x69,0x76,0x65,0x4b,
0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,0x63,0x73,0x12,
0x3a,0x0a,0x07,0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,
0x18,0x01,0x20,0x03,0x28,0x0b,0x32,0x20,0x2e,0x77,
0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,
0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,
0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,
0x52,0x07,0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x22,
0x6f,0x0a,0x1c,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,
0x66,0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,
0x75,0x6c,0x65,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,
0x6e,0x12,0x1a,0x0a,0x08,0x64,0x69,0x73,0x74,0x61,
0x6e,0x63,0x65,0x18,0x01,0x20,0x01,0x28,0x01,0x52,
0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63,0x65,0x12,
0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,
0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,
0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,
0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,
0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x07,
0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x22,0x6f,0x0a,
0x1c,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,
0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,
0x65,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x12,
0x1a,0x0a,0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63,
0x65,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x08,0x64,
0x69,0x73,0x74,0x61,0x6e,0x63,0x65,0x12,0x33,0x0a,
0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,
0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,
0x6c,0x65,0x22,0x6f,0x0a,0x1c,0x50,0x72,0x6f,0x74,
0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,0x76,0x65,
0x4d,0x6f,0x64,0x75,0x6c,0x65,0x56,0x65,0x6c,0x6f,
0x63,0x69,0x74,0x79,0x12,0x1a,0x0a,0x08,0x76,0x65,
0x6c,0x6f,0x63,0x69,0x74,0x79,0x18,0x01,0x20,0x01,
0x28,0x01,0x52,0x08,0x76,0x65,0x6c,0x6f,0x63,0x69,
0x74,0x79,0x12,0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,
0x65,0x18,0x02,0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,
0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,
0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,
0x74,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,
0x61,0x6e,0x67,0x6c,0x65,0x22,0x7b,0x0a,0x20,0x50,
0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,
0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x41,
0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,
0x6e,0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c,
0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20,
0x01,0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c,
0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x33,0x0a,
0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,
0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,
0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,
0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,
0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,
0x22,0x66,0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f,0x62,
0x75,0x66,0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,
0x64,0x75,0x6c,0x65,0x53,0x74,0x61,0x74,0x65,0x12,
0x14,0x0a,0x05,0x73,0x70,0x65,0x65,0x64,0x18,0x01,
0x20,0x01,0x28,0x01,0x52,0x05,0x73,0x70,0x65,0x65,
0x64,0x12,0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,
0x18,0x02,0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,
0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,
0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,
0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,
0x6e,0x67,0x6c,0x65,0x22,0x7b,0x0a,0x20,0x50,0x72,
0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,
0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x41,0x63,
0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,
0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c,0x65,
0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20,0x01,
0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c,0x65,
0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x33,0x0a,0x05,
0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,0x28,
0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,
0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,
0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,0x6e,
0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x42,
0x37,0x0a,0x15,0x6f,0x72,0x67,0x2e,0x77,0x70,0x69,
0x6c,0x69,0x62,0x2e,0x6d,0x61,0x74,0x68,0x2e,0x70,
0x72,0x6f,0x74,0x6f,0x42,0x1c,0x50,0x72,0x6f,0x74,
0x6f,0x62,0x75,0x66,0x4b,0x69,0x6e,0x65,0x6d,0x61,
0x74,0x69,0x63,0x73,0x44,0x65,0x73,0x63,0x72,0x69,
0x70,0x74,0x6f,0x72,0x50,0x01,0x4a,0x80,0x13,0x0a,
0x06,0x12,0x04,0x00,0x00,0x58,0x01,0x0a,0x08,0x0a,
0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a,
0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09,0x0a,
0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,0x08,
0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x2e,0x0a,0x09,
0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x2e,0x0a,
0x08,0x0a,0x01,0x08,0x12,0x03,0x08,0x00,0x3d,0x0a,
0x09,0x0a,0x02,0x08,0x08,0x12,0x03,0x08,0x00,0x3d,
0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x0a,0x00,0x22,
0x0a,0x09,0x0a,0x02,0x08,0x0a,0x12,0x03,0x0a,0x00,
0x22,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x0c,
0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,
0x12,0x03,0x0c,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,
0x00,0x02,0x00,0x12,0x03,0x0d,0x02,0x10,0x0a,0x0c,
0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x0d,
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,
0x01,0x12,0x03,0x0d,0x09,0x0b,0x0a,0x0c,0x0a,0x05,
0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x0d,0x0e,0x0f,
0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,
0x0e,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
0x01,0x05,0x12,0x03,0x0e,0x02,0x08,0x0a,0x0c,0x0a,
0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0e,0x09,
0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,
0x12,0x03,0x0e,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,
0x00,0x02,0x02,0x12,0x03,0x0f,0x02,0x13,0x0a,0x0c,
0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0f,
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,
0x01,0x12,0x03,0x0f,0x09,0x0e,0x0a,0x0c,0x0a,0x05,
0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0f,0x11,0x12,
0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x12,0x00,
0x16,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,
0x03,0x12,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,
0x02,0x00,0x12,0x03,0x13,0x02,0x10,0x0a,0x0c,0x0a,
0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x13,0x02,
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01,
0x12,0x03,0x13,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,
0x01,0x02,0x00,0x03,0x12,0x03,0x13,0x0e,0x0f,0x0a,
0x0b,0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x14,
0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,
0x05,0x12,0x03,0x14,0x02,0x08,0x0a,0x0c,0x0a,0x05,
0x04,0x01,0x02,0x01,0x01,0x12,0x03,0x14,0x09,0x0b,
0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12,
0x03,0x14,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,
0x02,0x02,0x12,0x03,0x15,0x02,0x13,0x0a,0x0c,0x0a,
0x05,0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x15,0x02,
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01,
0x12,0x03,0x15,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,
0x01,0x02,0x02,0x03,0x12,0x03,0x15,0x11,0x12,0x0a,
0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x18,0x00,0x1a,
0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03,
0x18,0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,
0x00,0x12,0x03,0x19,0x02,0x18,0x0a,0x0c,0x0a,0x05,
0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x19,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12,
0x03,0x19,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,
0x02,0x00,0x03,0x12,0x03,0x19,0x16,0x17,0x0a,0x0a,
0x0a,0x02,0x04,0x03,0x12,0x04,0x1c,0x00,0x1f,0x01,
0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x1c,
0x08,0x2c,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,
0x12,0x03,0x1d,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,
0x03,0x02,0x00,0x05,0x12,0x03,0x1d,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,
0x1d,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,
0x00,0x03,0x12,0x03,0x1d,0x10,0x11,0x0a,0x0b,0x0a,
0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x1e,0x02,0x13,
0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,
0x03,0x1e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,
0x02,0x01,0x01,0x12,0x03,0x1e,0x09,0x0e,0x0a,0x0c,
0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1e,
0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,
0x21,0x00,0x24,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,
0x01,0x12,0x03,0x21,0x08,0x33,0x0a,0x0b,0x0a,0x04,
0x04,0x04,0x02,0x00,0x12,0x03,0x22,0x02,0x12,0x0a,
0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03,
0x22,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,
0x00,0x01,0x12,0x03,0x22,0x09,0x0d,0x0a,0x0c,0x0a,
0x05,0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x22,0x10,
0x11,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,
0x03,0x23,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,
0x02,0x01,0x05,0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,
0x0a,0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x23,
0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,
0x03,0x12,0x03,0x23,0x11,0x12,0x0a,0x0a,0x0a,0x02,
0x04,0x05,0x12,0x04,0x26,0x00,0x29,0x01,0x0a,0x0a,
0x0a,0x03,0x04,0x05,0x01,0x12,0x03,0x26,0x08,0x2f,
0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,
0x27,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,
0x00,0x05,0x12,0x03,0x27,0x02,0x08,0x0a,0x0c,0x0a,
0x05,0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x27,0x09,
0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,
0x12,0x03,0x27,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,
0x05,0x02,0x01,0x12,0x03,0x28,0x02,0x13,0x0a,0x0c,
0x0a,0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x28,
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,
0x01,0x12,0x03,0x28,0x09,0x0e,0x0a,0x0c,0x0a,0x05,
0x04,0x05,0x02,0x01,0x03,0x12,0x03,0x28,0x11,0x12,
0x0a,0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x2b,0x00,
0x30,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,
0x03,0x2b,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,
0x02,0x00,0x12,0x03,0x2c,0x02,0x27,0x0a,0x0c,0x0a,
0x05,0x04,0x06,0x02,0x00,0x06,0x12,0x03,0x2c,0x02,
0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,
0x12,0x03,0x2c,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,
0x06,0x02,0x00,0x03,0x12,0x03,0x2c,0x25,0x26,0x0a,
0x0b,0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x2d,
0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,
0x06,0x12,0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a,0x05,
0x04,0x06,0x02,0x01,0x01,0x12,0x03,0x2d,0x18,0x23,
0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,
0x03,0x2d,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,
0x02,0x02,0x12,0x03,0x2e,0x02,0x26,0x0a,0x0c,0x0a,
0x05,0x04,0x06,0x02,0x02,0x06,0x12,0x03,0x2e,0x02,
0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,
0x12,0x03,0x2e,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,
0x06,0x02,0x02,0x03,0x12,0x03,0x2e,0x24,0x25,0x0a,
0x0b,0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2f,
0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,
0x06,0x12,0x03,0x2f,0x02,0x17,0x0a,0x0c,0x0a,0x05,
0x04,0x06,0x02,0x03,0x01,0x12,0x03,0x2f,0x18,0x22,
0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,
0x03,0x2f,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,
0x12,0x04,0x32,0x00,0x37,0x01,0x0a,0x0a,0x0a,0x03,
0x04,0x07,0x01,0x12,0x03,0x32,0x08,0x2a,0x0a,0x0b,
0x0a,0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x33,0x02,
0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05,
0x12,0x03,0x33,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,
0x07,0x02,0x00,0x01,0x12,0x03,0x33,0x09,0x13,0x0a,
0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03,
0x33,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,
0x01,0x12,0x03,0x34,0x02,0x19,0x0a,0x0c,0x0a,0x05,
0x04,0x07,0x02,0x01,0x05,0x12,0x03,0x34,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12,
0x03,0x34,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,
0x02,0x01,0x03,0x12,0x03,0x34,0x17,0x18,0x0a,0x0b,
0x0a,0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x35,0x02,
0x17,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05,
0x12,0x03,0x35,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,
0x07,0x02,0x02,0x01,0x12,0x03,0x35,0x09,0x12,0x0a,
0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03,
0x35,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,
0x03,0x12,0x03,0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,
0x04,0x07,0x02,0x03,0x05,0x12,0x03,0x36,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12,
0x03,0x36,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,
0x02,0x03,0x03,0x12,0x03,0x36,0x16,0x17,0x0a,0x0a,
0x0a,0x02,0x04,0x08,0x12,0x04,0x39,0x00,0x3e,0x01,
0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x39,
0x08,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,
0x12,0x03,0x3a,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,
0x08,0x02,0x00,0x05,0x12,0x03,0x3a,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03,
0x3a,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,
0x00,0x03,0x12,0x03,0x3a,0x16,0x17,0x0a,0x0b,0x0a,
0x04,0x04,0x08,0x02,0x01,0x12,0x03,0x3b,0x02,0x19,
0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12,
0x03,0x3b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,
0x02,0x01,0x01,0x12,0x03,0x3b,0x09,0x14,0x0a,0x0c,
0x0a,0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x3b,
0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,
0x12,0x03,0x3c,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,
0x08,0x02,0x02,0x05,0x12,0x03,0x3c,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03,
0x3c,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,
0x02,0x03,0x12,0x03,0x3c,0x15,0x16,0x0a,0x0b,0x0a,
0x04,0x04,0x08,0x02,0x03,0x12,0x03,0x3d,0x02,0x18,
0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12,
0x03,0x3d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,
0x02,0x03,0x01,0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,
0x0a,0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x3d,
0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,
0x40,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,
0x01,0x12,0x03,0x40,0x08,0x2e,0x0a,0x0b,0x0a,0x04,
0x04,0x09,0x02,0x00,0x12,0x03,0x41,0x02,0x18,0x0a,
0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03,
0x41,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,
0x00,0x01,0x12,0x03,0x41,0x09,0x13,0x0a,0x0c,0x0a,
0x05,0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x41,0x16,
0x17,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12,
0x03,0x42,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,
0x02,0x01,0x05,0x12,0x03,0x42,0x02,0x08,0x0a,0x0c,
0x0a,0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x42,
0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,
0x03,0x12,0x03,0x42,0x17,0x18,0x0a,0x0b,0x0a,0x04,
0x04,0x09,0x02,0x02,0x12,0x03,0x43,0x02,0x17,0x0a,
0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03,
0x43,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,
0x02,0x01,0x12,0x03,0x43,0x09,0x12,0x0a,0x0c,0x0a,
0x05,0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x43,0x15,
0x16,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12,
0x03,0x44,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,
0x02,0x03,0x05,0x12,0x03,0x44,0x02,0x08,0x0a,0x0c,
0x0a,0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x44,
0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,
0x03,0x12,0x03,0x44,0x16,0x17,0x0a,0x0a,0x0a,0x02,
0x04,0x0a,0x12,0x04,0x47,0x00,0x49,0x01,0x0a,0x0a,
0x0a,0x03,0x04,0x0a,0x01,0x12,0x03,0x47,0x08,0x25,
0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03,
0x48,0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,
0x00,0x04,0x12,0x03,0x48,0x02,0x0a,0x0a,0x0c,0x0a,
0x05,0x04,0x0a,0x02,0x00,0x06,0x12,0x03,0x48,0x0b,
0x20,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,
0x12,0x03,0x48,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,
0x0a,0x02,0x00,0x03,0x12,0x03,0x48,0x2b,0x2c,0x0a,
0x0a,0x0a,0x02,0x04,0x0b,0x12,0x04,0x4b,0x00,0x4e,
0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,
0x4b,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,
0x00,0x12,0x03,0x4c,0x02,0x16,0x0a,0x0c,0x0a,0x05,
0x04,0x0b,0x02,0x00,0x05,0x12,0x03,0x4c,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12,
0x03,0x4c,0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,
0x02,0x00,0x03,0x12,0x03,0x4c,0x14,0x15,0x0a,0x0b,
0x0a,0x04,0x04,0x0b,0x02,0x01,0x12,0x03,0x4d,0x02,
0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,
0x12,0x03,0x4d,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,
0x0b,0x02,0x01,0x01,0x12,0x03,0x4d,0x15,0x1a,0x0a,
0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03,
0x4d,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,
0x04,0x50,0x00,0x53,0x01,0x0a,0x0a,0x0a,0x03,0x04,
0x0c,0x01,0x12,0x03,0x50,0x08,0x21,0x0a,0x0b,0x0a,
0x04,0x04,0x0c,0x02,0x00,0x12,0x03,0x51,0x02,0x13,
0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12,
0x03,0x51,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,
0x02,0x00,0x01,0x12,0x03,0x51,0x09,0x0e,0x0a,0x0c,
0x0a,0x05,0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x51,
0x11,0x12,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,
0x12,0x03,0x52,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,
0x0c,0x02,0x01,0x06,0x12,0x03,0x52,0x02,0x14,0x0a,
0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03,
0x52,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,
0x01,0x03,0x12,0x03,0x52,0x1d,0x1e,0x0a,0x0a,0x0a,
0x02,0x04,0x0d,0x12,0x04,0x55,0x00,0x58,0x01,0x0a,
0x0a,0x0a,0x03,0x04,0x0d,0x01,0x12,0x03,0x55,0x08,
0x28,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,
0x03,0x56,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,
0x02,0x00,0x05,0x12,0x03,0x56,0x02,0x08,0x0a,0x0c,
0x0a,0x05,0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x56,
0x09,0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,
0x03,0x12,0x03,0x56,0x18,0x19,0x0a,0x0b,0x0a,0x04,
0x04,0x0d,0x02,0x01,0x12,0x03,0x57,0x02,0x1f,0x0a,
0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x06,0x12,0x03,
0x57,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,
0x01,0x01,0x12,0x03,0x57,0x15,0x1a,0x0a,0x0c,0x0a,
0x05,0x04,0x0d,0x02,0x01,0x03,0x12,0x03,0x57,0x1d,
0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33,
0x42,0x37,0x0a,0x15,0x6f,0x72,0x67,0x2e,0x77,0x70,
0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61,0x74,0x68,0x2e,
0x70,0x72,0x6f,0x74,0x6f,0x42,0x1c,0x50,0x72,0x6f,
0x74,0x6f,0x62,0x75,0x66,0x4b,0x69,0x6e,0x65,0x6d,
0x61,0x74,0x69,0x63,0x73,0x44,0x65,0x73,0x63,0x72,
0x69,0x70,0x74,0x6f,0x72,0x50,0x01,0x4a,0x80,0x13,
0x0a,0x06,0x12,0x04,0x00,0x00,0x58,0x01,0x0a,0x08,
0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,
0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09,
0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,
0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x2e,0x0a,
0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x2e,
0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x08,0x00,0x3d,
0x0a,0x09,0x0a,0x02,0x08,0x08,0x12,0x03,0x08,0x00,
0x3d,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x0a,0x00,
0x22,0x0a,0x09,0x0a,0x02,0x08,0x0a,0x12,0x03,0x0a,
0x00,0x22,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,
0x0c,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,
0x01,0x12,0x03,0x0c,0x08,0x21,0x0a,0x0b,0x0a,0x04,
0x04,0x00,0x02,0x00,0x12,0x03,0x0d,0x02,0x10,0x0a,
0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,
0x0d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
0x00,0x01,0x12,0x03,0x0d,0x09,0x0b,0x0a,0x0c,0x0a,
0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x0d,0x0e,
0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,
0x03,0x0e,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,
0x02,0x01,0x05,0x12,0x03,0x0e,0x02,0x08,0x0a,0x0c,
0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0e,
0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,
0x03,0x12,0x03,0x0e,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,
0x04,0x00,0x02,0x02,0x12,0x03,0x0f,0x02,0x13,0x0a,
0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,
0x0f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
0x02,0x01,0x12,0x03,0x0f,0x09,0x0e,0x0a,0x0c,0x0a,
0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0f,0x11,
0x12,0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x12,
0x00,0x16,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,
0x12,0x03,0x12,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,
0x01,0x02,0x00,0x12,0x03,0x13,0x02,0x10,0x0a,0x0c,
0x0a,0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x13,
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,
0x01,0x12,0x03,0x13,0x09,0x0b,0x0a,0x0c,0x0a,0x05,
0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x13,0x0e,0x0f,
0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,
0x14,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,
0x01,0x05,0x12,0x03,0x14,0x02,0x08,0x0a,0x0c,0x0a,
0x05,0x04,0x01,0x02,0x01,0x01,0x12,0x03,0x14,0x09,
0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,
0x12,0x03,0x14,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,
0x01,0x02,0x02,0x12,0x03,0x15,0x02,0x13,0x0a,0x0c,
0x0a,0x05,0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x15,
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,
0x01,0x12,0x03,0x15,0x09,0x0e,0x0a,0x0c,0x0a,0x05,
0x04,0x01,0x02,0x02,0x03,0x12,0x03,0x15,0x11,0x12,
0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x18,0x00,
0x1a,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,
0x03,0x18,0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,
0x02,0x00,0x12,0x03,0x19,0x02,0x18,0x0a,0x0c,0x0a,
0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x19,0x02,
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01,
0x12,0x03,0x19,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,
0x02,0x02,0x00,0x03,0x12,0x03,0x19,0x16,0x17,0x0a,
0x0a,0x0a,0x02,0x04,0x03,0x12,0x04,0x1c,0x00,0x1f,
0x01,0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,
0x1c,0x08,0x30,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,
0x00,0x12,0x03,0x1d,0x02,0x12,0x0a,0x0c,0x0a,0x05,
0x04,0x03,0x02,0x00,0x05,0x12,0x03,0x1d,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,
0x03,0x1d,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,
0x02,0x00,0x03,0x12,0x03,0x1d,0x10,0x11,0x0a,0x0b,
0x0a,0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x1e,0x02,
0x13,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,
0x12,0x03,0x1e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,
0x03,0x02,0x01,0x01,0x12,0x03,0x1e,0x09,0x0e,0x0a,
0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,
0x1e,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,
0x04,0x21,0x00,0x24,0x01,0x0a,0x0a,0x0a,0x03,0x04,
0x04,0x01,0x12,0x03,0x21,0x08,0x33,0x0a,0x0b,0x0a,
0x04,0x04,0x04,0x02,0x00,0x12,0x03,0x22,0x02,0x12,
0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x05,0x12,
0x03,0x22,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,
0x02,0x00,0x01,0x12,0x03,0x22,0x09,0x0d,0x0a,0x0c,
0x0a,0x05,0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x22,
0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,
0x12,0x03,0x23,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,
0x04,0x02,0x01,0x05,0x12,0x03,0x23,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,
0x23,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,
0x01,0x03,0x12,0x03,0x23,0x11,0x12,0x0a,0x0a,0x0a,
0x02,0x04,0x05,0x12,0x04,0x26,0x00,0x29,0x01,0x0a,
0x0a,0x0a,0x03,0x04,0x05,0x01,0x12,0x03,0x26,0x08,
0x2f,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,
0x03,0x27,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,
0x02,0x00,0x05,0x12,0x03,0x27,0x02,0x08,0x0a,0x0c,
0x0a,0x05,0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x27,
0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,
0x03,0x12,0x03,0x27,0x10,0x11,0x0a,0x0b,0x0a,0x04,
0x04,0x05,0x02,0x01,0x12,0x03,0x28,0x02,0x13,0x0a,
0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,
0x28,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,
0x01,0x01,0x12,0x03,0x28,0x09,0x0e,0x0a,0x0c,0x0a,
0x05,0x04,0x05,0x02,0x01,0x03,0x12,0x03,0x28,0x11,
0x12,0x0a,0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x2b,
0x00,0x30,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,
0x12,0x03,0x2b,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,
0x06,0x02,0x00,0x12,0x03,0x2c,0x02,0x27,0x0a,0x0c,
0x0a,0x05,0x04,0x06,0x02,0x00,0x06,0x12,0x03,0x2c,
0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,
0x01,0x12,0x03,0x2c,0x18,0x22,0x0a,0x0c,0x0a,0x05,
0x04,0x06,0x02,0x00,0x03,0x12,0x03,0x2c,0x25,0x26,
0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,
0x2d,0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,
0x01,0x06,0x12,0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a,
0x05,0x04,0x06,0x02,0x01,0x01,0x12,0x03,0x2d,0x18,
0x23,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,
0x12,0x03,0x2d,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,
0x06,0x02,0x02,0x12,0x03,0x2e,0x02,0x26,0x0a,0x0c,
0x0a,0x05,0x04,0x06,0x02,0x02,0x06,0x12,0x03,0x2e,
0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,
0x01,0x12,0x03,0x2e,0x18,0x21,0x0a,0x0c,0x0a,0x05,
0x04,0x06,0x02,0x02,0x03,0x12,0x03,0x2e,0x24,0x25,
0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,
0x2f,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,
0x03,0x06,0x12,0x03,0x2f,0x02,0x17,0x0a,0x0c,0x0a,
0x05,0x04,0x06,0x02,0x03,0x01,0x12,0x03,0x2f,0x18,
0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,
0x12,0x03,0x2f,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,
0x07,0x12,0x04,0x32,0x00,0x37,0x01,0x0a,0x0a,0x0a,
0x03,0x04,0x07,0x01,0x12,0x03,0x32,0x08,0x2a,0x0a,
0x0b,0x0a,0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x33,
0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,
0x05,0x12,0x03,0x33,0x02,0x08,0x0a,0x0c,0x0a,0x05,
0x04,0x07,0x02,0x00,0x01,0x12,0x03,0x33,0x09,0x13,
0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,
0x03,0x33,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,
0x02,0x01,0x12,0x03,0x34,0x02,0x19,0x0a,0x0c,0x0a,
0x05,0x04,0x07,0x02,0x01,0x05,0x12,0x03,0x34,0x02,
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,
0x12,0x03,0x34,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,
0x07,0x02,0x01,0x03,0x12,0x03,0x34,0x17,0x18,0x0a,
0x0b,0x0a,0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x35,
0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,
0x05,0x12,0x03,0x35,0x02,0x08,0x0a,0x0c,0x0a,0x05,
0x04,0x07,0x02,0x02,0x01,0x12,0x03,0x35,0x09,0x12,
0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,
0x03,0x35,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,
0x02,0x03,0x12,0x03,0x36,0x02,0x18,0x0a,0x0c,0x0a,
0x05,0x04,0x07,0x02,0x03,0x05,0x12,0x03,0x36,0x02,
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,
0x12,0x03,0x36,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,
0x07,0x02,0x03,0x03,0x12,0x03,0x36,0x16,0x17,0x0a,
0x0a,0x0a,0x02,0x04,0x08,0x12,0x04,0x39,0x00,0x3e,
0x01,0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,
0x39,0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,
0x00,0x12,0x03,0x3a,0x02,0x18,0x0a,0x0c,0x0a,0x05,
0x04,0x08,0x02,0x00,0x05,0x12,0x03,0x3a,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12,
0x03,0x3a,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,
0x02,0x00,0x03,0x12,0x03,0x3a,0x16,0x17,0x0a,0x0b,
0x0a,0x04,0x04,0x08,0x02,0x01,0x12,0x03,0x3b,0x02,
0x19,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05,
0x12,0x03,0x3b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,
0x08,0x02,0x01,0x01,0x12,0x03,0x3b,0x09,0x14,0x0a,
0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03,
0x3b,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,
0x02,0x12,0x03,0x3c,0x02,0x17,0x0a,0x0c,0x0a,0x05,
0x04,0x08,0x02,0x02,0x05,0x12,0x03,0x3c,0x02,0x08,
0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12,
0x03,0x3c,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,
0x02,0x02,0x03,0x12,0x03,0x3c,0x15,0x16,0x0a,0x0b,
0x0a,0x04,0x04,0x08,0x02,0x03,0x12,0x03,0x3d,0x02,
0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05,
0x12,0x03,0x3d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,
0x08,0x02,0x03,0x01,0x12,0x03,0x3d,0x09,0x13,0x0a,
0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03,
0x3d,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,
0x04,0x40,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03,0x04,
0x09,0x01,0x12,0x03,0x40,0x08,0x2e,0x0a,0x0b,0x0a,
0x04,0x04,0x09,0x02,0x00,0x12,0x03,0x41,0x02,0x18,
0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,
0x03,0x41,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,
0x02,0x00,0x01,0x12,0x03,0x41,0x09,0x13,0x0a,0x0c,
0x0a,0x05,0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x41,
0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,
0x12,0x03,0x42,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,
0x09,0x02,0x01,0x05,0x12,0x03,0x42,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,
0x42,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,
0x01,0x03,0x12,0x03,0x42,0x17,0x18,0x0a,0x0b,0x0a,
0x04,0x04,0x09,0x02,0x02,0x12,0x03,0x43,0x02,0x17,
0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12,
0x03,0x43,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,
0x02,0x02,0x01,0x12,0x03,0x43,0x09,0x12,0x0a,0x0c,
0x0a,0x05,0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x43,
0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,
0x12,0x03,0x44,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,
0x09,0x02,0x03,0x05,0x12,0x03,0x44,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03,
0x44,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,
0x03,0x03,0x12,0x03,0x44,0x16,0x17,0x0a,0x0a,0x0a,
0x02,0x04,0x0a,0x12,0x04,0x47,0x00,0x49,0x01,0x0a,
0x0a,0x0a,0x03,0x04,0x0a,0x01,0x12,0x03,0x47,0x08,
0x25,0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,
0x03,0x48,0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,
0x02,0x00,0x04,0x12,0x03,0x48,0x02,0x0a,0x0a,0x0c,
0x0a,0x05,0x04,0x0a,0x02,0x00,0x06,0x12,0x03,0x48,
0x0b,0x20,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,
0x01,0x12,0x03,0x48,0x21,0x28,0x0a,0x0c,0x0a,0x05,
0x04,0x0a,0x02,0x00,0x03,0x12,0x03,0x48,0x2b,0x2c,
0x0a,0x0a,0x0a,0x02,0x04,0x0b,0x12,0x04,0x4b,0x00,
0x4e,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,
0x03,0x4b,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,
0x02,0x00,0x12,0x03,0x4c,0x02,0x16,0x0a,0x0c,0x0a,
0x05,0x04,0x0b,0x02,0x00,0x05,0x12,0x03,0x4c,0x02,
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,
0x12,0x03,0x4c,0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,
0x0b,0x02,0x00,0x03,0x12,0x03,0x4c,0x14,0x15,0x0a,
0x0b,0x0a,0x04,0x04,0x0b,0x02,0x01,0x12,0x03,0x4d,
0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,
0x06,0x12,0x03,0x4d,0x02,0x14,0x0a,0x0c,0x0a,0x05,
0x04,0x0b,0x02,0x01,0x01,0x12,0x03,0x4d,0x15,0x1a,
0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x03,0x12,
0x03,0x4d,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,
0x12,0x04,0x50,0x00,0x53,0x01,0x0a,0x0a,0x0a,0x03,
0x04,0x0c,0x01,0x12,0x03,0x50,0x08,0x24,0x0a,0x0b,
0x0a,0x04,0x04,0x0c,0x02,0x00,0x12,0x03,0x51,0x02,
0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,
0x12,0x03,0x51,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,
0x0c,0x02,0x00,0x01,0x12,0x03,0x51,0x09,0x11,0x0a,
0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x03,0x12,0x03,
0x51,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,
0x01,0x12,0x03,0x52,0x02,0x1f,0x0a,0x0c,0x0a,0x05,
0x04,0x0c,0x02,0x01,0x06,0x12,0x03,0x52,0x02,0x14,
0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x01,0x12,
0x03,0x52,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,
0x02,0x01,0x03,0x12,0x03,0x52,0x1d,0x1e,0x0a,0x0a,
0x0a,0x02,0x04,0x0d,0x12,0x04,0x55,0x00,0x58,0x01,
0x0a,0x0a,0x0a,0x03,0x04,0x0d,0x01,0x12,0x03,0x55,
0x08,0x28,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,
0x12,0x03,0x56,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,
0x0d,0x02,0x00,0x05,0x12,0x03,0x56,0x02,0x08,0x0a,
0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x01,0x12,0x03,
0x56,0x09,0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,
0x00,0x03,0x12,0x03,0x56,0x18,0x19,0x0a,0x0b,0x0a,
0x04,0x04,0x0d,0x02,0x01,0x12,0x03,0x57,0x02,0x1f,
0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x06,0x12,
0x03,0x57,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0d,
0x02,0x01,0x01,0x12,0x03,0x57,0x15,0x1a,0x0a,0x0c,
0x0a,0x05,0x04,0x0d,0x02,0x01,0x03,0x12,0x03,0x57,
0x1d,0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33,
};
static const char file_name[] = "kinematics.proto";
static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds";
std::string_view wpi_proto_ProtobufChassisSpeeds::msg_name(void) noexcept { return wpi_proto_ProtobufChassisSpeeds_name; }
pb_filedesc_t wpi_proto_ProtobufChassisSpeeds::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufChassisSpeeds, wpi_proto_ProtobufChassisSpeeds, AUTO)
static const char wpi_proto_ProtobufChassisVelocities_name[] = "wpi.proto.ProtobufChassisVelocities";
std::string_view wpi_proto_ProtobufChassisVelocities::msg_name(void) noexcept { return wpi_proto_ProtobufChassisVelocities_name; }
pb_filedesc_t wpi_proto_ProtobufChassisVelocities::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufChassisVelocities, wpi_proto_ProtobufChassisVelocities, AUTO)
static const char wpi_proto_ProtobufChassisAccelerations_name[] = "wpi.proto.ProtobufChassisAccelerations";
@@ -458,10 +461,10 @@ pb_filedesc_t wpi_proto_ProtobufDifferentialDriveKinematics::file_descriptor(voi
PB_BIND(wpi_proto_ProtobufDifferentialDriveKinematics, wpi_proto_ProtobufDifferentialDriveKinematics, AUTO)
static const char wpi_proto_ProtobufDifferentialDriveWheelSpeeds_name[] = "wpi.proto.ProtobufDifferentialDriveWheelSpeeds";
std::string_view wpi_proto_ProtobufDifferentialDriveWheelSpeeds::msg_name(void) noexcept { return wpi_proto_ProtobufDifferentialDriveWheelSpeeds_name; }
pb_filedesc_t wpi_proto_ProtobufDifferentialDriveWheelSpeeds::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufDifferentialDriveWheelSpeeds, wpi_proto_ProtobufDifferentialDriveWheelSpeeds, AUTO)
static const char wpi_proto_ProtobufDifferentialDriveWheelVelocities_name[] = "wpi.proto.ProtobufDifferentialDriveWheelVelocities";
std::string_view wpi_proto_ProtobufDifferentialDriveWheelVelocities::msg_name(void) noexcept { return wpi_proto_ProtobufDifferentialDriveWheelVelocities_name; }
pb_filedesc_t wpi_proto_ProtobufDifferentialDriveWheelVelocities::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufDifferentialDriveWheelVelocities, wpi_proto_ProtobufDifferentialDriveWheelVelocities, AUTO)
static const char wpi_proto_ProtobufDifferentialDriveWheelAccelerations_name[] = "wpi.proto.ProtobufDifferentialDriveWheelAccelerations";
@@ -488,10 +491,10 @@ pb_filedesc_t wpi_proto_ProtobufMecanumDriveWheelPositions::file_descriptor(void
PB_BIND(wpi_proto_ProtobufMecanumDriveWheelPositions, wpi_proto_ProtobufMecanumDriveWheelPositions, AUTO)
static const char wpi_proto_ProtobufMecanumDriveWheelSpeeds_name[] = "wpi.proto.ProtobufMecanumDriveWheelSpeeds";
std::string_view wpi_proto_ProtobufMecanumDriveWheelSpeeds::msg_name(void) noexcept { return wpi_proto_ProtobufMecanumDriveWheelSpeeds_name; }
pb_filedesc_t wpi_proto_ProtobufMecanumDriveWheelSpeeds::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufMecanumDriveWheelSpeeds, wpi_proto_ProtobufMecanumDriveWheelSpeeds, AUTO)
static const char wpi_proto_ProtobufMecanumDriveWheelVelocities_name[] = "wpi.proto.ProtobufMecanumDriveWheelVelocities";
std::string_view wpi_proto_ProtobufMecanumDriveWheelVelocities::msg_name(void) noexcept { return wpi_proto_ProtobufMecanumDriveWheelVelocities_name; }
pb_filedesc_t wpi_proto_ProtobufMecanumDriveWheelVelocities::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufMecanumDriveWheelVelocities, wpi_proto_ProtobufMecanumDriveWheelVelocities, AUTO)
static const char wpi_proto_ProtobufMecanumDriveWheelAccelerations_name[] = "wpi.proto.ProtobufMecanumDriveWheelAccelerations";
@@ -512,10 +515,10 @@ pb_filedesc_t wpi_proto_ProtobufSwerveModulePosition::file_descriptor(void) noex
PB_BIND(wpi_proto_ProtobufSwerveModulePosition, wpi_proto_ProtobufSwerveModulePosition, AUTO)
static const char wpi_proto_ProtobufSwerveModuleState_name[] = "wpi.proto.ProtobufSwerveModuleState";
std::string_view wpi_proto_ProtobufSwerveModuleState::msg_name(void) noexcept { return wpi_proto_ProtobufSwerveModuleState_name; }
pb_filedesc_t wpi_proto_ProtobufSwerveModuleState::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufSwerveModuleState, wpi_proto_ProtobufSwerveModuleState, AUTO)
static const char wpi_proto_ProtobufSwerveModuleVelocity_name[] = "wpi.proto.ProtobufSwerveModuleVelocity";
std::string_view wpi_proto_ProtobufSwerveModuleVelocity::msg_name(void) noexcept { return wpi_proto_ProtobufSwerveModuleVelocity_name; }
pb_filedesc_t wpi_proto_ProtobufSwerveModuleVelocity::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
PB_BIND(wpi_proto_ProtobufSwerveModuleVelocity, wpi_proto_ProtobufSwerveModuleVelocity, AUTO)
static const char wpi_proto_ProtobufSwerveModuleAcceleration_name[] = "wpi.proto.ProtobufSwerveModuleAcceleration";

92
wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h generated
View File

@@ -16,7 +16,7 @@
#endif
/* Struct definitions */
typedef struct _wpi_proto_ProtobufChassisSpeeds {
typedef struct _wpi_proto_ProtobufChassisVelocities {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
static std::string_view msg_name(void) noexcept;
static pb_filedesc_t file_descriptor(void) noexcept;
@@ -24,7 +24,7 @@ typedef struct _wpi_proto_ProtobufChassisSpeeds {
double vx;
double vy;
double omega;
} wpi_proto_ProtobufChassisSpeeds;
} wpi_proto_ProtobufChassisVelocities;
typedef struct _wpi_proto_ProtobufChassisAccelerations {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
@@ -44,14 +44,14 @@ typedef struct _wpi_proto_ProtobufDifferentialDriveKinematics {
double trackwidth;
} wpi_proto_ProtobufDifferentialDriveKinematics;
typedef struct _wpi_proto_ProtobufDifferentialDriveWheelSpeeds {
typedef struct _wpi_proto_ProtobufDifferentialDriveWheelVelocities {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
static std::string_view msg_name(void) noexcept;
static pb_filedesc_t file_descriptor(void) noexcept;
double left;
double right;
} wpi_proto_ProtobufDifferentialDriveWheelSpeeds;
} wpi_proto_ProtobufDifferentialDriveWheelVelocities;
typedef struct _wpi_proto_ProtobufDifferentialDriveWheelAccelerations {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
@@ -93,7 +93,7 @@ typedef struct _wpi_proto_ProtobufMecanumDriveWheelPositions {
double rear_right;
} wpi_proto_ProtobufMecanumDriveWheelPositions;
typedef struct _wpi_proto_ProtobufMecanumDriveWheelSpeeds {
typedef struct _wpi_proto_ProtobufMecanumDriveWheelVelocities {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
static std::string_view msg_name(void) noexcept;
static pb_filedesc_t file_descriptor(void) noexcept;
@@ -102,7 +102,7 @@ typedef struct _wpi_proto_ProtobufMecanumDriveWheelSpeeds {
double front_right;
double rear_left;
double rear_right;
} wpi_proto_ProtobufMecanumDriveWheelSpeeds;
} wpi_proto_ProtobufMecanumDriveWheelVelocities;
typedef struct _wpi_proto_ProtobufMecanumDriveWheelAccelerations {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
@@ -132,14 +132,14 @@ typedef struct _wpi_proto_ProtobufSwerveModulePosition {
pb_callback_t angle;
} wpi_proto_ProtobufSwerveModulePosition;
typedef struct _wpi_proto_ProtobufSwerveModuleState {
typedef struct _wpi_proto_ProtobufSwerveModuleVelocity {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
static std::string_view msg_name(void) noexcept;
static pb_filedesc_t file_descriptor(void) noexcept;
double speed;
double velocity;
pb_callback_t angle;
} wpi_proto_ProtobufSwerveModuleState;
} wpi_proto_ProtobufSwerveModuleVelocity;
typedef struct _wpi_proto_ProtobufSwerveModuleAcceleration {
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
@@ -152,45 +152,45 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAcceleration {
/* Initializer values for message structs */
#define wpi_proto_ProtobufChassisSpeeds_init_default {0, 0, 0}
#define wpi_proto_ProtobufChassisVelocities_init_default {0, 0, 0}
#define wpi_proto_ProtobufChassisAccelerations_init_default {0, 0, 0}
#define wpi_proto_ProtobufDifferentialDriveKinematics_init_default {0}
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_init_default {0, 0}
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_init_default {0, 0}
#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_init_default {0, 0}
#define wpi_proto_ProtobufDifferentialDriveWheelPositions_init_default {0, 0}
#define wpi_proto_ProtobufMecanumDriveKinematics_init_default {{{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
#define wpi_proto_ProtobufMecanumDriveWheelPositions_init_default {0, 0, 0, 0}
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_init_default {0, 0, 0, 0}
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_init_default {0, 0, 0, 0}
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_init_default {0, 0, 0, 0}
#define wpi_proto_ProtobufSwerveDriveKinematics_init_default {{{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModulePosition_init_default {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModuleState_init_default {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModuleVelocity_init_default {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModuleAcceleration_init_default {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufChassisSpeeds_init_zero {0, 0, 0}
#define wpi_proto_ProtobufChassisVelocities_init_zero {0, 0, 0}
#define wpi_proto_ProtobufChassisAccelerations_init_zero {0, 0, 0}
#define wpi_proto_ProtobufDifferentialDriveKinematics_init_zero {0}
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_init_zero {0, 0}
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_init_zero {0, 0}
#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_init_zero {0, 0}
#define wpi_proto_ProtobufDifferentialDriveWheelPositions_init_zero {0, 0}
#define wpi_proto_ProtobufMecanumDriveKinematics_init_zero {{{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
#define wpi_proto_ProtobufMecanumDriveWheelPositions_init_zero {0, 0, 0, 0}
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_init_zero {0, 0, 0, 0}
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_init_zero {0, 0, 0, 0}
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_init_zero {0, 0, 0, 0}
#define wpi_proto_ProtobufSwerveDriveKinematics_init_zero {{{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModulePosition_init_zero {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModuleState_init_zero {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModuleVelocity_init_zero {0, {{NULL}, NULL}}
#define wpi_proto_ProtobufSwerveModuleAcceleration_init_zero {0, {{NULL}, NULL}}
/* Field tags (for use in manual encoding/decoding) */
#define wpi_proto_ProtobufChassisSpeeds_vx_tag 1
#define wpi_proto_ProtobufChassisSpeeds_vy_tag 2
#define wpi_proto_ProtobufChassisSpeeds_omega_tag 3
#define wpi_proto_ProtobufChassisVelocities_vx_tag 1
#define wpi_proto_ProtobufChassisVelocities_vy_tag 2
#define wpi_proto_ProtobufChassisVelocities_omega_tag 3
#define wpi_proto_ProtobufChassisAccelerations_ax_tag 1
#define wpi_proto_ProtobufChassisAccelerations_ay_tag 2
#define wpi_proto_ProtobufChassisAccelerations_alpha_tag 3
#define wpi_proto_ProtobufDifferentialDriveKinematics_trackwidth_tag 1
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_left_tag 1
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_right_tag 2
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_left_tag 1
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_right_tag 2
#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_left_tag 1
#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_right_tag 2
#define wpi_proto_ProtobufDifferentialDriveWheelPositions_left_tag 1
@@ -203,10 +203,10 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAcceleration {
#define wpi_proto_ProtobufMecanumDriveWheelPositions_front_right_tag 2
#define wpi_proto_ProtobufMecanumDriveWheelPositions_rear_left_tag 3
#define wpi_proto_ProtobufMecanumDriveWheelPositions_rear_right_tag 4
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_front_left_tag 1
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_front_right_tag 2
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_rear_left_tag 3
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_rear_right_tag 4
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_front_left_tag 1
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_front_right_tag 2
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_rear_left_tag 3
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_rear_right_tag 4
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_front_left_tag 1
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_front_right_tag 2
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_rear_left_tag 3
@@ -214,18 +214,18 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAcceleration {
#define wpi_proto_ProtobufSwerveDriveKinematics_modules_tag 1
#define wpi_proto_ProtobufSwerveModulePosition_distance_tag 1
#define wpi_proto_ProtobufSwerveModulePosition_angle_tag 2
#define wpi_proto_ProtobufSwerveModuleState_speed_tag 1
#define wpi_proto_ProtobufSwerveModuleState_angle_tag 2
#define wpi_proto_ProtobufSwerveModuleVelocity_velocity_tag 1
#define wpi_proto_ProtobufSwerveModuleVelocity_angle_tag 2
#define wpi_proto_ProtobufSwerveModuleAcceleration_acceleration_tag 1
#define wpi_proto_ProtobufSwerveModuleAcceleration_angle_tag 2
/* Struct field encoding specification for nanopb */
#define wpi_proto_ProtobufChassisSpeeds_FIELDLIST(X, a) \
#define wpi_proto_ProtobufChassisVelocities_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, vx, 1) \
X(a, STATIC, SINGULAR, DOUBLE, vy, 2) \
X(a, STATIC, SINGULAR, DOUBLE, omega, 3)
#define wpi_proto_ProtobufChassisSpeeds_CALLBACK NULL
#define wpi_proto_ProtobufChassisSpeeds_DEFAULT NULL
#define wpi_proto_ProtobufChassisVelocities_CALLBACK NULL
#define wpi_proto_ProtobufChassisVelocities_DEFAULT NULL
#define wpi_proto_ProtobufChassisAccelerations_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, ax, 1) \
@@ -239,11 +239,11 @@ X(a, STATIC, SINGULAR, DOUBLE, trackwidth, 1)
#define wpi_proto_ProtobufDifferentialDriveKinematics_CALLBACK NULL
#define wpi_proto_ProtobufDifferentialDriveKinematics_DEFAULT NULL
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_FIELDLIST(X, a) \
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, left, 1) \
X(a, STATIC, SINGULAR, DOUBLE, right, 2)
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_CALLBACK NULL
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_DEFAULT NULL
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_CALLBACK NULL
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_DEFAULT NULL
#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, left, 1) \
@@ -277,13 +277,13 @@ X(a, STATIC, SINGULAR, DOUBLE, rear_right, 4)
#define wpi_proto_ProtobufMecanumDriveWheelPositions_CALLBACK NULL
#define wpi_proto_ProtobufMecanumDriveWheelPositions_DEFAULT NULL
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_FIELDLIST(X, a) \
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, front_left, 1) \
X(a, STATIC, SINGULAR, DOUBLE, front_right, 2) \
X(a, STATIC, SINGULAR, DOUBLE, rear_left, 3) \
X(a, STATIC, SINGULAR, DOUBLE, rear_right, 4)
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_CALLBACK NULL
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_DEFAULT NULL
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_CALLBACK NULL
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_DEFAULT NULL
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, front_left, 1) \
@@ -306,12 +306,12 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2)
#define wpi_proto_ProtobufSwerveModulePosition_DEFAULT NULL
#define wpi_proto_ProtobufSwerveModulePosition_angle_MSGTYPE wpi_proto_ProtobufRotation2d
#define wpi_proto_ProtobufSwerveModuleState_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, speed, 1) \
#define wpi_proto_ProtobufSwerveModuleVelocity_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, velocity, 1) \
X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2)
#define wpi_proto_ProtobufSwerveModuleState_CALLBACK pb_default_field_callback
#define wpi_proto_ProtobufSwerveModuleState_DEFAULT NULL
#define wpi_proto_ProtobufSwerveModuleState_angle_MSGTYPE wpi_proto_ProtobufRotation2d
#define wpi_proto_ProtobufSwerveModuleVelocity_CALLBACK pb_default_field_callback
#define wpi_proto_ProtobufSwerveModuleVelocity_DEFAULT NULL
#define wpi_proto_ProtobufSwerveModuleVelocity_angle_MSGTYPE wpi_proto_ProtobufRotation2d
#define wpi_proto_ProtobufSwerveModuleAcceleration_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, acceleration, 1) \
@@ -324,18 +324,18 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2)
/* wpi_proto_ProtobufMecanumDriveKinematics_size depends on runtime parameters */
/* wpi_proto_ProtobufSwerveDriveKinematics_size depends on runtime parameters */
/* wpi_proto_ProtobufSwerveModulePosition_size depends on runtime parameters */
/* wpi_proto_ProtobufSwerveModuleState_size depends on runtime parameters */
/* wpi_proto_ProtobufSwerveModuleVelocity_size depends on runtime parameters */
/* wpi_proto_ProtobufSwerveModuleAcceleration_size depends on runtime parameters */
#define WPI_PROTO_KINEMATICS_NPB_H_MAX_SIZE wpi_proto_ProtobufMecanumDriveWheelPositions_size
#define wpi_proto_ProtobufChassisAccelerations_size 27
#define wpi_proto_ProtobufChassisSpeeds_size 27
#define wpi_proto_ProtobufChassisVelocities_size 27
#define wpi_proto_ProtobufDifferentialDriveKinematics_size 9
#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_size 18
#define wpi_proto_ProtobufDifferentialDriveWheelPositions_size 18
#define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_size 18
#define wpi_proto_ProtobufDifferentialDriveWheelVelocities_size 18
#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_size 36
#define wpi_proto_ProtobufMecanumDriveWheelPositions_size 36
#define wpi_proto_ProtobufMecanumDriveWheelSpeeds_size 36
#define wpi_proto_ProtobufMecanumDriveWheelVelocities_size 36
#endif

4
wpimath/src/main/java/org/wpilib/math/controller/BangBangController.java
View File

@@ -15,8 +15,8 @@ import org.wpilib.util.sendable.SendableRegistry;
* for velocity control of high-inertia mechanisms, and poorly on most other things.
*
* <p>Note that this is an *asymmetric* bang-bang controller - it will not exert any control effort
* in the reverse direction (e.g. it won't try to slow down an over-speeding shooter wheel). This
* asymmetry is *extremely important.* Bang-bang control is extremely simple, but also potentially
* in the reverse direction (e.g. it won't try to slow down an overspeeding shooter wheel). This
* asymmetry is *extremely important*. Bang-bang control is extremely simple, but also potentially
* hazardous. Always ensure that your motor controllers are set to "coast" before attempting to
* control them with a bang-bang controller.
*/

10
wpimath/src/main/java/org/wpilib/math/controller/LTVUnicycleController.java
View File

@@ -5,7 +5,7 @@
package org.wpilib.math.controller;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.linalg.DARE;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
@@ -105,7 +105,7 @@ public class LTVUnicycleController {
* @param angularVelocityRef The desired angular velocity in radians per second.
* @return The linear and angular velocity outputs of the LTV controller.
*/
public ChassisSpeeds calculate(
public ChassisVelocities calculate(
Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
// The change in global pose for a unicycle is defined by the following
// three equations.
@@ -142,7 +142,7 @@ public class LTVUnicycleController {
// [0 0 0] [0 1]
if (!m_enabled) {
return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
return new ChassisVelocities(linearVelocityRef, 0.0, angularVelocityRef);
}
// The DARE is ill-conditioned if the velocity is close to zero, so don't
@@ -188,7 +188,7 @@ public class LTVUnicycleController {
m_poseError.getRotation().getRadians());
var u = K.times(e);
return new ChassisSpeeds(
return new ChassisVelocities(
linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0));
}
@@ -202,7 +202,7 @@ public class LTVUnicycleController {
* @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
* @return The linear and angular velocity outputs of the LTV controller.
*/
public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
public ChassisVelocities calculate(Pose2d currentPose, Trajectory.State desiredState) {
return calculate(
currentPose,
desiredState.pose,

2
wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java
View File

@@ -20,7 +20,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/**
* Represents the acceleration of a robot chassis.
* Represents robot chassis accelerations.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have an ay
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum

235
wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java
View File

@@ -1,235 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import static org.wpilib.units.Units.RadiansPerSecond;
import java.util.Objects;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Twist2d;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.ChassisSpeedsProto;
import org.wpilib.math.kinematics.struct.ChassisSpeedsStruct;
import org.wpilib.math.util.MathUtil;
import org.wpilib.units.measure.AngularVelocity;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/**
* Represents the speed of a robot chassis. Although this class contains similar members compared to
* a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
* w.r.t to the robot frame of reference, a ChassisSpeeds object represents a robot's velocity.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
* will often have all three components.
*/
public class ChassisSpeeds
implements ProtobufSerializable, StructSerializable, Interpolatable<ChassisSpeeds> {
/** Velocity along the x-axis in meters per second. (Fwd is +) */
public double vx;
/** Velocity along the y-axis in meters per second. (Left is +) */
public double vy;
/** Angular velocity of the robot frame in radians per second. (CCW is +) */
public double omega;
/** ChassisSpeeds protobuf for serialization. */
public static final ChassisSpeedsProto proto = new ChassisSpeedsProto();
/** ChassisSpeeds struct for serialization. */
public static final ChassisSpeedsStruct struct = new ChassisSpeedsStruct();
/** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
public ChassisSpeeds() {}
/**
* Constructs a ChassisSpeeds object.
*
* @param vx Forward velocity in meters per second.
* @param vy Sideways velocity in meters per second.
* @param omega Angular velocity in radians per second.
*/
public ChassisSpeeds(double vx, double vy, double omega) {
this.vx = vx;
this.vy = vy;
this.omega = omega;
}
/**
* Constructs a ChassisSpeeds object.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
*/
public ChassisSpeeds(LinearVelocity vx, LinearVelocity vy, AngularVelocity omega) {
this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
}
/**
* Creates a Twist2d from ChassisSpeeds.
*
* @param dt The duration of the timestep in seconds.
* @return Twist2d.
*/
public Twist2d toTwist2d(double dt) {
return new Twist2d(vx * dt, vy * dt, omega * dt);
}
/**
* Discretizes a continuous-time chassis speed.
*
* <p>This function converts this continuous-time chassis speed into a discrete-time one such that
* when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
* velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
* along the y-axis, and omega * dt around the z-axis).
*
* <p>This is useful for compensating for translational skew when translating and rotating a
* holonomic (swerve or mecanum) drivetrain. However, scaling down the ChassisSpeeds after
* discretizing (e.g., when desaturating swerve module speeds) rotates the direction of net motion
* in the opposite direction of rotational velocity, introducing a different translational skew
* which is not accounted for by discretization.
*
* @param dt The duration of the timestep in seconds the speeds should be applied for.
* @return Discretized ChassisSpeeds.
*/
public ChassisSpeeds discretize(double dt) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredTransform = new Transform2d(vx * dt, vy * dt, new Rotation2d(omega * dt));
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = desiredTransform.log();
// Turn the chassis translation/rotation deltas into average velocities
return new ChassisSpeeds(twist.dx / dt, twist.dy / dt, twist.dtheta / dt);
}
/**
* Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
*/
public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated = new Translation2d(vx, vy).rotateBy(robotAngle.unaryMinus());
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omega);
}
/**
* Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
*/
public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vx, vy).rotateBy(robotAngle);
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omega);
}
/**
* Adds two ChassisSpeeds and returns the sum.
*
* <p>For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} =
* ChassisSpeeds{3.0, 2.0, 1.0}
*
* @param other The ChassisSpeeds to add.
* @return The sum of the ChassisSpeeds.
*/
public ChassisSpeeds plus(ChassisSpeeds other) {
return new ChassisSpeeds(vx + other.vx, vy + other.vy, omega + other.omega);
}
/**
* Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
*
* <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} =
* ChassisSpeeds{4.0, 2.0, 1.0}
*
* @param other The ChassisSpeeds to subtract.
* @return The difference between the two ChassisSpeeds.
*/
public ChassisSpeeds minus(ChassisSpeeds other) {
return new ChassisSpeeds(vx - other.vx, vy - other.vy, omega - other.omega);
}
/**
* Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components
* of the ChassisSpeeds.
*
* @return The inverse of the current ChassisSpeeds.
*/
public ChassisSpeeds unaryMinus() {
return new ChassisSpeeds(-vx, -vy, -omega);
}
/**
* Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
*
* <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 = ChassisSpeeds{4.0, 5.0, 1.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled ChassisSpeeds.
*/
public ChassisSpeeds times(double scalar) {
return new ChassisSpeeds(vx * scalar, vy * scalar, omega * scalar);
}
/**
* Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
*
* <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 = ChassisSpeeds{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled ChassisSpeeds.
*/
public ChassisSpeeds div(double scalar) {
return new ChassisSpeeds(vx / scalar, vy / scalar, omega / scalar);
}
@Override
public ChassisSpeeds interpolate(ChassisSpeeds endValue, double t) {
if (t <= 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
return new ChassisSpeeds(
MathUtil.lerp(this.vx, endValue.vx, t),
MathUtil.lerp(this.vy, endValue.vy, t),
MathUtil.lerp(this.omega, endValue.omega, t));
}
}
@Override
public final int hashCode() {
return Objects.hash(vx, vy, omega);
}
@Override
public boolean equals(Object o) {
return o == this
|| o instanceof ChassisSpeeds c && vx == c.vx && vy == c.vy && omega == c.omega;
}
@Override
public String toString() {
return String.format(
"ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", vx, vy, omega);
}
}

238
wpimath/src/main/java/org/wpilib/math/kinematics/ChassisVelocities.java Normal file
View File

@@ -0,0 +1,238 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import static org.wpilib.units.Units.RadiansPerSecond;
import java.util.Objects;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Twist2d;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.ChassisVelocitiesProto;
import org.wpilib.math.kinematics.struct.ChassisVelocitiesStruct;
import org.wpilib.math.util.MathUtil;
import org.wpilib.units.measure.AngularVelocity;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/**
* Represents robot chassis velocities.
*
* <p>Although this class contains similar members compared to a Twist2d, they do NOT represent the
* same thing. Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
* a ChassisVelocities object represents a robot's velocities.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
* will often have all three components.
*/
public class ChassisVelocities
implements ProtobufSerializable, StructSerializable, Interpolatable<ChassisVelocities> {
/** Velocity along the x-axis in meters per second. (Fwd is +) */
public double vx;
/** Velocity along the y-axis in meters per second. (Left is +) */
public double vy;
/** Angular velocity of the robot frame in radians per second. (CCW is +) */
public double omega;
/** ChassisVelocities protobuf for serialization. */
public static final ChassisVelocitiesProto proto = new ChassisVelocitiesProto();
/** ChassisVelocities struct for serialization. */
public static final ChassisVelocitiesStruct struct = new ChassisVelocitiesStruct();
/** Constructs a ChassisVelocities with zeros for dx, dy, and theta. */
public ChassisVelocities() {}
/**
* Constructs a ChassisVelocities object.
*
* @param vx Forward velocity in meters per second.
* @param vy Sideways velocity in meters per second.
* @param omega Angular velocity in radians per second.
*/
public ChassisVelocities(double vx, double vy, double omega) {
this.vx = vx;
this.vy = vy;
this.omega = omega;
}
/**
* Constructs a ChassisVelocities object.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
*/
public ChassisVelocities(LinearVelocity vx, LinearVelocity vy, AngularVelocity omega) {
this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
}
/**
* Creates a Twist2d from ChassisVelocities.
*
* @param dt The duration of the timestep in seconds.
* @return Twist2d.
*/
public Twist2d toTwist2d(double dt) {
return new Twist2d(vx * dt, vy * dt, omega * dt);
}
/**
* Discretizes continuous-time chassis velocities.
*
* <p>This function converts these continuous-time chassis velocities into discrete-time ones such
* that when the discrete-time chassis velocities are applied for one timestep, the robot moves as
* if the velocity components are independent (i.e., the robot moves v_x * dt along the x-axis,
* v_y * dt along the y-axis, and omega * dt around the z-axis).
*
* <p>This is useful for compensating for translational skew when translating and rotating a
* holonomic (swerve or mecanum) drivetrain. However, scaling down the ChassisVelocities after
* discretizing (e.g., when desaturating swerve module velocities) rotates the direction of net
* motion in the opposite direction of rotational velocity, introducing a different translational
* skew which is not accounted for by discretization.
*
* @param dt The duration of the timestep in seconds the velocities should be applied for.
* @return Discretized ChassisVelocities.
*/
public ChassisVelocities discretize(double dt) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredTransform = new Transform2d(vx * dt, vy * dt, new Rotation2d(omega * dt));
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = desiredTransform.log();
// Turn the chassis translation/rotation deltas into average velocities
return new ChassisVelocities(twist.dx / dt, twist.dy / dt, twist.dtheta / dt);
}
/**
* Converts this field-relative set of velocities into a robot-relative ChassisVelocities object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisVelocities object representing the velocities in the robot's frame of reference.
*/
public ChassisVelocities toRobotRelative(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated = new Translation2d(vx, vy).rotateBy(robotAngle.unaryMinus());
return new ChassisVelocities(rotated.getX(), rotated.getY(), omega);
}
/**
* Converts this robot-relative set of velocities into a field-relative ChassisVelocities object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisVelocities object representing the velocities in the field's frame of reference.
*/
public ChassisVelocities toFieldRelative(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vx, vy).rotateBy(robotAngle);
return new ChassisVelocities(rotated.getX(), rotated.getY(), omega);
}
/**
* Adds two ChassisVelocities and returns the sum.
*
* <p>For example, ChassisVelocities{1.0, 0.5, 0.75} + ChassisVelocities{2.0, 1.5, 0.25} =
* ChassisVelocities{3.0, 2.0, 1.0}
*
* @param other The ChassisVelocities to add.
* @return The sum of the ChassisVelocities.
*/
public ChassisVelocities plus(ChassisVelocities other) {
return new ChassisVelocities(vx + other.vx, vy + other.vy, omega + other.omega);
}
/**
* Subtracts the other ChassisVelocities from the current ChassisVelocities and returns the
* difference.
*
* <p>For example, ChassisVelocities{5.0, 4.0, 2.0} - ChassisVelocities{1.0, 2.0, 1.0} =
* ChassisVelocities{4.0, 2.0, 1.0}
*
* @param other The ChassisVelocities to subtract.
* @return The difference between the two ChassisVelocities.
*/
public ChassisVelocities minus(ChassisVelocities other) {
return new ChassisVelocities(vx - other.vx, vy - other.vy, omega - other.omega);
}
/**
* Returns the inverse of the current ChassisVelocities. This is equivalent to negating all
* components of the ChassisVelocities.
*
* @return The inverse of the current ChassisVelocities.
*/
public ChassisVelocities unaryMinus() {
return new ChassisVelocities(-vx, -vy, -omega);
}
/**
* Multiplies the ChassisVelocities by a scalar and returns the new ChassisVelocities.
*
* <p>For example, ChassisVelocities{2.0, 2.5, 1.0} * 2 = ChassisVelocities{4.0, 5.0, 1.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled ChassisVelocities.
*/
public ChassisVelocities times(double scalar) {
return new ChassisVelocities(vx * scalar, vy * scalar, omega * scalar);
}
/**
* Divides the ChassisVelocities by a scalar and returns the new ChassisVelocities.
*
* <p>For example, ChassisVelocities{2.0, 2.5, 1.0} / 2 = ChassisVelocities{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled ChassisVelocities.
*/
public ChassisVelocities div(double scalar) {
return new ChassisVelocities(vx / scalar, vy / scalar, omega / scalar);
}
@Override
public ChassisVelocities interpolate(ChassisVelocities endValue, double t) {
if (t <= 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
return new ChassisVelocities(
MathUtil.lerp(this.vx, endValue.vx, t),
MathUtil.lerp(this.vy, endValue.vy, t),
MathUtil.lerp(this.omega, endValue.omega, t));
}
}
@Override
public final int hashCode() {
return Objects.hash(vx, vy, omega);
}
@Override
public boolean equals(Object o) {
return o == this
|| o instanceof ChassisVelocities c && vx == c.vx && vy == c.vy && omega == c.omega;
}
@Override
public String toString() {
return String.format(
"ChassisVelocities(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", vx, vy, omega);
}
}

34
wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java
View File

@@ -18,14 +18,14 @@ import org.wpilib.util.struct.StructSerializable;
* Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
* velocities for a differential drive.
*
* <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
* <p>Inverse kinematics converts a desired chassis velocity into left and right velocity components
* whereas forward kinematics converts left and right component velocities into a linear and angular
* chassis speed.
* chassis velocity.
*/
public class DifferentialDriveKinematics
implements Kinematics<
DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations>,
ProtobufSerializable,
StructSerializable {
@@ -64,31 +64,31 @@ public class DifferentialDriveKinematics
}
/**
* Returns a chassis speed from left and right component velocities using forward kinematics.
* Returns a chassis velocity from left and right component velocities using forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
* @param wheelVelocities The left and right velocities.
* @return The chassis velocity.
*/
@Override
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.left + wheelSpeeds.right) / 2,
public ChassisVelocities toChassisVelocities(DifferentialDriveWheelVelocities wheelVelocities) {
return new ChassisVelocities(
(wheelVelocities.left + wheelVelocities.right) / 2,
0,
(wheelSpeeds.right - wheelSpeeds.left) / trackwidth);
(wheelVelocities.right - wheelVelocities.left) / trackwidth);
}
/**
* Returns left and right component velocities from a chassis speed using inverse kinematics.
* Returns left and right component velocities from a chassis velocity using inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
* chassis' speed.
* @param chassisVelocities The linear and angular (dx and dtheta) components that represent the
* chassis' velocity.
* @return The left and right velocities.
*/
@Override
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega,
chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega);
public DifferentialDriveWheelVelocities toWheelVelocities(ChassisVelocities chassisVelocities) {
return new DifferentialDriveWheelVelocities(
chassisVelocities.vx - trackwidth / 2 * chassisVelocities.omega,
chassisVelocities.vx + trackwidth / 2 * chassisVelocities.omega);
}
@Override

178
wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeeds.java
View File

@@ -1,178 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.DifferentialDriveWheelSpeedsProto;
import org.wpilib.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/** Represents the wheel speeds for a differential drive drivetrain. */
public class DifferentialDriveWheelSpeeds
implements Interpolatable<DifferentialDriveWheelSpeeds>,
ProtobufSerializable,
StructSerializable {
/** Speed of the left side of the robot in meters per second. */
public double left;
/** Speed of the right side of the robot in meters per second. */
public double right;
/** DifferentialDriveWheelSpeeds protobuf for serialization. */
public static final DifferentialDriveWheelSpeedsProto proto =
new DifferentialDriveWheelSpeedsProto();
/** DifferentialDriveWheelSpeeds struct for serialization. */
public static final DifferentialDriveWheelSpeedsStruct struct =
new DifferentialDriveWheelSpeedsStruct();
/** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
public DifferentialDriveWheelSpeeds() {}
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param left The left speed in meters per second.
* @param right The right speed in meters per second.
*/
public DifferentialDriveWheelSpeeds(double left, double right) {
this.left = left;
this.right = right;
}
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param left The left speed in meters per second.
* @param right The right speed in meters per second.
*/
public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) {
this(left.in(MetersPerSecond), right.in(MetersPerSecond));
}
/**
* Renormalizes the wheel speeds if any either side is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
* above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
*/
public void desaturate(double attainableMaxSpeed) {
double realMaxSpeed = Math.max(Math.abs(left), Math.abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}
/**
* Renormalizes the wheel speeds if any either side is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
* above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
*/
public void desaturate(LinearVelocity attainableMaxSpeed) {
desaturate(attainableMaxSpeed.in(MetersPerSecond));
}
/**
* Adds two DifferentialDriveWheelSpeeds and returns the sum.
*
* <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
* = DifferentialDriveWheelSpeeds{3.0, 2.0}
*
* @param other The DifferentialDriveWheelSpeeds to add.
* @return The sum of the DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
return new DifferentialDriveWheelSpeeds(left + other.left, right + other.right);
}
/**
* Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds
* and returns the difference.
*
* <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0}
* = DifferentialDriveWheelSpeeds{4.0, 2.0}
*
* @param other The DifferentialDriveWheelSpeeds to subtract.
* @return The difference between the two DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
return new DifferentialDriveWheelSpeeds(left - other.left, right - other.right);
}
/**
* Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
* all components of the DifferentialDriveWheelSpeeds.
*
* @return The inverse of the current DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds unaryMinus() {
return new DifferentialDriveWheelSpeeds(-left, -right);
}
/**
* Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
* DifferentialDriveWheelSpeeds.
*
* <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0,
* 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds times(double scalar) {
return new DifferentialDriveWheelSpeeds(left * scalar, right * scalar);
}
/**
* Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
* DifferentialDriveWheelSpeeds.
*
* <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0,
* 1.25}
*
* @param scalar The scalar to divide by.
* @return The scaled DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds div(double scalar) {
return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar);
}
/**
* Returns the linear interpolation of this DifferentialDriveWheelSpeeds and another.
*
* @param endValue The end value for the interpolation.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
@Override
public DifferentialDriveWheelSpeeds interpolate(DifferentialDriveWheelSpeeds endValue, double t) {
// Clamp t to [0, 1]
t = Math.max(0.0, Math.min(1.0, t));
return new DifferentialDriveWheelSpeeds(
left + t * (endValue.left - left), right + t * (endValue.right - right));
}
@Override
public String toString() {
return String.format(
"DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", left, right);
}
}

181
wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java Normal file
View File

@@ -0,0 +1,181 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.DifferentialDriveWheelVelocitiesProto;
import org.wpilib.math.kinematics.struct.DifferentialDriveWheelVelocitiesStruct;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/** Represents the wheel velocities for a differential drive drivetrain. */
public class DifferentialDriveWheelVelocities
implements Interpolatable<DifferentialDriveWheelVelocities>,
ProtobufSerializable,
StructSerializable {
/** Velocity of the left side of the robot in meters per second. */
public double left;
/** Velocity of the right side of the robot in meters per second. */
public double right;
/** DifferentialDriveWheelVelocities protobuf for serialization. */
public static final DifferentialDriveWheelVelocitiesProto proto =
new DifferentialDriveWheelVelocitiesProto();
/** DifferentialDriveWheelVelocities struct for serialization. */
public static final DifferentialDriveWheelVelocitiesStruct struct =
new DifferentialDriveWheelVelocitiesStruct();
/** Constructs a DifferentialDriveWheelVelocities with zeros for left and right velocities. */
public DifferentialDriveWheelVelocities() {}
/**
* Constructs a DifferentialDriveWheelVelocities.
*
* @param left The left velocity in meters per second.
* @param right The right velocity in meters per second.
*/
public DifferentialDriveWheelVelocities(double left, double right) {
this.left = left;
this.right = right;
}
/**
* Constructs a DifferentialDriveWheelVelocities.
*
* @param left The left velocity in meters per second.
* @param right The right velocity in meters per second.
*/
public DifferentialDriveWheelVelocities(LinearVelocity left, LinearVelocity right) {
this(left.in(MetersPerSecond), right.in(MetersPerSecond));
}
/**
* Renormalizes the wheel velocities if any either side is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more wheels may be
* above the max attainable velocity for the driving motor on that wheel. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
*
* @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
* reach.
*/
public void desaturate(double attainableMaxVelocity) {
double realMaxVelocity = Math.max(Math.abs(left), Math.abs(right));
if (realMaxVelocity > attainableMaxVelocity) {
left = left / realMaxVelocity * attainableMaxVelocity;
right = right / realMaxVelocity * attainableMaxVelocity;
}
}
/**
* Renormalizes the wheel velocities if any either side is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more wheels may be
* above the max attainable velocity for the driving motor on that wheel. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
*
* @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
* reach.
*/
public void desaturate(LinearVelocity attainableMaxVelocity) {
desaturate(attainableMaxVelocity.in(MetersPerSecond));
}
/**
* Adds two DifferentialDriveWheelVelocities and returns the sum.
*
* <p>For example, DifferentialDriveWheelVelocities{1.0, 0.5} +
* DifferentialDriveWheelVelocities{2.0, 1.5} = DifferentialDriveWheelVelocities{3.0, 2.0}
*
* @param other The DifferentialDriveWheelVelocities to add.
* @return The sum of the DifferentialDriveWheelVelocities.
*/
public DifferentialDriveWheelVelocities plus(DifferentialDriveWheelVelocities other) {
return new DifferentialDriveWheelVelocities(left + other.left, right + other.right);
}
/**
* Subtracts the other DifferentialDriveWheelVelocities from the current
* DifferentialDriveWheelVelocities and returns the difference.
*
* <p>For example, DifferentialDriveWheelVelocities{5.0, 4.0} -
* DifferentialDriveWheelVelocities{1.0, 2.0} = DifferentialDriveWheelVelocities{4.0, 2.0}
*
* @param other The DifferentialDriveWheelVelocities to subtract.
* @return The difference between the two DifferentialDriveWheelVelocities.
*/
public DifferentialDriveWheelVelocities minus(DifferentialDriveWheelVelocities other) {
return new DifferentialDriveWheelVelocities(left - other.left, right - other.right);
}
/**
* Returns the inverse of the current DifferentialDriveWheelVelocities. This is equivalent to
* negating all components of the DifferentialDriveWheelVelocities.
*
* @return The inverse of the current DifferentialDriveWheelVelocities.
*/
public DifferentialDriveWheelVelocities unaryMinus() {
return new DifferentialDriveWheelVelocities(-left, -right);
}
/**
* Multiplies the DifferentialDriveWheelVelocities by a scalar and returns the new
* DifferentialDriveWheelVelocities.
*
* <p>For example, DifferentialDriveWheelVelocities{2.0, 2.5} * 2 =
* DifferentialDriveWheelVelocities{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled DifferentialDriveWheelVelocities.
*/
public DifferentialDriveWheelVelocities times(double scalar) {
return new DifferentialDriveWheelVelocities(left * scalar, right * scalar);
}
/**
* Divides the DifferentialDriveWheelVelocities by a scalar and returns the new
* DifferentialDriveWheelVelocities.
*
* <p>For example, DifferentialDriveWheelVelocities{2.0, 2.5} / 2 =
* DifferentialDriveWheelVelocities{1.0, 1.25}
*
* @param scalar The scalar to divide by.
* @return The scaled DifferentialDriveWheelVelocities.
*/
public DifferentialDriveWheelVelocities div(double scalar) {
return new DifferentialDriveWheelVelocities(left / scalar, right / scalar);
}
/**
* Returns the linear interpolation of this DifferentialDriveWheelVelocities and another.
*
* @param endValue The end value for the interpolation.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
@Override
public DifferentialDriveWheelVelocities interpolate(
DifferentialDriveWheelVelocities endValue, double t) {
// Clamp t to [0, 1]
t = Math.max(0.0, Math.min(1.0, t));
return new DifferentialDriveWheelVelocities(
left + t * (endValue.left - left), right + t * (endValue.right - right));
}
@Override
public String toString() {
return String.format(
"DifferentialDriveWheelVelocities(Left: %.2f m/s, Right: %.2f m/s)", left, right);
}
}

30
wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java
View File

@@ -8,33 +8,33 @@ import org.wpilib.math.geometry.Twist2d;
import org.wpilib.math.interpolation.Interpolator;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds and
* chassis accelerations into wheel accelerations. Robot code should not use this directly- Instead,
* use the particular type for your drivetrain (e.g., {@link DifferentialDriveKinematics}).
* Helper class that converts a chassis velocity (dx and dtheta components) into wheel velocities
* and chassis accelerations into wheel accelerations. Robot code should not use this directly-
* Instead, use the particular type for your drivetrain (e.g., {@link DifferentialDriveKinematics}).
*
* @param <P> The type of the wheel positions.
* @param <S> The type of the wheel speeds.
* @param <S> The type of the wheel velocities.
* @param <A> The type of the wheel accelerations.
*/
public interface Kinematics<P, S, A> extends Interpolator<P> {
/**
* Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This
* method is often used for odometry -- determining the robot's position on the field using data
* from the real-world speed of each wheel on the robot.
* Performs forward kinematics to return the resulting chassis velocity from the wheel velocities.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world velocity of each wheel on the robot.
*
* @param wheelSpeeds The speeds of the wheels.
* @return The chassis speed.
* @param wheelVelocities The velocities of the wheels.
* @return The chassis velocity.
*/
ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
ChassisVelocities toChassisVelocities(S wheelVelocities);
/**
* Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
* method is often used to convert joystick values into wheel speeds.
* Performs inverse kinematics to return the wheel velocities from a desired chassis velocity.
* This method is often used to convert joystick values into wheel velocities.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
* @param chassisVelocities The desired chassis velocity.
* @return The wheel velocities.
*/
S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
S toWheelVelocities(ChassisVelocities chassisVelocities);
/**
* Performs forward kinematics to return the resulting chassis accelerations from the wheel

97
wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java
View File

@@ -15,27 +15,29 @@ import org.wpilib.util.struct.StructSerializable;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
* wheel speeds.
* wheel velocities.
*
* <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel speeds)
* uses the relative locations of the wheels with respect to the center of rotation. The center of
* rotation for inverse kinematics is also variable. This means that you can set your center of
* rotation in a corner of the robot to perform special evasion maneuvers.
* <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel
* velocities) uses the relative locations of the wheels with respect to the center of rotation. The
* center of rotation for inverse kinematics is also variable. This means that you can set your
* center of rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>Forward kinematics (converting an array of wheel speeds into the overall chassis motion) is
* performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
* <p>Forward kinematics (converting an array of wheel velocities into the overall chassis motion)
* is performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
* system (more equations than variables), we use a least-squares approximation.
*
* <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] We take the
* Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelSpeeds] to get our
* chassis speeds.
* <p>The inverse kinematics: [wheelVelocities] = [wheelLocations] * [chassisVelocities] We take the
* Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelVelocities] to get our
* chassis velocities.
*
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class MecanumDriveKinematics
implements Kinematics<
MecanumDriveWheelPositions, MecanumDriveWheelSpeeds, MecanumDriveWheelAccelerations>,
MecanumDriveWheelPositions,
MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations>,
ProtobufSerializable,
StructSerializable {
private final SimpleMatrix m_inverseKinematics;
@@ -85,24 +87,24 @@ public class MecanumDriveKinematics
}
/**
* Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
* method is often used to convert joystick values into wheel speeds.
* Performs inverse kinematics to return the wheel velocities from a desired chassis velocity.
* This method is often used to convert joystick values into wheel velocities.
*
* <p>This function also supports variable centers of rotation. During normal operations, the
* center of rotation is usually the same as the physical center of the robot; therefore, the
* argument is defaulted to that use case. However, if you wish to change the center of rotation
* for evasive maneuvers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @param centerOfRotation The center of rotation. For example, if you set the center of rotation
* at one corner of the robot and provide a chassis speed that only has a dtheta component,
* at one corner of the robot and provide a chassis velocity that only has a dtheta component,
* the robot will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
* may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
* MecanumDriveWheelSpeeds#desaturate(double)} function to rectify this issue.
* @return The wheel velocities. Use caution because they are not normalized. Sometimes, a user
* input may cause one of the wheel velocities to go above the attainable max velocity. Use
* the {@link MecanumDriveWheelVelocities#desaturate(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation) {
public MecanumDriveWheelVelocities toWheelVelocities(
ChassisVelocities chassisVelocities, Translation2d centerOfRotation) {
// We have a new center of rotation. We need to compute the matrix again.
if (!centerOfRotation.equals(m_prevCoR)) {
var fl = m_frontLeftWheel.minus(centerOfRotation);
@@ -114,11 +116,12 @@ public class MecanumDriveKinematics
m_prevCoR = centerOfRotation;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega);
var chassisVelocitiesVector = new SimpleMatrix(3, 1);
chassisVelocitiesVector.setColumn(
0, 0, chassisVelocities.vx, chassisVelocities.vy, chassisVelocities.omega);
var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector);
return new MecanumDriveWheelSpeeds(
var wheelsVector = m_inverseKinematics.mult(chassisVelocitiesVector);
return new MecanumDriveWheelVelocities(
wheelsVector.get(0, 0),
wheelsVector.get(1, 0),
wheelsVector.get(2, 0),
@@ -126,41 +129,41 @@ public class MecanumDriveKinematics
}
/**
* Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
* information.
* Performs inverse kinematics. See {@link #toWheelVelocities(ChassisVelocities, Translation2d)}
* for more information.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
* @param chassisVelocities The desired chassis velocity.
* @return The wheel velocities.
*/
@Override
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toWheelSpeeds(chassisSpeeds, Translation2d.kZero);
public MecanumDriveWheelVelocities toWheelVelocities(ChassisVelocities chassisVelocities) {
return toWheelVelocities(chassisVelocities, Translation2d.kZero);
}
/**
* Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world speed of each wheel on the robot.
* Performs forward kinematics to return the resulting chassis state from the given wheel
* velocities. This method is often used for odometry -- determining the robot's position on the
* field using data from the real-world velocity of each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @return The resulting chassis speed.
* @param wheelVelocities The current mecanum drive wheel velocities.
* @return The resulting chassis velocity.
*/
@Override
public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
var wheelSpeedsVector = new SimpleMatrix(4, 1);
wheelSpeedsVector.setColumn(
public ChassisVelocities toChassisVelocities(MecanumDriveWheelVelocities wheelVelocities) {
var wheelVelocitiesVector = new SimpleMatrix(4, 1);
wheelVelocitiesVector.setColumn(
0,
0,
wheelSpeeds.frontLeft,
wheelSpeeds.frontRight,
wheelSpeeds.rearLeft,
wheelSpeeds.rearRight);
var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector);
wheelVelocities.frontLeft,
wheelVelocities.frontRight,
wheelVelocities.rearLeft,
wheelVelocities.rearRight);
var chassisVelocitiesVector = m_forwardKinematics.mult(wheelVelocitiesVector);
return new ChassisSpeeds(
chassisSpeedsVector.get(0, 0),
chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
return new ChassisVelocities(
chassisVelocitiesVector.get(0, 0),
chassisVelocitiesVector.get(1, 0),
chassisVelocitiesVector.get(2, 0));
}
/**

217
wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeeds.java
View File

@@ -1,217 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.MecanumDriveWheelSpeedsProto;
import org.wpilib.math.kinematics.struct.MecanumDriveWheelSpeedsStruct;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/** Represents the wheel speeds for a mecanum drive drivetrain. */
public class MecanumDriveWheelSpeeds
implements Interpolatable<MecanumDriveWheelSpeeds>, ProtobufSerializable, StructSerializable {
/** Speed of the front left wheel in meters per second. */
public double frontLeft;
/** Speed of the front right wheel in meters per second. */
public double frontRight;
/** Speed of the rear left wheel in meters per second. */
public double rearLeft;
/** Speed of the rear right wheel in meters per second. */
public double rearRight;
/** MecanumDriveWheelSpeeds protobuf for serialization. */
public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto();
/** MecanumDriveWheelSpeeds struct for serialization. */
public static final MecanumDriveWheelSpeedsStruct struct = new MecanumDriveWheelSpeedsStruct();
/** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
public MecanumDriveWheelSpeeds() {}
/**
* Constructs a MecanumDriveWheelSpeeds.
*
* @param frontLeft Speed of the front left wheel in meters per second.
* @param frontRight Speed of the front right wheel in meters per second.
* @param rearLeft Speed of the rear left wheel in meters per second.
* @param rearRight Speed of the rear right wheel in meters per second.
*/
public MecanumDriveWheelSpeeds(
double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
/**
* Constructs a MecanumDriveWheelSpeeds.
*
* @param frontLeft Speed of the front left wheel in meters per second.
* @param frontRight Speed of the front right wheel in meters per second.
* @param rearLeft Speed of the rear left wheel in meters per second.
* @param rearRight Speed of the rear right wheel in meters per second.
*/
public MecanumDriveWheelSpeeds(
LinearVelocity frontLeft,
LinearVelocity frontRight,
LinearVelocity rearLeft,
LinearVelocity rearRight) {
this(
frontLeft.in(MetersPerSecond),
frontRight.in(MetersPerSecond),
rearLeft.in(MetersPerSecond),
rearRight.in(MetersPerSecond));
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
* above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeed) {
double realMaxSpeed = Math.max(Math.abs(frontLeft), Math.abs(frontRight));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeft));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRight));
if (realMaxSpeed > attainableMaxSpeed) {
return new MecanumDriveWheelSpeeds(
frontLeft / realMaxSpeed * attainableMaxSpeed,
frontRight / realMaxSpeed * attainableMaxSpeed,
rearLeft / realMaxSpeed * attainableMaxSpeed,
rearRight / realMaxSpeed * attainableMaxSpeed);
}
return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight);
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
* above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds desaturate(LinearVelocity attainableMaxSpeed) {
return desaturate(attainableMaxSpeed.in(MetersPerSecond));
}
/**
* Adds two MecanumDriveWheelSpeeds and returns the sum.
*
* <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5,
* 0.5, 1.0} = MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
*
* @param other The MecanumDriveWheelSpeeds to add.
* @return The sum of the MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) {
return new MecanumDriveWheelSpeeds(
frontLeft + other.frontLeft,
frontRight + other.frontRight,
rearLeft + other.rearLeft,
rearRight + other.rearRight);
}
/**
* Subtracts the other MecanumDriveWheelSpeeds from the current MecanumDriveWheelSpeeds and
* returns the difference.
*
* <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelSpeeds{1.0, 2.0,
* 3.0, 0.5} = MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
*
* @param other The MecanumDriveWheelSpeeds to subtract.
* @return The difference between the two MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) {
return new MecanumDriveWheelSpeeds(
frontLeft - other.frontLeft,
frontRight - other.frontRight,
rearLeft - other.rearLeft,
rearRight - other.rearRight);
}
/**
* Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all
* components of the MecanumDriveWheelSpeeds.
*
* @return The inverse of the current MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds unaryMinus() {
return new MecanumDriveWheelSpeeds(-frontLeft, -frontRight, -rearLeft, -rearRight);
}
/**
* Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
*
* <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelSpeeds{4.0,
* 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds times(double scalar) {
return new MecanumDriveWheelSpeeds(
frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar);
}
/**
* Divides the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
*
* <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelSpeeds{1.0,
* 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds div(double scalar) {
return new MecanumDriveWheelSpeeds(
frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar);
}
/**
* Returns the linear interpolation of this MecanumDriveWheelSpeeds and another.
*
* @param endValue The end value for the interpolation.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
@Override
public MecanumDriveWheelSpeeds interpolate(MecanumDriveWheelSpeeds endValue, double t) {
// Clamp t to [0, 1]
t = Math.max(0.0, Math.min(1.0, t));
return new MecanumDriveWheelSpeeds(
frontLeft + t * (endValue.frontLeft - frontLeft),
frontRight + t * (endValue.frontRight - frontRight),
rearLeft + t * (endValue.rearLeft - rearLeft),
rearRight + t * (endValue.rearRight - rearRight));
}
@Override
public String toString() {
return String.format(
"MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
frontLeft, frontRight, rearLeft, rearRight);
}
}

226
wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java Normal file
View File

@@ -0,0 +1,226 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.MecanumDriveWheelVelocitiesProto;
import org.wpilib.math.kinematics.struct.MecanumDriveWheelVelocitiesStruct;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/** Represents the wheel velocities for a mecanum drive drivetrain. */
public class MecanumDriveWheelVelocities
implements Interpolatable<MecanumDriveWheelVelocities>,
ProtobufSerializable,
StructSerializable {
/** Velocity of the front left wheel in meters per second. */
public double frontLeft;
/** Velocity of the front right wheel in meters per second. */
public double frontRight;
/** Velocity of the rear left wheel in meters per second. */
public double rearLeft;
/** Velocity of the rear right wheel in meters per second. */
public double rearRight;
/** MecanumDriveWheelVelocities protobuf for serialization. */
public static final MecanumDriveWheelVelocitiesProto proto =
new MecanumDriveWheelVelocitiesProto();
/** MecanumDriveWheelVelocities struct for serialization. */
public static final MecanumDriveWheelVelocitiesStruct struct =
new MecanumDriveWheelVelocitiesStruct();
/** Constructs a MecanumDriveWheelVelocities with zeros for all member fields. */
public MecanumDriveWheelVelocities() {}
/**
* Constructs a MecanumDriveWheelVelocities.
*
* @param frontLeft Velocity of the front left wheel in meters per second.
* @param frontRight Velocity of the front right wheel in meters per second.
* @param rearLeft Velocity of the rear left wheel in meters per second.
* @param rearRight Velocity of the rear right wheel in meters per second.
*/
public MecanumDriveWheelVelocities(
double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
/**
* Constructs a MecanumDriveWheelVelocities.
*
* @param frontLeft Velocity of the front left wheel in meters per second.
* @param frontRight Velocity of the front right wheel in meters per second.
* @param rearLeft Velocity of the rear left wheel in meters per second.
* @param rearRight Velocity of the rear right wheel in meters per second.
*/
public MecanumDriveWheelVelocities(
LinearVelocity frontLeft,
LinearVelocity frontRight,
LinearVelocity rearLeft,
LinearVelocity rearRight) {
this(
frontLeft.in(MetersPerSecond),
frontRight.in(MetersPerSecond),
rearLeft.in(MetersPerSecond),
rearRight.in(MetersPerSecond));
}
/**
* Renormalizes the wheel velocities if any individual velocity is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more wheels may be
* above the max attainable velocity for the driving motor on that wheel. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
*
* @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
* reach.
* @return Desaturated MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities desaturate(double attainableMaxVelocity) {
double realMaxVelocity = Math.max(Math.abs(frontLeft), Math.abs(frontRight));
realMaxVelocity = Math.max(realMaxVelocity, Math.abs(rearLeft));
realMaxVelocity = Math.max(realMaxVelocity, Math.abs(rearRight));
if (realMaxVelocity > attainableMaxVelocity) {
return new MecanumDriveWheelVelocities(
frontLeft / realMaxVelocity * attainableMaxVelocity,
frontRight / realMaxVelocity * attainableMaxVelocity,
rearLeft / realMaxVelocity * attainableMaxVelocity,
rearRight / realMaxVelocity * attainableMaxVelocity);
}
return new MecanumDriveWheelVelocities(frontLeft, frontRight, rearLeft, rearRight);
}
/**
* Renormalizes the wheel velocities if any individual velocity is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more wheels may be
* above the max attainable velocity for the driving motor on that wheel. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
*
* @param attainableMaxVelocity The absolute max velocity that a wheel can reach.
* @return Desaturated MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
return desaturate(attainableMaxVelocity.in(MetersPerSecond));
}
/**
* Adds two MecanumDriveWheelVelocities and returns the sum.
*
* <p>For example, MecanumDriveWheelVelocities{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelVelocities{2.0, 1.5, 0.5, 1.0} = MecanumDriveWheelVelocities{3.0, 2.0, 2.5,
* 2.5}
*
* @param other The MecanumDriveWheelVelocities to add.
* @return The sum of the MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities plus(MecanumDriveWheelVelocities other) {
return new MecanumDriveWheelVelocities(
frontLeft + other.frontLeft,
frontRight + other.frontRight,
rearLeft + other.rearLeft,
rearRight + other.rearRight);
}
/**
* Subtracts the other MecanumDriveWheelVelocities from the current MecanumDriveWheelVelocities
* and returns the difference.
*
* <p>For example, MecanumDriveWheelVelocities{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelVelocities{1.0, 2.0, 3.0, 0.5} = MecanumDriveWheelVelocities{4.0, 2.0, 3.0,
* 2.0}
*
* @param other The MecanumDriveWheelVelocities to subtract.
* @return The difference between the two MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities minus(MecanumDriveWheelVelocities other) {
return new MecanumDriveWheelVelocities(
frontLeft - other.frontLeft,
frontRight - other.frontRight,
rearLeft - other.rearLeft,
rearRight - other.rearRight);
}
/**
* Returns the inverse of the current MecanumDriveWheelVelocities. This is equivalent to negating
* all components of the MecanumDriveWheelVelocities.
*
* @return The inverse of the current MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities unaryMinus() {
return new MecanumDriveWheelVelocities(-frontLeft, -frontRight, -rearLeft, -rearRight);
}
/**
* Multiplies the MecanumDriveWheelVelocities by a scalar and returns the new
* MecanumDriveWheelVelocities.
*
* <p>For example, MecanumDriveWheelVelocities{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelVelocities{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities times(double scalar) {
return new MecanumDriveWheelVelocities(
frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar);
}
/**
* Divides the MecanumDriveWheelVelocities by a scalar and returns the new
* MecanumDriveWheelVelocities.
*
* <p>For example, MecanumDriveWheelVelocities{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelVelocities{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled MecanumDriveWheelVelocities.
*/
public MecanumDriveWheelVelocities div(double scalar) {
return new MecanumDriveWheelVelocities(
frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar);
}
/**
* Returns the linear interpolation of this MecanumDriveWheelVelocities and another.
*
* @param endValue The end value for the interpolation.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
@Override
public MecanumDriveWheelVelocities interpolate(MecanumDriveWheelVelocities endValue, double t) {
// Clamp t to [0, 1]
t = Math.max(0.0, Math.min(1.0, t));
return new MecanumDriveWheelVelocities(
frontLeft + t * (endValue.frontLeft - frontLeft),
frontRight + t * (endValue.frontRight - frontRight),
rearLeft + t * (endValue.rearLeft - rearLeft),
rearRight + t * (endValue.rearRight - rearRight));
}
@Override
public String toString() {
return String.format(
"MecanumDriveWheelVelocities(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
frontLeft, frontRight, rearLeft, rearRight);
}
}

294
wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java
View File

@@ -23,7 +23,7 @@ import org.wpilib.util.struct.StructSerializable;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
* module states (speed and angle).
* module states (velocity and angle).
*
* <p>The inverse kinematics (converting from a desired chassis velocity to individual module
* states) uses the relative locations of the modules with respect to the center of rotation. The
@@ -34,16 +34,17 @@ import org.wpilib.util.struct.StructSerializable;
* performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
* system (more equations than variables), we use a least-squares approximation.
*
* <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the
* Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our
* chassis speeds.
* <p>The inverse kinematics: [moduleVelocities] = [moduleLocations] * [chassisVelocities] We take
* the Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleVelocities] to
* get our chassis velocities.
*
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
@SuppressWarnings("overrides")
public class SwerveDriveKinematics
implements Kinematics<SwerveModulePosition[], SwerveModuleState[], SwerveModuleAcceleration[]>,
implements Kinematics<
SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>,
ProtobufSerializable,
StructSerializable {
private final SimpleMatrix m_firstOrderInverseKinematics;
@@ -102,112 +103,115 @@ public class SwerveDriveKinematics
/**
* Performs inverse kinematics to return the module states from a desired chassis velocity. This
* method is often used to convert joystick values into module speeds and angles.
* method is often used to convert joystick values into module velocities and angles.
*
* <p>This function also supports variable centers of rotation. During normal operations, the
* center of rotation is usually the same as the physical center of the robot; therefore, the
* argument is defaulted to that use case. However, if you wish to change the center of rotation
* for evasive maneuvers, vision alignment, or for any other use case, you can do so.
*
* <p>In the case that the desired chassis speeds are zero (i.e. the robot will be stationary),
* the previously calculated module angle will be maintained.
* <p>In the case that the desired chassis velocities are zero (i.e. the robot will be
* stationary), the previously calculated module angle will be maintained.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @param centerOfRotation The center of rotation. For example, if you set the center of rotation
* at one corner of the robot and provide a chassis speed that only has a dtheta component,
* at one corner of the robot and provide a chassis velocity that only has a dtheta component,
* the robot will rotate around that corner.
* @return An array containing the module states. Use caution because these module states are not
* normalized. Sometimes, a user input may cause one of the module speeds to go above the
* attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
* DesaturateWheelSpeeds} function to rectify this issue.
* normalized. Sometimes, a user input may cause one of the module velocities to go above the
* attainable max velocity. Use the {@link #desaturateWheelVelocities(SwerveModuleVelocity[],
* double) DesaturateWheelVelocities} function to rectify this issue.
*/
public SwerveModuleState[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation) {
var moduleStates = new SwerveModuleState[m_numModules];
public SwerveModuleVelocity[] toSwerveModuleVelocities(
ChassisVelocities chassisVelocities, Translation2d centerOfRotation) {
var moduleVelocities = new SwerveModuleVelocity[m_numModules];
if (chassisSpeeds.vx == 0.0 && chassisSpeeds.vy == 0.0 && chassisSpeeds.omega == 0.0) {
if (chassisVelocities.vx == 0.0
&& chassisVelocities.vy == 0.0
&& chassisVelocities.omega == 0.0) {
for (int i = 0; i < m_numModules; i++) {
moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]);
moduleVelocities[i] = new SwerveModuleVelocity(0.0, m_moduleHeadings[i]);
}
return moduleStates;
return moduleVelocities;
}
if (!centerOfRotation.equals(m_prevCoR)) {
setInverseKinematics(centerOfRotation);
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega);
var chassisVelocitiesVector = new SimpleMatrix(3, 1);
chassisVelocitiesVector.setColumn(
0, 0, chassisVelocities.vx, chassisVelocities.vy, chassisVelocities.omega);
var moduleStatesMatrix = m_firstOrderInverseKinematics.mult(chassisSpeedsVector);
var moduleVelocitiesMatrix = m_firstOrderInverseKinematics.mult(chassisVelocitiesVector);
for (int i = 0; i < m_numModules; i++) {
double x = moduleStatesMatrix.get(i * 2, 0);
double y = moduleStatesMatrix.get(i * 2 + 1, 0);
double x = moduleVelocitiesMatrix.get(i * 2, 0);
double y = moduleVelocitiesMatrix.get(i * 2 + 1, 0);
double speed = Math.hypot(x, y);
Rotation2d angle = speed > 1e-6 ? new Rotation2d(x, y) : m_moduleHeadings[i];
double velocity = Math.hypot(x, y);
Rotation2d angle = velocity > 1e-6 ? new Rotation2d(x, y) : m_moduleHeadings[i];
moduleStates[i] = new SwerveModuleState(speed, angle);
moduleVelocities[i] = new SwerveModuleVelocity(velocity, angle);
m_moduleHeadings[i] = angle;
}
return moduleStates;
return moduleVelocities;
}
/**
* Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
* toSwerveModuleStates for more information.
* Performs inverse kinematics. See {@link #toSwerveModuleVelocities(ChassisVelocities,
* Translation2d)} toSwerveModuleVelocities for more information.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @return An array containing the module states.
*/
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
return toSwerveModuleStates(chassisSpeeds, Translation2d.kZero);
public SwerveModuleVelocity[] toSwerveModuleVelocities(ChassisVelocities chassisVelocities) {
return toSwerveModuleVelocities(chassisVelocities, Translation2d.kZero);
}
@Override
public SwerveModuleState[] toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toSwerveModuleStates(chassisSpeeds);
public SwerveModuleVelocity[] toWheelVelocities(ChassisVelocities chassisVelocities) {
return toSwerveModuleVelocities(chassisVelocities);
}
/**
* Performs forward kinematics to return the resulting chassis state from the given module states.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world speed and angle of each module on the robot.
* data from the real-world velocity and angle of each module on the robot.
*
* @param moduleStates The state of the modules (as a SwerveModuleState type) as measured from
* respective encoders and gyros. The order of the swerve module states should be same as
* @param moduleVelocities The state of the modules (as a SwerveModuleVelocity type) as measured
* from respective encoders and gyros. The order of the swerve module states should be same as
* passed into the constructor of this class.
* @return The resulting chassis speed.
* @return The resulting chassis velocity.
*/
@Override
public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) {
if (moduleStates.length != m_numModules) {
public ChassisVelocities toChassisVelocities(SwerveModuleVelocity... moduleVelocities) {
if (moduleVelocities.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of module locations provided in "
+ "constructor");
}
var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
var moduleVelocitiesMatrix = new SimpleMatrix(m_numModules * 2, 1);
for (int i = 0; i < m_numModules; i++) {
var module = moduleStates[i];
moduleStatesMatrix.set(i * 2, 0, module.speed * module.angle.getCos());
moduleStatesMatrix.set(i * 2 + 1, module.speed * module.angle.getSin());
var module = moduleVelocities[i];
moduleVelocitiesMatrix.set(i * 2, 0, module.velocity * module.angle.getCos());
moduleVelocitiesMatrix.set(i * 2 + 1, module.velocity * module.angle.getSin());
}
var chassisSpeedsVector = m_firstOrderForwardKinematics.mult(moduleStatesMatrix);
return new ChassisSpeeds(
chassisSpeedsVector.get(0, 0),
chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
var chassisVelocitiesVector = m_firstOrderForwardKinematics.mult(moduleVelocitiesMatrix);
return new ChassisVelocities(
chassisVelocitiesVector.get(0, 0),
chassisVelocitiesVector.get(1, 0),
chassisVelocitiesVector.get(2, 0));
}
/**
* Performs forward kinematics to return the resulting chassis state from the given module states.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world speed and angle of each module on the robot.
* data from the real-world velocity and angle of each module on the robot.
*
* @param moduleDeltas The latest change in position of the modules (as a SwerveModulePosition
* type) as measured from respective encoders and gyros. The order of the swerve module states
@@ -246,138 +250,140 @@ public class SwerveDriveKinematics
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
* Renormalizes the wheel velocities if any individual velocity is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
* above the max attainable speed for the driving motor on that module. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between modules.
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more modules may be
* above the max attainable velocity for the driving motor on that module. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between modules.
*
* <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
* of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
* discretization did not account for this translational skew.
* <p>Scaling down the module velocities rotates the direction of net motion in the opposite
* direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
* because the discretization did not account for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
* @param moduleVelocities Reference to array of module states. The array will be mutated with the
* normalized velocities!
* @param attainableMaxVelocity The absolute max velocity in meters per second that a module can
* reach.
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeed) {
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
public static void desaturateWheelVelocities(
SwerveModuleVelocity[] moduleVelocities, double attainableMaxVelocity) {
double realMaxVelocity = 0;
for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
realMaxVelocity = Math.max(realMaxVelocity, Math.abs(moduleVelocity.velocity));
}
if (realMaxSpeed > attainableMaxSpeed) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speed = moduleState.speed / realMaxSpeed * attainableMaxSpeed;
if (realMaxVelocity > attainableMaxVelocity) {
for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
moduleVelocity.velocity = moduleVelocity.velocity / realMaxVelocity * attainableMaxVelocity;
}
}
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
* Renormalizes the wheel velocities if any individual velocity is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
* above the max attainable speed for the driving motor on that module. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between modules.
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more modules may be
* above the max attainable velocity for the driving motor on that module. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between modules.
*
* <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
* of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
* discretization did not account for this translational skew.
* <p>Scaling down the module velocities rotates the direction of net motion in the opposite
* direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
* because the discretization did not account for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
* @param moduleVelocities Reference to array of module states. The array will be mutated with the
* normalized velocities!
* @param attainableMaxVelocity The absolute max velocity in meters per second that a module can
* reach.
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, LinearVelocity attainableMaxSpeed) {
desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
public static void desaturateWheelVelocities(
SwerveModuleVelocity[] moduleVelocities, LinearVelocity attainableMaxVelocity) {
desaturateWheelVelocities(moduleVelocities, attainableMaxVelocity.in(MetersPerSecond));
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well
* as getting rid of joystick saturation at edges of joystick.
* Renormalizes the wheel velocities if any individual velocity is above the specified maximum, as
* well as getting rid of joystick saturation at edges of joystick.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
* above the max attainable speed for the driving motor on that module. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between modules.
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more modules may be
* above the max attainable velocity for the driving motor on that module. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between modules.
*
* <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
* of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
* discretization did not account for this translational skew.
* <p>Scaling down the module velocities rotates the direction of net motion in the opposite
* direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
* because the discretization did not account for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeed The absolute max speed in meters per second that a module can
* reach
* @param attainableMaxTranslationalSpeed The absolute max speed in meters per second that your
* robot can reach while translating
* @param attainableMaxRotationalVelocity The absolute max speed in radians per second the robot
* can reach while rotating
* @param moduleVelocities Reference to array of module states. The array will be mutated with the
* normalized velocities!
* @param desiredChassisVelocity The desired velocity of the robot
* @param attainableMaxModuleVelocity The absolute max velocity in meters per second that a module
* can reach
* @param attainableMaxTranslationalVelocity The absolute max velocity in meters per second that
* your robot can reach while translating
* @param attainableMaxRotationalVelocity The absolute max velocity in radians per second the
* robot can reach while rotating
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates,
ChassisSpeeds desiredChassisSpeed,
double attainableMaxModuleSpeed,
double attainableMaxTranslationalSpeed,
public static void desaturateWheelVelocities(
SwerveModuleVelocity[] moduleVelocities,
ChassisVelocities desiredChassisVelocity,
double attainableMaxModuleVelocity,
double attainableMaxTranslationalVelocity,
double attainableMaxRotationalVelocity) {
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
double realMaxVelocity = 0;
for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
realMaxVelocity = Math.max(realMaxVelocity, Math.abs(moduleVelocity.velocity));
}
if (attainableMaxTranslationalSpeed == 0
if (attainableMaxTranslationalVelocity == 0
|| attainableMaxRotationalVelocity == 0
|| realMaxSpeed == 0) {
|| realMaxVelocity == 0) {
return;
}
double translationalK =
Math.hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy)
/ attainableMaxTranslationalSpeed;
double rotationalK = Math.abs(desiredChassisSpeed.omega) / attainableMaxRotationalVelocity;
Math.hypot(desiredChassisVelocity.vx, desiredChassisVelocity.vy)
/ attainableMaxTranslationalVelocity;
double rotationalK = Math.abs(desiredChassisVelocity.omega) / attainableMaxRotationalVelocity;
double k = Math.max(translationalK, rotationalK);
double scale = Math.min(k * attainableMaxModuleSpeed / realMaxSpeed, 1);
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speed *= scale;
double scale = Math.min(k * attainableMaxModuleVelocity / realMaxVelocity, 1);
for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
moduleVelocity.velocity *= scale;
}
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well
* as getting rid of joystick saturation at edges of joystick.
* Renormalizes the wheel velocities if any individual velocity is above the specified maximum, as
* well as getting rid of joystick saturation at edges of joystick.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
* above the max attainable speed for the driving motor on that module. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between modules.
* <p>Sometimes, after inverse kinematics, the requested velocity from one or more modules may be
* above the max attainable velocity for the driving motor on that module. To fix this issue, one
* can reduce all the wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of velocities between modules.
*
* <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
* of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
* discretization did not account for this translational skew.
* <p>Scaling down the module velocities rotates the direction of net motion in the opposite
* direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
* because the discretization did not account for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeed The absolute max speed that a module can reach
* @param attainableMaxTranslationalSpeed The absolute max speed that your robot can reach while
* translating
* @param attainableMaxRotationalVelocity The absolute max speed the robot can reach while
* @param moduleVelocities Reference to array of module states. The array will be mutated with the
* normalized velocities!
* @param desiredChassisVelocity The desired velocity of the robot
* @param attainableMaxModuleVelocity The absolute max velocity that a module can reach
* @param attainableMaxTranslationalVelocity The absolute max velocity that your robot can reach
* while translating
* @param attainableMaxRotationalVelocity The absolute max velocity the robot can reach while
* rotating
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates,
ChassisSpeeds desiredChassisSpeed,
LinearVelocity attainableMaxModuleSpeed,
LinearVelocity attainableMaxTranslationalSpeed,
public static void desaturateWheelVelocities(
SwerveModuleVelocity[] moduleVelocities,
ChassisVelocities desiredChassisVelocity,
LinearVelocity attainableMaxModuleVelocity,
LinearVelocity attainableMaxTranslationalVelocity,
AngularVelocity attainableMaxRotationalVelocity) {
desaturateWheelSpeeds(
moduleStates,
desiredChassisSpeed,
attainableMaxModuleSpeed.in(MetersPerSecond),
attainableMaxTranslationalSpeed.in(MetersPerSecond),
desaturateWheelVelocities(
moduleVelocities,
desiredChassisVelocity,
attainableMaxModuleVelocity.in(MetersPerSecond),
attainableMaxTranslationalVelocity.in(MetersPerSecond),
attainableMaxRotationalVelocity.in(RadiansPerSecond));
}

158
wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java
View File

@@ -1,158 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import java.util.Objects;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.SwerveModuleStateProto;
import org.wpilib.math.kinematics.struct.SwerveModuleStateStruct;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/** Represents the state of one swerve module. */
public class SwerveModuleState
implements Interpolatable<SwerveModuleState>,
Comparable<SwerveModuleState>,
ProtobufSerializable,
StructSerializable {
/** Speed of the wheel of the module in meters per second. */
public double speed;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.kZero;
/** SwerveModuleState protobuf for serialization. */
public static final SwerveModuleStateProto proto = new SwerveModuleStateProto();
/** SwerveModuleState struct for serialization. */
public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct();
/** Constructs a SwerveModuleState with zeros for speed and angle. */
public SwerveModuleState() {}
/**
* Constructs a SwerveModuleState.
*
* @param speed The speed of the wheel of the module in meters per second.
* @param angle The angle of the module.
*/
public SwerveModuleState(double speed, Rotation2d angle) {
this.speed = speed;
this.angle = angle;
}
/**
* Constructs a SwerveModuleState.
*
* @param speed The speed of the wheel of the module.
* @param angle The angle of the module.
*/
public SwerveModuleState(LinearVelocity speed, Rotation2d angle) {
this(speed.in(MetersPerSecond), angle);
}
@Override
public boolean equals(Object obj) {
return obj instanceof SwerveModuleState other
&& Math.abs(other.speed - speed) < 1E-9
&& angle.equals(other.angle);
}
@Override
public int hashCode() {
return Objects.hash(speed, angle);
}
/**
* Compares two swerve module states. One swerve module is "greater" than the other if its speed
* is higher than the other.
*
* @param other The other swerve module.
* @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
*/
@Override
public int compareTo(SwerveModuleState other) {
return Double.compare(this.speed, other.speed);
}
@Override
public String toString() {
return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speed, angle);
}
/**
* Minimize the change in heading this swerve module state would require by potentially reversing
* the direction the wheel spins. If this is used with the PIDController class's continuous input
* functionality, the furthest a wheel will ever rotate is 90 degrees.
*
* @param currentAngle The current module angle.
*/
public void optimize(Rotation2d currentAngle) {
var delta = angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
speed *= -1;
angle = angle.rotateBy(Rotation2d.kPi);
}
}
/**
* Minimize the change in heading the desired swerve module state would require by potentially
* reversing the direction the wheel spins. If this is used with the PIDController class's
* continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
*
* @param desiredState The desired state.
* @param currentAngle The current module angle.
* @return Optimized swerve module state.
* @deprecated Use the instance method instead.
*/
@Deprecated
public static SwerveModuleState optimize(
SwerveModuleState desiredState, Rotation2d currentAngle) {
var delta = desiredState.angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
return new SwerveModuleState(
-desiredState.speed, desiredState.angle.rotateBy(Rotation2d.kPi));
} else {
return new SwerveModuleState(desiredState.speed, desiredState.angle);
}
}
/**
* Returns the linear interpolation of this SwerveModuleState and another. The angle is
* interpolated using the shortest path between the two angles.
*
* @param endValue The end value for the interpolation.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
@Override
public SwerveModuleState interpolate(SwerveModuleState endValue, double t) {
// Clamp t to [0, 1]
t = Math.max(0.0, Math.min(1.0, t));
// Interpolate speed linearly
double interpolatedSpeed = speed + t * (endValue.speed - speed);
// Interpolate angle using the shortest path
Rotation2d interpolatedAngle = angle.interpolate(endValue.angle, t);
return new SwerveModuleState(interpolatedSpeed, interpolatedAngle);
}
/**
* Scales speed by cosine of angle error. This scales down movement perpendicular to the desired
* direction of travel that can occur when modules change directions. This results in smoother
* driving.
*
* @param currentAngle The current module angle.
*/
public void cosineScale(Rotation2d currentAngle) {
speed *= angle.minus(currentAngle).getCos();
}
}

158
wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java Normal file
View File

@@ -0,0 +1,158 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.wpilib.units.Units.MetersPerSecond;
import java.util.Objects;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.kinematics.proto.SwerveModuleVelocityProto;
import org.wpilib.math.kinematics.struct.SwerveModuleVelocityStruct;
import org.wpilib.units.measure.LinearVelocity;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
/** Represents the velocity of one swerve module. */
public class SwerveModuleVelocity
implements Interpolatable<SwerveModuleVelocity>,
Comparable<SwerveModuleVelocity>,
ProtobufSerializable,
StructSerializable {
/** Velocity of the wheel of the module in meters per second. */
public double velocity;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.kZero;
/** SwerveModuleVelocity protobuf for serialization. */
public static final SwerveModuleVelocityProto proto = new SwerveModuleVelocityProto();
/** SwerveModuleVelocity struct for serialization. */
public static final SwerveModuleVelocityStruct struct = new SwerveModuleVelocityStruct();
/** Constructs a SwerveModuleVelocity with zeros for velocity and angle. */
public SwerveModuleVelocity() {}
/**
* Constructs a SwerveModuleVelocity.
*
* @param velocity The velocity of the wheel of the module in meters per second.
* @param angle The angle of the module.
*/
public SwerveModuleVelocity(double velocity, Rotation2d angle) {
this.velocity = velocity;
this.angle = angle;
}
/**
* Constructs a SwerveModuleVelocity.
*
* @param velocity The velocity of the wheel of the module.
* @param angle The angle of the module.
*/
public SwerveModuleVelocity(LinearVelocity velocity, Rotation2d angle) {
this(velocity.in(MetersPerSecond), angle);
}
@Override
public boolean equals(Object obj) {
return obj instanceof SwerveModuleVelocity other
&& Math.abs(other.velocity - velocity) < 1E-9
&& angle.equals(other.angle);
}
@Override
public int hashCode() {
return Objects.hash(velocity, angle);
}
/**
* Compares two swerve module velocities. One swerve module is "greater" than the other if its
* velocity is higher than the other.
*
* @param other The other swerve module.
* @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
*/
@Override
public int compareTo(SwerveModuleVelocity other) {
return Double.compare(this.velocity, other.velocity);
}
@Override
public String toString() {
return String.format("SwerveModuleVelocity(Velocity: %.2f m/s, Angle: %s)", velocity, angle);
}
/**
* Minimize the change in heading this swerve module velocity would require by potentially
* reversing the direction the wheel spins. If this is used with the PIDController class's
* continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
*
* @param currentAngle The current module angle.
*/
public void optimize(Rotation2d currentAngle) {
var delta = angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
velocity *= -1;
angle = angle.rotateBy(Rotation2d.kPi);
}
}
/**
* Minimize the change in heading the desired swerve module velocity would require by potentially
* reversing the direction the wheel spins. If this is used with the PIDController class's
* continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
*
* @param desiredVelocity The desired velocity.
* @param currentAngle The current module angle.
* @return Optimized swerve module velocity.
* @deprecated Use the instance method instead.
*/
@Deprecated
public static SwerveModuleVelocity optimize(
SwerveModuleVelocity desiredVelocity, Rotation2d currentAngle) {
var delta = desiredVelocity.angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
return new SwerveModuleVelocity(
-desiredVelocity.velocity, desiredVelocity.angle.rotateBy(Rotation2d.kPi));
} else {
return new SwerveModuleVelocity(desiredVelocity.velocity, desiredVelocity.angle);
}
}
/**
* Returns the linear interpolation of this SwerveModuleVelocity and another. The angle is
* interpolated using the shortest path between the two angles.
*
* @param endValue The end value for the interpolation.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
@Override
public SwerveModuleVelocity interpolate(SwerveModuleVelocity endValue, double t) {
// Clamp t to [0, 1]
t = Math.max(0.0, Math.min(1.0, t));
// Interpolate velocity linearly
double interpolatedVelocity = velocity + t * (endValue.velocity - velocity);
// Interpolate angle using the shortest path
Rotation2d interpolatedAngle = angle.interpolate(endValue.angle, t);
return new SwerveModuleVelocity(interpolatedVelocity, interpolatedAngle);
}
/**
* Scales velocity by cosine of angle error. This scales down movement perpendicular to the
* desired direction of travel that can occur when modules change directions. This results in
* smoother driving.
*
* @param currentAngle The current module angle.
*/
public void cosineScale(Rotation2d currentAngle) {
velocity *= angle.minus(currentAngle).getCos();
}
}

39
wpimath/src/main/java/org/wpilib/math/kinematics/proto/ChassisSpeedsProto.java
View File

@@ -1,39 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.proto.ProtobufChassisSpeeds;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class ChassisSpeedsProto implements Protobuf<ChassisSpeeds, ProtobufChassisSpeeds> {
@Override
public Class<ChassisSpeeds> getTypeClass() {
return ChassisSpeeds.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufChassisSpeeds.getDescriptor();
}
@Override
public ProtobufChassisSpeeds createMessage() {
return ProtobufChassisSpeeds.newInstance();
}
@Override
public ChassisSpeeds unpack(ProtobufChassisSpeeds msg) {
return new ChassisSpeeds(msg.getVx(), msg.getVy(), msg.getOmega());
}
@Override
public void pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) {
msg.setVx(value.vx);
msg.setVy(value.vy);
msg.setOmega(value.omega);
}
}

40
wpimath/src/main/java/org/wpilib/math/kinematics/proto/ChassisVelocitiesProto.java Normal file
View File

@@ -0,0 +1,40 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.proto.ProtobufChassisVelocities;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class ChassisVelocitiesProto
implements Protobuf<ChassisVelocities, ProtobufChassisVelocities> {
@Override
public Class<ChassisVelocities> getTypeClass() {
return ChassisVelocities.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufChassisVelocities.getDescriptor();
}
@Override
public ProtobufChassisVelocities createMessage() {
return ProtobufChassisVelocities.newInstance();
}
@Override
public ChassisVelocities unpack(ProtobufChassisVelocities msg) {
return new ChassisVelocities(msg.getVx(), msg.getVy(), msg.getOmega());
}
@Override
public void pack(ProtobufChassisVelocities msg, ChassisVelocities value) {
msg.setVx(value.vx);
msg.setVy(value.vy);
msg.setOmega(value.omega);
}
}

39
wpimath/src/main/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java
View File

@@ -1,39 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.proto.ProtobufDifferentialDriveWheelSpeeds;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class DifferentialDriveWheelSpeedsProto
implements Protobuf<DifferentialDriveWheelSpeeds, ProtobufDifferentialDriveWheelSpeeds> {
@Override
public Class<DifferentialDriveWheelSpeeds> getTypeClass() {
return DifferentialDriveWheelSpeeds.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufDifferentialDriveWheelSpeeds.getDescriptor();
}
@Override
public ProtobufDifferentialDriveWheelSpeeds createMessage() {
return ProtobufDifferentialDriveWheelSpeeds.newInstance();
}
@Override
public DifferentialDriveWheelSpeeds unpack(ProtobufDifferentialDriveWheelSpeeds msg) {
return new DifferentialDriveWheelSpeeds(msg.getLeft(), msg.getRight());
}
@Override
public void pack(ProtobufDifferentialDriveWheelSpeeds msg, DifferentialDriveWheelSpeeds value) {
msg.setLeft(value.left);
msg.setRight(value.right);
}
}

41
wpimath/src/main/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.java Normal file
View File

@@ -0,0 +1,41 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
import org.wpilib.math.proto.ProtobufDifferentialDriveWheelVelocities;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class DifferentialDriveWheelVelocitiesProto
implements Protobuf<
DifferentialDriveWheelVelocities, ProtobufDifferentialDriveWheelVelocities> {
@Override
public Class<DifferentialDriveWheelVelocities> getTypeClass() {
return DifferentialDriveWheelVelocities.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufDifferentialDriveWheelVelocities.getDescriptor();
}
@Override
public ProtobufDifferentialDriveWheelVelocities createMessage() {
return ProtobufDifferentialDriveWheelVelocities.newInstance();
}
@Override
public DifferentialDriveWheelVelocities unpack(ProtobufDifferentialDriveWheelVelocities msg) {
return new DifferentialDriveWheelVelocities(msg.getLeft(), msg.getRight());
}
@Override
public void pack(
ProtobufDifferentialDriveWheelVelocities msg, DifferentialDriveWheelVelocities value) {
msg.setLeft(value.left);
msg.setRight(value.right);
}
}

42
wpimath/src/main/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java
View File

@@ -1,42 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.kinematics.MecanumDriveWheelSpeeds;
import org.wpilib.math.proto.ProtobufMecanumDriveWheelSpeeds;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class MecanumDriveWheelSpeedsProto
implements Protobuf<MecanumDriveWheelSpeeds, ProtobufMecanumDriveWheelSpeeds> {
@Override
public Class<MecanumDriveWheelSpeeds> getTypeClass() {
return MecanumDriveWheelSpeeds.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufMecanumDriveWheelSpeeds.getDescriptor();
}
@Override
public ProtobufMecanumDriveWheelSpeeds createMessage() {
return ProtobufMecanumDriveWheelSpeeds.newInstance();
}
@Override
public MecanumDriveWheelSpeeds unpack(ProtobufMecanumDriveWheelSpeeds msg) {
return new MecanumDriveWheelSpeeds(
msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
}
@Override
public void pack(ProtobufMecanumDriveWheelSpeeds msg, MecanumDriveWheelSpeeds value) {
msg.setFrontLeft(value.frontLeft);
msg.setFrontRight(value.frontRight);
msg.setRearLeft(value.rearLeft);
msg.setRearRight(value.rearRight);
}
}

42
wpimath/src/main/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.java Normal file
View File

@@ -0,0 +1,42 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.kinematics.MecanumDriveWheelVelocities;
import org.wpilib.math.proto.ProtobufMecanumDriveWheelVelocities;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class MecanumDriveWheelVelocitiesProto
implements Protobuf<MecanumDriveWheelVelocities, ProtobufMecanumDriveWheelVelocities> {
@Override
public Class<MecanumDriveWheelVelocities> getTypeClass() {
return MecanumDriveWheelVelocities.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufMecanumDriveWheelVelocities.getDescriptor();
}
@Override
public ProtobufMecanumDriveWheelVelocities createMessage() {
return ProtobufMecanumDriveWheelVelocities.newInstance();
}
@Override
public MecanumDriveWheelVelocities unpack(ProtobufMecanumDriveWheelVelocities msg) {
return new MecanumDriveWheelVelocities(
msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
}
@Override
public void pack(ProtobufMecanumDriveWheelVelocities msg, MecanumDriveWheelVelocities value) {
msg.setFrontLeft(value.frontLeft);
msg.setFrontRight(value.frontRight);
msg.setRearLeft(value.rearLeft);
msg.setRearRight(value.rearRight);
}
}

40
wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleStateProto.java
View File

@@ -1,40 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.proto.ProtobufSwerveModuleState;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class SwerveModuleStateProto
implements Protobuf<SwerveModuleState, ProtobufSwerveModuleState> {
@Override
public Class<SwerveModuleState> getTypeClass() {
return SwerveModuleState.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufSwerveModuleState.getDescriptor();
}
@Override
public ProtobufSwerveModuleState createMessage() {
return ProtobufSwerveModuleState.newInstance();
}
@Override
public SwerveModuleState unpack(ProtobufSwerveModuleState msg) {
return new SwerveModuleState(msg.getSpeed(), Rotation2d.proto.unpack(msg.getAngle()));
}
@Override
public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) {
msg.setSpeed(value.speed);
Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
}
}

40
wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleVelocityProto.java Normal file
View File

@@ -0,0 +1,40 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModuleVelocity;
import org.wpilib.math.proto.ProtobufSwerveModuleVelocity;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class SwerveModuleVelocityProto
implements Protobuf<SwerveModuleVelocity, ProtobufSwerveModuleVelocity> {
@Override
public Class<SwerveModuleVelocity> getTypeClass() {
return SwerveModuleVelocity.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufSwerveModuleVelocity.getDescriptor();
}
@Override
public ProtobufSwerveModuleVelocity createMessage() {
return ProtobufSwerveModuleVelocity.newInstance();
}
@Override
public SwerveModuleVelocity unpack(ProtobufSwerveModuleVelocity msg) {
return new SwerveModuleVelocity(msg.getVelocity(), Rotation2d.proto.unpack(msg.getAngle()));
}
@Override
public void pack(ProtobufSwerveModuleVelocity msg, SwerveModuleVelocity value) {
msg.setVelocity(value.velocity);
Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
}
}

16
wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisSpeedsStruct.java → wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisVelocitiesStruct.java
View File

@@ -5,18 +5,18 @@
package org.wpilib.math.kinematics.struct;
import java.nio.ByteBuffer;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.util.struct.Struct;
public class ChassisSpeedsStruct implements Struct<ChassisSpeeds> {
public class ChassisVelocitiesStruct implements Struct<ChassisVelocities> {
@Override
public Class<ChassisSpeeds> getTypeClass() {
return ChassisSpeeds.class;
public Class<ChassisVelocities> getTypeClass() {
return ChassisVelocities.class;
}
@Override
public String getTypeName() {
return "ChassisSpeeds";
return "ChassisVelocities";
}
@Override
@@ -30,15 +30,15 @@ public class ChassisSpeedsStruct implements Struct<ChassisSpeeds> {
}
@Override
public ChassisSpeeds unpack(ByteBuffer bb) {
public ChassisVelocities unpack(ByteBuffer bb) {
double vx = bb.getDouble();
double vy = bb.getDouble();
double omega = bb.getDouble();
return new ChassisSpeeds(vx, vy, omega);
return new ChassisVelocities(vx, vy, omega);
}
@Override
public void pack(ByteBuffer bb, ChassisSpeeds value) {
public void pack(ByteBuffer bb, ChassisVelocities value) {
bb.putDouble(value.vx);
bb.putDouble(value.vy);
bb.putDouble(value.omega);

17
wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java → wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.java
View File

@@ -5,18 +5,19 @@
package org.wpilib.math.kinematics.struct;
import java.nio.ByteBuffer;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
import org.wpilib.util.struct.Struct;
public class DifferentialDriveWheelSpeedsStruct implements Struct<DifferentialDriveWheelSpeeds> {
public class DifferentialDriveWheelVelocitiesStruct
implements Struct<DifferentialDriveWheelVelocities> {
@Override
public Class<DifferentialDriveWheelSpeeds> getTypeClass() {
return DifferentialDriveWheelSpeeds.class;
public Class<DifferentialDriveWheelVelocities> getTypeClass() {
return DifferentialDriveWheelVelocities.class;
}
@Override
public String getTypeName() {
return "DifferentialDriveWheelSpeeds";
return "DifferentialDriveWheelVelocities";
}
@Override
@@ -30,14 +31,14 @@ public class DifferentialDriveWheelSpeedsStruct implements Struct<DifferentialDr
}
@Override
public DifferentialDriveWheelSpeeds unpack(ByteBuffer bb) {
public DifferentialDriveWheelVelocities unpack(ByteBuffer bb) {
double left = bb.getDouble();
double right = bb.getDouble();
return new DifferentialDriveWheelSpeeds(left, right);
return new DifferentialDriveWheelVelocities(left, right);
}
@Override
public void pack(ByteBuffer bb, DifferentialDriveWheelSpeeds value) {
public void pack(ByteBuffer bb, DifferentialDriveWheelVelocities value) {
bb.putDouble(value.left);
bb.putDouble(value.right);
}

16
wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java → wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.java
View File

@@ -5,18 +5,18 @@
package org.wpilib.math.kinematics.struct;
import java.nio.ByteBuffer;
import org.wpilib.math.kinematics.MecanumDriveWheelSpeeds;
import org.wpilib.math.kinematics.MecanumDriveWheelVelocities;
import org.wpilib.util.struct.Struct;
public class MecanumDriveWheelSpeedsStruct implements Struct<MecanumDriveWheelSpeeds> {
public class MecanumDriveWheelVelocitiesStruct implements Struct<MecanumDriveWheelVelocities> {
@Override
public Class<MecanumDriveWheelSpeeds> getTypeClass() {
return MecanumDriveWheelSpeeds.class;
public Class<MecanumDriveWheelVelocities> getTypeClass() {
return MecanumDriveWheelVelocities.class;
}
@Override
public String getTypeName() {
return "MecanumDriveWheelSpeeds";
return "MecanumDriveWheelVelocities";
}
@Override
@@ -30,16 +30,16 @@ public class MecanumDriveWheelSpeedsStruct implements Struct<MecanumDriveWheelSp
}
@Override
public MecanumDriveWheelSpeeds unpack(ByteBuffer bb) {
public MecanumDriveWheelVelocities unpack(ByteBuffer bb) {
double frontLeft = bb.getDouble();
double frontRight = bb.getDouble();
double rearLeft = bb.getDouble();
double rearRight = bb.getDouble();
return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight);
return new MecanumDriveWheelVelocities(frontLeft, frontRight, rearLeft, rearRight);
}
@Override
public void pack(ByteBuffer bb, MecanumDriveWheelSpeeds value) {
public void pack(ByteBuffer bb, MecanumDriveWheelVelocities value) {
bb.putDouble(value.frontLeft);
bb.putDouble(value.frontRight);
bb.putDouble(value.rearLeft);

22
wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleStateStruct.java → wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleVelocityStruct.java
View File

@@ -6,18 +6,18 @@ package org.wpilib.math.kinematics.struct;
import java.nio.ByteBuffer;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.kinematics.SwerveModuleVelocity;
import org.wpilib.util.struct.Struct;
public class SwerveModuleStateStruct implements Struct<SwerveModuleState> {
public class SwerveModuleVelocityStruct implements Struct<SwerveModuleVelocity> {
@Override
public Class<SwerveModuleState> getTypeClass() {
return SwerveModuleState.class;
public Class<SwerveModuleVelocity> getTypeClass() {
return SwerveModuleVelocity.class;
}
@Override
public String getTypeName() {
return "SwerveModuleState";
return "SwerveModuleVelocity";
}
@Override
@@ -27,7 +27,7 @@ public class SwerveModuleStateStruct implements Struct<SwerveModuleState> {
@Override
public String getSchema() {
return "double speed;Rotation2d angle";
return "double velocity;Rotation2d angle";
}
@Override
@@ -36,15 +36,15 @@ public class SwerveModuleStateStruct implements Struct<SwerveModuleState> {
}
@Override
public SwerveModuleState unpack(ByteBuffer bb) {
double speed = bb.getDouble();
public SwerveModuleVelocity unpack(ByteBuffer bb) {
double velocity = bb.getDouble();
Rotation2d angle = Rotation2d.struct.unpack(bb);
return new SwerveModuleState(speed, angle);
return new SwerveModuleVelocity(velocity, angle);
}
@Override
public void pack(ByteBuffer bb, SwerveModuleState value) {
bb.putDouble(value.speed);
public void pack(ByteBuffer bb, SwerveModuleVelocity value) {
bb.putDouble(value.velocity);
Rotation2d.struct.pack(bb, value.angle);
}
}

20
wpimath/src/main/java/org/wpilib/math/system/DCMotor.java
View File

@@ -71,14 +71,14 @@ public class DCMotor implements ProtobufSerializable, StructSerializable {
}
/**
* Calculate current drawn by motor with given speed and input voltage.
* Calculate current drawn by motor with given velocity and input voltage.
*
* @param speed The current angular velocity of the motor.
* @param velocity The current angular velocity of the motor.
* @param voltageInput The voltage being applied to the motor.
* @return The estimated current.
*/
public double getCurrent(double speed, double voltageInput) {
return -1.0 / Kv / R * speed + 1.0 / R * voltageInput;
public double getCurrent(double velocity, double voltageInput) {
return -1.0 / Kv / R * velocity + 1.0 / R * voltageInput;
}
/**
@@ -105,21 +105,21 @@ public class DCMotor implements ProtobufSerializable, StructSerializable {
* Calculate the voltage provided to the motor for a given torque and angular velocity.
*
* @param torque The torque produced by the motor in Newton-meters.
* @param speed The current angular velocity of the motor in radians per second.
* @param velocity The current angular velocity of the motor in radians per second.
* @return The voltage of the motor.
*/
public double getVoltage(double torque, double speed) {
return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
public double getVoltage(double torque, double velocity) {
return 1.0 / Kv * velocity + 1.0 / Kt * R * torque;
}
/**
* Calculates the angular speed produced by the motor at a given torque and input voltage.
* Calculates the angular velocity produced by the motor at a given torque and input voltage.
*
* @param torque The torque produced by the motor in Newton-meters.
* @param voltageInput The voltage applied to the motor.
* @return The angular speed of the motor.
* @return The angular velocity of the motor.
*/
public double getSpeed(double torque, double voltageInput) {
public double getVelocity(double torque, double voltageInput) {
return voltageInput * Kv - 1.0 / Kt * torque * R * Kv;
}

4
wpimath/src/main/java/org/wpilib/math/trajectory/Trajectory.java
View File

@@ -259,7 +259,7 @@ public class Trajectory implements ProtobufSerializable {
@JsonProperty("time")
public double time;
/** The speed at that point of the trajectory in meters per second. */
/** The velocity at that point of the trajectory in meters per second. */
@JsonProperty("velocity")
public double velocity;
@@ -284,7 +284,7 @@ public class Trajectory implements ProtobufSerializable {
* Constructs a State with the specified parameters.
*
* @param time The time elapsed since the beginning of the trajectory in seconds.
* @param velocity The speed at that point of the trajectory in m/s.
* @param velocity The velocity at that point of the trajectory in m/s.
* @param acceleration The acceleration at that point of the trajectory in m/s².
* @param pose The pose at that point of the trajectory.
* @param curvature The curvature at that point of the trajectory in rad/m.

36
wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java
View File

@@ -47,7 +47,7 @@ public class TrapezoidProfile {
private State m_current = new State();
private double m_endAccel;
private double m_endFullSpeed;
private double m_endFullVelocity;
private double m_endDecel;
/** Profile constraints. */
@@ -156,24 +156,24 @@ public class TrapezoidProfile {
cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist =
double fullVelocityDist =
fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < 0) {
// Handle the case where the profile never reaches full velocity
if (fullVelocityDist < 0) {
accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = 0;
fullVelocityDist = 0;
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd;
m_endFullVelocity = m_endAccel + fullVelocityDist / m_constraints.maxVelocity;
m_endDecel = m_endFullVelocity + accelerationTime - cutoffEnd;
State result = new State(m_current.position, m_current.velocity);
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
} else if (t < m_endFullVelocity) {
result.velocity = m_constraints.maxVelocity;
result.position +=
(m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
@@ -203,16 +203,16 @@ public class TrapezoidProfile {
double velocity = m_current.velocity * m_direction;
double endAccel = m_endAccel * m_direction;
double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
double endFullVelocity = m_endFullVelocity * m_direction - endAccel;
if (target < position) {
endAccel = -endAccel;
endFullSpeed = -endFullSpeed;
endFullVelocity = -endFullVelocity;
velocity = -velocity;
}
endAccel = Math.max(endAccel, 0);
endFullSpeed = Math.max(endFullSpeed, 0);
endFullVelocity = Math.max(endFullVelocity, 0);
final double acceleration = m_constraints.maxAcceleration;
final double deceleration = -m_constraints.maxAcceleration;
@@ -231,18 +231,18 @@ public class TrapezoidProfile {
decelVelocity = velocity;
}
double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
double fullVelocityDist = m_constraints.maxVelocity * endFullVelocity;
double decelDist;
if (accelDist > distToTarget) {
accelDist = distToTarget;
fullSpeedDist = 0;
fullVelocityDist = 0;
decelDist = 0;
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
} else if (accelDist + fullVelocityDist > distToTarget) {
fullVelocityDist = distToTarget - accelDist;
decelDist = 0;
} else {
decelDist = distToTarget - fullSpeedDist - accelDist;
decelDist = distToTarget - fullVelocityDist - accelDist;
}
double accelTime =
@@ -254,9 +254,9 @@ public class TrapezoidProfile {
+ Math.sqrt(Math.abs(decelVelocity * decelVelocity + 2 * deceleration * decelDist)))
/ deceleration;
double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
double fullVelocityTime = fullVelocityDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + decelTime;
return accelTime + fullVelocityTime + decelTime;
}
/**

4
wpimath/src/main/java/org/wpilib/math/trajectory/constraint/CentripetalAccelerationConstraint.java
View File

@@ -49,11 +49,11 @@ public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
* curvature, and velocity.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @param velocity The velocity at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override

28
wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
View File

@@ -5,7 +5,7 @@
package org.wpilib.math.trajectory.constraint;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
/**
@@ -14,18 +14,18 @@ import org.wpilib.math.kinematics.DifferentialDriveKinematics;
* drivetrain stay below a certain limit.
*/
public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeed;
private final double m_maxVelocity;
private final DifferentialDriveKinematics m_kinematics;
/**
* Constructs a differential drive dynamics constraint.
*
* @param kinematics A kinematics component describing the drive geometry.
* @param maxSpeed The max speed that a side of the robot can travel at in m/s.
* @param maxVelocity The max velocity that a side of the robot can travel at in m/s.
*/
public DifferentialDriveKinematicsConstraint(
final DifferentialDriveKinematics kinematics, double maxSpeed) {
m_maxSpeed = maxSpeed;
final DifferentialDriveKinematics kinematics, double maxVelocity) {
m_maxVelocity = maxVelocity;
m_kinematics = kinematics;
}
@@ -40,24 +40,24 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai
*/
@Override
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(velocity, 0, velocity * curvature);
// Create an object to represent the current chassis velocities.
var chassisVelocities = new ChassisVelocities(velocity, 0, velocity * curvature);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.desaturate(m_maxSpeed);
// Get the wheel velocities and normalize them to within the max velocity.
var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
wheelVelocities.desaturate(m_maxVelocity);
// Return the new linear chassis speed.
return m_kinematics.toChassisSpeeds(wheelSpeeds).vx;
// Return the new linear chassis velocity.
return m_kinematics.toChassisVelocities(wheelVelocities).vx;
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
* curvature, and velocity.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @param velocity The velocity at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override

18
wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
View File

@@ -8,7 +8,7 @@ import static org.wpilib.util.ErrorMessages.requireNonNullParam;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
/**
@@ -49,18 +49,18 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
@Override
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
var wheelSpeeds =
m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocity, 0, velocity * curvature));
var wheelVelocities =
m_kinematics.toWheelVelocities(new ChassisVelocities(velocity, 0, velocity * curvature));
double maxWheelSpeed = Math.max(wheelSpeeds.left, wheelSpeeds.right);
double minWheelSpeed = Math.min(wheelSpeeds.left, wheelSpeeds.right);
double maxWheelVelocity = Math.max(wheelVelocities.left, wheelVelocities.right);
double minWheelVelocity = Math.min(wheelVelocities.left, wheelVelocities.right);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
// and max/min wheel velocities
double maxWheelAcceleration =
m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelVelocity);
double minWheelAcceleration =
m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelVelocity);
// Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
// increased by half of the trackwidth T. Inner wheel has radius decreased
@@ -68,7 +68,7 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
// Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
// Inner wheel is similar.
// sgn(speed) term added to correctly account for which wheel is on
// sgn(velocity) term added to correctly account for which wheel is on
// outside of turn:
// If moving forward, max acceleration constraint corresponds to wheel on outside of turn
// If moving backward, max acceleration constraint corresponds to wheel on inside of turn

31
wpimath/src/main/java/org/wpilib/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
View File

@@ -5,7 +5,7 @@
package org.wpilib.math.trajectory.constraint;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.MecanumDriveKinematics;
/**
@@ -14,18 +14,18 @@ import org.wpilib.math.kinematics.MecanumDriveKinematics;
* drivetrain stay below a certain limit.
*/
public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeed;
private final double m_maxVelocity;
private final MecanumDriveKinematics m_kinematics;
/**
* Constructs a mecanum drive kinematics constraint.
*
* @param kinematics Mecanum drive kinematics.
* @param maxSpeed The max speed that a side of the robot can travel at in m/s.
* @param maxVelocity The max velocity that a side of the robot can travel at in m/s.
*/
public MecanumDriveKinematicsConstraint(
final MecanumDriveKinematics kinematics, double maxSpeed) {
m_maxSpeed = maxSpeed;
final MecanumDriveKinematics kinematics, double maxVelocity) {
m_maxVelocity = maxVelocity;
m_kinematics = kinematics;
}
@@ -46,26 +46,27 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
// Represents the velocity of the chassis in the y direction
var ydVelocity = velocity * pose.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
// Create an object to represent the current chassis velocities.
var chassisVelocities = new ChassisVelocities(xdVelocity, ydVelocity, velocity * curvature);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeed);
// Get the wheel velocities and normalize them to within the max velocity.
var wheelVelocities =
m_kinematics.toWheelVelocities(chassisVelocities).desaturate(m_maxVelocity);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
// Convert normalized wheel velocities back to chassis velocities
var normVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
// Return the new linear chassis speed.
return Math.hypot(normSpeeds.vx, normSpeeds.vy);
// Return the new linear chassis velocity.
return Math.hypot(normVelocities.vx, normVelocities.vy);
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
* curvature, and velocity.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @param velocity The velocity at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override

33
wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
View File

@@ -5,7 +5,7 @@
package org.wpilib.math.trajectory.constraint;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
/**
@@ -14,17 +14,18 @@ import org.wpilib.math.kinematics.SwerveDriveKinematics;
* stay below a certain limit.
*/
public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeed;
private final double m_maxVelocity;
private final SwerveDriveKinematics m_kinematics;
/**
* Constructs a swerve drive kinematics constraint.
*
* @param kinematics Swerve drive kinematics.
* @param maxSpeed The max speed that a side of the robot can travel at in m/s.
* @param maxVelocity The max velocity that a side of the robot can travel at in m/s.
*/
public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics, double maxSpeed) {
m_maxSpeed = maxSpeed;
public SwerveDriveKinematicsConstraint(
final SwerveDriveKinematics kinematics, double maxVelocity) {
m_maxVelocity = maxVelocity;
m_kinematics = kinematics;
}
@@ -45,27 +46,27 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
// Represents the velocity of the chassis in the y direction
var ydVelocity = velocity * pose.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
// Create an object to represent the current chassis velocities.
var chassisVelocities = new ChassisVelocities(xdVelocity, ydVelocity, velocity * curvature);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeed);
// Get the wheel velocities and normalize them to within the max velocity.
var wheelVelocities = m_kinematics.toSwerveModuleVelocities(chassisVelocities);
SwerveDriveKinematics.desaturateWheelVelocities(wheelVelocities, m_maxVelocity);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
// Convert normalized wheel velocities back to chassis velocities
var normVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
// Return the new linear chassis speed.
return Math.hypot(normSpeeds.vx, normSpeeds.vy);
// Return the new linear chassis velocity.
return Math.hypot(normVelocities.vx, normVelocities.vy);
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
* curvature, and velocity.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @param velocity The velocity at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override

4
wpimath/src/main/java/org/wpilib/math/trajectory/constraint/TrajectoryConstraint.java
View File

@@ -24,11 +24,11 @@ public interface TrajectoryConstraint {
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
* curvature, and velocity.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @param velocity The velocity at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity);

6
wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
View File

@@ -10,7 +10,7 @@
using namespace wpi::math;
ChassisSpeeds LTVUnicycleController::Calculate(
ChassisVelocities LTVUnicycleController::Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
wpi::units::meters_per_second_t linearVelocityRef,
wpi::units::radians_per_second_t angularVelocityRef) {
@@ -48,7 +48,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
// [0 0 0] [0 1]
if (!m_enabled) {
return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
return ChassisVelocities{linearVelocityRef, 0_mps, angularVelocityRef};
}
// The DARE is ill-conditioned if the velocity is close to zero, so don't
@@ -78,7 +78,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
m_poseError.Rotation().Radians().value()};
Eigen::Vector2d u = K * e;
return ChassisSpeeds{
return ChassisVelocities{
linearVelocityRef + wpi::units::meters_per_second_t{u(0)}, 0_mps,
angularVelocityRef + wpi::units::radians_per_second_t{u(1)}};
}

45
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -6,8 +6,8 @@
using namespace wpi::math;
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
MecanumDriveWheelVelocities MecanumDriveKinematics::ToWheelVelocities(
const ChassisVelocities& chassisVelocities,
const Translation2d& centerOfRotation) const {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation != m_previousCoR) {
@@ -21,32 +21,33 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Vector3d chassisVelocitiesVector{chassisVelocities.vx.value(),
chassisVelocities.vy.value(),
chassisVelocities.omega.value()};
Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisVelocitiesVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = wpi::units::meters_per_second_t{wheelsVector(0)};
wheelSpeeds.frontRight = wpi::units::meters_per_second_t{wheelsVector(1)};
wheelSpeeds.rearLeft = wpi::units::meters_per_second_t{wheelsVector(2)};
wheelSpeeds.rearRight = wpi::units::meters_per_second_t{wheelsVector(3)};
return wheelSpeeds;
MecanumDriveWheelVelocities wheelVelocities;
wheelVelocities.frontLeft = wpi::units::meters_per_second_t{wheelsVector(0)};
wheelVelocities.frontRight = wpi::units::meters_per_second_t{wheelsVector(1)};
wheelVelocities.rearLeft = wpi::units::meters_per_second_t{wheelsVector(2)};
wheelVelocities.rearRight = wpi::units::meters_per_second_t{wheelsVector(3)};
return wheelVelocities;
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Eigen::Vector4d wheelSpeedsVector{
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
ChassisVelocities MecanumDriveKinematics::ToChassisVelocities(
const MecanumDriveWheelVelocities& wheelVelocities) const {
Eigen::Vector4d wheelVelocitiesVector{
wheelVelocities.frontLeft.value(), wheelVelocities.frontRight.value(),
wheelVelocities.rearLeft.value(), wheelVelocities.rearRight.value()};
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(wheelSpeedsVector);
Eigen::Vector3d chassisVelocitiesVector =
m_forwardKinematics.solve(wheelVelocitiesVector);
return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT
wpi::units::meters_per_second_t{chassisSpeedsVector(1)},
wpi::units::radians_per_second_t{chassisSpeedsVector(2)}};
return {
wpi::units::meters_per_second_t{chassisVelocitiesVector(0)}, // NOLINT
wpi::units::meters_per_second_t{chassisVelocitiesVector(1)},
wpi::units::radians_per_second_t{chassisVelocitiesVector(2)}};
}
Twist2d MecanumDriveKinematics::ToTwist2d(

16
wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp → wpimath/src/main/native/cpp/kinematics/proto/ChassisVelocitiesProto.cpp
View File

@@ -2,27 +2,27 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/proto/ChassisSpeedsProto.hpp"
#include "wpi/math/kinematics/proto/ChassisVelocitiesProto.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::ChassisSpeeds>
wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Unpack(InputStream& stream) {
wpi_proto_ProtobufChassisSpeeds msg;
std::optional<wpi::math::ChassisVelocities>
wpi::util::Protobuf<wpi::math::ChassisVelocities>::Unpack(InputStream& stream) {
wpi_proto_ProtobufChassisVelocities msg;
if (!stream.Decode(msg)) {
return {};
}
return wpi::math::ChassisSpeeds{
return wpi::math::ChassisVelocities{
wpi::units::meters_per_second_t{msg.vx},
wpi::units::meters_per_second_t{msg.vy},
wpi::units::radians_per_second_t{msg.omega},
};
}
bool wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Pack(
OutputStream& stream, const wpi::math::ChassisSpeeds& value) {
wpi_proto_ProtobufChassisSpeeds msg{
bool wpi::util::Protobuf<wpi::math::ChassisVelocities>::Pack(
OutputStream& stream, const wpi::math::ChassisVelocities& value) {
wpi_proto_ProtobufChassisVelocities msg{
.vx = value.vx.value(),
.vy = value.vy.value(),
.omega = value.omega.value(),

30
wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp
View File

@@ -1,30 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::DifferentialDriveWheelSpeeds> wpi::util::Protobuf<
wpi::math::DifferentialDriveWheelSpeeds>::Unpack(InputStream& stream) {
wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg;
if (!stream.Decode(msg)) {
return {};
}
return wpi::math::DifferentialDriveWheelSpeeds{
wpi::units::meters_per_second_t{msg.left},
wpi::units::meters_per_second_t{msg.right},
};
}
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Pack(
OutputStream& stream,
const wpi::math::DifferentialDriveWheelSpeeds& value) {
wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg{
.left = value.left.value(),
.right = value.right.value(),
};
return stream.Encode(msg);
}

30
wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelVelocitiesProto.cpp Normal file
View File

@@ -0,0 +1,30 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::DifferentialDriveWheelVelocities> wpi::util::Protobuf<
wpi::math::DifferentialDriveWheelVelocities>::Unpack(InputStream& stream) {
wpi_proto_ProtobufDifferentialDriveWheelVelocities msg;
if (!stream.Decode(msg)) {
return {};
}
return wpi::math::DifferentialDriveWheelVelocities{
wpi::units::meters_per_second_t{msg.left},
wpi::units::meters_per_second_t{msg.right},
};
}
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVelocities>::Pack(
OutputStream& stream,
const wpi::math::DifferentialDriveWheelVelocities& value) {
wpi_proto_ProtobufDifferentialDriveWheelVelocities msg{
.left = value.left.value(),
.right = value.right.value(),
};
return stream.Encode(msg);
}

16
wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp → wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelVelocitiesProto.cpp
View File

@@ -2,18 +2,18 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp"
#include "wpi/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::MecanumDriveWheelSpeeds> wpi::util::Protobuf<
wpi::math::MecanumDriveWheelSpeeds>::Unpack(InputStream& stream) {
wpi_proto_ProtobufMecanumDriveWheelSpeeds msg;
std::optional<wpi::math::MecanumDriveWheelVelocities> wpi::util::Protobuf<
wpi::math::MecanumDriveWheelVelocities>::Unpack(InputStream& stream) {
wpi_proto_ProtobufMecanumDriveWheelVelocities msg;
if (!stream.Decode(msg)) {
return {};
}
return wpi::math::MecanumDriveWheelSpeeds{
return wpi::math::MecanumDriveWheelVelocities{
wpi::units::meters_per_second_t{msg.front_left},
wpi::units::meters_per_second_t{msg.front_right},
wpi::units::meters_per_second_t{msg.rear_left},
@@ -21,9 +21,9 @@ std::optional<wpi::math::MecanumDriveWheelSpeeds> wpi::util::Protobuf<
};
}
bool wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds>::Pack(
OutputStream& stream, const wpi::math::MecanumDriveWheelSpeeds& value) {
wpi_proto_ProtobufMecanumDriveWheelSpeeds msg{
bool wpi::util::Protobuf<wpi::math::MecanumDriveWheelVelocities>::Pack(
OutputStream& stream, const wpi::math::MecanumDriveWheelVelocities& value) {
wpi_proto_ProtobufMecanumDriveWheelVelocities msg{
.front_left = value.frontLeft.value(),
.front_right = value.frontRight.value(),
.rear_left = value.rearLeft.value(),

22
wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp → wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleVelocityProto.cpp
View File

@@ -2,16 +2,16 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/proto/SwerveModuleStateProto.hpp"
#include "wpi/math/kinematics/proto/SwerveModuleVelocityProto.hpp"
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::SwerveModuleState>
wpi::util::Protobuf<wpi::math::SwerveModuleState>::Unpack(InputStream& stream) {
std::optional<wpi::math::SwerveModuleVelocity> wpi::util::Protobuf<
wpi::math::SwerveModuleVelocity>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Rotation2d> angle;
wpi_proto_ProtobufSwerveModuleState msg{
.speed = 0,
wpi_proto_ProtobufSwerveModuleVelocity msg{
.velocity = 0,
.angle = angle.Callback(),
};
if (!stream.Decode(msg)) {
@@ -24,17 +24,17 @@ wpi::util::Protobuf<wpi::math::SwerveModuleState>::Unpack(InputStream& stream) {
return {};
}
return wpi::math::SwerveModuleState{
wpi::units::meters_per_second_t{msg.speed},
return wpi::math::SwerveModuleVelocity{
wpi::units::meters_per_second_t{msg.velocity},
iangle[0],
};
}
bool wpi::util::Protobuf<wpi::math::SwerveModuleState>::Pack(
OutputStream& stream, const wpi::math::SwerveModuleState& value) {
bool wpi::util::Protobuf<wpi::math::SwerveModuleVelocity>::Pack(
OutputStream& stream, const wpi::math::SwerveModuleVelocity& value) {
wpi::util::PackCallback angle{&value.angle};
wpi_proto_ProtobufSwerveModuleState msg{
.speed = value.speed.value(),
wpi_proto_ProtobufSwerveModuleVelocity msg{
.velocity = value.velocity.value(),
.angle = angle.Callback(),
};
return stream.Encode(msg);

10
wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp → wpimath/src/main/native/cpp/kinematics/struct/ChassisVelocitiesStruct.cpp
View File

@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp"
#include "wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp"
namespace {
constexpr size_t kVxOff = 0;
@@ -10,10 +10,10 @@ constexpr size_t kVyOff = kVxOff + 8;
constexpr size_t kOmegaOff = kVyOff + 8;
} // namespace
using StructType = wpi::util::Struct<wpi::math::ChassisSpeeds>;
using StructType = wpi::util::Struct<wpi::math::ChassisVelocities>;
wpi::math::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
return wpi::math::ChassisSpeeds{
wpi::math::ChassisVelocities StructType::Unpack(std::span<const uint8_t> data) {
return wpi::math::ChassisVelocities{
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kVxOff>(data)},
wpi::units::meters_per_second_t{
@@ -24,7 +24,7 @@ wpi::math::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
}
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::ChassisSpeeds& value) {
const wpi::math::ChassisVelocities& value) {
wpi::util::PackStruct<kVxOff>(data, value.vx.value());
wpi::util::PackStruct<kVyOff>(data, value.vy.value());
wpi::util::PackStruct<kOmegaOff>(data, value.omega.value());

14
wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp → wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.cpp
View File

@@ -2,18 +2,19 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.hpp"
namespace {
constexpr size_t kLeftOff = 0;
constexpr size_t kRightOff = kLeftOff + 8;
} // namespace
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
using StructType =
wpi::util::Struct<wpi::math::DifferentialDriveWheelVelocities>;
wpi::math::DifferentialDriveWheelSpeeds StructType::Unpack(
wpi::math::DifferentialDriveWheelVelocities StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::DifferentialDriveWheelSpeeds{
return wpi::math::DifferentialDriveWheelVelocities{
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kLeftOff>(data)},
wpi::units::meters_per_second_t{
@@ -21,8 +22,9 @@ wpi::math::DifferentialDriveWheelSpeeds StructType::Unpack(
};
}
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::DifferentialDriveWheelSpeeds& value) {
void StructType::Pack(
std::span<uint8_t> data,
const wpi::math::DifferentialDriveWheelVelocities& value) {
wpi::util::PackStruct<kLeftOff>(data, value.left.value());
wpi::util::PackStruct<kRightOff>(data, value.right.value());
}

10
wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp → wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelVelocitiesStruct.cpp
View File

@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp"
#include "wpi/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.hpp"
namespace {
constexpr size_t kFrontLeftOff = 0;
@@ -11,11 +11,11 @@ constexpr size_t kRearLeftOff = kFrontRightOff + 8;
constexpr size_t kRearRightOff = kRearLeftOff + 8;
} // namespace
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds>;
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelVelocities>;
wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(
wpi::math::MecanumDriveWheelVelocities StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::MecanumDriveWheelSpeeds{
return wpi::math::MecanumDriveWheelVelocities{
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kFrontLeftOff>(data)},
wpi::units::meters_per_second_t{
@@ -28,7 +28,7 @@ wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(
}
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::MecanumDriveWheelSpeeds& value) {
const wpi::math::MecanumDriveWheelVelocities& value) {
wpi::util::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
wpi::util::PackStruct<kFrontRightOff>(data, value.frontRight.value());
wpi::util::PackStruct<kRearLeftOff>(data, value.rearLeft.value());

26
wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp
View File

@@ -1,26 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/struct/SwerveModuleStateStruct.hpp"
namespace {
constexpr size_t kSpeedOff = 0;
constexpr size_t kAngleOff = kSpeedOff + 8;
} // namespace
using StructType = wpi::util::Struct<wpi::math::SwerveModuleState>;
wpi::math::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
return wpi::math::SwerveModuleState{
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kSpeedOff>(data)},
wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),
};
}
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::SwerveModuleState& value) {
wpi::util::PackStruct<kSpeedOff>(data, value.speed.value());
wpi::util::PackStruct<kAngleOff>(data, value.angle);
}

27
wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleVelocityStruct.cpp Normal file
View File

@@ -0,0 +1,27 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp"
namespace {
constexpr size_t kVelocityOff = 0;
constexpr size_t kAngleOff = kVelocityOff + 8;
} // namespace
using StructType = wpi::util::Struct<wpi::math::SwerveModuleVelocity>;
wpi::math::SwerveModuleVelocity StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::SwerveModuleVelocity{
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kVelocityOff>(data)},
wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),
};
}
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::SwerveModuleVelocity& value) {
wpi::util::PackStruct<kVelocityOff>(data, value.velocity.value());
wpi::util::PackStruct<kAngleOff>(data, value.angle);
}

2
wpimath/src/main/native/include/wpi/math/controller/BangBangController.hpp
View File

@@ -23,7 +23,7 @@ namespace wpi::math {
*
* <p>Note that this is an *asymmetric* bang-bang controller - it will not exert
* any control effort in the reverse direction (e.g. it won't try to slow down
* an over-speeding shooter wheel). This asymmetry is *extremely important.*
* an overspeeding shooter wheel). This asymmetry is *extremely important*.
* Bang-bang control is extremely simple, but also potentially hazardous. Always
* ensure that your motor controllers are set to "coast" before attempting to
* control them with a bang-bang controller.

13
wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp
View File

@@ -7,7 +7,7 @@
#include <Eigen/Core>
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/trajectory/Trajectory.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/angular_velocity.hpp"
@@ -104,9 +104,10 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param linearVelocityRef The desired linear velocity.
* @param angularVelocityRef The desired angular velocity.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
wpi::units::meters_per_second_t linearVelocityRef,
wpi::units::radians_per_second_t angularVelocityRef);
ChassisVelocities Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
wpi::units::meters_per_second_t linearVelocityRef,
wpi::units::radians_per_second_t angularVelocityRef);
/**
* Returns the linear and angular velocity outputs of the LTV controller.
@@ -118,8 +119,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param desiredState The desired pose, linear velocity, and angular velocity
* from a trajectory.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState) {
ChassisVelocities Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredState.velocity * desiredState.curvature);
}

2
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp
View File

@@ -31,7 +31,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
: public PoseEstimator<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

2
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp
View File

@@ -35,7 +35,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
: public PoseEstimator3d<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

3
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp
View File

@@ -28,7 +28,8 @@ namespace wpi::math {
* odometry.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
: public PoseEstimator<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public PoseEstimator<MecanumDriveWheelPositions,
MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

2
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp
View File

@@ -33,7 +33,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
: public PoseEstimator3d<MecanumDriveWheelPositions,
MecanumDriveWheelSpeeds,
MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

11
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -39,10 +39,10 @@ namespace wpi::math {
* never call it, then this class will behave like regular encoder odometry.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT PoseEstimator {
public:
@@ -64,8 +64,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
* less.
*/
PoseEstimator(
Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
kinematics,
Odometry<WheelPositions, WheelVelocities, WheelAccelerations>& odometry,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry) {
@@ -478,7 +479,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
Odometry<WheelPositions, WheelVelocities, WheelAccelerations>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::util::array<double, 3> m_q{wpi::util::empty_array};

11
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -44,10 +44,10 @@ namespace wpi::math {
* never call it, then this class will behave like regular encoder odometry.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT PoseEstimator3d {
public:
@@ -69,8 +69,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
* vision pose measurement less.
*/
PoseEstimator3d(
Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
kinematics,
Odometry3d<WheelPositions, WheelVelocities, WheelAccelerations>& odometry,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
: m_odometry(odometry) {
@@ -492,7 +493,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
Odometry3d<WheelPositions, WheelVelocities, WheelAccelerations>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::util::array<double, 4> m_q{wpi::util::empty_array};

2
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp
View File

@@ -29,7 +29,7 @@ template <size_t NumModules>
class SwerveDrivePoseEstimator
: public PoseEstimator<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

2
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp
View File

@@ -33,7 +33,7 @@ template <size_t NumModules>
class SwerveDrivePoseEstimator3d
: public PoseEstimator3d<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

13
wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp
View File

@@ -12,10 +12,7 @@
namespace wpi::math {
/**
* Represents the acceleration of a robot chassis. Although this struct contains
* similar members compared to a ChassisSpeeds, they do NOT represent the same
* thing. Whereas a ChassisSpeeds object represents a robot's velocity, a
* ChassisAccelerations object represents a robot's acceleration.
* Represents robot chassis accelerations.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have an ay component because it can never move sideways. Holonomic
@@ -82,7 +79,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
/**
* Adds two ChassisAccelerations and returns the sum.
*
* <p>For example, ChassisAccelerations{1.0, 0.5, 1.5} +
* For example, ChassisAccelerations{1.0, 0.5, 1.5} +
* ChassisAccelerations{2.0, 1.5, 0.5} = ChassisAccelerations{3.0, 2.0, 2.0}
*
* @param other The ChassisAccelerations to add.
@@ -98,7 +95,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
* Subtracts the other ChassisAccelerations from the current
* ChassisAccelerations and returns the difference.
*
* <p>For example, ChassisAccelerations{5.0, 4.0, 2.0} -
* For example, ChassisAccelerations{5.0, 4.0, 2.0} -
* ChassisAccelerations{1.0, 2.0, 1.0} = ChassisAccelerations{4.0, 2.0, 1.0}
*
* @param other The ChassisAccelerations to subtract.
@@ -124,7 +121,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
* Multiplies the ChassisAccelerations by a scalar and returns the new
* ChassisAccelerations.
*
* <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2
* For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2
* = ChassisAccelerations{4.0, 5.0, 2.0}
*
* @param scalar The scalar to multiply by.
@@ -139,7 +136,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
* Divides the ChassisAccelerations by a scalar and returns the new
* ChassisAccelerations.
*
* <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2
* For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2
* = ChassisAccelerations{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.

200
wpimath/src/main/native/include/wpi/math/kinematics/ChassisSpeeds.hpp
View File

@@ -1,200 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
* reference, a ChassisSpeeds struct represents a robot's velocity.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
struct WPILIB_DLLEXPORT ChassisSpeeds {
/**
* Velocity along the x-axis. (Fwd is +)
*/
wpi::units::meters_per_second_t vx = 0_mps;
/**
* Velocity along the y-axis. (Left is +)
*/
wpi::units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
wpi::units::radians_per_second_t omega = 0_rad_per_s;
/**
* Creates a Twist2d from ChassisSpeeds.
*
* @param dt The duration of the timestep.
*
* @return Twist2d.
*/
constexpr Twist2d ToTwist2d(wpi::units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
/**
* Discretizes a continuous-time chassis speed.
*
* This function converts a continuous-time chassis speed into a discrete-time
* one such that when the discrete-time chassis speed is applied for one
* timestep, the robot moves as if the velocity components are independent
* (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
* y-axis, and omega * dt around the z-axis).
*
* This is useful for compensating for translational skew when translating and
* rotating a holonomic (swerve or mecanum) drivetrain. However, scaling down
* the ChassisSpeeds after discretizing (e.g., when desaturating swerve module
* speeds) rotates the direction of net motion in the opposite direction of
* rotational velocity, introducing a different translational skew which is
* not accounted for by discretization.
*
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
*/
constexpr ChassisSpeeds Discretize(wpi::units::second_t dt) const {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
// Find the chassis translation/rotation deltas in the robot frame that move
// the robot from its current pose to the desired pose
auto twist = desiredTransform.Log();
// Turn the chassis translation/rotation deltas into average velocities
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}
/**
* Converts this field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Converts this robot-relative set of speeds into a field-relative
* ChassisSpeeds object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Adds two ChassisSpeeds and returns the sum.
*
* <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
* = ChassisSpeeds{3.0, 2.0, 2.0}
*
* @param other The ChassisSpeeds to add.
*
* @return The sum of the ChassisSpeeds.
*/
constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
return {vx + other.vx, vy + other.vy, omega + other.omega};
}
/**
* Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
* returns the difference.
*
* <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
* = ChassisSpeeds{4.0, 2.0, 1.0}
*
* @param other The ChassisSpeeds to subtract.
*
* @return The difference between the two ChassisSpeeds.
*/
constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current ChassisSpeeds.
* This is equivalent to negating all components of the ChassisSpeeds.
*
* @return The inverse of the current ChassisSpeeds.
*/
constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
/**
* Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
*
* <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
* = ChassisSpeeds{4.0, 5.0, 1.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled ChassisSpeeds.
*/
constexpr ChassisSpeeds operator*(double scalar) const {
return {scalar * vx, scalar * vy, scalar * omega};
}
/**
* Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
*
* <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
* = ChassisSpeeds{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.
*
* @return The scaled ChassisSpeeds.
*/
constexpr ChassisSpeeds operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Compares the ChassisSpeeds with another ChassisSpeed.
*
* @param other The other ChassisSpeeds.
*
* @return The result of the comparison. Is true if they are the same.
*/
constexpr bool operator==(const ChassisSpeeds& other) const = default;
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/ChassisSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp"

207
wpimath/src/main/native/include/wpi/math/kinematics/ChassisVelocities.hpp Normal file
View File

@@ -0,0 +1,207 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents robot chassis velocities.
*
* Although this struct contains similar members compared to a Twist2d, they do
* NOT represent the same thing. Whereas a Twist2d represents a change in pose
* w.r.t to the robot frame of reference, a ChassisVelocities struct represents
* a robot's velocities.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
struct WPILIB_DLLEXPORT ChassisVelocities {
/**
* Velocity along the x-axis. (Fwd is +)
*/
wpi::units::meters_per_second_t vx = 0_mps;
/**
* Velocity along the y-axis. (Left is +)
*/
wpi::units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
wpi::units::radians_per_second_t omega = 0_rad_per_s;
/**
* Creates a Twist2d from ChassisVelocities.
*
* @param dt The duration of the timestep.
*
* @return Twist2d.
*/
constexpr Twist2d ToTwist2d(wpi::units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
/**
* Discretizes continuous-time chassis velocities.
*
* This function converts continuous-time chassis velocities into
* discrete-time ones such that when the discrete-time chassis velocities are
* applied for one timestep, the robot moves as if the velocity components are
* independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
* along the y-axis, and omega * dt around the z-axis).
*
* This is useful for compensating for translational skew when translating and
* rotating a holonomic (swerve or mecanum) drivetrain. However, scaling down
* the ChassisVelocities after discretizing (e.g., when desaturating swerve
* module velocities) rotates the direction of net motion in the opposite
* direction of rotational velocity, introducing a different translational
* skew which is not accounted for by discretization.
*
* @param dt The duration of the timestep the velocities should be applied
* for.
* @return Discretized ChassisVelocities.
*/
constexpr ChassisVelocities Discretize(wpi::units::second_t dt) const {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
// Find the chassis translation/rotation deltas in the robot frame that move
// the robot from its current pose to the desired pose
auto twist = desiredTransform.Log();
// Turn the chassis translation/rotation deltas into average velocities
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}
/**
* Converts this field-relative set of velocities into a robot-relative
* ChassisVelocities object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisVelocities object representing the velocities in the robot's
* frame of reference.
*/
constexpr ChassisVelocities ToRobotRelative(
const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Converts this robot-relative set of velocities into a field-relative
* ChassisVelocities object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisVelocities object representing the velocities in the field's
* frame of reference.
*/
constexpr ChassisVelocities ToFieldRelative(
const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Adds two ChassisVelocities and returns the sum.
*
* For example, ChassisVelocities{1.0, 0.5, 1.5} +
* ChassisVelocities{2.0, 1.5, 0.5} = ChassisVelocities{3.0, 2.0, 2.0}
*
* @param other The ChassisVelocities to add.
*
* @return The sum of the ChassisVelocities.
*/
constexpr ChassisVelocities operator+(const ChassisVelocities& other) const {
return {vx + other.vx, vy + other.vy, omega + other.omega};
}
/**
* Subtracts the other ChassisVelocities from the current ChassisVelocities
* and returns the difference.
*
* For example, ChassisVelocities{5.0, 4.0, 2.0} -
* ChassisVelocities{1.0, 2.0, 1.0} = ChassisVelocities{4.0, 2.0, 1.0}
*
* @param other The ChassisVelocities to subtract.
*
* @return The difference between the two ChassisVelocities.
*/
constexpr ChassisVelocities operator-(const ChassisVelocities& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current ChassisVelocities.
* This is equivalent to negating all components of the ChassisVelocities.
*
* @return The inverse of the current ChassisVelocities.
*/
constexpr ChassisVelocities operator-() const { return {-vx, -vy, -omega}; }
/**
* Multiplies the ChassisVelocities by a scalar and returns the new
* ChassisVelocities.
*
* For example, ChassisVelocities{2.0, 2.5, 1.0} * 2
* = ChassisVelocities{4.0, 5.0, 1.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled ChassisVelocities.
*/
constexpr ChassisVelocities operator*(double scalar) const {
return {scalar * vx, scalar * vy, scalar * omega};
}
/**
* Divides the ChassisVelocities by a scalar and returns the new
* ChassisVelocities.
*
* For example, ChassisVelocities{2.0, 2.5, 1.0} / 2
* = ChassisVelocities{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.
*
* @return The scaled ChassisVelocities.
*/
constexpr ChassisVelocities operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Compares the ChassisVelocities with another ChassisVelocity.
*
* @param other The other ChassisVelocities.
*
* @return The result of the comparison. Is true if they are the same.
*/
constexpr bool operator==(const ChassisVelocities& other) const = default;
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/ChassisVelocitiesProto.hpp"
#include "wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp"

47
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp
View File

@@ -8,10 +8,10 @@
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/units/angle.hpp"
@@ -23,22 +23,22 @@ namespace wpi::math {
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
*
* Inverse kinematics converts a desired chassis speed into left and right
* Inverse kinematics converts a desired chassis velocity into left and right
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
* component velocities into a linear and angular chassis velocity.
*/
class WPILIB_DLLEXPORT DifferentialDriveKinematics
: public Kinematics<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**
* Constructs a differential drive kinematics object.
*
* @param trackwidth The trackwidth of the drivetrain. Theoretically, this is
* the distance between the left wheels and right wheels. However, the
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
* the distance between the left wheels and right wheels. However, the
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
constexpr explicit DifferentialDriveKinematics(wpi::units::meter_t trackwidth)
: trackwidth(trackwidth) {
@@ -49,30 +49,33 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
}
/**
* Returns a chassis speed from left and right component velocities using
* Returns a chassis velocity from left and right component velocities using
* forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
* @param wheelVelocities The left and right velocities.
* @return The chassis velocity.
*/
constexpr ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / trackwidth * 1_rad};
constexpr ChassisVelocities ToChassisVelocities(
const DifferentialDriveWheelVelocities& wheelVelocities) const override {
return {
(wheelVelocities.left + wheelVelocities.right) / 2.0, 0_mps,
(wheelVelocities.right - wheelVelocities.left) / trackwidth * 1_rad};
}
/**
* Returns left and right component velocities from a chassis speed using
* Returns left and right component velocities from a chassis velocity using
* inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that
* represent the chassis' speed.
* @param chassisVelocities The linear and angular (dx and dtheta) components
* that represent the chassis' velocity.
* @return The left and right velocities.
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return {chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega / 1_rad};
constexpr DifferentialDriveWheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const override {
return {
chassisVelocities.vx - trackwidth / 2 * chassisVelocities.omega / 1_rad,
chassisVelocities.vx +
trackwidth / 2 * chassisVelocities.omega / 1_rad};
}
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry.hpp"
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -27,7 +27,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDriveOdometry
: public Odometry<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry3d.hpp"
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -27,7 +27,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
: public Odometry3d<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

8
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
View File

@@ -25,7 +25,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
/**
* Adds two DifferentialDriveWheelAccelerations and returns the sum.
*
* <p>For example, DifferentialDriveWheelAccelerations{1.0, 0.5} +
* For example, DifferentialDriveWheelAccelerations{1.0, 0.5} +
* DifferentialDriveWheelAccelerations{2.0, 1.5} =
* DifferentialDriveWheelAccelerations{3.0, 2.0}
*
@@ -42,7 +42,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
* Subtracts the other DifferentialDriveWheelAccelerations from the current
* DifferentialDriveWheelAccelerations and returns the difference.
*
* <p>For example, DifferentialDriveWheelAccelerations{5.0, 4.0} -
* For example, DifferentialDriveWheelAccelerations{5.0, 4.0} -
* DifferentialDriveWheelAccelerations{1.0, 2.0} =
* DifferentialDriveWheelAccelerations{4.0, 2.0}
*
@@ -70,7 +70,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
* Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns
* the new DifferentialDriveWheelAccelerations.
*
* <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2
* For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2
* = DifferentialDriveWheelAccelerations{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
@@ -85,7 +85,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
* Divides the DifferentialDriveWheelAccelerations by a scalar and returns the
* new DifferentialDriveWheelAccelerations.
*
* <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2
* For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2
* = DifferentialDriveWheelAccelerations{1.0, 1.25}
*
* @param scalar The scalar to divide by.

126
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
View File

@@ -1,126 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
wpi::units::meters_per_second_t left = 0_mps;
/**
* Speed of the right side of the robot.
*/
wpi::units::meters_per_second_t right = 0_mps;
/**
* Renormalizes the wheel speeds if either side is above the specified
* maximum.
*
* Sometimes, after inverse kinematics, the requested speed from one or more
* wheels may be above the max attainable speed for the driving motor on that
* wheel. To fix this issue, one can reduce all the wheel speeds to make sure
* that all requested module speeds are at-or-below the absolute threshold,
* while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
constexpr void Desaturate(
wpi::units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed = wpi::units::math::max(wpi::units::math::abs(left),
wpi::units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}
/**
* Adds two DifferentialDriveWheelSpeeds and returns the sum.
*
* <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} +
* DifferentialDriveWheelSpeeds{2.0, 1.5} =
* DifferentialDriveWheelSpeeds{3.0, 2.0}
*
* @param other The DifferentialDriveWheelSpeeds to add.
*
* @return The sum of the DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator+(
const DifferentialDriveWheelSpeeds& other) const {
return {left + other.left, right + other.right};
}
/**
* Subtracts the other DifferentialDriveWheelSpeeds from the current
* DifferentialDriveWheelSpeeds and returns the difference.
*
* <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} -
* DifferentialDriveWheelSpeeds{1.0, 2.0} =
* DifferentialDriveWheelSpeeds{4.0, 2.0}
*
* @param other The DifferentialDriveWheelSpeeds to subtract.
*
* @return The difference between the two DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator-(
const DifferentialDriveWheelSpeeds& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current DifferentialDriveWheelSpeeds.
* This is equivalent to negating all components of the
* DifferentialDriveWheelSpeeds.
*
* @return The inverse of the current DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator-() const {
return {-left, -right};
}
/**
* Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
* DifferentialDriveWheelSpeeds.
*
* <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2
* = DifferentialDriveWheelSpeeds{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
return {scalar * left, scalar * right};
}
/**
* Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
* DifferentialDriveWheelSpeeds.
*
* <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2
* = DifferentialDriveWheelSpeeds{1.0, 1.25}
*
* @param scalar The scalar to divide by.
*
* @return The scaled DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp"

124
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp Normal file
View File

@@ -0,0 +1,124 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel velocities for a differential drive drivetrain.
*/
struct WPILIB_DLLEXPORT DifferentialDriveWheelVelocities {
/**
* Velocity of the left side of the robot.
*/
wpi::units::meters_per_second_t left = 0_mps;
/**
* Velocity of the right side of the robot.
*/
wpi::units::meters_per_second_t right = 0_mps;
/**
* Renormalizes the wheel velocities if either side is above the specified
* maximum.
*
* Sometimes, after inverse kinematics, the requested velocity from one or
* more wheels may be above the max attainable velocity for the driving motor
* on that wheel. To fix this issue, one can reduce all the wheel velocities
* to make sure that all requested module velocities are at-or-below the
* absolute threshold, while maintaining the ratio of velocities between
* wheels.
*
* @param attainableMaxVelocity The absolute max velocity that a wheel can
* reach.
*/
constexpr void Desaturate(
wpi::units::meters_per_second_t attainableMaxVelocity) {
auto realMaxVelocity = wpi::units::math::max(wpi::units::math::abs(left),
wpi::units::math::abs(right));
if (realMaxVelocity > attainableMaxVelocity) {
left = left / realMaxVelocity * attainableMaxVelocity;
right = right / realMaxVelocity * attainableMaxVelocity;
}
}
/**
* Adds two DifferentialDriveWheelVelocities and returns the sum.
*
* For example, DifferentialDriveWheelVelocities{1.0, 0.5} +
* DifferentialDriveWheelVelocities{2.0, 1.5} =
* DifferentialDriveWheelVelocities{3.0, 2.0}
*
* @param other The DifferentialDriveWheelVelocities to add.
* @return The sum of the DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator+(
const DifferentialDriveWheelVelocities& other) const {
return {left + other.left, right + other.right};
}
/**
* Subtracts the other DifferentialDriveWheelVelocities from the current
* DifferentialDriveWheelVelocities and returns the difference.
*
* For example, DifferentialDriveWheelVelocities{5.0, 4.0} -
* DifferentialDriveWheelVelocities{1.0, 2.0} =
* DifferentialDriveWheelVelocities{4.0, 2.0}
*
* @param other The DifferentialDriveWheelVelocities to subtract.
* @return The difference between the two DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator-(
const DifferentialDriveWheelVelocities& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current DifferentialDriveWheelVelocities.
* This is equivalent to negating all components of the
* DifferentialDriveWheelVelocities.
*
* @return The inverse of the current DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator-() const {
return {-left, -right};
}
/**
* Multiplies the DifferentialDriveWheelVelocities by a scalar and returns the
* new DifferentialDriveWheelVelocities.
*
* For example, DifferentialDriveWheelVelocities{2.0, 2.5} * 2
* = DifferentialDriveWheelVelocities{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator*(double scalar) const {
return {scalar * left, scalar * right};
}
/**
* Divides the DifferentialDriveWheelVelocities by a scalar and returns the
* new DifferentialDriveWheelVelocities.
*
* For example, DifferentialDriveWheelVelocities{2.0, 2.5} / 2
* = DifferentialDriveWheelVelocities{1.0, 1.25}
*
* @param scalar The scalar to divide by.
* @return The scaled DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.hpp"

38
wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp
View File

@@ -6,20 +6,20 @@
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds. Robot code should not use this directly-
* into individual wheel velocities. Robot code should not use this directly-
* Instead, use the particular type for your drivetrain (e.g.,
* DifferentialDriveKinematics).
*
* Inverse kinematics converts a desired chassis speed into wheel speeds whereas
* forward kinematics converts wheel speeds into chassis speed.
* Inverse kinematics converts a desired chassis velocity into wheel velocities
* whereas forward kinematics converts wheel velocities into chassis velocity.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
requires std::copy_constructible<WheelPositions> &&
std::assignable_from<WheelPositions&, const WheelPositions&>
@@ -28,27 +28,27 @@ class WPILIB_DLLEXPORT Kinematics {
virtual ~Kinematics() noexcept = default;
/**
* Performs forward kinematics to return the resulting chassis speed from the
* wheel speeds. This method is often used for odometry -- determining the
* robot's position on the field using data from the real-world speed of each
* wheel on the robot.
* Performs forward kinematics to return the resulting chassis velocity from
* the wheel velocities. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world velocity
* of each wheel on the robot.
*
* @param wheelSpeeds The speeds of the wheels.
* @return The chassis speed.
* @param wheelVelocities The velocities of the wheels.
* @return The chassis velocity.
*/
virtual ChassisSpeeds ToChassisSpeeds(
const WheelSpeeds& wheelSpeeds) const = 0;
virtual ChassisVelocities ToChassisVelocities(
const WheelVelocities& wheelVelocities) const = 0;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* Performs inverse kinematics to return the wheel velocities from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
* wheel velocities.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
* @param chassisVelocities The desired chassis velocity.
* @return The wheel velocities.
*/
virtual WheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const = 0;
virtual WheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const = 0;
/**
* Performs forward kinematics to return the resulting Twist2d from the given

88
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp
View File

@@ -9,11 +9,11 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -22,41 +22,42 @@ namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds.
* into individual wheel velocities.
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual wheel speeds) uses the relative locations of the wheels with
* individual wheel velocities) uses the relative locations of the wheels with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion maneuvers.
*
* Forward kinematics (converting an array of wheel speeds into the overall
* Forward kinematics (converting an array of wheel velocities into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
* multiply by [wheelSpeeds] to get our chassis speeds.
* The inverse kinematics: [wheelVelocities] = [wheelLocations] *
* [chassisVelocities] We take the Moore-Penrose pseudoinverse of
* [wheelLocations] and then multiply by [wheelVelocities] to get our chassis
* velocities.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
class WPILIB_DLLEXPORT MecanumDriveKinematics
: public Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheel The location of the front-left wheel relative to the
* physical center of the robot.
* physical center of the robot.
* @param frontRightWheel The location of the front-right wheel relative to
* the physical center of the robot.
* the physical center of the robot.
* @param rearLeftWheel The location of the rear-left wheel relative to the
* physical center of the robot.
* physical center of the robot.
* @param rearRightWheel The location of the rear-right wheel relative to the
* physical center of the robot.
* physical center of the robot.
*/
explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
Translation2d frontRightWheel,
@@ -75,9 +76,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* Performs inverse kinematics to return the wheel velocities from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
* wheel velocities.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
@@ -85,45 +86,44 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and
* provide a chassis speed that only has a dtheta
* component, the robot will rotate around that
* corner.
* center of rotation at one corner of the robot and provide a chassis
* velocity that only has a dtheta component, the robot will rotate around
* that corner.
*
* @return The wheel speeds. Use caution because they are not normalized.
* Sometimes, a user input may cause one of the wheel speeds to go
* above the attainable max velocity. Use the
* MecanumDriveWheelSpeeds::Normalize() function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the wheel speeds to variables:
* @return The wheel velocities. Use caution because they are not normalized.
* Sometimes, a user input may cause one of the wheel velocities to go
* above the attainable max velocity. Use the
* MecanumDriveWheelVelocities::Normalize() function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the wheel velocities to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
* auto [fl, fr, bl, br] = kinematics.ToWheelVelocities(chassisVelocities);
* @endcode
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
MecanumDriveWheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities,
const Translation2d& centerOfRotation) const;
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return ToWheelSpeeds(chassisSpeeds, {});
MecanumDriveWheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const override {
return ToWheelVelocities(chassisVelocities, {});
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given wheel speeds. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed of
* each wheel on the robot.
* given wheel velocities. This method is often used for odometry --
* determining the robot's position on the field using data from the
* real-world velocity of each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @param wheelVelocities The current mecanum drive wheel velocities.
*
* @return The resulting chassis speed.
* @return The resulting chassis velocity.
*/
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
ChassisVelocities ToChassisVelocities(
const MecanumDriveWheelVelocities& wheelVelocities) const override;
Twist2d ToTwist2d(const MecanumDriveWheelPositions& start,
const MecanumDriveWheelPositions& end) const override;
@@ -136,7 +136,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
*
* @param wheelDeltas The change in distance driven by each wheel.
*
* @return The resulting chassis speed.
* @return The resulting chassis velocity.
*/
Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
@@ -200,13 +200,13 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical
* center of the robot.
* center of the robot.
* @param fr The location of the front-right wheel relative to the physical
* center of the robot.
* center of the robot.
* @param rl The location of the rear-left wheel relative to the physical
* center of the robot.
* center of the robot.
* @param rr The location of the rear-right wheel relative to the physical
* center of the robot.
* center of the robot.
*/
void SetInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr) const;

4
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -23,7 +23,7 @@ namespace wpi::math {
* when using computer-vision systems.
*/
class WPILIB_DLLEXPORT MecanumDriveOdometry
: public Odometry<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public Odometry<MecanumDriveWheelPositions, MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry3d.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -23,7 +23,7 @@ namespace wpi::math {
* when using computer-vision systems.
*/
class WPILIB_DLLEXPORT MecanumDriveOdometry3d
: public Odometry3d<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public Odometry3d<MecanumDriveWheelPositions, MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

8
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
View File

@@ -35,7 +35,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
/**
* Adds two MecanumDriveWheelAccelerations and returns the sum.
*
* <p>For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} +
* For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelAccelerations{2.0, 1.5, 0.5, 1.0} =
* MecanumDriveWheelAccelerations{3.0, 2.0, 2.5, 2.5}
*
@@ -52,7 +52,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
* Subtracts the other MecanumDriveWheelAccelerations from the current
* MecanumDriveWheelAccelerations and returns the difference.
*
* <p>For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} -
* For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelAccelerations{1.0, 2.0, 3.0, 0.5} =
* MecanumDriveWheelAccelerations{4.0, 2.0, 3.0, 2.0}
*
@@ -79,7 +79,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
* Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the
* new MecanumDriveWheelAccelerations.
*
* <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 =
* For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
@@ -94,7 +94,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
* Divides the MecanumDriveWheelAccelerations by a scalar and returns the new
* MecanumDriveWheelAccelerations.
*
* <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 =
* For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelAccelerations{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.

149
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
View File

@@ -1,149 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <algorithm>
#include <array>
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
/**
* Speed of the front-left wheel.
*/
wpi::units::meters_per_second_t frontLeft = 0_mps;
/**
* Speed of the front-right wheel.
*/
wpi::units::meters_per_second_t frontRight = 0_mps;
/**
* Speed of the rear-left wheel.
*/
wpi::units::meters_per_second_t rearLeft = 0_mps;
/**
* Speed of the rear-right wheel.
*/
wpi::units::meters_per_second_t rearRight = 0_mps;
/**
* Renormalizes the wheel speeds if any individual speed is above the
* specified maximum.
*
* Sometimes, after inverse kinematics, the requested speed from one or
* more wheels may be above the max attainable speed for the driving motor on
* that wheel. To fix this issue, one can reduce all the wheel speeds to make
* sure that all requested module speeds are at-or-below the absolute
* threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds Desaturate(
wpi::units::meters_per_second_t attainableMaxSpeed) const {
std::array<wpi::units::meters_per_second_t, 4> wheelSpeeds{
frontLeft, frontRight, rearLeft, rearRight};
wpi::units::meters_per_second_t realMaxSpeed =
wpi::units::math::abs(*std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a) < wpi::units::math::abs(b);
}));
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
return MecanumDriveWheelSpeeds{wheelSpeeds[0], wheelSpeeds[1],
wheelSpeeds[2], wheelSpeeds[3]};
}
return *this;
}
/**
* Adds two MecanumDriveWheelSpeeds and returns the sum.
*
* <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelSpeeds{2.0, 1.5, 0.5, 1.0} =
* MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
*
* @param other The MecanumDriveWheelSpeeds to add.
* @return The sum of the MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator+(
const MecanumDriveWheelSpeeds& other) const {
return {frontLeft + other.frontLeft, frontRight + other.frontRight,
rearLeft + other.rearLeft, rearRight + other.rearRight};
}
/**
* Subtracts the other MecanumDriveWheelSpeeds from the current
* MecanumDriveWheelSpeeds and returns the difference.
*
* <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelSpeeds{1.0, 2.0, 3.0, 0.5} =
* MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
*
* @param other The MecanumDriveWheelSpeeds to subtract.
* @return The difference between the two MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator-(
const MecanumDriveWheelSpeeds& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current MecanumDriveWheelSpeeds.
* This is equivalent to negating all components of the
* MecanumDriveWheelSpeeds.
*
* @return The inverse of the current MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator-() const {
return {-frontLeft, -frontRight, -rearLeft, -rearRight};
}
/**
* Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new
* MecanumDriveWheelSpeeds.
*
* <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelSpeeds{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator*(double scalar) const {
return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
scalar * rearRight};
}
/**
* Divides the MecanumDriveWheelSpeeds by a scalar and returns the new
* MecanumDriveWheelSpeeds.
*
* <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelSpeeds{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp"

153
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp Normal file
View File

@@ -0,0 +1,153 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <algorithm>
#include <array>
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel velocities for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelVelocities {
/**
* Velocity of the front-left wheel.
*/
wpi::units::meters_per_second_t frontLeft = 0_mps;
/**
* Velocity of the front-right wheel.
*/
wpi::units::meters_per_second_t frontRight = 0_mps;
/**
* Velocity of the rear-left wheel.
*/
wpi::units::meters_per_second_t rearLeft = 0_mps;
/**
* Velocity of the rear-right wheel.
*/
wpi::units::meters_per_second_t rearRight = 0_mps;
/**
* Renormalizes the wheel velocities if any individual velocity is above the
* specified maximum.
*
* Sometimes, after inverse kinematics, the requested velocity from one or
* more wheels may be above the max attainable velocity for the driving motor
* on that wheel. To fix this issue, one can reduce all the wheel velocities
* to make sure that all requested module velocities are at-or-below the
* absolute threshold, while maintaining the ratio of velocities between
* wheels.
*
* @param attainableMaxVelocity The absolute max velocity that a wheel can
* reach.
* @return Desaturated MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities Desaturate(
wpi::units::meters_per_second_t attainableMaxVelocity) const {
std::array<wpi::units::meters_per_second_t, 4> wheelVelocities{
frontLeft, frontRight, rearLeft, rearRight};
wpi::units::meters_per_second_t realMaxVelocity =
wpi::units::math::abs(*std::max_element(
wheelVelocities.begin(), wheelVelocities.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a) < wpi::units::math::abs(b);
}));
if (realMaxVelocity > attainableMaxVelocity) {
for (int i = 0; i < 4; ++i) {
wheelVelocities[i] =
wheelVelocities[i] / realMaxVelocity * attainableMaxVelocity;
}
return MecanumDriveWheelVelocities{wheelVelocities[0], wheelVelocities[1],
wheelVelocities[2],
wheelVelocities[3]};
}
return *this;
}
/**
* Adds two MecanumDriveWheelVelocities and returns the sum.
*
* For example, MecanumDriveWheelVelocities{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelVelocities{2.0, 1.5, 0.5, 1.0} =
* MecanumDriveWheelVelocities{3.0, 2.0, 2.5, 2.5}
*
* @param other The MecanumDriveWheelVelocities to add.
* @return The sum of the MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator+(
const MecanumDriveWheelVelocities& other) const {
return {frontLeft + other.frontLeft, frontRight + other.frontRight,
rearLeft + other.rearLeft, rearRight + other.rearRight};
}
/**
* Subtracts the other MecanumDriveWheelVelocities from the current
* MecanumDriveWheelVelocities and returns the difference.
*
* For example, MecanumDriveWheelVelocities{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelVelocities{1.0, 2.0, 3.0, 0.5} =
* MecanumDriveWheelVelocities{4.0, 2.0, 3.0, 2.0}
*
* @param other The MecanumDriveWheelVelocities to subtract.
* @return The difference between the two MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator-(
const MecanumDriveWheelVelocities& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current MecanumDriveWheelVelocities.
* This is equivalent to negating all components of the
* MecanumDriveWheelVelocities.
*
* @return The inverse of the current MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator-() const {
return {-frontLeft, -frontRight, -rearLeft, -rearRight};
}
/**
* Multiplies the MecanumDriveWheelVelocities by a scalar and returns the new
* MecanumDriveWheelVelocities.
*
* For example, MecanumDriveWheelVelocities{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelVelocities{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator*(double scalar) const {
return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
scalar * rearRight};
}
/**
* Divides the MecanumDriveWheelVelocities by a scalar and returns the new
* MecanumDriveWheelVelocities.
*
* For example, MecanumDriveWheelVelocities{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelVelocities{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.hpp"
#include "wpi/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.hpp"

8
wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp
View File

@@ -23,10 +23,10 @@ namespace wpi::math {
* when using computer-vision systems.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT Odometry {
public:
@@ -38,7 +38,7 @@ class WPILIB_DLLEXPORT Odometry {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit Odometry(const Kinematics<WheelPositions, WheelSpeeds,
explicit Odometry(const Kinematics<WheelPositions, WheelVelocities,
WheelAccelerations>& kinematics,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions,
@@ -135,7 +135,7 @@ class WPILIB_DLLEXPORT Odometry {
}
private:
const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
const Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
m_kinematics;
Pose2d m_pose;

8
wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp
View File

@@ -23,10 +23,10 @@ namespace wpi::math {
* when using computer-vision systems.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT Odometry3d {
public:
@@ -38,7 +38,7 @@ class WPILIB_DLLEXPORT Odometry3d {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit Odometry3d(const Kinematics<WheelPositions, WheelSpeeds,
explicit Odometry3d(const Kinematics<WheelPositions, WheelVelocities,
WheelAccelerations>& kinematics,
const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions,
@@ -147,7 +147,7 @@ class WPILIB_DLLEXPORT Odometry3d {
}
private:
const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
const Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
m_kinematics;
Pose3d m_pose;

327
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
View File

@@ -14,11 +14,11 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/units/math.hpp"
@@ -30,7 +30,7 @@ namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
* into individual module states (velocity and angle).
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual module states) uses the relative locations of the modules with
@@ -43,9 +43,10 @@ namespace wpi::math {
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
* multiply by [moduleStates] to get our chassis speeds.
* The inverse kinematics: [moduleVelocities] = [moduleLocations] *
* [chassisVelocities] We take the Moore-Penrose pseudoinverse of
* [moduleLocations] and then multiply by [moduleVelocities] to get our chassis
* velocities.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
@@ -54,7 +55,7 @@ template <size_t NumModules>
class SwerveDriveKinematics
: public Kinematics<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**
@@ -66,7 +67,7 @@ class SwerveDriveKinematics
* kinematics methods.
*
* @param moduleTranslations The locations of the modules relative to the
* physical center of the robot.
* physical center of the robot.
*/
template <std::convertible_to<Translation2d>... ModuleTranslations>
requires(sizeof...(ModuleTranslations) == NumModules)
@@ -120,8 +121,9 @@ class SwerveDriveKinematics
/**
* Reset the internal swerve module headings.
*
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
* headings should be same as passed into the constructor of this class.
*/
template <std::convertible_to<Rotation2d>... ModuleHeadings>
requires(sizeof...(ModuleHeadings) == NumModules)
@@ -132,8 +134,9 @@ class SwerveDriveKinematics
/**
* Reset the internal swerve module headings.
*
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
* headings should be same as passed into the constructor of this class.
*/
void ResetHeadings(wpi::util::array<Rotation2d, NumModules> moduleHeadings) {
for (size_t i = 0; i < NumModules; i++) {
@@ -144,7 +147,7 @@ class SwerveDriveKinematics
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
* module velocities and angles.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
@@ -152,39 +155,41 @@ class SwerveDriveKinematics
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
*
* In the case that the desired chassis speeds are zero (i.e. the robot will
* be stationary), the previously calculated module angle will be maintained.
* In the case that the desired chassis velocities are zero (i.e. the robot
* will be stationary), the previously calculated module angle will be
* maintained.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis speed
* that only has a dtheta component, the robot will rotate around that corner.
*
* center of rotation at one corner of the robot and provide a chassis
* velocity that only has a dtheta component, the robot will rotate around
* that corner.
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* DesaturateWheelSpeeds(wpi::util::array<SwerveModuleState, NumModules>*,
* wpi::units::meters_per_second_t) function to rectify this issue. In
* addition, you can leverage the power of C++17 to directly assign the module
* states to variables:
* module states are not normalized. Sometimes, a user input may cause one
* of the module velocities to go above the attainable max velocity. Use
* the DesaturateWheelVelocities(wpi::util::array<SwerveModuleVelocity,
* NumModules>*, wpi::units::meters_per_second_t) function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the module states to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
* auto [fl, fr, bl, br] =
* kinematics.ToSwerveModuleVelocities(chassisVelocities);
* @endcode
*/
wpi::util::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
wpi::util::array<SwerveModuleVelocity, NumModules> ToSwerveModuleVelocities(
const ChassisVelocities& chassisVelocities,
const Translation2d& centerOfRotation = Translation2d{}) const {
wpi::util::array<SwerveModuleState, NumModules> moduleStates(
wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities(
wpi::util::empty_array);
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
if (chassisVelocities.vx == 0_mps && chassisVelocities.vy == 0_mps &&
chassisVelocities.omega == 0_rad_per_s) {
for (size_t i = 0; i < NumModules; i++) {
moduleStates[i] = {0_mps, m_moduleHeadings[i]};
moduleVelocities[i] = {0_mps, m_moduleHeadings[i]};
}
return moduleStates;
return moduleVelocities;
}
// We have a new center of rotation. We need to compute the matrix again.
@@ -192,83 +197,85 @@ class SwerveDriveKinematics
SetInverseKinematics(centerOfRotation);
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Vector3d chassisVelocitiesVector{chassisVelocities.vx.value(),
chassisVelocities.vy.value(),
chassisVelocities.omega.value()};
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_firstOrderInverseKinematics * chassisSpeedsVector;
Matrixd<NumModules * 2, 1> moduleVelocityMatrix =
m_firstOrderInverseKinematics * chassisVelocitiesVector;
for (size_t i = 0; i < NumModules; i++) {
wpi::units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
wpi::units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
wpi::units::meters_per_second_t x{moduleVelocityMatrix(i * 2, 0)};
wpi::units::meters_per_second_t y{moduleVelocityMatrix(i * 2 + 1, 0)};
auto speed = wpi::units::math::hypot(x, y);
auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
auto velocity = wpi::units::math::hypot(x, y);
auto rotation = velocity > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
moduleStates[i] = {speed, rotation};
moduleVelocities[i] = {velocity, rotation};
m_moduleHeadings[i] = rotation;
}
return moduleStates;
return moduleVelocities;
}
wpi::util::array<SwerveModuleState, NumModules> ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return ToSwerveModuleStates(chassisSpeeds);
wpi::util::array<SwerveModuleVelocity, NumModules> ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const override {
return ToSwerveModuleVelocities(chassisVelocities);
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
* the robot's position on the field using data from the real-world velocity
* and angle of each module on the robot.
*
* @param moduleStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
*
* @return The resulting chassis speed.
* @param moduleVelocities The state of the modules (as a SwerveModuleVelocity
* type) as measured from respective encoders and gyros. The order of the
* swerve module states should be same as passed into the constructor of
* this class.
* @return The resulting chassis velocity.
*/
template <std::convertible_to<SwerveModuleState>... ModuleStates>
requires(sizeof...(ModuleStates) == NumModules)
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
return this->ToChassisSpeeds(
wpi::util::array<SwerveModuleState, NumModules>{moduleStates...});
template <std::convertible_to<SwerveModuleVelocity>... ModuleVelocities>
requires(sizeof...(ModuleVelocities) == NumModules)
ChassisVelocities ToChassisVelocities(
ModuleVelocities&&... moduleVelocities) const {
return this->ToChassisVelocities(
wpi::util::array<SwerveModuleVelocity, NumModules>{
moduleVelocities...});
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
* the robot's position on the field using data from the real-world velocity
* and angle of each module on the robot.
*
* @param moduleStates The state of the modules as an wpi::util::array of type
* SwerveModuleState, NumModules long as measured from respective encoders
* and gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
*
* @return The resulting chassis speed.
* @param moduleVelocities The state of the modules as an wpi::util::array of
* type SwerveModuleVelocity, NumModules long as measured from respective
* encoders and gyros. The order of the swerve module states should be
* same as passed into the constructor of this class.
* @return The resulting chassis velocity.
*/
ChassisSpeeds ToChassisSpeeds(
const wpi::util::array<SwerveModuleState, NumModules>& moduleStates)
const override {
Matrixd<NumModules * 2, 1> moduleStateMatrix;
ChassisVelocities ToChassisVelocities(
const wpi::util::array<SwerveModuleVelocity, NumModules>&
moduleVelocities) const override {
Matrixd<NumModules * 2, 1> moduleVelocityMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) =
module.speed.value() * module.angle.Sin();
SwerveModuleVelocity module = moduleVelocities[i];
moduleVelocityMatrix(i * 2, 0) =
module.velocity.value() * module.angle.Cos();
moduleVelocityMatrix(i * 2 + 1, 0) =
module.velocity.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_firstOrderForwardKinematics.solve(moduleStateMatrix);
Eigen::Vector3d chassisVelocitiesVector =
m_firstOrderForwardKinematics.solve(moduleVelocityMatrix);
return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)},
wpi::units::meters_per_second_t{chassisSpeedsVector(1)},
wpi::units::radians_per_second_t{chassisSpeedsVector(2)}};
return {wpi::units::meters_per_second_t{chassisVelocitiesVector(0)},
wpi::units::meters_per_second_t{chassisVelocitiesVector(1)},
wpi::units::radians_per_second_t{chassisVelocitiesVector(2)}};
}
/**
@@ -278,10 +285,9 @@ class SwerveDriveKinematics
* real-world position delta and angle of each module on the robot.
*
* @param moduleDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* SwerveModulePosition type) as measured from respective encoders and
* gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
* @return The resulting Twist2d.
*/
template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
@@ -298,10 +304,9 @@ class SwerveDriveKinematics
* real-world position delta and angle of each module on the robot.
*
* @param moduleDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* SwerveModulePosition type) as measured from respective encoders and
* gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
* @return The resulting Twist2d.
*/
Twist2d ToTwist2d(
@@ -337,105 +342,109 @@ class SwerveDriveKinematics
}
/**
* Renormalizes the wheel speeds if any individual speed is above the
* Renormalizes the wheel velocities if any individual velocity is above the
* specified maximum.
*
* Sometimes, after inverse kinematics, the requested speed
* from one or more modules may be above the max attainable speed for the
* Sometimes, after inverse kinematics, the requested velocity
* from one or more modules may be above the max attainable velocity for the
* driving motor on that module. To fix this issue, one can reduce all the
* wheel speeds to make sure that all requested module speeds are at-or-below
* the absolute threshold, while maintaining the ratio of speeds between
* modules.
* wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of
* velocities between modules.
*
* Scaling down the module speeds rotates the direction of net motion in the
* opposite direction of rotational velocity, which makes discretizing the
* chassis speeds inaccurate because the discretization did not account for
* this translational skew.
* Scaling down the module velocities rotates the direction of net motion in
* the opposite direction of rotational velocity, which makes discretizing the
* chassis velocities inaccurate because the discretization did not account
* for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param attainableMaxSpeed The absolute max speed that a module can reach.
* @param moduleVelocities Reference to array of module states. The array will
* be mutated with the normalized velocities!
* @param attainableMaxVelocity The absolute max velocity that a module can
* reach.
*/
static void DesaturateWheelSpeeds(
wpi::util::array<SwerveModuleState, NumModules>* moduleStates,
wpi::units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = wpi::units::math::abs(
static void DesaturateWheelVelocities(
wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
wpi::units::meters_per_second_t attainableMaxVelocity) {
auto& states = *moduleVelocities;
auto realMaxVelocity = wpi::units::math::abs(
std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
return wpi::units::math::abs(a.velocity) <
wpi::units::math::abs(b.velocity);
})
->speed);
->velocity);
if (realMaxSpeed > attainableMaxSpeed) {
if (realMaxVelocity > attainableMaxVelocity) {
for (auto& module : states) {
module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
module.velocity =
module.velocity / realMaxVelocity * attainableMaxVelocity;
}
}
}
/**
* Renormalizes the wheel speeds if any individual speed is above the
* Renormalizes the wheel velocities if any individual velocity is above the
* specified maximum, as well as getting rid of joystick saturation at edges
* of joystick.
*
* Sometimes, after inverse kinematics, the requested speed
* from one or more modules may be above the max attainable speed for the
* Sometimes, after inverse kinematics, the requested velocity
* from one or more modules may be above the max attainable velocity for the
* driving motor on that module. To fix this issue, one can reduce all the
* wheel speeds to make sure that all requested module speeds are at-or-below
* the absolute threshold, while maintaining the ratio of speeds between
* modules.
* wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of
* velocities between modules.
*
* Scaling down the module speeds rotates the direction of net motion in the
* opposite direction of rotational velocity, which makes discretizing the
* chassis speeds inaccurate because the discretization did not account for
* this translational skew.
* Scaling down the module velocities rotates the direction of net motion in
* the opposite direction of rotational velocity, which makes discretizing the
* chassis velocities inaccurate because the discretization did not account
* for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeed The absolute max speed a module can reach
* @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
* can reach while translating
* @param attainableMaxRobotRotationSpeed The absolute max speed the robot can
* reach while rotating
* @param moduleVelocities Reference to array of module states. The array will
* be mutated with the normalized velocities!
* @param desiredChassisVelocity The desired velocity of the robot
* @param attainableMaxModuleVelocity The absolute max velocity a module can
* reach
* @param attainableMaxRobotTranslationVelocity The absolute max velocity the
* robot can reach while translating
* @param attainableMaxRobotRotationVelocity The absolute max velocity the
* robot can reach while rotating
*/
static void DesaturateWheelSpeeds(
wpi::util::array<SwerveModuleState, NumModules>* moduleStates,
ChassisSpeeds desiredChassisSpeed,
wpi::units::meters_per_second_t attainableMaxModuleSpeed,
wpi::units::meters_per_second_t attainableMaxRobotTranslationSpeed,
wpi::units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
static void DesaturateWheelVelocities(
wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
ChassisVelocities desiredChassisVelocity,
wpi::units::meters_per_second_t attainableMaxModuleVelocity,
wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
auto& states = *moduleVelocities;
auto realMaxSpeed = wpi::units::math::abs(
auto realMaxVelocity = wpi::units::math::abs(
std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
return wpi::units::math::abs(a.velocity) <
wpi::units::math::abs(b.velocity);
})
->speed);
->velocity);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s ||
realMaxSpeed == 0_mps) {
if (attainableMaxRobotTranslationVelocity == 0_mps ||
attainableMaxRobotRotationVelocity == 0_rad_per_s ||
realMaxVelocity == 0_mps) {
return;
}
auto translationalK = wpi::units::math::hypot(desiredChassisSpeed.vx,
desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
auto translationalK = wpi::units::math::hypot(desiredChassisVelocity.vx,
desiredChassisVelocity.vy) /
attainableMaxRobotTranslationVelocity;
auto rotationalK = wpi::units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed;
auto rotationalK = wpi::units::math::abs(desiredChassisVelocity.omega) /
attainableMaxRobotRotationVelocity;
auto k = wpi::units::math::max(translationalK, rotationalK);
auto scale = wpi::units::math::min(
k * attainableMaxModuleSpeed / realMaxSpeed, wpi::units::scalar_t{1});
auto scale =
wpi::units::math::min(k * attainableMaxModuleVelocity / realMaxVelocity,
wpi::units::scalar_t{1});
for (auto& module : states) {
module.speed = module.speed * scale;
module.velocity = module.velocity * scale;
}
}
@@ -461,7 +470,7 @@ class SwerveDriveKinematics
* calculations -- converting desired robot accelerations into individual
* module accelerations.
*
* <p>This function also supports variable centers of rotation. During normal
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
@@ -470,9 +479,9 @@ class SwerveDriveKinematics
* @param chassisAccelerations The desired chassis accelerations.
* @param angularVelocity The desired robot angular velocity.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis
* acceleration that only has a dtheta component, the robot will rotate around
* that corner.
* center of rotation at one corner of the robot and provide a chassis
* acceleration that only has a dtheta component, the robot will rotate
* around that corner.
* @return An array containing the module accelerations.
*/
wpi::util::array<SwerveModuleAcceleration, NumModules>
@@ -551,8 +560,9 @@ class SwerveDriveKinematics
* data from the real-world acceleration of each module on the robot.
*
* @param moduleAccelerations The accelerations of the modules as measured
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this class.
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this
* class.
* @return The resulting chassis accelerations.
*/
template <
@@ -572,8 +582,9 @@ class SwerveDriveKinematics
* data from the real-world acceleration of each module on the robot.
*
* @param moduleAccelerations The accelerations of the modules as measured
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this class.
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this
* class.
* @return The resulting chassis accelerations.
*/
ChassisAccelerations ToChassisAccelerations(

4
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/Odometry.hpp"
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -28,7 +28,7 @@ namespace wpi::math {
template <size_t NumModules>
class SwerveDriveOdometry
: public Odometry<wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/Odometry3d.hpp"
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
@@ -28,7 +28,7 @@ template <size_t NumModules>
class SwerveDriveOdometry3d
: public Odometry3d<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

42
wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleState.hpp → wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
View File

@@ -12,13 +12,13 @@
namespace wpi::math {
/**
* Represents the state of one swerve module.
* Represents the velocity of one swerve module.
*/
struct WPILIB_DLLEXPORT SwerveModuleState {
struct WPILIB_DLLEXPORT SwerveModuleVelocity {
/**
* Speed of the wheel of the module.
* Velocity of the wheel of the module.
*/
wpi::units::meters_per_second_t speed = 0_mps;
wpi::units::meters_per_second_t velocity = 0_mps;
/**
* Angle of the module.
@@ -26,18 +26,18 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
Rotation2d angle;
/**
* Checks equality between this SwerveModuleState and another object.
* Checks equality between this SwerveModuleVelocity and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const SwerveModuleState& other) const {
return wpi::units::math::abs(speed - other.speed) < 1E-9_mps &&
constexpr bool operator==(const SwerveModuleVelocity& other) const {
return wpi::units::math::abs(velocity - other.velocity) < 1E-9_mps &&
angle == other.angle;
}
/**
* Minimize the change in the heading this swerve module state would
* Minimize the change in the heading this swerve module velocity would
* require by potentially reversing the direction the wheel spins. If this is
* used with the PIDController class's continuous input functionality, the
* furthest a wheel will ever rotate is 90 degrees.
@@ -47,43 +47,45 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
constexpr void Optimize(const Rotation2d& currentAngle) {
auto delta = angle - currentAngle;
if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
speed *= -1;
velocity *= -1;
angle = angle + Rotation2d{180_deg};
}
}
/**
* Minimize the change in heading the desired swerve module state would
* Minimize the change in heading the desired swerve module velocity would
* require by potentially reversing the direction the wheel spins. If this is
* used with the PIDController class's continuous input functionality, the
* furthest a wheel will ever rotate is 90 degrees.
*
* @param desiredState The desired state.
* @param desiredVelocity The desired velocity.
* @param currentAngle The current module angle.
*/
[[deprecated("Use instance method instead.")]]
constexpr static SwerveModuleState Optimize(
const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
auto delta = desiredState.angle - currentAngle;
constexpr static SwerveModuleVelocity Optimize(
const SwerveModuleVelocity& desiredVelocity,
const Rotation2d& currentAngle) {
auto delta = desiredVelocity.angle - currentAngle;
if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
return {-desiredVelocity.velocity,
desiredVelocity.angle + Rotation2d{180_deg}};
} else {
return {desiredState.speed, desiredState.angle};
return {desiredVelocity.velocity, desiredVelocity.angle};
}
}
/**
* Scales speed by cosine of angle error. This scales down movement
* Scales velocity by cosine of angle error. This scales down movement
* perpendicular to the desired direction of travel that can occur when
* modules change directions. This results in smoother driving.
*
* @param currentAngle The current module angle.
*/
constexpr void CosineScale(const Rotation2d& currentAngle) {
speed *= (angle - currentAngle).Cos();
velocity *= (angle - currentAngle).Cos();
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/SwerveModuleStateProto.hpp"
#include "wpi/math/kinematics/struct/SwerveModuleStateStruct.hpp"
#include "wpi/math/kinematics/proto/SwerveModuleVelocityProto.hpp"
#include "wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp"

19
wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisSpeedsProto.hpp
View File

@@ -1,19 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ChassisSpeeds> {
using MessageStruct = wpi_proto_ProtobufChassisSpeeds;
using InputStream = wpi::util::ProtoInputStream<wpi::math::ChassisSpeeds>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ChassisSpeeds>;
static std::optional<wpi::math::ChassisSpeeds> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ChassisSpeeds& value);
};

22
wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisVelocitiesProto.hpp Normal file
View File

@@ -0,0 +1,22 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ChassisVelocities> {
using MessageStruct = wpi_proto_ProtobufChassisVelocities;
using InputStream = wpi::util::ProtoInputStream<wpi::math::ChassisVelocities>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::ChassisVelocities>;
static std::optional<wpi::math::ChassisVelocities> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::ChassisVelocities& value);
};

14
wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp → wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.hpp
View File

@@ -4,21 +4,21 @@
#pragma once
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelSpeeds;
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVelocities> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelVelocities;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelSpeeds>;
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelVelocities>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveWheelSpeeds>;
static std::optional<wpi::math::DifferentialDriveWheelSpeeds> Unpack(
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveWheelVelocities>;
static std::optional<wpi::math::DifferentialDriveWheelVelocities> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::DifferentialDriveWheelSpeeds& value);
const wpi::math::DifferentialDriveWheelVelocities& value);
};

24
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelSpeeds;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelSpeeds>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelSpeeds>;
static std::optional<wpi::math::MecanumDriveWheelSpeeds> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::MecanumDriveWheelSpeeds& value);
};

24
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.hpp Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::MecanumDriveWheelVelocities> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelVelocities;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelVelocities>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelVelocities>;
static std::optional<wpi::math::MecanumDriveWheelVelocities> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::MecanumDriveWheelVelocities& value);
};

15
wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp → wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleVelocityProto.hpp
View File

@@ -4,19 +4,20 @@
#pragma once
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::SwerveModuleState> {
using MessageStruct = wpi_proto_ProtobufSwerveModuleState;
using InputStream = wpi::util::ProtoInputStream<wpi::math::SwerveModuleState>;
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::SwerveModuleVelocity> {
using MessageStruct = wpi_proto_ProtobufSwerveModuleVelocity;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::SwerveModuleVelocity>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::SwerveModuleState>;
static std::optional<wpi::math::SwerveModuleState> Unpack(
wpi::util::ProtoOutputStream<wpi::math::SwerveModuleVelocity>;
static std::optional<wpi::math::SwerveModuleVelocity> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::SwerveModuleState& value);
const wpi::math::SwerveModuleVelocity& value);
};

14
wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp
View File

@@ -4,21 +4,23 @@
#pragma once
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ChassisSpeeds> {
static constexpr std::string_view GetTypeName() { return "ChassisSpeeds"; }
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ChassisVelocities> {
static constexpr std::string_view GetTypeName() {
return "ChassisVelocities";
}
static constexpr size_t GetSize() { return 24; }
static constexpr std::string_view GetSchema() {
return "double vx;double vy;double omega";
}
static wpi::math::ChassisSpeeds Unpack(std::span<const uint8_t> data);
static wpi::math::ChassisVelocities Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::ChassisSpeeds& value);
const wpi::math::ChassisVelocities& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::ChassisSpeeds>);
static_assert(wpi::util::StructSerializable<wpi::math::ChassisVelocities>);

12
wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.hpp
View File

@@ -4,26 +4,26 @@
#pragma once
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds> {
wpi::util::Struct<wpi::math::DifferentialDriveWheelVelocities> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelSpeeds";
return "DifferentialDriveWheelVelocities";
}
static constexpr size_t GetSize() { return 16; }
static constexpr std::string_view GetSchema() {
return "double left;double right";
}
static wpi::math::DifferentialDriveWheelSpeeds Unpack(
static wpi::math::DifferentialDriveWheelVelocities Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::DifferentialDriveWheelSpeeds& value);
const wpi::math::DifferentialDriveWheelVelocities& value);
};
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelSpeeds>);
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVelocities>);

13
wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.hpp
View File

@@ -4,14 +4,15 @@
#pragma once
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::MecanumDriveWheelVelocities> {
static constexpr std::string_view GetTypeName() {
return "MecanumDriveWheelSpeeds";
return "MecanumDriveWheelVelocities";
}
static constexpr size_t GetSize() { return 32; }
static constexpr std::string_view GetSchema() {
@@ -19,11 +20,11 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds> {
"double rear_right";
}
static wpi::math::MecanumDriveWheelSpeeds Unpack(
static wpi::math::MecanumDriveWheelVelocities Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::MecanumDriveWheelSpeeds& value);
const wpi::math::MecanumDriveWheelVelocities& value);
};
static_assert(
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelSpeeds>);
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelVelocities>);

16
wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleStateStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp
View File

@@ -4,30 +4,30 @@
#pragma once
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::SwerveModuleState> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::SwerveModuleVelocity> {
static constexpr std::string_view GetTypeName() {
return "SwerveModuleState";
return "SwerveModuleVelocity";
}
static constexpr size_t GetSize() {
return 8 + wpi::util::GetStructSize<wpi::math::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "double speed;Rotation2d angle";
return "double velocity;Rotation2d angle";
}
static wpi::math::SwerveModuleState Unpack(std::span<const uint8_t> data);
static wpi::math::SwerveModuleVelocity Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::SwerveModuleState& value);
const wpi::math::SwerveModuleVelocity& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::util::ForEachStructSchema<wpi::math::Rotation2d>(fn);
}
};
static_assert(wpi::util::StructSerializable<wpi::math::SwerveModuleState>);
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveModuleState>);
static_assert(wpi::util::StructSerializable<wpi::math::SwerveModuleVelocity>);
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveModuleVelocity>);

40
wpimath/src/main/native/include/wpi/math/system/DCMotor.hpp
View File

@@ -53,11 +53,11 @@ class WPILIB_DLLEXPORT DCMotor {
* Constructs a DC motor.
*
* @param nominalVoltage Voltage at which the motor constants were measured.
* @param stallTorque Torque when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
* @param stallTorque Torque when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
*/
constexpr DCMotor(wpi::units::volt_t nominalVoltage,
wpi::units::newton_meter_t stallTorque,
@@ -75,15 +75,15 @@ class WPILIB_DLLEXPORT DCMotor {
Kt(this->stallTorque / this->stallCurrent) {}
/**
* Returns current drawn by motor with given speed and input voltage.
* Returns current drawn by motor with given velocity and input voltage.
*
* @param speed The current angular velocity of the motor.
* @param velocity The current angular velocity of the motor.
* @param inputVoltage The voltage being applied to the motor.
*/
constexpr wpi::units::ampere_t Current(
wpi::units::radians_per_second_t speed,
wpi::units::radians_per_second_t velocity,
wpi::units::volt_t inputVoltage) const {
return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
return -1.0 / Kv / R * velocity + 1.0 / R * inputVoltage;
}
/**
@@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT DCMotor {
/**
* Returns torque produced by the motor with a given current.
*
* @param current The current drawn by the motor.
* @param current The current drawn by the motor.
*/
constexpr wpi::units::newton_meter_t Torque(
wpi::units::ampere_t current) const {
@@ -110,23 +110,23 @@ class WPILIB_DLLEXPORT DCMotor {
* Returns the voltage provided to the motor for a given torque and
* angular velocity.
*
* @param torque The torque produced by the motor.
* @param speed The current angular velocity of the motor.
* @param torque The torque produced by the motor.
* @param velocity The current angular velocity of the motor.
*/
constexpr wpi::units::volt_t Voltage(
wpi::units::newton_meter_t torque,
wpi::units::radians_per_second_t speed) const {
return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
wpi::units::radians_per_second_t velocity) const {
return 1.0 / Kv * velocity + 1.0 / Kt * R * torque;
}
/**
* Returns the angular speed produced by the motor at a given torque and input
* voltage.
* Returns the angular velocity produced by the motor at a given torque and
* input voltage.
*
* @param torque The torque produced by the motor.
* @param inputVoltage The input voltage provided to the motor.
* @param torque The torque produced by the motor.
* @param inputVoltage The input voltage provided to the motor.
*/
constexpr wpi::units::radians_per_second_t Speed(
constexpr wpi::units::radians_per_second_t Velocity(
wpi::units::newton_meter_t torque,
wpi::units::volt_t inputVoltage) const {
return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
@@ -135,7 +135,7 @@ class WPILIB_DLLEXPORT DCMotor {
/**
* Returns a copy of this motor with the given gearbox reduction applied.
*
* @param gearboxReduction The gearbox reduction.
* @param gearboxReduction The gearbox reduction.
*/
constexpr DCMotor WithReduction(double gearboxReduction) {
return DCMotor(nominalVoltage, stallTorque * gearboxReduction, stallCurrent,

2
wpimath/src/main/native/include/wpi/math/trajectory/Trajectory.hpp
View File

@@ -34,7 +34,7 @@ class WPILIB_DLLEXPORT Trajectory {
/// The time elapsed since the beginning of the trajectory.
wpi::units::second_t t = 0_s;
/// The speed at that point of the trajectory.
/// The velocity at that point of the trajectory.
wpi::units::meters_per_second_t velocity = 0_mps;
/// The acceleration at that point of the trajectory.

Some files were not shown because too many files have changed in this diff Show More