Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01: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

52
wpimath/src/test/python/kinematics/test_chassis_speeds.py → wpimath/src/test/python/kinematics/test_chassis_velocities.py
View File

@@ -2,16 +2,16 @@ import pytest
import math
import numpy as np
from wpimath import ChassisSpeeds, Twist2d, Pose2d, Rotation2d
from wpimath import ChassisVelocities, Twist2d, Pose2d, Rotation2d
def test_discretize():
target = ChassisSpeeds(vx=1, vy=0, omega=0.5)
target = ChassisVelocities(vx=1, vy=0, omega=0.5)
duration = 1
dt = 0.01
speeds = target.discretize(duration)
twist = Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt)
velocities = target.discretize(duration)
twist = Twist2d(velocities.vx * dt, velocities.vy * dt, velocities.omega * dt)
pose = Pose2d()
time = 0
@@ -27,28 +27,28 @@ def test_discretize():
def test_to_robot_relative():
chassis_speeds = ChassisSpeeds(vx=1, vy=0, omega=0.5).toRobotRelative(
chassis_velocities = ChassisVelocities(vx=1, vy=0, omega=0.5).toRobotRelative(
Rotation2d.fromDegrees(-90)
)
assert chassis_speeds.vx == pytest.approx(0.0, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(1.0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0.5, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(0.0, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(1.0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0.5, abs=1e-9)
def test_to_field_relative():
chassis_speeds = ChassisSpeeds(vx=1, vy=0, omega=0.5).toFieldRelative(
chassis_velocities = ChassisVelocities(vx=1, vy=0, omega=0.5).toFieldRelative(
Rotation2d.fromDegrees(45)
)
assert chassis_speeds.vx == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_speeds.vy == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0.5, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_velocities.vy == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0.5, abs=1e-9)
def test_plus() -> None:
left = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
right = ChassisSpeeds(vx=2.0, vy=1.5, omega=0.25)
left = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
right = ChassisVelocities(vx=2.0, vy=1.5, omega=0.25)
result = left + right
@@ -58,8 +58,8 @@ def test_plus() -> None:
def test_minus() -> None:
left = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
right = ChassisSpeeds(vx=2.0, vy=0.5, omega=0.25)
left = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
right = ChassisVelocities(vx=2.0, vy=0.5, omega=0.25)
result = left - right
@@ -69,9 +69,9 @@ def test_minus() -> None:
def test_neg() -> None:
speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
velocities = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
result = -speeds
result = -velocities
assert math.isclose(-1.0, result.vx)
assert math.isclose(-0.5, result.vy)
@@ -79,9 +79,9 @@ def test_neg() -> None:
def test_multiplication() -> None:
speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
velocities = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
result = speeds * 2
result = velocities * 2
assert math.isclose(2.0, result.vx)
assert math.isclose(1.0, result.vy)
@@ -89,9 +89,9 @@ def test_multiplication() -> None:
def test_division() -> None:
speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
velocities = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
result = speeds / 2
result = velocities / 2
assert math.isclose(0.5, result.vx)
assert math.isclose(0.25, result.vy)
@@ -99,9 +99,9 @@ def test_division() -> None:
def test_unpack() -> None:
speeds = ChassisSpeeds(1, 1, 0.5)
velocities = ChassisVelocities(1, 1, 0.5)
vx, vy, omega = speeds
vx, vy, omega = velocities
assert math.isclose(1, vx)
assert math.isclose(1, vy)
@@ -109,6 +109,6 @@ def test_unpack() -> None:
def test_repr() -> None:
speeds = ChassisSpeeds(2.0, 1.0, 0.0)
velocities = ChassisVelocities(2.0, 1.0, 0.0)
assert repr(speeds) == "ChassisSpeeds(vx=2.000000, vy=1.000000, omega=0.000000)"
assert repr(velocities) == "ChassisVelocities(vx=2.000000, vy=1.000000, omega=0.000000)"

58
wpimath/src/test/python/kinematics/test_differential_drive_kinematics.py
View File

@@ -3,67 +3,67 @@ import math
import numpy as np
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
DifferentialDriveKinematics,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
Rotation2d,
)
def test_inverse_kinematics_from_zero():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
chassis_speeds = ChassisSpeeds()
wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
chassis_velocities = ChassisVelocities()
wheel_velocities = kinematics.toWheelVelocities(chassis_velocities)
assert wheel_speeds.left == pytest.approx(0, abs=1e-9)
assert wheel_speeds.right == pytest.approx(0, abs=1e-9)
assert wheel_velocities.left == pytest.approx(0, abs=1e-9)
assert wheel_velocities.right == pytest.approx(0, abs=1e-9)
def test_forward_kinematics_from_zero():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
wheel_speeds = DifferentialDriveWheelSpeeds()
chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
wheel_velocities = DifferentialDriveWheelVelocities()
chassis_velocities = kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0, abs=1e-9)
def test_inverse_kinematics_for_straight_line():
kinematics = DifferentialDriveKinematics(trackwidth=(0.381 * 2))
chassis_speeds = ChassisSpeeds(vx=3.0, vy=0, omega=0)
wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
chassis_velocities = ChassisVelocities(vx=3.0, vy=0, omega=0)
wheel_velocities = kinematics.toWheelVelocities(chassis_velocities)
assert wheel_speeds.left == pytest.approx(3, abs=1e-9)
assert wheel_speeds.right == pytest.approx(3, abs=1e-9)
assert wheel_velocities.left == pytest.approx(3, abs=1e-9)
assert wheel_velocities.right == pytest.approx(3, abs=1e-9)
def test_forward_kinematics_for_straight_line():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
wheel_speeds = DifferentialDriveWheelSpeeds(left=3.0, right=3.0)
chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
wheel_velocities = DifferentialDriveWheelVelocities(left=3.0, right=3.0)
chassis_velocities = kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(3, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(3, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0, abs=1e-9)
def test_inverse_kinematics_for_rotate_in_place():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
chassis_speeds = ChassisSpeeds(vx=0.0, vy=0.0, omega=math.pi)
wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
chassis_velocities = ChassisVelocities(vx=0.0, vy=0.0, omega=math.pi)
wheel_velocities = kinematics.toWheelVelocities(chassis_velocities)
assert wheel_speeds.left == pytest.approx(-0.381 * math.pi, abs=1e-9)
assert wheel_speeds.right == pytest.approx(0.381 * math.pi, abs=1e-9)
assert wheel_velocities.left == pytest.approx(-0.381 * math.pi, abs=1e-9)
assert wheel_velocities.right == pytest.approx(0.381 * math.pi, abs=1e-9)
def test_forward_kinematics_for_rotate_in_place():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
wheel_speeds = DifferentialDriveWheelSpeeds(
wheel_velocities = DifferentialDriveWheelVelocities(
left=0.381 * math.pi, right=-0.381 * math.pi
)
chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(-math.pi, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(-math.pi, abs=1e-9)

50
wpimath/src/test/python/kinematics/test_differential_drive_wheel_speeds.py
View File

@@ -1,50 +0,0 @@
import pytest
from wpimath import DifferentialDriveWheelSpeeds
def test_plus():
left = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
right = DifferentialDriveWheelSpeeds(left=2.0, right=1.5)
result = left + right
assert result.left == 3.0
assert result.right == 2.0
def test_minus():
left = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
right = DifferentialDriveWheelSpeeds(left=2.0, right=0.5)
result = left - right
assert result.left == -1.0
assert result.right == 0.0
def test_unary_minus():
speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
result = -speeds
assert result.left == -1.0
assert result.right == -0.5
def test_multiplication():
speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
result = speeds * 2
assert result.left == 2.0
assert result.right == 1.0
def test_division():
speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
result = speeds / 2
assert result.left == 0.5
assert result.right == 0.25

50
wpimath/src/test/python/kinematics/test_differential_drive_wheel_velocities.py Normal file
View File

@@ -0,0 +1,50 @@
import pytest
from wpimath import DifferentialDriveWheelVelocities
def test_plus():
left = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
right = DifferentialDriveWheelVelocities(left=2.0, right=1.5)
result = left + right
assert result.left == 3.0
assert result.right == 2.0
def test_minus():
left = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
right = DifferentialDriveWheelVelocities(left=2.0, right=0.5)
result = left - right
assert result.left == -1.0
assert result.right == 0.0
def test_unary_minus():
velocities = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
result = -velocities
assert result.left == -1.0
assert result.right == -0.5
def test_multiplication():
velocities = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
result = velocities * 2
assert result.left == 2.0
assert result.right == 1.0
def test_division():
velocities = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
result = velocities / 2
assert result.left == 0.5
assert result.right == 0.25

162
wpimath/src/test/python/kinematics/test_mecanum_drive_kinematics.py
View File

@@ -3,8 +3,8 @@ import math
from wpimath import (
MecanumDriveKinematics,
ChassisSpeeds,
MecanumDriveWheelSpeeds,
ChassisVelocities,
MecanumDriveWheelVelocities,
MecanumDriveWheelPositions,
Rotation2d,
Translation2d,
@@ -27,24 +27,24 @@ def kinematics_test():
def test_straight_line_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=5, vy=0, omega=0)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=5, vy=0, omega=0)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(5.0, abs=0.1)
assert module_states.frontRight == pytest.approx(5.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(5.0, abs=0.1)
assert module_states.rearRight == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(5.0, abs=0.1)
def test_straight_line_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=5, frontRight=5, rearLeft=5, rearRight=5
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(5.0, abs=0.1)
assert chassis_speeds.vy == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vx == pytest.approx(5.0, abs=0.1)
assert chassis_velocities.vy == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.0, abs=0.1)
def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
@@ -59,24 +59,24 @@ def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
def test_strafe_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=4, omega=0)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=0, vy=4, omega=0)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(-4.0, abs=0.1)
assert module_states.frontRight == pytest.approx(4.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(4.0, abs=0.1)
assert module_states.rearRight == pytest.approx(-4.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(-4.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(4.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(4.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(-4.0, abs=0.1)
def test_strafe_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-5, frontRight=5, rearLeft=5, rearRight=-5
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.vy == pytest.approx(5.0, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vx == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vy == pytest.approx(5.0, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.0, abs=0.1)
def test_strafe_forward_kinematics_with_deltas(kinematics_test):
@@ -91,27 +91,27 @@ def test_strafe_forward_kinematics_with_deltas(kinematics_test):
def test_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=0, vy=0, omega=2 * math.pi)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(-150.79644737, abs=0.1)
assert module_states.frontRight == pytest.approx(150.79644737, abs=0.1)
assert module_states.rearLeft == pytest.approx(-150.79644737, abs=0.1)
assert module_states.rearRight == pytest.approx(150.79644737, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(-150.79644737, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(150.79644737, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-150.79644737, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(150.79644737, abs=0.1)
def test_rotation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-150.79644737,
frontRight=150.79644737,
rearLeft=-150.79644737,
rearRight=150.79644737,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.vy == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=0.1)
assert chassis_velocities.vx == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vy == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.omega == pytest.approx(2 * math.pi, abs=0.1)
def test_rotation_forward_kinematics_with_deltas(kinematics_test):
@@ -129,28 +129,28 @@ def test_rotation_forward_kinematics_with_deltas(kinematics_test):
def test_mixed_rotation_translation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=2, vy=3, omega=1)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=2, vy=3, omega=1)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(-25.0, abs=0.1)
assert module_states.frontRight == pytest.approx(29.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(-19.0, abs=0.1)
assert module_states.rearRight == pytest.approx(23.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(-25.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(29.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-19.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(23.0, abs=0.1)
def test_mixed_rotation_translation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-17.677670,
frontRight=20.506097,
rearLeft=-13.435,
rearRight=16.26,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(1.41335, abs=0.1)
assert chassis_speeds.vy == pytest.approx(2.1221, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
assert chassis_velocities.vx == pytest.approx(1.41335, abs=0.1)
assert chassis_velocities.vy == pytest.approx(2.1221, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.707, abs=0.1)
def test_mixed_rotation_translation_forward_kinematics_with_deltas(kinematics_test):
@@ -166,29 +166,29 @@ def test_mixed_rotation_translation_forward_kinematics_with_deltas(kinematics_te
def test_off_center_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=1)
module_states = kinematics_test.kinematics.toWheelSpeeds(
speeds, kinematics_test.m_fl
velocities = ChassisVelocities(vx=0, vy=0, omega=1)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(
velocities, kinematics_test.m_fl
)
assert module_states.frontLeft == pytest.approx(0, abs=0.1)
assert module_states.frontRight == pytest.approx(24.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(-24.0, abs=0.1)
assert module_states.rearRight == pytest.approx(48.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(24.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-24.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(48.0, abs=0.1)
def test_off_center_rotation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=0,
frontRight=16.971,
rearLeft=-16.971,
rearRight=33.941,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(8.48525, abs=0.1)
assert chassis_speeds.vy == pytest.approx(-8.48525, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
assert chassis_velocities.vx == pytest.approx(8.48525, abs=0.1)
assert chassis_velocities.vy == pytest.approx(-8.48525, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.707, abs=0.1)
def test_off_center_rotation_forward_kinematics_with_deltas(kinematics_test):
@@ -203,29 +203,29 @@ def test_off_center_rotation_forward_kinematics_with_deltas(kinematics_test):
def test_off_center_translation_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=5, vy=2, omega=1)
module_states = kinematics_test.kinematics.toWheelSpeeds(
speeds, kinematics_test.m_fl
velocities = ChassisVelocities(vx=5, vy=2, omega=1)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(
velocities, kinematics_test.m_fl
)
assert module_states.frontLeft == pytest.approx(3.0, abs=0.1)
assert module_states.frontRight == pytest.approx(31.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(-17.0, abs=0.1)
assert module_states.rearRight == pytest.approx(51.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(3.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(31.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-17.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(51.0, abs=0.1)
def test_off_center_translation_rotation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=2.12,
frontRight=21.92,
rearLeft=-12.02,
rearRight=36.06,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(12.02, abs=0.1)
assert chassis_speeds.vy == pytest.approx(-7.07, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
assert chassis_velocities.vx == pytest.approx(12.02, abs=0.1)
assert chassis_velocities.vy == pytest.approx(-7.07, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.707, abs=0.1)
def test_off_center_translation_rotation_forward_kinematics_with_deltas(
@@ -242,20 +242,20 @@ def test_off_center_translation_rotation_forward_kinematics_with_deltas(
def test_desaturate(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=5, frontRight=6, rearLeft=4, rearRight=7
).desaturate(5.5)
k_factor = 5.5 / 7.0
assert wheel_speeds.frontLeft == pytest.approx(5.0 * k_factor, abs=1e-9)
assert wheel_speeds.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearRight == pytest.approx(7.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontLeft == pytest.approx(5.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearRight == pytest.approx(7.0 * k_factor, abs=1e-9)
def test_desaturate_negative_speeds(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
def test_desaturate_negative_velocities(kinematics_test):
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-5,
frontRight=6,
rearLeft=4,
@@ -264,7 +264,7 @@ def test_desaturate_negative_speeds(kinematics_test):
k_factor = 5.5 / 7.0
assert wheel_speeds.frontLeft == pytest.approx(-5.0 * k_factor, abs=1e-9)
assert wheel_speeds.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearRight == pytest.approx(-7.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontLeft == pytest.approx(-5.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearRight == pytest.approx(-7.0 * k_factor, abs=1e-9)

42
wpimath/src/test/python/kinematics/test_mecanum_drive_odometry.py
View File

@@ -3,7 +3,7 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
MecanumDriveKinematics,
MecanumDriveOdometry,
MecanumDriveWheelPositions,
@@ -130,23 +130,23 @@ def test_accuracy_facing_trajectory():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity,
0,
ground_truth_state.velocity * ground_truth_state.curvature,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
ground_truth_state.pose.rotation()
@@ -199,23 +199,23 @@ def test_accuracy_facing_x_axis():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity * ground_truth_state.pose.rotation().cos(),
ground_truth_state.velocity * ground_truth_state.pose.rotation().sin(),
0,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
Rotation2d(random.gauss(0.0, 1.0) * 0.05), wheel_positions

42
wpimath/src/test/python/kinematics/test_mecanum_drive_odometry3d.py
View File

@@ -3,7 +3,7 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
MecanumDriveKinematics,
MecanumDriveOdometry3d,
MecanumDriveWheelPositions,
@@ -155,23 +155,23 @@ def test_accuracy_facing_trajectory():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity,
0,
ground_truth_state.velocity * ground_truth_state.curvature,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
Rotation3d(
@@ -228,23 +228,23 @@ def test_accuracy_facing_x_axis():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity * ground_truth_state.pose.rotation().cos(),
ground_truth_state.velocity * ground_truth_state.pose.rotation().sin(),
0,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
Rotation3d(0, 0, random.gauss(0.0, 1.0) * 0.05),

22
wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_speeds.py → wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_velocities.py
View File

@@ -1,16 +1,16 @@
import pytest
from wpimath import MecanumDriveWheelSpeeds
from wpimath import MecanumDriveWheelVelocities
def test_plus():
left = MecanumDriveWheelSpeeds(
left = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
right = MecanumDriveWheelSpeeds(
right = MecanumDriveWheelVelocities(
frontLeft=2.0,
frontRight=1.5,
rearLeft=0.5,
@@ -26,13 +26,13 @@ def test_plus():
def test_minus():
left = MecanumDriveWheelSpeeds(
left = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
right = MecanumDriveWheelSpeeds(
right = MecanumDriveWheelVelocities(
frontLeft=2.0,
frontRight=1.5,
rearLeft=0.5,
@@ -48,14 +48,14 @@ def test_minus():
def test_unary_minus():
speeds = MecanumDriveWheelSpeeds(
velocities = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
result = -speeds
result = -velocities
assert result.frontLeft == -1.0
assert result.frontRight == -0.5
@@ -64,14 +64,14 @@ def test_unary_minus():
def test_multiplication():
speeds = MecanumDriveWheelSpeeds(
velocities = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
result = speeds * 2
result = velocities * 2
assert result.frontLeft == 2.0
assert result.frontRight == 1.0
@@ -80,14 +80,14 @@ def test_multiplication():
def test_division():
speeds = MecanumDriveWheelSpeeds(
velocities = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
result = speeds / 2
result = velocities / 2
assert result.frontLeft == 0.5
assert result.frontRight == 0.25

34
wpimath/src/test/python/kinematics/test_swerve.py
View File

@@ -4,11 +4,11 @@ from wpimath import (
Pose2d,
Rotation2d,
Translation2d,
ChassisSpeeds,
ChassisVelocities,
SwerveDrive4Kinematics,
SwerveDrive4Odometry,
SwerveModulePosition,
SwerveModuleState,
SwerveModuleVelocity,
)
@@ -22,13 +22,13 @@ def s4():
def test_swerve4_straightline(s4: SwerveDrive4Kinematics):
chassisSpeeds = ChassisSpeeds(5, 0, 0)
chassisVelocities = ChassisVelocities(5, 0, 0)
fl, fr, bl, br = s4.toSwerveModuleStates(chassisSpeeds)
assert fl.speed == pytest.approx(5.0)
assert fr.speed == pytest.approx(5.0)
assert bl.speed == pytest.approx(5.0)
assert br.speed == pytest.approx(5.0)
fl, fr, bl, br = s4.toSwerveModuleVelocities(chassisVelocities)
assert fl.velocity == pytest.approx(5.0)
assert fr.velocity == pytest.approx(5.0)
assert bl.velocity == pytest.approx(5.0)
assert br.velocity == pytest.approx(5.0)
assert fl.angle.radians() == pytest.approx(0.0)
assert fr.angle.radians() == pytest.approx(0.0)
@@ -37,19 +37,19 @@ def test_swerve4_straightline(s4: SwerveDrive4Kinematics):
def test_swerve4_normalize():
s1 = SwerveModuleState(5)
s2 = SwerveModuleState(6)
s3 = SwerveModuleState(4)
s4 = SwerveModuleState(7)
s1 = SwerveModuleVelocity(5)
s2 = SwerveModuleVelocity(6)
s3 = SwerveModuleVelocity(4)
s4 = SwerveModuleVelocity(7)
states = SwerveDrive4Kinematics.desaturateWheelSpeeds((s1, s2, s3, s4), 5.5)
states = SwerveDrive4Kinematics.desaturateWheelVelocities((s1, s2, s3, s4), 5.5)
kFactor = 5.5 / 7.0
assert states[0].speed == pytest.approx(5.0 * kFactor)
assert states[1].speed == pytest.approx(6.0 * kFactor)
assert states[2].speed == pytest.approx(4.0 * kFactor)
assert states[3].speed == pytest.approx(7.0 * kFactor)
assert states[0].velocity == pytest.approx(5.0 * kFactor)
assert states[1].velocity == pytest.approx(6.0 * kFactor)
assert states[2].velocity == pytest.approx(4.0 * kFactor)
assert states[3].velocity == pytest.approx(7.0 * kFactor)
def test_swerve4_odometry(s4: SwerveDrive4Kinematics):

220
wpimath/src/test/python/kinematics/test_swerve_drive_kinematics.py
View File

@@ -3,9 +3,9 @@ import math
from wpimath import (
SwerveDrive4Kinematics,
ChassisSpeeds,
ChassisVelocities,
Rotation2d,
SwerveModuleState,
SwerveModuleVelocity,
SwerveModulePosition,
Translation2d,
)
@@ -29,15 +29,15 @@ def kinematics_test():
def test_straight_line_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=5.0, vy=0.0, omega=0.0)
states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
velocities = ChassisVelocities(vx=5.0, vy=0.0, omega=0.0)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(5.0, abs=kEpsilon)
assert fr.speed == pytest.approx(5.0, abs=kEpsilon)
assert bl.speed == pytest.approx(5.0, abs=kEpsilon)
assert br.speed == pytest.approx(5.0, abs=kEpsilon)
assert fl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(5.0, abs=kEpsilon)
assert bl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert br.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fl.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
assert fr.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
@@ -46,14 +46,14 @@ def test_straight_line_inverse_kinematics(kinematics_test):
def test_straight_line_forward_kinematics(kinematics_test):
state = SwerveModuleState(speed=5.0, angle=Rotation2d.fromDegrees(0))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
state = SwerveModuleVelocity(velocity=5.0, angle=Rotation2d.fromDegrees(0))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities(
(state, state, state, state)
)
assert chassis_speeds.vx == pytest.approx(5.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(5.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(0.0, abs=kEpsilon)
def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
@@ -66,15 +66,15 @@ def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
def test_straight_strafe_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=5, omega=0)
states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
velocities = ChassisVelocities(vx=0, vy=5, omega=0)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(5.0, abs=kEpsilon)
assert fr.speed == pytest.approx(5.0, abs=kEpsilon)
assert bl.speed == pytest.approx(5.0, abs=kEpsilon)
assert br.speed == pytest.approx(5.0, abs=kEpsilon)
assert fl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(5.0, abs=kEpsilon)
assert bl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert br.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
@@ -83,14 +83,14 @@ def test_straight_strafe_inverse_kinematics(kinematics_test):
def test_straight_strafe_forward_kinematics(kinematics_test):
state = SwerveModuleState(speed=5, angle=Rotation2d.fromDegrees(90))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
state = SwerveModuleVelocity(velocity=5, angle=Rotation2d.fromDegrees(90))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities(
(state, state, state, state)
)
assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(5.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(5.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(0.0, abs=kEpsilon)
def test_straight_strafe_forward_kinematics_with_deltas(kinematics_test):
@@ -103,15 +103,15 @@ def test_straight_strafe_forward_kinematics_with_deltas(kinematics_test):
def test_turn_in_place_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
velocities = ChassisVelocities(vx=0, vy=0, omega=2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(106.63, abs=kEpsilon)
assert fr.speed == pytest.approx(106.63, abs=kEpsilon)
assert bl.speed == pytest.approx(106.63, abs=kEpsilon)
assert br.speed == pytest.approx(106.63, abs=kEpsilon)
assert fl.velocity == pytest.approx(106.63, abs=kEpsilon)
assert fr.velocity == pytest.approx(106.63, abs=kEpsilon)
assert bl.velocity == pytest.approx(106.63, abs=kEpsilon)
assert br.velocity == pytest.approx(106.63, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(135.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
@@ -120,16 +120,16 @@ def test_turn_in_place_inverse_kinematics(kinematics_test):
def test_conserve_wheel_angle(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
states = kinematics_test.m_kinematics.toSwerveModuleStates(ChassisSpeeds())
velocities = ChassisVelocities(vx=0, vy=0, omega=2 * math.pi)
kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(ChassisVelocities())
fl, fr, bl, br = states
assert fl.speed == pytest.approx(0.0, abs=kEpsilon)
assert fr.speed == pytest.approx(0.0, abs=kEpsilon)
assert bl.speed == pytest.approx(0.0, abs=kEpsilon)
assert br.speed == pytest.approx(0.0, abs=kEpsilon)
assert fl.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(0.0, abs=kEpsilon)
assert bl.velocity == pytest.approx(0.0, abs=kEpsilon)
assert br.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(135.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
@@ -143,14 +143,14 @@ def test_reset_wheel_angle(kinematics_test):
bl_angle = Rotation2d.fromDegrees(180)
br_angle = Rotation2d.fromDegrees(270)
kinematics_test.m_kinematics.resetHeadings((fl_angle, fr_angle, bl_angle, br_angle))
states = kinematics_test.m_kinematics.toSwerveModuleStates(ChassisSpeeds())
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(ChassisVelocities())
fl_mod, fr_mod, bl_mod, br_mod = states
assert fl_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert fr_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert bl_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert br_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert fl_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fr_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert bl_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert br_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fl_mod.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
assert fr_mod.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
@@ -159,16 +159,16 @@ def test_reset_wheel_angle(kinematics_test):
def test_turn_in_place_forward_kinematics(kinematics_test):
fl = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(135))
fr = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(45))
bl = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(-135))
br = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(-45))
fl = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(135))
fr = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(45))
bl = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(-135))
br = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(-45))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities((fl, fr, bl, br))
assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
def test_turn_in_place_forward_kinematics_with_deltas(kinematics_test):
@@ -185,17 +185,17 @@ def test_turn_in_place_forward_kinematics_with_deltas(kinematics_test):
def test_off_center_cor_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(0, 0, 2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleStates(
speeds, kinematics_test.m_fl
velocities = ChassisVelocities(0, 0, 2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(
velocities, kinematics_test.m_fl
)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(0.0, abs=kEpsilon)
assert fr.speed == pytest.approx(150.796, abs=kEpsilon)
assert bl.speed == pytest.approx(150.796, abs=kEpsilon)
assert br.speed == pytest.approx(213.258, abs=kEpsilon)
assert fl.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(150.796, abs=kEpsilon)
assert bl.velocity == pytest.approx(150.796, abs=kEpsilon)
assert br.velocity == pytest.approx(213.258, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
@@ -204,16 +204,16 @@ def test_off_center_cor_rotation_inverse_kinematics(kinematics_test):
def test_off_center_cor_rotation_forward_kinematics(kinematics_test):
fl = SwerveModuleState(speed=0.0, angle=Rotation2d.fromDegrees(0))
fr = SwerveModuleState(speed=150.796, angle=Rotation2d.fromDegrees(0))
bl = SwerveModuleState(speed=150.796, angle=Rotation2d.fromDegrees(-90))
br = SwerveModuleState(speed=213.258, angle=Rotation2d.fromDegrees(-45))
fl = SwerveModuleVelocity(velocity=0.0, angle=Rotation2d.fromDegrees(0))
fr = SwerveModuleVelocity(velocity=150.796, angle=Rotation2d.fromDegrees(0))
bl = SwerveModuleVelocity(velocity=150.796, angle=Rotation2d.fromDegrees(-90))
br = SwerveModuleVelocity(velocity=213.258, angle=Rotation2d.fromDegrees(-45))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities((fl, fr, bl, br))
assert chassis_speeds.vx == pytest.approx(75.398, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(-75.398, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(75.398, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(-75.398, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
def test_off_center_cor_rotation_forward_kinematics_with_deltas(kinematics_test):
@@ -230,17 +230,17 @@ def test_off_center_cor_rotation_forward_kinematics_with_deltas(kinematics_test)
def test_off_center_cor_rotation_and_translation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(0, 3.0, 1.5)
states = kinematics_test.m_kinematics.toSwerveModuleStates(
speeds, Translation2d(x=24, y=0)
velocities = ChassisVelocities(0, 3.0, 1.5)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(
velocities, Translation2d(x=24, y=0)
)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(23.43, abs=kEpsilon)
assert fr.speed == pytest.approx(23.43, abs=kEpsilon)
assert bl.speed == pytest.approx(54.08, abs=kEpsilon)
assert br.speed == pytest.approx(54.08, abs=kEpsilon)
assert fl.velocity == pytest.approx(23.43, abs=kEpsilon)
assert fr.velocity == pytest.approx(23.43, abs=kEpsilon)
assert bl.velocity == pytest.approx(54.08, abs=kEpsilon)
assert br.velocity == pytest.approx(54.08, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(-140.19, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(-39.81, abs=kEpsilon)
@@ -249,16 +249,16 @@ def test_off_center_cor_rotation_and_translation_inverse_kinematics(kinematics_t
def test_off_center_cor_rotation_and_translation_forward_kinematics(kinematics_test):
fl = SwerveModuleState(speed=23.43, angle=Rotation2d.fromDegrees(-140.19))
fr = SwerveModuleState(speed=23.43, angle=Rotation2d.fromDegrees(-39.81))
bl = SwerveModuleState(speed=54.08, angle=Rotation2d.fromDegrees(-109.44))
br = SwerveModuleState(speed=54.08, angle=Rotation2d.fromDegrees(-70.56))
fl = SwerveModuleVelocity(velocity=23.43, angle=Rotation2d.fromDegrees(-140.19))
fr = SwerveModuleVelocity(velocity=23.43, angle=Rotation2d.fromDegrees(-39.81))
bl = SwerveModuleVelocity(velocity=54.08, angle=Rotation2d.fromDegrees(-109.44))
br = SwerveModuleVelocity(velocity=54.08, angle=Rotation2d.fromDegrees(-70.56))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities((fl, fr, bl, br))
assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(-33.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(1.5, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(-33.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(1.5, abs=kEpsilon)
def test_off_center_cor_rotation_and_translation_forward_kinematics_with_deltas(
@@ -277,54 +277,54 @@ def test_off_center_cor_rotation_and_translation_forward_kinematics_with_deltas(
def test_desaturate(kinematics_test):
state1 = SwerveModuleState(speed=5.0, angle=Rotation2d.fromDegrees(0))
state2 = SwerveModuleState(speed=6.0, angle=Rotation2d.fromDegrees(0))
state3 = SwerveModuleState(speed=4.0, angle=Rotation2d.fromDegrees(0))
state4 = SwerveModuleState(speed=7.0, angle=Rotation2d.fromDegrees(0))
state1 = SwerveModuleVelocity(velocity=5.0, angle=Rotation2d.fromDegrees(0))
state2 = SwerveModuleVelocity(velocity=6.0, angle=Rotation2d.fromDegrees(0))
state3 = SwerveModuleVelocity(velocity=4.0, angle=Rotation2d.fromDegrees(0))
state4 = SwerveModuleVelocity(velocity=7.0, angle=Rotation2d.fromDegrees(0))
arr = [state1, state2, state3, state4]
arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(arr, 5.5)
arr = kinematics_test.m_kinematics.desaturateWheelVelocities(arr, 5.5)
k_factor = 5.5 / 7.0
assert arr[0].speed == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].speed == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].speed == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].speed == pytest.approx(7.0 * k_factor, abs=kEpsilon)
assert arr[0].velocity == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].velocity == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].velocity == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].velocity == pytest.approx(7.0 * k_factor, abs=kEpsilon)
def test_desaturate_smooth(kinematics_test):
state1 = SwerveModuleState(speed=5.0, angle=Rotation2d(0))
state2 = SwerveModuleState(speed=6.0, angle=Rotation2d(0))
state3 = SwerveModuleState(speed=4.0, angle=Rotation2d(0))
state4 = SwerveModuleState(speed=7.0, angle=Rotation2d(0))
state1 = SwerveModuleVelocity(velocity=5.0, angle=Rotation2d(0))
state2 = SwerveModuleVelocity(velocity=6.0, angle=Rotation2d(0))
state3 = SwerveModuleVelocity(velocity=4.0, angle=Rotation2d(0))
state4 = SwerveModuleVelocity(velocity=7.0, angle=Rotation2d(0))
arr = [state1, state2, state3, state4]
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities(
(arr[0], arr[1], arr[2], arr[3])
)
arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(
arr, chassis_speeds, 5.5, 5.5, 3.5
arr = kinematics_test.m_kinematics.desaturateWheelVelocities(
arr, chassis_velocities, 5.5, 5.5, 3.5
)
k_factor = 5.5 / 7.0
assert arr[0].speed == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].speed == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].speed == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].speed == pytest.approx(7.0 * k_factor, abs=kEpsilon)
assert arr[0].velocity == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].velocity == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].velocity == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].velocity == pytest.approx(7.0 * k_factor, abs=kEpsilon)
def test_desaturate_negative_speed(kinematics_test):
state1 = SwerveModuleState(speed=1.0, angle=Rotation2d(0))
state2 = SwerveModuleState(speed=1.0, angle=Rotation2d(0))
state3 = SwerveModuleState(speed=-2.0, angle=Rotation2d(0))
state4 = SwerveModuleState(speed=-2.0, angle=Rotation2d(0))
def test_desaturate_negative_velocity(kinematics_test):
state1 = SwerveModuleVelocity(velocity=1.0, angle=Rotation2d(0))
state2 = SwerveModuleVelocity(velocity=1.0, angle=Rotation2d(0))
state3 = SwerveModuleVelocity(velocity=-2.0, angle=Rotation2d(0))
state4 = SwerveModuleVelocity(velocity=-2.0, angle=Rotation2d(0))
arr = [state1, state2, state3, state4]
arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(arr, 1.0)
arr = kinematics_test.m_kinematics.desaturateWheelVelocities(arr, 1.0)
assert arr[0].speed == pytest.approx(0.5, abs=kEpsilon)
assert arr[1].speed == pytest.approx(0.5, abs=kEpsilon)
assert arr[2].speed == pytest.approx(-1.0, abs=kEpsilon)
assert arr[3].speed == pytest.approx(-1.0, abs=kEpsilon)
assert arr[0].velocity == pytest.approx(0.5, abs=kEpsilon)
assert arr[1].velocity == pytest.approx(0.5, abs=kEpsilon)
assert arr[2].velocity == pytest.approx(-1.0, abs=kEpsilon)
assert arr[3].velocity == pytest.approx(-1.0, abs=kEpsilon)

24
wpimath/src/test/python/kinematics/test_swerve_drive_odometry.py
View File

@@ -3,12 +3,12 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
Pose2d,
Rotation2d,
SwerveDrive4Kinematics,
SwerveDrive4Odometry,
SwerveModuleState,
SwerveModuleVelocity,
SwerveModulePosition,
TrajectoryGenerator,
TrajectoryConfig,
@@ -154,23 +154,23 @@ def test_accuracy_facing_trajectory():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
module_states = kinematics.toSwerveModuleStates(
ChassisSpeeds(
module_velocities = kinematics.toSwerveModuleVelocities(
ChassisVelocities(
vx=ground_truth_state.velocity,
vy=0,
omega=ground_truth_state.velocity * ground_truth_state.curvature,
)
)
fl.distance += module_states[0].speed * dt
fr.distance += module_states[1].speed * dt
bl.distance += module_states[2].speed * dt
br.distance += module_states[3].speed * dt
fl.distance += module_velocities[0].velocity * dt
fr.distance += module_velocities[1].velocity * dt
bl.distance += module_velocities[2].velocity * dt
br.distance += module_velocities[3].velocity * dt
fl.angle = module_states[0].angle
fr.angle = module_states[1].angle
bl.angle = module_states[2].angle
br.angle = module_states[3].angle
fl.angle = module_velocities[0].angle
fr.angle = module_velocities[1].angle
bl.angle = module_velocities[2].angle
br.angle = module_velocities[3].angle
xhat = odometry.update(
ground_truth_state.pose.rotation()

2
wpimath/src/test/python/kinematics/test_swerve_drive_odometry3d.py
View File

@@ -3,7 +3,7 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
Pose3d,
Pose2d,
Rotation2d,

0
wpimath/src/test/python/kinematics/test_swerve_module_accelerations.py → wpimath/src/test/python/kinematics/test_swerve_module_acceleration.py
View File

76
wpimath/src/test/python/kinematics/test_swerve_module_state.py → wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
View File

@@ -1,140 +1,140 @@
import pytest
import math
from wpimath import Rotation2d, SwerveModuleState
from wpimath import Rotation2d, SwerveModuleVelocity
kEpsilon = 1e-9
def test_optimize():
angle_a = Rotation2d.fromDegrees(45)
ref_a = SwerveModuleState(-2, Rotation2d.fromDegrees(180))
ref_a = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(180))
ref_a.optimize(angle_a)
assert ref_a.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(-50)
ref_b = SwerveModuleState(4.7, Rotation2d.fromDegrees(41))
ref_b = SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41))
ref_b.optimize(angle_b)
assert ref_b.speed == pytest.approx(-4.7, abs=kEpsilon)
assert ref_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
def test_no_optimize():
angle_a = Rotation2d.fromDegrees(0)
ref_a = SwerveModuleState(2, Rotation2d.fromDegrees(89))
ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(89))
ref_a.optimize(angle_a)
assert ref_a.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(0)
ref_b = SwerveModuleState(-2, Rotation2d.fromDegrees(-2))
ref_b = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(-2))
ref_b.optimize(angle_b)
assert ref_b.speed == pytest.approx(-2.0, abs=kEpsilon)
assert ref_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
def test_cosine_scaling():
angle_a = Rotation2d.fromDegrees(0)
ref_a = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_a.cosineScale(angle_a)
assert ref_a.speed == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(45)
ref_b = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_b = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_b.cosineScale(angle_b)
assert ref_b.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_b.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_c = Rotation2d.fromDegrees(-45)
ref_c = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_c = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_c.cosineScale(angle_c)
assert ref_c.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_c.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_d = Rotation2d.fromDegrees(135)
ref_d = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_d = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_d.cosineScale(angle_d)
assert ref_d.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_d.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_e = Rotation2d.fromDegrees(-135)
ref_e = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_e = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_e.cosineScale(angle_e)
assert ref_e.speed == pytest.approx(-2.0, abs=kEpsilon)
assert ref_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_f = Rotation2d.fromDegrees(180)
ref_f = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_f = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_f.cosineScale(angle_f)
assert ref_f.speed == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_g = Rotation2d.fromDegrees(0)
ref_g = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_g = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_g.cosineScale(angle_g)
assert ref_g.speed == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_h = Rotation2d.fromDegrees(45)
ref_h = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_h = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_h.cosineScale(angle_h)
assert ref_h.speed == pytest.approx(-2.0, abs=kEpsilon)
assert ref_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_i = Rotation2d.fromDegrees(-45)
ref_i = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_i = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_i.cosineScale(angle_i)
assert ref_i.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_i.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_j = Rotation2d.fromDegrees(135)
ref_j = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_j = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_j.cosineScale(angle_j)
assert ref_j.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_j.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_k = Rotation2d.fromDegrees(-135)
ref_k = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_k = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_k.cosineScale(angle_k)
assert ref_k.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_k.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_l = Rotation2d.fromDegrees(180)
ref_l = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_l = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_l.cosineScale(angle_l)
assert ref_l.speed == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
def test_equality():
state1 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
state2 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
state1 = SwerveModuleVelocity(2, Rotation2d.fromDegrees(90))
state2 = SwerveModuleVelocity(2, Rotation2d.fromDegrees(90))
assert state1 == state2
def test_inequality():
state1 = SwerveModuleState(1, Rotation2d.fromDegrees(90))
state2 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
state3 = SwerveModuleState(1, Rotation2d.fromDegrees(89))
state1 = SwerveModuleVelocity(1, Rotation2d.fromDegrees(90))
state2 = SwerveModuleVelocity(2, Rotation2d.fromDegrees(90))
state3 = SwerveModuleVelocity(1, Rotation2d.fromDegrees(89))
assert state1 != state2
assert state1 != state3

16
wpimath/src/test/python/test_trajectory_exponential_profile.py
View File

@@ -113,29 +113,29 @@ def test_switch_goal_in_middle(profile):
assert state == goal
def test_top_speed(profile):
def test_top_velocity(profile):
goal = ExponentialProfileMeterVolts.State(40, 0)
state = ExponentialProfileMeterVolts.State(0, 0)
max_speed = 0
max_velocity = 0
for _ in range(900):
state = check_dynamics(profile, state, goal)
max_speed = max(max_speed, state.velocity)
max_velocity = max(max_velocity, state.velocity)
assert_near(constraints.maxVelocity(), max_speed, 10e-5)
assert_near(constraints.maxVelocity(), max_velocity, 10e-5)
assert state == goal
def test_top_speed_backward(profile):
def test_top_velocity_backward(profile):
goal = ExponentialProfileMeterVolts.State(-40, 0)
state = ExponentialProfileMeterVolts.State(0, 0)
max_speed = 0
max_velocity = 0
for _ in range(900):
state = check_dynamics(profile, state, goal)
max_speed = min(max_speed, state.velocity)
max_velocity = min(max_velocity, state.velocity)
assert_near(-constraints.maxVelocity(), max_speed, 10e-5)
assert_near(-constraints.maxVelocity(), max_velocity, 10e-5)
assert state == goal

2
wpimath/src/test/python/trajectory/test_trapezoidal_profile.py
View File

@@ -78,7 +78,7 @@ def test_switch_goal_in_middle():
assert state == goal
def test_top_speed():
def test_top_velocity():
constraints = TrapezoidProfile.Constraints(0.75, 0.75)
goal = TrapezoidProfile.State(4.0, 0.0)
state = TrapezoidProfile.State()