[wpimath] Replace Speeds with Velocities (#8479)

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

View File

@@ -2,7 +2,7 @@ extra_includes:
- wpystruct.h
classes:
wpi::math::ChassisSpeeds:
wpi::math::ChassisVelocities:
force_no_default_constructor: true
attributes:
vx:
@@ -13,11 +13,11 @@ classes:
Discretize:
overloads:
wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t, wpi::units::second_t:
const ChassisSpeeds&, wpi::units::second_t:
const ChassisVelocities&, wpi::units::second_t:
operator+:
operator-:
overloads:
const ChassisSpeeds& [const]:
const ChassisVelocities& [const]:
'[const]':
operator*:
operator/:
@@ -26,7 +26,7 @@ classes:
ToFieldRelative:
inline_code: |
cls_ChassisSpeeds
cls_ChassisVelocities
.def(
py::init<
wpi::units::meters_per_second_t, wpi::units::meters_per_second_t,
@@ -35,34 +35,34 @@ inline_code: |
py::arg("vx") = 0, py::arg("vy") = 0, py::arg("omega") = 0
)
.def_static("fromFeet", [](wpi::units::feet_per_second_t vx, wpi::units::feet_per_second_t vy, wpi::units::radians_per_second_t omega){
return ChassisSpeeds{vx, vy, omega};
return ChassisVelocities{vx, vy, omega};
}, py::arg("vx") = 0, py::arg("vy") = 0, py::arg("omega") = 0)
.def_property("vx_fps",
[](ChassisSpeeds * self) -> wpi::units::feet_per_second_t {
[](ChassisVelocities * self) -> wpi::units::feet_per_second_t {
return self->vx;
},
[](ChassisSpeeds * self, wpi::units::feet_per_second_t vx) {
[](ChassisVelocities * self, wpi::units::feet_per_second_t vx) {
self->vx = vx;
}
)
.def_property("vy_fps",
[](ChassisSpeeds * self) -> wpi::units::feet_per_second_t {
[](ChassisVelocities * self) -> wpi::units::feet_per_second_t {
return self->vy;
},
[](ChassisSpeeds * self, wpi::units::feet_per_second_t vy) {
[](ChassisVelocities * self, wpi::units::feet_per_second_t vy) {
self->vy = vy;
}
)
.def_property("omega_dps",
[](ChassisSpeeds * self) -> wpi::units::degrees_per_second_t {
[](ChassisVelocities * self) -> wpi::units::degrees_per_second_t {
return self->omega;
},
[](ChassisSpeeds * self, wpi::units::degrees_per_second_t omega) {
[](ChassisVelocities * self, wpi::units::degrees_per_second_t omega) {
self->omega = omega;
}
)
.def("__len__", [](const ChassisSpeeds &self) { return 3; })
.def("__getitem__", [](const ChassisSpeeds &self, int index) {
.def("__len__", [](const ChassisVelocities &self) { return 3; })
.def("__getitem__", [](const ChassisVelocities &self, int index) {
switch (index) {
case 0:
return self.vx();
@@ -71,14 +71,14 @@ inline_code: |
case 2:
return self.omega();
default:
throw std::out_of_range("ChassisSpeeds index out of range");
throw std::out_of_range("ChassisVelocities index out of range");
}
})
.def("__repr__", [](const ChassisSpeeds &cs) -> std::string {
return "ChassisSpeeds(vx=" + std::to_string(cs.vx()) + ", "
.def("__repr__", [](const ChassisVelocities &cs) -> std::string {
return "ChassisVelocities(vx=" + std::to_string(cs.vx()) + ", "
"vy=" + std::to_string(cs.vy()) + ", "
"omega=" + std::to_string(cs.omega()) + ")";
})
;
SetupWPyStruct<wpi::math::ChassisSpeeds>(cls_ChassisSpeeds);
SetupWPyStruct<wpi::math::ChassisVelocities>(cls_ChassisVelocities);

View File

@@ -20,7 +20,7 @@ classes:
wpi::units::newton_meter_t [const]:
Torque:
Voltage:
Speed:
Velocity:
WithReduction:
CIM:
MiniCIM:

View File

@@ -7,8 +7,8 @@ classes:
trackwidth:
methods:
DifferentialDriveKinematics:
ToChassisSpeeds:
ToWheelSpeeds:
ToChassisVelocities:
ToWheelVelocities:
ToTwist2d:
overloads:
const wpi::units::meter_t, const wpi::units::meter_t [const]:

View File

@@ -10,7 +10,7 @@ classes:
inline_code: |-
cls_DifferentialDriveKinematicsConstraint
.def_static("fromFps", [](const DifferentialDriveKinematics& kinematics,
wpi::units::feet_per_second_t maxSpeed) {
return std::make_shared<DifferentialDriveKinematicsConstraint>(kinematics, maxSpeed);
}, py::arg("kinematics"), py::arg("maxSpeed"))
wpi::units::feet_per_second_t maxVelocity) {
return std::make_shared<DifferentialDriveKinematicsConstraint>(kinematics, maxVelocity);
}, py::arg("kinematics"), py::arg("maxVelocity"))
;

View File

@@ -2,7 +2,7 @@ extra_includes:
- wpystruct.h
classes:
wpi::math::DifferentialDriveWheelSpeeds:
wpi::math::DifferentialDriveWheelVelocities:
force_no_default_constructor: true
attributes:
left:
@@ -12,42 +12,42 @@ classes:
operator+:
operator-:
overloads:
const DifferentialDriveWheelSpeeds& [const]:
const DifferentialDriveWheelVelocities& [const]:
'[const]':
operator*:
operator/:
inline_code: |
cls_DifferentialDriveWheelSpeeds
cls_DifferentialDriveWheelVelocities
.def(
py::init<wpi::units::meters_per_second_t, wpi::units::meters_per_second_t>(),
py::arg("left") = 0, py::arg("right") = 0
)
.def_static("fromFeet", [](wpi::units::feet_per_second_t left, wpi::units::feet_per_second_t right){
return DifferentialDriveWheelSpeeds{left, right};
return DifferentialDriveWheelVelocities{left, right};
}, py::arg("left"), py::arg("right"))
.def_property("left_fps",
[](DifferentialDriveWheelSpeeds * self) -> wpi::units::feet_per_second_t {
[](DifferentialDriveWheelVelocities * self) -> wpi::units::feet_per_second_t {
return self->left;
},
[](DifferentialDriveWheelSpeeds * self, wpi::units::feet_per_second_t left) {
[](DifferentialDriveWheelVelocities * self, wpi::units::feet_per_second_t left) {
self->left = left;
}
)
.def_property("right_fps",
[](DifferentialDriveWheelSpeeds * self) -> wpi::units::feet_per_second_t {
[](DifferentialDriveWheelVelocities * self) -> wpi::units::feet_per_second_t {
return self->right;
},
[](DifferentialDriveWheelSpeeds * self, wpi::units::feet_per_second_t right) {
[](DifferentialDriveWheelVelocities * self, wpi::units::feet_per_second_t right) {
self->right = right;
}
)
.def("__repr__", [](const DifferentialDriveWheelSpeeds &dds) -> std::string {
return "DifferentialDriveWheelSpeeds(left=" + std::to_string(dds.left()) + ", "
.def("__repr__", [](const DifferentialDriveWheelVelocities &dds) -> std::string {
return "DifferentialDriveWheelVelocities(left=" + std::to_string(dds.left()) + ", "
"right=" + std::to_string(dds.right()) + ")";
})
;
SetupWPyStruct<wpi::math::DifferentialDriveWheelSpeeds>(cls_DifferentialDriveWheelSpeeds);
SetupWPyStruct<wpi::math::DifferentialDriveWheelVelocities>(cls_DifferentialDriveWheelVelocities);

View File

@@ -1,10 +1,10 @@
extra_includes:
- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
- wpi/math/kinematics/DifferentialDriveWheelPositions.hpp
- wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
- wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
- wpi/math/kinematics/MecanumDriveWheelPositions.hpp
- wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
- wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
- wpi/math/kinematics/SwerveModuleAcceleration.hpp
- wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -15,11 +15,11 @@ classes:
- wpi::util::array
template_params:
- WheelPositions
- WheelSpeeds
- WheelVelocities
- WheelAccelerations
methods:
ToChassisSpeeds:
ToWheelSpeeds:
ToChassisVelocities:
ToWheelVelocities:
ToTwist2d:
Interpolate:
ToChassisAccelerations:
@@ -31,35 +31,35 @@ templates:
qualname: wpi::math::Kinematics
params:
- wpi::math::DifferentialDriveWheelPositions
- wpi::math::DifferentialDriveWheelSpeeds
- wpi::math::DifferentialDriveWheelVelocities
- wpi::math::DifferentialDriveWheelAccelerations
MecanumDriveKinematicsBase:
qualname: wpi::math::Kinematics
params:
- wpi::math::MecanumDriveWheelPositions
- wpi::math::MecanumDriveWheelSpeeds
- wpi::math::MecanumDriveWheelVelocities
- wpi::math::MecanumDriveWheelAccelerations
SwerveDrive2KinematicsBase:
qualname: wpi::math::Kinematics
params:
- wpi::util::array<wpi::math::SwerveModulePosition,2>
- wpi::util::array<wpi::math::SwerveModuleState,2>
- wpi::util::array<wpi::math::SwerveModuleVelocity,2>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
SwerveDrive3KinematicsBase:
qualname: wpi::math::Kinematics
params:
- wpi::util::array<wpi::math::SwerveModulePosition,3>
- wpi::util::array<wpi::math::SwerveModuleState,3>
- wpi::util::array<wpi::math::SwerveModuleVelocity,3>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
SwerveDrive4KinematicsBase:
qualname: wpi::math::Kinematics
params:
- wpi::util::array<wpi::math::SwerveModulePosition,4>
- wpi::util::array<wpi::math::SwerveModuleState,4>
- wpi::util::array<wpi::math::SwerveModuleVelocity,4>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
SwerveDrive6KinematicsBase:
qualname: wpi::math::Kinematics
params:
- wpi::util::array<wpi::math::SwerveModulePosition,6>
- wpi::util::array<wpi::math::SwerveModuleState,6>
- wpi::util::array<wpi::math::SwerveModuleVelocity,6>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>

View File

@@ -5,11 +5,11 @@ classes:
wpi::math::MecanumDriveKinematics:
methods:
MecanumDriveKinematics:
ToWheelSpeeds:
ToWheelVelocities:
doc: |
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
@@ -17,25 +17,25 @@ classes:
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
provide a chassis velocity that only has a dtheta
component, the robot will rotate around that
corner.
:returns: The wheel speeds. Use caution because they are not normalized.
Sometimes, a user input may cause one of the wheel speeds to go
:returns: 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
:meth:`MecanumDriveWheelSpeeds.normalize` method to rectify
:meth:`MecanumDriveWheelVelocities.normalize` method to rectify
this issue. In addition, you can use Python unpacking syntax
to directly assign the wheel speeds to variables::
to directly assign the wheel velocities to variables::
fl, fr, bl, br = kinematics.toWheelSpeeds(chassisSpeeds)
fl, fr, bl, br = kinematics.toWheelVelocities(chassisVelocities)
overloads:
const ChassisSpeeds&, const Translation2d& [const]:
const ChassisSpeeds& [const]:
ToChassisSpeeds:
const ChassisVelocities&, const Translation2d& [const]:
const ChassisVelocities& [const]:
ToChassisVelocities:
ToTwist2d:
overloads:
const MecanumDriveWheelPositions&, const MecanumDriveWheelPositions& [const]:

View File

@@ -10,7 +10,7 @@ classes:
inline_code: |-
cls_MecanumDriveKinematicsConstraint
.def_static("fromFps", [](const wpi::math::MecanumDriveKinematics& kinematics,
wpi::units::feet_per_second_t maxSpeed) {
return std::make_shared<wpi::math::MecanumDriveKinematicsConstraint>(kinematics, maxSpeed);
}, py::arg("kinematics"), py::arg("maxSpeed"))
wpi::units::feet_per_second_t maxVelocity) {
return std::make_shared<wpi::math::MecanumDriveKinematicsConstraint>(kinematics, maxVelocity);
}, py::arg("kinematics"), py::arg("maxVelocity"))
;

View File

@@ -2,7 +2,7 @@ extra_includes:
- wpystruct.h
classes:
wpi::math::MecanumDriveWheelSpeeds:
wpi::math::MecanumDriveWheelVelocities:
force_no_default_constructor: true
attributes:
frontLeft:
@@ -14,13 +14,13 @@ classes:
operator+:
operator-:
overloads:
const MecanumDriveWheelSpeeds& [const]:
const MecanumDriveWheelVelocities& [const]:
'[const]':
operator*:
operator/:
inline_code: |
cls_MecanumDriveWheelSpeeds
cls_MecanumDriveWheelVelocities
.def(
py::init<
wpi::units::meters_per_second_t, wpi::units::meters_per_second_t,
@@ -35,47 +35,47 @@ inline_code: |
wpi::units::feet_per_second_t rearLeft,
wpi::units::feet_per_second_t rearRight
){
return MecanumDriveWheelSpeeds{frontLeft, frontRight, rearLeft, rearRight};
return MecanumDriveWheelVelocities{frontLeft, frontRight, rearLeft, rearRight};
}, py::arg("frontLeft"), py::arg("frontRight"),
py::arg("rearLeft"), py::arg("rearRight"))
.def_property("frontLeft_fps",
[](MecanumDriveWheelSpeeds * self) -> wpi::units::feet_per_second_t {
[](MecanumDriveWheelVelocities * self) -> wpi::units::feet_per_second_t {
return self->frontLeft;
},
[](MecanumDriveWheelSpeeds * self, wpi::units::feet_per_second_t fps) {
[](MecanumDriveWheelVelocities * self, wpi::units::feet_per_second_t fps) {
self->frontLeft = fps;
}
)
.def_property("frontRight_fps",
[](MecanumDriveWheelSpeeds * self) -> wpi::units::feet_per_second_t {
[](MecanumDriveWheelVelocities * self) -> wpi::units::feet_per_second_t {
return self->frontRight;
},
[](MecanumDriveWheelSpeeds * self, wpi::units::feet_per_second_t fps) {
[](MecanumDriveWheelVelocities * self, wpi::units::feet_per_second_t fps) {
self->frontRight = fps;
}
)
.def_property("rearLeft_fps",
[](MecanumDriveWheelSpeeds * self) -> wpi::units::feet_per_second_t {
[](MecanumDriveWheelVelocities * self) -> wpi::units::feet_per_second_t {
return self->rearLeft;
},
[](MecanumDriveWheelSpeeds * self, wpi::units::feet_per_second_t fps) {
[](MecanumDriveWheelVelocities * self, wpi::units::feet_per_second_t fps) {
self->rearLeft = fps;
}
)
.def_property("rearRight_fps",
[](MecanumDriveWheelSpeeds * self) -> wpi::units::feet_per_second_t {
[](MecanumDriveWheelVelocities * self) -> wpi::units::feet_per_second_t {
return self->rearRight;
},
[](MecanumDriveWheelSpeeds * self, wpi::units::feet_per_second_t fps) {
[](MecanumDriveWheelVelocities * self, wpi::units::feet_per_second_t fps) {
self->rearRight = fps;
}
)
.def("__repr__", [](const MecanumDriveWheelSpeeds &ms) -> std::string {
return "MecanumDriveWheelSpeeds(frontLeft=" + std::to_string(ms.frontLeft()) + ", "
.def("__repr__", [](const MecanumDriveWheelVelocities &ms) -> std::string {
return "MecanumDriveWheelVelocities(frontLeft=" + std::to_string(ms.frontLeft()) + ", "
"frontRight=" + std::to_string(ms.frontRight()) + ", "
"rearLeft=" + std::to_string(ms.rearLeft()) + ", "
"rearRight=" + std::to_string(ms.rearRight()) + ")";
})
;
SetupWPyStruct<wpi::math::MecanumDriveWheelSpeeds>(cls_MecanumDriveWheelSpeeds);
SetupWPyStruct<wpi::math::MecanumDriveWheelVelocities>(cls_MecanumDriveWheelVelocities);

View File

@@ -1,10 +1,10 @@
extra_includes:
- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
- wpi/math/kinematics/DifferentialDriveWheelPositions.hpp
- wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
- wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
- wpi/math/kinematics/MecanumDriveWheelPositions.hpp
- wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
- wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
- wpi/math/kinematics/SwerveModuleAcceleration.hpp
- wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -12,7 +12,7 @@ classes:
wpi::math::Odometry:
template_params:
- WheelPositions
- WheelSpeeds
- WheelVelocities
- WheelAccelerations
methods:
Odometry:
@@ -28,35 +28,35 @@ templates:
qualname: wpi::math::Odometry
params:
- wpi::math::DifferentialDriveWheelPositions
- wpi::math::DifferentialDriveWheelSpeeds
- wpi::math::DifferentialDriveWheelVelocities
- wpi::math::DifferentialDriveWheelAccelerations
MecanumDriveOdometryBase:
qualname: wpi::math::Odometry
params:
- wpi::math::MecanumDriveWheelPositions
- wpi::math::MecanumDriveWheelSpeeds
- wpi::math::MecanumDriveWheelVelocities
- wpi::math::MecanumDriveWheelAccelerations
SwerveDrive2OdometryBase:
qualname: wpi::math::Odometry
params:
- wpi::util::array<wpi::math::SwerveModulePosition,2>
- wpi::util::array<wpi::math::SwerveModuleState,2>
- wpi::util::array<wpi::math::SwerveModuleVelocity,2>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
SwerveDrive3OdometryBase:
qualname: wpi::math::Odometry
params:
- wpi::util::array<wpi::math::SwerveModulePosition,3>
- wpi::util::array<wpi::math::SwerveModuleState,3>
- wpi::util::array<wpi::math::SwerveModuleVelocity,3>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
SwerveDrive4OdometryBase:
qualname: wpi::math::Odometry
params:
- wpi::util::array<wpi::math::SwerveModulePosition,4>
- wpi::util::array<wpi::math::SwerveModuleState,4>
- wpi::util::array<wpi::math::SwerveModuleVelocity,4>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
SwerveDrive6OdometryBase:
qualname: wpi::math::Odometry
params:
- wpi::util::array<wpi::math::SwerveModulePosition,6>
- wpi::util::array<wpi::math::SwerveModuleState,6>
- wpi::util::array<wpi::math::SwerveModuleVelocity,6>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>

View File

@@ -1,10 +1,10 @@
extra_includes:
- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
- wpi/math/kinematics/DifferentialDriveWheelPositions.hpp
- wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
- wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
- wpi/math/kinematics/MecanumDriveWheelPositions.hpp
- wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
- wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
- wpi/math/kinematics/SwerveModuleAcceleration.hpp
- wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -12,7 +12,7 @@ classes:
wpi::math::Odometry3d:
template_params:
- WheelPositions
- WheelSpeeds
- WheelVelocities
- WheelAccelerations
methods:
Odometry3d:
@@ -29,35 +29,35 @@ templates:
qualname: wpi::math::Odometry3d
params:
- wpi::math::DifferentialDriveWheelPositions
- wpi::math::DifferentialDriveWheelSpeeds
- wpi::math::DifferentialDriveWheelVelocities
- wpi::math::DifferentialDriveWheelAccelerations
MecanumDriveOdometry3dBase:
qualname: wpi::math::Odometry3d
params:
- wpi::math::MecanumDriveWheelPositions
- wpi::math::MecanumDriveWheelSpeeds
- wpi::math::MecanumDriveWheelVelocities
- wpi::math::MecanumDriveWheelAccelerations
SwerveDrive2Odometry3dBase:
qualname: wpi::math::Odometry3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,2>
- wpi::util::array<wpi::math::SwerveModuleState,2>
- wpi::util::array<wpi::math::SwerveModuleVelocity,2>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
SwerveDrive3Odometry3dBase:
qualname: wpi::math::Odometry3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,3>
- wpi::util::array<wpi::math::SwerveModuleState,3>
- wpi::util::array<wpi::math::SwerveModuleVelocity,3>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
SwerveDrive4Odometry3dBase:
qualname: wpi::math::Odometry3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,4>
- wpi::util::array<wpi::math::SwerveModuleState,4>
- wpi::util::array<wpi::math::SwerveModuleVelocity,4>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
SwerveDrive6Odometry3dBase:
qualname: wpi::math::Odometry3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,6>
- wpi::util::array<wpi::math::SwerveModuleState,6>
- wpi::util::array<wpi::math::SwerveModuleVelocity,6>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>

View File

@@ -1,10 +1,10 @@
extra_includes:
- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
- wpi/math/kinematics/DifferentialDriveWheelPositions.hpp
- wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
- wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
- wpi/math/kinematics/MecanumDriveWheelPositions.hpp
- wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
- wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
- wpi/math/kinematics/SwerveModuleAcceleration.hpp
- wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -13,7 +13,7 @@ classes:
wpi::math::PoseEstimator:
template_params:
- WheelPositions
- WheelSpeeds
- WheelVelocities
- WheelAccelerations
methods:
PoseEstimator:
@@ -36,35 +36,35 @@ templates:
qualname: wpi::math::PoseEstimator
params:
- wpi::math::DifferentialDriveWheelPositions
- wpi::math::DifferentialDriveWheelSpeeds
- wpi::math::DifferentialDriveWheelVelocities
- wpi::math::DifferentialDriveWheelAccelerations
MecanumDrivePoseEstimatorBase:
qualname: wpi::math::PoseEstimator
params:
- wpi::math::MecanumDriveWheelPositions
- wpi::math::MecanumDriveWheelSpeeds
- wpi::math::MecanumDriveWheelVelocities
- wpi::math::MecanumDriveWheelAccelerations
SwerveDrive2PoseEstimatorBase:
qualname: wpi::math::PoseEstimator
params:
- wpi::util::array<wpi::math::SwerveModulePosition,2>
- wpi::util::array<wpi::math::SwerveModuleState,2>
- wpi::util::array<wpi::math::SwerveModuleVelocity,2>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
SwerveDrive3PoseEstimatorBase:
qualname: wpi::math::PoseEstimator
params:
- wpi::util::array<wpi::math::SwerveModulePosition,3>
- wpi::util::array<wpi::math::SwerveModuleState,3>
- wpi::util::array<wpi::math::SwerveModuleVelocity,3>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
SwerveDrive4PoseEstimatorBase:
qualname: wpi::math::PoseEstimator
params:
- wpi::util::array<wpi::math::SwerveModulePosition,4>
- wpi::util::array<wpi::math::SwerveModuleState,4>
- wpi::util::array<wpi::math::SwerveModuleVelocity,4>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
SwerveDrive6PoseEstimatorBase:
qualname: wpi::math::PoseEstimator
params:
- wpi::util::array<wpi::math::SwerveModulePosition,6>
- wpi::util::array<wpi::math::SwerveModuleState,6>
- wpi::util::array<wpi::math::SwerveModuleVelocity,6>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>

View File

@@ -1,10 +1,10 @@
extra_includes:
- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
- wpi/math/kinematics/DifferentialDriveWheelPositions.hpp
- wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
- wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
- wpi/math/kinematics/MecanumDriveWheelPositions.hpp
- wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
- wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
- wpi/math/kinematics/SwerveModuleAcceleration.hpp
- wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -12,7 +12,7 @@ classes:
wpi::math::PoseEstimator3d:
template_params:
- WheelPositions
- WheelSpeeds
- WheelVelocities
- WheelAccelerations
methods:
PoseEstimator3d:
@@ -36,35 +36,35 @@ templates:
qualname: wpi::math::PoseEstimator3d
params:
- wpi::math::DifferentialDriveWheelPositions
- wpi::math::DifferentialDriveWheelSpeeds
- wpi::math::DifferentialDriveWheelVelocities
- wpi::math::DifferentialDriveWheelAccelerations
MecanumDrivePoseEstimator3dBase:
qualname: wpi::math::PoseEstimator3d
params:
- wpi::math::MecanumDriveWheelPositions
- wpi::math::MecanumDriveWheelSpeeds
- wpi::math::MecanumDriveWheelVelocities
- wpi::math::MecanumDriveWheelAccelerations
SwerveDrive2PoseEstimator3dBase:
qualname: wpi::math::PoseEstimator3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,2>
- wpi::util::array<wpi::math::SwerveModuleState,2>
- wpi::util::array<wpi::math::SwerveModuleVelocity,2>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
SwerveDrive3PoseEstimator3dBase:
qualname: wpi::math::PoseEstimator3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,3>
- wpi::util::array<wpi::math::SwerveModuleState,3>
- wpi::util::array<wpi::math::SwerveModuleVelocity,3>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
SwerveDrive4PoseEstimator3dBase:
qualname: wpi::math::PoseEstimator3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,4>
- wpi::util::array<wpi::math::SwerveModuleState,4>
- wpi::util::array<wpi::math::SwerveModuleVelocity,4>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
SwerveDrive6PoseEstimator3dBase:
qualname: wpi::math::PoseEstimator3d
params:
- wpi::util::array<wpi::math::SwerveModulePosition,6>
- wpi::util::array<wpi::math::SwerveModuleState,6>
- wpi::util::array<wpi::math::SwerveModuleVelocity,6>
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>

View File

@@ -18,11 +18,11 @@ classes:
ignore: true
wpi::util::array<Rotation2d, NumModules>:
ToSwerveModuleStates:
ToSwerveModuleVelocities:
doc: |
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
@@ -30,25 +30,25 @@ classes:
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
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.
:returns: 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
:meth:`desaturateWheelSpeeds` function to rectify this issue.
the module velocities to go above the attainable max velocity. Use the
:meth:`desaturateWheelVelocities` function to rectify this issue.
In addition, you can use Python unpacking syntax
to directly assign the module states to variables::
fl, fr, bl, br = kinematics.toSwerveModuleStates(chassisSpeeds)
ToWheelSpeeds:
ToChassisSpeeds:
fl, fr, bl, br = kinematics.toSwerveModuleVelocities(chassisVelocities)
ToWheelVelocities:
ToChassisVelocities:
overloads:
ModuleStates&&... [const]:
ModuleVelocities&&... [const]:
ignore: true
const wpi::util::array<SwerveModuleState, NumModules>& [const]:
const wpi::util::array<SwerveModuleVelocity, NumModules>& [const]:
ToTwist2d:
overloads:
ModuleDeltas&&... [const]:
@@ -56,23 +56,23 @@ classes:
wpi::util::array<SwerveModulePosition, NumModules> [const]:
? const wpi::util::array<SwerveModulePosition, NumModules>&, const wpi::util::array<SwerveModulePosition, NumModules>& [const]
:
DesaturateWheelSpeeds:
DesaturateWheelVelocities:
overloads:
wpi::util::array<SwerveModuleState, NumModules>*, wpi::units::meters_per_second_t:
wpi::util::array<SwerveModuleVelocity, NumModules>*, wpi::units::meters_per_second_t:
cpp_code: |
[](wpi::util::array<SwerveModuleState, NumModules> moduleStates, wpi::units::meters_per_second_t attainableMaxSpeed) {
wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(&moduleStates, attainableMaxSpeed);
return moduleStates;
[](wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities, wpi::units::meters_per_second_t attainableMaxVelocity) {
wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(&moduleVelocities, attainableMaxVelocity);
return moduleVelocities;
}
? wpi::util::array<SwerveModuleState, NumModules>*, ChassisSpeeds, wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t
? wpi::util::array<SwerveModuleVelocity, NumModules>*, ChassisVelocities, wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t
: cpp_code: |
[](wpi::util::array<SwerveModuleState, NumModules> moduleStates,
ChassisSpeeds currentChassisSpeed,
wpi::units::meters_per_second_t attainableMaxModuleSpeed,
wpi::units::meters_per_second_t attainableMaxRobotTranslationSpeed,
wpi::units::radians_per_second_t attainableMaxRobotRotationSpeed) {
wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(&moduleStates, currentChassisSpeed, attainableMaxModuleSpeed, attainableMaxRobotTranslationSpeed, attainableMaxRobotRotationSpeed);
return moduleStates;
[](wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
ChassisVelocities currentChassisVelocity,
wpi::units::meters_per_second_t attainableMaxModuleVelocity,
wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(&moduleVelocities, currentChassisVelocity, attainableMaxModuleVelocity, attainableMaxRobotTranslationVelocity, attainableMaxRobotRotationVelocity);
return moduleVelocities;
}
Interpolate:

View File

@@ -11,9 +11,9 @@ classes:
template_inline_code: |
cls_SwerveDriveKinematicsConstraint
.def_static("fromFps", [](const wpi::math::SwerveDriveKinematics<NumModules>& kinematics,
wpi::units::feet_per_second_t maxSpeed) {
return std::make_shared<wpi::math::SwerveDriveKinematicsConstraint<NumModules>>(kinematics, maxSpeed);
}, py::arg("kinematics"), py::arg("maxSpeed"))
wpi::units::feet_per_second_t maxVelocity) {
return std::make_shared<wpi::math::SwerveDriveKinematicsConstraint<NumModules>>(kinematics, maxVelocity);
}, py::arg("kinematics"), py::arg("maxVelocity"))
;
templates:

View File

@@ -1,5 +1,5 @@
extra_includes:
- wpi/math/kinematics/SwerveModuleState.hpp
- wpi/math/kinematics/SwerveModuleVelocity.hpp
classes:
wpi::math::SwerveDrivePoseEstimator:

View File

@@ -1,5 +1,5 @@
extra_includes:
- wpi/math/kinematics/SwerveModuleState.hpp
- wpi/math/kinematics/SwerveModuleVelocity.hpp
classes:
wpi::math::SwerveDrivePoseEstimator3d:

View File

@@ -1,41 +0,0 @@
extra_includes:
- wpystruct.h
classes:
wpi::math::SwerveModuleState:
force_no_default_constructor: true
attributes:
speed:
angle:
methods:
operator==:
Optimize:
overloads:
const Rotation2d&:
const SwerveModuleState&, const Rotation2d&:
ignore: true
CosineScale:
inline_code: |
cls_SwerveModuleState
.def(
py::init<
wpi::units::meters_per_second_t, wpi::math::Rotation2d
>(),
py::arg("speed") = 0, py::arg("angle") = wpi::math::Rotation2d()
)
.def_property("speed_fps",
[](SwerveModuleState * self) -> wpi::units::feet_per_second_t {
return self->speed;
},
[](SwerveModuleState * self, wpi::units::feet_per_second_t speed) {
self->speed = speed;
}
)
.def("__repr__", [](const SwerveModuleState &ss) -> std::string {
return "SwerveModuleState(speed=" + std::to_string(ss.speed()) + ", "
"angle=" + std::to_string(ss.angle.Radians()()) + ")";
})
;
SetupWPyStruct<wpi::math::SwerveModuleState>(cls_SwerveModuleState);

View File

@@ -0,0 +1,41 @@
extra_includes:
- wpystruct.h
classes:
wpi::math::SwerveModuleVelocity:
force_no_default_constructor: true
attributes:
velocity:
angle:
methods:
operator==:
Optimize:
overloads:
const Rotation2d&:
const SwerveModuleVelocity&, const Rotation2d&:
ignore: true
CosineScale:
inline_code: |
cls_SwerveModuleVelocity
.def(
py::init<
wpi::units::meters_per_second_t, wpi::math::Rotation2d
>(),
py::arg("velocity") = 0, py::arg("angle") = wpi::math::Rotation2d()
)
.def_property("velocity_fps",
[](SwerveModuleVelocity * self) -> wpi::units::feet_per_second_t {
return self->velocity;
},
[](SwerveModuleVelocity * self, wpi::units::feet_per_second_t velocity) {
self->velocity = velocity;
}
)
.def("__repr__", [](const SwerveModuleVelocity &ss) -> std::string {
return "SwerveModuleVelocity(velocity=" + std::to_string(ss.velocity()) + ", "
"angle=" + std::to_string(ss.angle.Radians()()) + ")";
})
;
SetupWPyStruct<wpi::math::SwerveModuleVelocity>(cls_SwerveModuleVelocity);