diff --git a/.github/workflows/dist.yml b/.github/workflows/dist.yml index 40985f740..4fe46edf4 100644 --- a/.github/workflows/dist.yml +++ b/.github/workflows/dist.yml @@ -153,7 +153,7 @@ jobs: - "ubuntu-24.04" - "ubuntu-24.04-arm" - "macos-15" - - "windows-2022" + - "windows-2025" python_version: - "3.13" @@ -201,7 +201,7 @@ jobs: - "ubuntu-24.04-arm" - "macos-15-intel" # x86_64 - "macos-15" # arm64 - - "windows-2022" + - "windows-2025" python_version: - '3.11' - '3.12' diff --git a/examples/robot/DifferentialDrivePoseEstimator/drivetrain.py b/examples/robot/DifferentialDrivePoseEstimator/drivetrain.py index 2bb4a201b..bb7c2fe2f 100644 --- a/examples/robot/DifferentialDrivePoseEstimator/drivetrain.py +++ b/examples/robot/DifferentialDrivePoseEstimator/drivetrain.py @@ -59,7 +59,6 @@ def __init__(self, camera_to_object_topic: ntcore.DoubleArrayTopic) -> None: # Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The # numbers used below are robot specific, and should be tuned. self.pose_estimator = wpimath.DifferentialDrivePoseEstimator( - self.kinematics, self.imu.get_rotation2d(), self.left_encoder.get_distance(), self.right_encoder.get_distance(), diff --git a/examples/robot/ElevatorExponentialProfile/robot.py b/examples/robot/ElevatorExponentialProfile/robot.py index 34839e950..375f1def4 100644 --- a/examples/robot/ElevatorExponentialProfile/robot.py +++ b/examples/robot/ElevatorExponentialProfile/robot.py @@ -23,22 +23,20 @@ def __init__(self) -> None: # Create a motion profile with the given maximum voltage and characteristics kV, kA # These gains should match your feedforward kV, kA for best results. - self.profile = wpimath.ExponentialProfileMeterVolts( - wpimath.ExponentialProfileMeterVolts.Constraints.from_characteristics( - 10, 1, 1 - ) + self.profile = wpimath.ExponentialProfile( + wpimath.ExponentialProfile.Constraints.from_characteristics(10, 1, 1) ) - self.goal = wpimath.ExponentialProfileMeterVolts.State(0, 0) - self.setpoint = wpimath.ExponentialProfileMeterVolts.State(0, 0) + self.goal = wpimath.ExponentialProfile.State(0, 0) + self.setpoint = wpimath.ExponentialProfile.State(0, 0) # Note: These gains are fake, and will have to be tuned for your robot. self.motor.set_pid(1.3, 0.0, 0.7) def teleop_periodic(self) -> None: if self.joystick.get_raw_button_pressed(2): - self.goal = wpimath.ExponentialProfileMeterVolts.State(5, 0) + self.goal = wpimath.ExponentialProfile.State(5, 0) elif self.joystick.get_raw_button_pressed(3): - self.goal = wpimath.ExponentialProfileMeterVolts.State(0, 0) + self.goal = wpimath.ExponentialProfile.State(0, 0) # Retrieve the profiled setpoint for the next timestep. This setpoint moves # toward the goal while obeying the constraints. diff --git a/examples/robot/ElevatorExponentialSimulation/subsystems/elevator.py b/examples/robot/ElevatorExponentialSimulation/subsystems/elevator.py index 55248f2ef..887e914cb 100644 --- a/examples/robot/ElevatorExponentialSimulation/subsystems/elevator.py +++ b/examples/robot/ElevatorExponentialSimulation/subsystems/elevator.py @@ -17,15 +17,15 @@ def __init__(self) -> None: # This gearbox represents a gearbox containing 4 Vex 775pro motors. self.elevator_gearbox = wpimath.DCMotor.neo(2) - self.profile = wpimath.ExponentialProfileMeterVolts( - wpimath.ExponentialProfileMeterVolts.Constraints.from_characteristics( + self.profile = wpimath.ExponentialProfile( + wpimath.ExponentialProfile.Constraints.from_characteristics( constants.ELEVATOR_MAX_V, constants.ELEVATOR_KV, constants.ELEVATOR_KA, ) ) - self.setpoint = wpimath.ExponentialProfileMeterVolts.State(0, 0) + self.setpoint = wpimath.ExponentialProfile.State(0, 0) # Standard classes for controlling our elevator self.pid_controller = wpimath.PIDController( @@ -104,7 +104,7 @@ def reach_goal(self, goal: float) -> None: :param goal: the position to maintain """ - goal_state = wpimath.ExponentialProfileMeterVolts.State(goal, 0) + goal_state = wpimath.ExponentialProfile.State(goal, 0) next_state = self.profile.calculate(0.020, self.setpoint, goal_state) @@ -126,9 +126,7 @@ def stop(self) -> None: def reset(self) -> None: """Reset Exponential profile to begin from current position on enable.""" - self.setpoint = wpimath.ExponentialProfileMeterVolts.State( - self.encoder.get_distance(), 0 - ) + self.setpoint = wpimath.ExponentialProfile.State(self.encoder.get_distance(), 0) def update_telemetry(self) -> None: """Update telemetry, including the mechanism visualization.""" diff --git a/examples/robot/ElevatorProfiledPID/robot.py b/examples/robot/ElevatorProfiledPID/robot.py index 0ab29b784..e5dac0506 100644 --- a/examples/robot/ElevatorProfiledPID/robot.py +++ b/examples/robot/ElevatorProfiledPID/robot.py @@ -25,7 +25,7 @@ class MyRobot(wpilib.TimedRobot): def __init__(self) -> None: super().__init__() self.joystick = wpilib.Joystick(1) - self.encoder = wpilib.Encoder(1, 2) + self.encoder = wpilib.Encoder(2, 3) self.motor = wpilib.PWMSparkMax(1) # Create a PID controller whose setpoint's change is subject to maximum diff --git a/examples/robot/Encoder/robot.py b/examples/robot/Encoder/robot.py index adad7e241..4a0802bf0 100755 --- a/examples/robot/Encoder/robot.py +++ b/examples/robot/Encoder/robot.py @@ -28,13 +28,7 @@ def __init__(self): """Robot initialization function""" super().__init__() - self.encoder = wpilib.Encoder(1, 2, False, wpilib.Encoder.EncodingType.X4) - - # Defines the number of samples to average when determining the rate. - # On a quadrature encoder, values range from 1-255; - # larger values result in smoother but potentially - # less accurate rates than lower values. - self.encoder.set_samples_to_average(5) + self.encoder = wpilib.Encoder(2, 3, False, wpilib.Encoder.EncodingType.X4) # Defines how far the mechanism attached to the encoder moves per pulse. In # this case, we assume that a 360 count encoder is directly @@ -42,13 +36,6 @@ def __init__(self): # and that we want to measure distance in inches. self.encoder.set_distance_per_pulse(1.0 / 360.0 * 2.0 * math.pi * 1.5) - # Defines the lowest rate at which the encoder will - # not be considered stopped, for the purposes of - # the GetStopped() method. Units are in distance / second, - # where distance refers to the units of distance - # that you are using, in this case inches. - self.encoder.set_min_rate(1.0) - def teleop_periodic(self): wpilib.SmartDashboard.put_number( "Encoder Distance", self.encoder.get_distance() diff --git a/examples/robot/SimpleDifferentialDriveSimulation/robot.py b/examples/robot/SimpleDifferentialDriveSimulation/robot.py index 672d92685..76e5f2a35 100644 --- a/examples/robot/SimpleDifferentialDriveSimulation/robot.py +++ b/examples/robot/SimpleDifferentialDriveSimulation/robot.py @@ -26,7 +26,7 @@ def __init__(self) -> None: self.timer = wpilib.Timer() # Called once at the beginning of the robot program. - self.trajectory = wpimath.TrajectoryGenerator.generate_trajectory( + self.trajectory = wpimath.DrivetrainSplineTrajectoryGenerator.generate( wpimath.Pose2d(2, 2, wpimath.Rotation2d()), [], wpimath.Pose2d(6, 4, wpimath.Rotation2d()), diff --git a/rdev.toml b/rdev.toml index 32788dbc0..e214e3398 100644 --- a/rdev.toml +++ b/rdev.toml @@ -22,7 +22,7 @@ wrapper = "2027.0.0a6.post4" [params] -wpilib_bin_version = "2027.0.0-alpha-6-167-gdb3a9c428" +wpilib_bin_version = "2027.0.0-alpha-6-218-g3fa405550" wpilib_bin_url = "https://frcmaven.wpi.edu/artifactory/development-2027" # wpilib_bin_url = "https://frcmaven.wpi.edu/artifactory/development-2027" diff --git a/snippets/robot/QuickVision/robot.py b/snippets/robot/QuickVision/robot.py index a1af6979c..b738976b9 100644 --- a/snippets/robot/QuickVision/robot.py +++ b/snippets/robot/QuickVision/robot.py @@ -13,9 +13,9 @@ class MyRobot(wpilib.TimedRobot): """ Uses the CameraServer class to automatically capture video from a USB webcam and send it to the dashboard without doing any vision processing. This is the easiest way to get camera images - to the dashboard. Just add this to the robot_init() method in your program. + to the dashboard. Just add this to the robot class constructor. """ - def __init__(self): + def __init__(self) -> None: super().__init__() CameraServer().launch() diff --git a/subprojects/robotpy-cscore/pyproject.toml b/subprojects/robotpy-cscore/pyproject.toml index 096aa50bd..d2a2a6c7d 100644 --- a/subprojects/robotpy-cscore/pyproject.toml +++ b/subprojects/robotpy-cscore/pyproject.toml @@ -44,7 +44,7 @@ version_file = "cscore/version.py" artifact_id = "cscore-cpp" group_id = "org.wpilib.cscore" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" staticlibs = ["cscore"] extract_to = "lib" @@ -53,7 +53,7 @@ extract_to = "lib" artifact_id = "cameraserver-cpp" group_id = "org.wpilib.cameraserver" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" staticlibs = ["cameraserver"] extract_to = "lib" diff --git a/subprojects/robotpy-hal/hal/_initialize.py b/subprojects/robotpy-hal/hal/_initialize.py index eafffb4ea..af18c0d70 100644 --- a/subprojects/robotpy-hal/hal/_initialize.py +++ b/subprojects/robotpy-hal/hal/_initialize.py @@ -6,4 +6,4 @@ if _sse: _wpi_hal.set_show_extensions_not_found_messages(False) -_wpi_hal.initialize(500, 0) +_wpi_hal.initialize() diff --git a/subprojects/robotpy-hal/semiwrap/UsageReporting.yml b/subprojects/robotpy-hal/semiwrap/UsageReporting.yml index 1d1901167..cdf470a25 100644 --- a/subprojects/robotpy-hal/semiwrap/UsageReporting.yml +++ b/subprojects/robotpy-hal/semiwrap/UsageReporting.yml @@ -6,3 +6,28 @@ functions: overloads: std::string_view, std::string_view: std::string_view, int, std::string_view: + HAL_PublishCanVersion: + overloads: + uint8_t, uint32_t, std::string_view, std::string_view: + cpp_code: | + [](uint8_t busId, uint32_t deviceId, std::string_view name, + std::string_view version) { + py::gil_scoped_release release; + return HAL_PublishCanVersion(busId, deviceId, name, version); + } + HAL_PublishVersion: + overloads: + std::string_view, std::string_view: + cpp_code: | + [](std::string_view name, std::string_view version) { + py::gil_scoped_release release; + return HAL_PublishVersion(name, version); + } + HAL_PublishWpilibVersion: + overloads: + std::string_view: + cpp_code: | + [](std::string_view version) { + py::gil_scoped_release release; + return HAL_PublishWpilibVersion(version); + } diff --git a/subprojects/robotpy-halsim-ds-socket/pyproject.toml b/subprojects/robotpy-halsim-ds-socket/pyproject.toml index 913c48cca..989bbe7db 100644 --- a/subprojects/robotpy-halsim-ds-socket/pyproject.toml +++ b/subprojects/robotpy-halsim-ds-socket/pyproject.toml @@ -30,7 +30,7 @@ version_file = "halsim_ds_socket/version.py" artifact_id = "halsim_ds_socket" group_id = "org.wpilib.halsim" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" use_headers = false extract_to = "halsim_ds_socket" diff --git a/subprojects/robotpy-halsim-ws/pyproject.toml b/subprojects/robotpy-halsim-ws/pyproject.toml index 9c64ce64d..71fb21490 100644 --- a/subprojects/robotpy-halsim-ws/pyproject.toml +++ b/subprojects/robotpy-halsim-ws/pyproject.toml @@ -35,7 +35,7 @@ version_file = "halsim_ws/version.py" artifact_id = "halsim_ws_server" group_id = "org.wpilib.halsim" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" use_headers = false extract_to = "halsim_ws/server" @@ -45,7 +45,7 @@ libs = ["halsim_ws_server"] artifact_id = "halsim_ws_client" group_id = "org.wpilib.halsim" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" use_headers = false extract_to = "halsim_ws/client" diff --git a/subprojects/robotpy-native-apriltag/pyproject.toml b/subprojects/robotpy-native-apriltag/pyproject.toml index 093156e4c..7ed2bfc0f 100644 --- a/subprojects/robotpy-native-apriltag/pyproject.toml +++ b/subprojects/robotpy-native-apriltag/pyproject.toml @@ -27,7 +27,7 @@ packages = ["src/native"] artifact_id = "apriltag-cpp" group_id = "org.wpilib.apriltag" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/apriltag" libs = ["apriltag"] diff --git a/subprojects/robotpy-native-datalog/pyproject.toml b/subprojects/robotpy-native-datalog/pyproject.toml index 2a18b8a37..e05f229d6 100644 --- a/subprojects/robotpy-native-datalog/pyproject.toml +++ b/subprojects/robotpy-native-datalog/pyproject.toml @@ -26,7 +26,7 @@ packages = ["src/native"] artifact_id = "datalog-cpp" group_id = "org.wpilib.datalog" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/datalog" libs = ["datalog"] diff --git a/subprojects/robotpy-native-halsim-gui/pyproject.toml b/subprojects/robotpy-native-halsim-gui/pyproject.toml index 8a93d8a80..88af25001 100644 --- a/subprojects/robotpy-native-halsim-gui/pyproject.toml +++ b/subprojects/robotpy-native-halsim-gui/pyproject.toml @@ -29,7 +29,7 @@ packages = ["src/native"] artifact_id = "halsim_gui" group_id = "org.wpilib.halsim" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" use_headers = true extract_to = "src/native/halsim_gui" diff --git a/subprojects/robotpy-native-ntcore/pyproject.toml b/subprojects/robotpy-native-ntcore/pyproject.toml index 5b3f02bf0..566916a1e 100644 --- a/subprojects/robotpy-native-ntcore/pyproject.toml +++ b/subprojects/robotpy-native-ntcore/pyproject.toml @@ -29,7 +29,7 @@ packages = ["src/native"] artifact_id = "ntcore-cpp" group_id = "org.wpilib.ntcore" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/ntcore" libs = ["ntcore"] diff --git a/subprojects/robotpy-native-romi/pyproject.toml b/subprojects/robotpy-native-romi/pyproject.toml index 557bceb82..91f0fdd9d 100644 --- a/subprojects/robotpy-native-romi/pyproject.toml +++ b/subprojects/robotpy-native-romi/pyproject.toml @@ -25,7 +25,7 @@ packages = ["src/native"] artifact_id = "romiVendordep-cpp" group_id = "org.wpilib.romiVendordep" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/romi" libs = ["romiVendordep"] diff --git a/subprojects/robotpy-native-wpihal/pyproject.toml b/subprojects/robotpy-native-wpihal/pyproject.toml index aae31cc6d..cd40f4835 100644 --- a/subprojects/robotpy-native-wpihal/pyproject.toml +++ b/subprojects/robotpy-native-wpihal/pyproject.toml @@ -27,7 +27,7 @@ packages = ["src/native"] artifact_id = "hal-cpp" group_id = "org.wpilib.hal" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/wpihal" libs = ["wpiHal"] diff --git a/subprojects/robotpy-native-wpilib/pyproject.toml b/subprojects/robotpy-native-wpilib/pyproject.toml index e3a3a12c2..08518dcd3 100644 --- a/subprojects/robotpy-native-wpilib/pyproject.toml +++ b/subprojects/robotpy-native-wpilib/pyproject.toml @@ -33,7 +33,7 @@ packages = ["src/native"] artifact_id = "wpilibc-cpp" group_id = "org.wpilib.wpilibc" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/wpilib" libs = ["wpilibc"] diff --git a/subprojects/robotpy-native-wpimath/pyproject.toml b/subprojects/robotpy-native-wpimath/pyproject.toml index f848ef9fc..fa5844ef1 100644 --- a/subprojects/robotpy-native-wpimath/pyproject.toml +++ b/subprojects/robotpy-native-wpimath/pyproject.toml @@ -25,7 +25,7 @@ packages = ["src/native"] artifact_id = "wpimath-cpp" group_id = "org.wpilib.wpimath" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/wpimath" libs = ["wpimath"] diff --git a/subprojects/robotpy-native-wpinet/pyproject.toml b/subprojects/robotpy-native-wpinet/pyproject.toml index 816c58820..f430868b2 100644 --- a/subprojects/robotpy-native-wpinet/pyproject.toml +++ b/subprojects/robotpy-native-wpinet/pyproject.toml @@ -25,7 +25,7 @@ packages = ["src/native"] artifact_id = "wpinet-cpp" group_id = "org.wpilib.wpinet" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/wpinet" libs = ["wpinet"] diff --git a/subprojects/robotpy-native-wpiutil/pyproject.toml b/subprojects/robotpy-native-wpiutil/pyproject.toml index 9a498df79..e0959a83e 100644 --- a/subprojects/robotpy-native-wpiutil/pyproject.toml +++ b/subprojects/robotpy-native-wpiutil/pyproject.toml @@ -24,7 +24,7 @@ packages = ["src/native"] artifact_id = "wpiutil-cpp" group_id = "org.wpilib.wpiutil" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/wpiutil" libs = ["wpiutil"] diff --git a/subprojects/robotpy-native-xrp/pyproject.toml b/subprojects/robotpy-native-xrp/pyproject.toml index 87b826d18..db76de782 100644 --- a/subprojects/robotpy-native-xrp/pyproject.toml +++ b/subprojects/robotpy-native-xrp/pyproject.toml @@ -25,7 +25,7 @@ packages = ["src/native"] artifact_id = "xrpVendordep-cpp" group_id = "org.wpilib.xrpVendordep" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" extract_to = "src/native/xrp" libs = ["xrpVendordep"] diff --git a/subprojects/robotpy-wpilib/semiwrap/CounterBase.yml b/subprojects/robotpy-wpilib/semiwrap/CounterBase.yml index c6b8641d2..6a5447844 100644 --- a/subprojects/robotpy-wpilib/semiwrap/CounterBase.yml +++ b/subprojects/robotpy-wpilib/semiwrap/CounterBase.yml @@ -6,7 +6,5 @@ classes: CounterBase: Get: Reset: - GetPeriod: - SetMaxPeriod: GetStopped: GetDirection: diff --git a/subprojects/robotpy-wpilib/semiwrap/DifferentialDrive.yml b/subprojects/robotpy-wpilib/semiwrap/DifferentialDrive.yml index 9f6c7cc39..4a6e63e62 100644 --- a/subprojects/robotpy-wpilib/semiwrap/DifferentialDrive.yml +++ b/subprojects/robotpy-wpilib/semiwrap/DifferentialDrive.yml @@ -37,7 +37,9 @@ classes: import wpilib.drive class Robot(wpilib.TimedRobot): - def robotInit(self): + def __init__(self) -> None: + super().__init__() + self.front_left = wpilib.PWMVictorSPX(1) self.rear_left = wpilib.PWMVictorSPX(2) self.left = wpilib.MotorControllerGroup(self.front_left, self.rear_left) @@ -53,7 +55,9 @@ classes: import wpilib.drive class Robot(wpilib.TimedRobot): - def robotInit(self): + def __init__(self) -> None: + super().__init__() + self.front_left = wpilib.PWMVictorSPX(1) self.mid_left = wpilib.PWMVictorSPX(2) self.rear_left = wpilib.PWMVictorSPX(3) diff --git a/subprojects/robotpy-wpilib/semiwrap/Encoder.yml b/subprojects/robotpy-wpilib/semiwrap/Encoder.yml index c055be7d2..a6484de71 100644 --- a/subprojects/robotpy-wpilib/semiwrap/Encoder.yml +++ b/subprojects/robotpy-wpilib/semiwrap/Encoder.yml @@ -24,20 +24,15 @@ classes: default: wpi::Encoder::EncodingType::X4 Get: Reset: - GetPeriod: - SetMaxPeriod: GetStopped: GetDirection: GetRaw: GetEncodingScale: GetDistance: GetRate: - SetMinRate: SetDistancePerPulse: GetDistancePerPulse: SetReverseDirection: - SetSamplesToAverage: - GetSamplesToAverage: SetSimDevice: GetFPGAIndex: InitSendable: diff --git a/subprojects/robotpy-wpilib/semiwrap/OpModeRobot.yml b/subprojects/robotpy-wpilib/semiwrap/OpModeRobot.yml index 20f7f43d5..fa4a6695f 100644 --- a/subprojects/robotpy-wpilib/semiwrap/OpModeRobot.yml +++ b/subprojects/robotpy-wpilib/semiwrap/OpModeRobot.yml @@ -29,6 +29,10 @@ classes: AddPeriodic: GetLoopStartTime: LoopFunc: + StartCurrentOpMode: + ignore: true + EndCurrentOpMode: + ignore: true attributes: DEFAULT_PERIOD: wpi::OpModeRobot: diff --git a/subprojects/robotpy-wpilib/semiwrap/PeriodicPriorityQueue.yml b/subprojects/robotpy-wpilib/semiwrap/PeriodicPriorityQueue.yml index 369920f5a..9fb74a917 100644 --- a/subprojects/robotpy-wpilib/semiwrap/PeriodicPriorityQueue.yml +++ b/subprojects/robotpy-wpilib/semiwrap/PeriodicPriorityQueue.yml @@ -22,6 +22,7 @@ classes: func: period: expirationTime: + id: methods: Callback: overloads: diff --git a/subprojects/robotpy-wpilib/semiwrap/simulation/EncoderSim.yml b/subprojects/robotpy-wpilib/semiwrap/simulation/EncoderSim.yml index 9b4288ba4..3f3605873 100644 --- a/subprojects/robotpy-wpilib/semiwrap/simulation/EncoderSim.yml +++ b/subprojects/robotpy-wpilib/semiwrap/simulation/EncoderSim.yml @@ -17,24 +17,16 @@ classes: RegisterCountCallback: GetCount: SetCount: - RegisterPeriodCallback: - GetPeriod: - SetPeriod: + RegisterRateCallback: RegisterResetCallback: GetReset: SetReset: - RegisterMaxPeriodCallback: - GetMaxPeriod: - SetMaxPeriod: RegisterDirectionCallback: GetDirection: SetDirection: RegisterReverseDirectionCallback: GetReverseDirection: SetReverseDirection: - RegisterSamplesToAverageCallback: - GetSamplesToAverage: - SetSamplesToAverage: RegisterDistancePerPulseCallback: GetDistancePerPulse: SetDistancePerPulse: diff --git a/subprojects/robotpy-wpilib/wpilib/_impl/start.py b/subprojects/robotpy-wpilib/wpilib/_impl/start.py index fe333b91b..cca50e7c8 100644 --- a/subprojects/robotpy-wpilib/wpilib/_impl/start.py +++ b/subprojects/robotpy-wpilib/wpilib/_impl/start.py @@ -156,6 +156,7 @@ def start(self, robot_cls: wpilib.RobotBase) -> bool: def _start(self, robot_cls: wpilib.RobotBase) -> bool: hal.report_usage("Language", "Python") + hal.publish_wpilib_version(f"{wpilib.__version__} (Python)") is_simulation = wpilib.RobotBase.is_simulation() diff --git a/subprojects/robotpy-wpimath/pyproject.toml b/subprojects/robotpy-wpimath/pyproject.toml index 570a30b87..c13df751e 100644 --- a/subprojects/robotpy-wpimath/pyproject.toml +++ b/subprojects/robotpy-wpimath/pyproject.toml @@ -1581,13 +1581,13 @@ DifferentialSample = "wpi/math/trajectory/DifferentialSample.hpp" DifferentialTrajectory = "wpi/math/trajectory/DifferentialTrajectory.hpp" DrivetrainSplineSample = "wpi/math/trajectory/DrivetrainSplineSample.hpp" DrivetrainSplineTrajectory = "wpi/math/trajectory/DrivetrainSplineTrajectory.hpp" +DrivetrainSplineTrajectoryGenerator = "wpi/math/trajectory/DrivetrainSplineTrajectoryGenerator.hpp" +DrivetrainSplineTrajectoryParameterizer = "wpi/math/trajectory/DrivetrainSplineTrajectoryParameterizer.hpp" ExponentialProfile = "wpi/math/trajectory/ExponentialProfile.hpp" HolonomicSample = "wpi/math/trajectory/HolonomicSample.hpp" HolonomicTrajectory = "wpi/math/trajectory/HolonomicTrajectory.hpp" Trajectory = "wpi/math/trajectory/Trajectory.hpp" TrajectoryConfig = "wpi/math/trajectory/TrajectoryConfig.hpp" -TrajectoryGenerator = "wpi/math/trajectory/TrajectoryGenerator.hpp" -TrajectoryParameterizer = "wpi/math/trajectory/TrajectoryParameterizer.hpp" TrajectorySample = "wpi/math/trajectory/TrajectorySample.hpp" TrapezoidProfile = "wpi/math/trajectory/TrapezoidProfile.hpp" diff --git a/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator.yml b/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator.yml index 7e8a6ec11..95d59ce74 100644 --- a/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator.yml +++ b/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator.yml @@ -37,8 +37,8 @@ classes: methods: DifferentialDrivePoseEstimator: overloads: - DifferentialDriveKinematics&, const Rotation2d&, wpi::units::meter_t, wpi::units::meter_t, const Pose2d&: - ? DifferentialDriveKinematics&, const Rotation2d&, wpi::units::meter_t, wpi::units::meter_t, const Pose2d&, const wpi::util::array&, const wpi::util::array& + const Rotation2d&, wpi::units::meter_t, wpi::units::meter_t, const Pose2d&: + ? const Rotation2d&, wpi::units::meter_t, wpi::units::meter_t, const Pose2d&, const wpi::util::array&, const wpi::util::array& : ResetPosition: Update: diff --git a/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator3d.yml b/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator3d.yml index b63aca0ae..726e6a498 100644 --- a/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator3d.yml +++ b/subprojects/robotpy-wpimath/semiwrap/DifferentialDrivePoseEstimator3d.yml @@ -4,8 +4,8 @@ classes: methods: DifferentialDrivePoseEstimator3d: overloads: - DifferentialDriveKinematics&, const Rotation3d&, wpi::units::meter_t, wpi::units::meter_t, const Pose3d&: - ? DifferentialDriveKinematics&, const Rotation3d&, wpi::units::meter_t, wpi::units::meter_t, const Pose3d&, const wpi::util::array&, const wpi::util::array& + const Rotation3d&, wpi::units::meter_t, wpi::units::meter_t, const Pose3d&: + ? const Rotation3d&, wpi::units::meter_t, wpi::units::meter_t, const Pose3d&, const wpi::util::array&, const wpi::util::array& : ResetPosition: Update: diff --git a/subprojects/robotpy-wpimath/semiwrap/TrajectoryGenerator.yml b/subprojects/robotpy-wpimath/semiwrap/DrivetrainSplineTrajectoryGenerator.yml similarity index 91% rename from subprojects/robotpy-wpimath/semiwrap/TrajectoryGenerator.yml rename to subprojects/robotpy-wpimath/semiwrap/DrivetrainSplineTrajectoryGenerator.yml index df9cdd0af..ac9333fef 100644 --- a/subprojects/robotpy-wpimath/semiwrap/TrajectoryGenerator.yml +++ b/subprojects/robotpy-wpimath/semiwrap/DrivetrainSplineTrajectoryGenerator.yml @@ -3,12 +3,12 @@ extra_includes: - wpi/math/spline/QuinticHermiteSpline.hpp classes: - wpi::math::TrajectoryGenerator: + wpi::math::DrivetrainSplineTrajectoryGenerator: force_type_casters: - wpi::units::unit_t - wpi::units::curvature_t methods: - GenerateTrajectory: + Generate: overloads: Spline<3>::ControlVector, const std::vector&, Spline<3>::ControlVector, const TrajectoryConfig&: const Pose2d&, const std::vector&, const Pose2d&, const TrajectoryConfig&: diff --git a/subprojects/robotpy-wpimath/semiwrap/TrajectoryParameterizer.yml b/subprojects/robotpy-wpimath/semiwrap/DrivetrainSplineTrajectoryParameterizer.yml similarity index 57% rename from subprojects/robotpy-wpimath/semiwrap/TrajectoryParameterizer.yml rename to subprojects/robotpy-wpimath/semiwrap/DrivetrainSplineTrajectoryParameterizer.yml index ccacb6634..6ef50b193 100644 --- a/subprojects/robotpy-wpimath/semiwrap/TrajectoryParameterizer.yml +++ b/subprojects/robotpy-wpimath/semiwrap/DrivetrainSplineTrajectoryParameterizer.yml @@ -1,7 +1,7 @@ classes: - wpi::math::TrajectoryParameterizer: + wpi::math::DrivetrainSplineTrajectoryParameterizer: force_type_casters: - wpi::units::unit_t - wpi::units::curvature_t methods: - TimeParameterizeTrajectory: + Parameterize: diff --git a/subprojects/robotpy-wpimath/semiwrap/ExponentialProfile.yml b/subprojects/robotpy-wpimath/semiwrap/ExponentialProfile.yml index 83fded75b..e64a9cb39 100644 --- a/subprojects/robotpy-wpimath/semiwrap/ExponentialProfile.yml +++ b/subprojects/robotpy-wpimath/semiwrap/ExponentialProfile.yml @@ -53,7 +53,7 @@ classes: IsFinished: templates: - ExponentialProfileMeterVolts: + ExponentialProfile: qualname: wpi::math::ExponentialProfile params: - wpi::units::meter diff --git a/subprojects/robotpy-wpimath/semiwrap/Odometry.yml b/subprojects/robotpy-wpimath/semiwrap/Odometry.yml index 959eb2e52..1da16591a 100644 --- a/subprojects/robotpy-wpimath/semiwrap/Odometry.yml +++ b/subprojects/robotpy-wpimath/semiwrap/Odometry.yml @@ -1,16 +1,19 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveKinematics.hpp - wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp +- wpi/math/kinematics/MecanumDriveKinematics.hpp - wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelVelocities.hpp -- wpi/math/kinematics/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp +- wpi/math/kinematics/SwerveModuleAcceleration.hpp classes: wpi::math::Odometry: template_params: + - Kinematics - WheelPositions - WheelVelocities - WheelAccelerations @@ -27,36 +30,42 @@ templates: DifferentialDriveOdometryBase: qualname: wpi::math::Odometry params: + - wpi::math::DifferentialDriveKinematics - wpi::math::DifferentialDriveWheelPositions - wpi::math::DifferentialDriveWheelVelocities - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveOdometryBase: qualname: wpi::math::Odometry params: + - wpi::math::MecanumDriveKinematics - wpi::math::MecanumDriveWheelPositions - wpi::math::MecanumDriveWheelVelocities - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2OdometryBase: qualname: wpi::math::Odometry params: + - wpi::math::SwerveDriveKinematics<2> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive3OdometryBase: qualname: wpi::math::Odometry params: + - wpi::math::SwerveDriveKinematics<3> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive4OdometryBase: qualname: wpi::math::Odometry params: + - wpi::math::SwerveDriveKinematics<4> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive6OdometryBase: qualname: wpi::math::Odometry params: + - wpi::math::SwerveDriveKinematics<6> - wpi::util::array - wpi::util::array - wpi::util::array diff --git a/subprojects/robotpy-wpimath/semiwrap/Odometry3d.yml b/subprojects/robotpy-wpimath/semiwrap/Odometry3d.yml index c1e822bb2..ee35f64d1 100644 --- a/subprojects/robotpy-wpimath/semiwrap/Odometry3d.yml +++ b/subprojects/robotpy-wpimath/semiwrap/Odometry3d.yml @@ -1,16 +1,19 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveKinematics.hpp - wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp +- wpi/math/kinematics/MecanumDriveKinematics.hpp - wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelVelocities.hpp -- wpi/math/kinematics/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp +- wpi/math/kinematics/SwerveModuleAcceleration.hpp classes: wpi::math::Odometry3d: template_params: + - Kinematics - WheelPositions - WheelVelocities - WheelAccelerations @@ -28,36 +31,42 @@ templates: DifferentialDriveOdometry3dBase: qualname: wpi::math::Odometry3d params: + - wpi::math::DifferentialDriveKinematics - wpi::math::DifferentialDriveWheelPositions - wpi::math::DifferentialDriveWheelVelocities - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveOdometry3dBase: qualname: wpi::math::Odometry3d params: + - wpi::math::MecanumDriveKinematics - wpi::math::MecanumDriveWheelPositions - wpi::math::MecanumDriveWheelVelocities - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2Odometry3dBase: qualname: wpi::math::Odometry3d params: + - wpi::math::SwerveDriveKinematics<2> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive3Odometry3dBase: qualname: wpi::math::Odometry3d params: + - wpi::math::SwerveDriveKinematics<3> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive4Odometry3dBase: qualname: wpi::math::Odometry3d params: + - wpi::math::SwerveDriveKinematics<4> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive6Odometry3dBase: qualname: wpi::math::Odometry3d params: + - wpi::math::SwerveDriveKinematics<6> - wpi::util::array - wpi::util::array - wpi::util::array diff --git a/subprojects/robotpy-wpimath/semiwrap/PoseEstimator.yml b/subprojects/robotpy-wpimath/semiwrap/PoseEstimator.yml index 5080bce18..7551fb243 100644 --- a/subprojects/robotpy-wpimath/semiwrap/PoseEstimator.yml +++ b/subprojects/robotpy-wpimath/semiwrap/PoseEstimator.yml @@ -1,17 +1,20 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveKinematics.hpp - wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp +- wpi/math/kinematics/MecanumDriveKinematics.hpp - wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelVelocities.hpp -- wpi/math/kinematics/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp +- wpi/math/kinematics/SwerveModuleAcceleration.hpp classes: wpi::math::PoseEstimator: template_params: + - Kinematics - WheelPositions - WheelVelocities - WheelAccelerations @@ -35,36 +38,42 @@ templates: DifferentialDrivePoseEstimatorBase: qualname: wpi::math::PoseEstimator params: + - wpi::math::DifferentialDriveKinematics - wpi::math::DifferentialDriveWheelPositions - wpi::math::DifferentialDriveWheelVelocities - wpi::math::DifferentialDriveWheelAccelerations MecanumDrivePoseEstimatorBase: qualname: wpi::math::PoseEstimator params: + - wpi::math::MecanumDriveKinematics - wpi::math::MecanumDriveWheelPositions - wpi::math::MecanumDriveWheelVelocities - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: + - wpi::math::SwerveDriveKinematics<2> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive3PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: + - wpi::math::SwerveDriveKinematics<3> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive4PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: + - wpi::math::SwerveDriveKinematics<4> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive6PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: + - wpi::math::SwerveDriveKinematics<6> - wpi::util::array - wpi::util::array - wpi::util::array diff --git a/subprojects/robotpy-wpimath/semiwrap/PoseEstimator3d.yml b/subprojects/robotpy-wpimath/semiwrap/PoseEstimator3d.yml index 260872bd7..aaa3484fb 100644 --- a/subprojects/robotpy-wpimath/semiwrap/PoseEstimator3d.yml +++ b/subprojects/robotpy-wpimath/semiwrap/PoseEstimator3d.yml @@ -1,16 +1,19 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveKinematics.hpp - wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp +- wpi/math/kinematics/MecanumDriveKinematics.hpp - wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelVelocities.hpp -- wpi/math/kinematics/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp +- wpi/math/kinematics/SwerveModuleAcceleration.hpp classes: wpi::math::PoseEstimator3d: template_params: + - Kinematics - WheelPositions - WheelVelocities - WheelAccelerations @@ -35,36 +38,42 @@ templates: DifferentialDrivePoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: + - wpi::math::DifferentialDriveKinematics - wpi::math::DifferentialDriveWheelPositions - wpi::math::DifferentialDriveWheelVelocities - wpi::math::DifferentialDriveWheelAccelerations MecanumDrivePoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: + - wpi::math::MecanumDriveKinematics - wpi::math::MecanumDriveWheelPositions - wpi::math::MecanumDriveWheelVelocities - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: + - wpi::math::SwerveDriveKinematics<2> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive3PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: + - wpi::math::SwerveDriveKinematics<3> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive4PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: + - wpi::math::SwerveDriveKinematics<4> - wpi::util::array - wpi::util::array - wpi::util::array SwerveDrive6PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: + - wpi::math::SwerveDriveKinematics<6> - wpi::util::array - wpi::util::array - wpi::util::array diff --git a/subprojects/robotpy-wpimath/semiwrap/Rotation3d.yml b/subprojects/robotpy-wpimath/semiwrap/Rotation3d.yml index dcffe2027..f8678c3fb 100644 --- a/subprojects/robotpy-wpimath/semiwrap/Rotation3d.yml +++ b/subprojects/robotpy-wpimath/semiwrap/Rotation3d.yml @@ -9,6 +9,9 @@ functions: ignore: true classes: wpi::math::Rotation3d: + force_type_casters: + - wpi::units::radians_per_second + - wpi::units::second_t methods: Rotation3d: overloads: @@ -32,6 +35,7 @@ classes: operator==: RotateBy: Interpolate: + Integrate: GetQuaternion: X: Y: diff --git a/subprojects/robotpy-wpimath/semiwrap/TrapezoidProfile.yml b/subprojects/robotpy-wpimath/semiwrap/TrapezoidProfile.yml index 26cfbdb0d..efc419bb8 100644 --- a/subprojects/robotpy-wpimath/semiwrap/TrapezoidProfile.yml +++ b/subprojects/robotpy-wpimath/semiwrap/TrapezoidProfile.yml @@ -90,6 +90,13 @@ classes: methods: operator==: cpp_code: py::self == State() + wpi::math::TrapezoidProfile::ProfileTiming: + attributes: + t_1: + t_2: + t_3: + methods: + operator==: templates: TrapezoidProfile: qualname: wpi::math::TrapezoidProfile diff --git a/subprojects/robotpy-wpimath/tests/geometry/test_rotation3d.py b/subprojects/robotpy-wpimath/tests/geometry/test_rotation3d.py index ec952d558..1bf42ac77 100644 --- a/subprojects/robotpy-wpimath/tests/geometry/test_rotation3d.py +++ b/subprojects/robotpy-wpimath/tests/geometry/test_rotation3d.py @@ -249,6 +249,29 @@ def test_rotate_by_non_zero_z(): assert expected == rot +def test_integrate(): + rot = Rotation3d(0, 0, math.radians(90)) + + integrated1 = rot.integrate(0, 0, math.radians(20), 1) + expected1 = Rotation3d(0, 0, math.radians(110)) + assert expected1 == integrated1 + + integrated2 = rot.integrate(0, math.radians(20), 0, 1) + expected2 = Rotation3d(0, math.radians(20), math.radians(90)) + assert expected2 == integrated2 + + integrated3 = rot.integrate(0, math.radians(20), math.radians(20), 1) + expected3 = Rotation3d( + Quaternion( + 0.5635121137168105, + -0.12216409746525868, + 0.1221640974652587, + 0.8078403086473278, + ) + ) + assert expected3 == integrated3 + + def test_axis_angle(): x_axis = np.array([1.0, 0.0, 0.0]) y_axis = np.array([0.0, 1.0, 0.0]) diff --git a/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry.py b/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry.py index 6ade2530e..3f1075b85 100644 --- a/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry.py +++ b/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry.py @@ -3,12 +3,12 @@ import random from wpimath import ( + DrivetrainSplineTrajectoryGenerator, MecanumDriveKinematics, MecanumDriveOdometry, MecanumDriveWheelPositions, Pose2d, Rotation2d, - TrajectoryGenerator, TrajectoryConfig, Translation2d, ) @@ -109,7 +109,7 @@ def test_accuracy_facing_trajectory(): odometry = MecanumDriveOdometry(kinematics, Rotation2d(), wheel_positions) - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d.from_degrees(45)), Pose2d(x=3, y=0, rotation=Rotation2d.from_degrees(-90)), @@ -176,7 +176,7 @@ def test_accuracy_facing_x_axis(): odometry = MecanumDriveOdometry(kinematics, Rotation2d(), wheel_positions) - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d.from_degrees(45)), Pose2d(x=3, y=0, rotation=Rotation2d.from_degrees(-90)), diff --git a/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry3d.py b/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry3d.py index dd4a8b723..65f033ac7 100644 --- a/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry3d.py +++ b/subprojects/robotpy-wpimath/tests/kinematics/test_mecanum_drive_odometry3d.py @@ -4,6 +4,7 @@ from wpimath import ( ChassisVelocities, + DrivetrainSplineTrajectoryGenerator, MecanumDriveKinematics, MecanumDriveOdometry3d, MecanumDriveWheelPositions, @@ -11,7 +12,6 @@ Pose3d, Rotation2d, Rotation3d, - TrajectoryGenerator, TrajectoryConfig, Translation2d, ) @@ -137,7 +137,7 @@ def test_accuracy_facing_trajectory(): odometry = MecanumDriveOdometry3d(kinematics, Rotation3d(), wheel_positions) - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d.from_degrees(45)), Pose2d(x=3, y=0, rotation=Rotation2d.from_degrees(-90)), @@ -208,7 +208,7 @@ def test_accuracy_facing_x_axis(): odometry = MecanumDriveOdometry3d(kinematics, Rotation3d(), wheel_positions) - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d(45)), Pose2d(x=3, y=0, rotation=Rotation2d(-90)), diff --git a/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry.py b/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry.py index 6e04bfb83..96d6db3ae 100644 --- a/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry.py +++ b/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry.py @@ -4,13 +4,13 @@ from wpimath import ( ChassisVelocities, + DrivetrainSplineTrajectoryGenerator, Pose2d, Rotation2d, SwerveDrive4Kinematics, SwerveDrive4Odometry, SwerveModuleVelocity, SwerveModulePosition, - TrajectoryGenerator, TrajectoryConfig, Translation2d, ) @@ -134,7 +134,7 @@ def test_accuracy_facing_trajectory(): bl = SwerveModulePosition() br = SwerveModulePosition() - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d.from_degrees(45)), Pose2d(x=3, y=0, rotation=Rotation2d.from_degrees(-90)), @@ -207,7 +207,7 @@ def test_accuracy_facing_x_axis(): bl = SwerveModulePosition() br = SwerveModulePosition() - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d.from_degrees(45)), Pose2d(x=3, y=0, rotation=Rotation2d.from_degrees(-90)), diff --git a/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry3d.py b/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry3d.py index d5ba1deeb..26cd0e0db 100644 --- a/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry3d.py +++ b/subprojects/robotpy-wpimath/tests/kinematics/test_swerve_drive_odometry3d.py @@ -4,6 +4,7 @@ from wpimath import ( ChassisVelocities, + DrivetrainSplineTrajectoryGenerator, Pose3d, Pose2d, Rotation2d, @@ -11,7 +12,6 @@ SwerveDrive4Kinematics, SwerveDrive4Odometry3d, SwerveModulePosition, - TrajectoryGenerator, TrajectoryConfig, Translation2d, ) @@ -166,7 +166,7 @@ def test_accuracy_facing_x_axis(): bl = SwerveModulePosition() br = SwerveModulePosition() - trajectory = TrajectoryGenerator.generate_trajectory( + trajectory = DrivetrainSplineTrajectoryGenerator.generate( [ Pose2d(x=0, y=0, rotation=Rotation2d.from_degrees(45)), Pose2d(x=3, y=0, rotation=Rotation2d.from_degrees(-90)), diff --git a/subprojects/robotpy-wpimath/tests/test_trajectory.py b/subprojects/robotpy-wpimath/tests/test_trajectory.py index 4740ceada..08e6a38a9 100644 --- a/subprojects/robotpy-wpimath/tests/test_trajectory.py +++ b/subprojects/robotpy-wpimath/tests/test_trajectory.py @@ -3,6 +3,8 @@ from wpimath import ( CubicHermiteSpline, DrivetrainSplineTrajectory, + DrivetrainSplineTrajectoryGenerator, + DrivetrainSplineTrajectoryParameterizer, Ellipse2d, EllipticalRegionConstraint, MaxVelocityConstraint, @@ -13,8 +15,6 @@ SplineHelper, TrajectoryConfig, TrajectoryConstraint, - TrajectoryGenerator, - TrajectoryParameterizer, Transform2d, Translation2d, ) @@ -39,11 +39,35 @@ def get_test_trajectory(config: TrajectoryConfig) -> DrivetrainSplineTrajectory: ).translation(), ] - return TrajectoryGenerator.generate_trajectory( + return DrivetrainSplineTrajectoryGenerator.generate( side_start, vector, cross_scale, config ) +# +# DrivetrainSplineTrajectoryParameterizer +# + + +def test_drivetrain_spline_trajectory_parameterizer(): + start = Pose2d(1, 1, 0) + end = Pose2d(2, 2, math.pi / 2) + + # generate the spline from start and end poses + vec1, vec2 = SplineHelper.cubic_control_vectors_from_waypoints(start, [], end) + spline = CubicHermiteSpline(vec1.x, vec2.x, vec1.y, vec2.y) + + # sample the pose and curvature along the spline + points: list[tuple[Pose2d, float]] = [] + for i in range(100): + points.append(spline.get_point(i / 100)) + + trajectory = DrivetrainSplineTrajectoryParameterizer.parameterize( + points, [], 0, 0, 4, 3, False + ) + assert trajectory is not None + + # # EllipticalRegionConstraint tests # @@ -120,27 +144,3 @@ def test_trajectory_constraint_min_max(): repr(min_max) == "TrajectoryConstraint.MinMax(min_acceleration=0.0, max_acceleration=1.0)" ) - - -# -# TrajectoryParameterizer -# - - -def test_trajectory_parameterizer(): - start = Pose2d(1, 1, 0) - end = Pose2d(2, 2, math.pi / 2) - - # generate the spline from start and end poses - vec1, vec2 = SplineHelper.cubic_control_vectors_from_waypoints(start, [], end) - spline = CubicHermiteSpline(vec1.x, vec2.x, vec1.y, vec2.y) - - # sample the pose and curvature along the spline - points: list[tuple[Pose2d, float]] = [] - for i in range(100): - points.append(spline.get_point(i / 100)) - - trajectory = TrajectoryParameterizer.time_parameterize_trajectory( - points, [], 0, 0, 4, 3, False - ) - assert trajectory is not None diff --git a/subprojects/robotpy-wpimath/tests/test_trajectory_exponential_profile.py b/subprojects/robotpy-wpimath/tests/test_trajectory_exponential_profile.py index cf4e4ceaa..f87e1d594 100644 --- a/subprojects/robotpy-wpimath/tests/test_trajectory_exponential_profile.py +++ b/subprojects/robotpy-wpimath/tests/test_trajectory_exponential_profile.py @@ -4,13 +4,11 @@ import pytest -from wpimath import ExponentialProfileMeterVolts, SimpleMotorFeedforwardMeters +from wpimath import ExponentialProfile, SimpleMotorFeedforwardMeters kDt = 0.01 feedforward = SimpleMotorFeedforwardMeters(0, 2.5629, 0.43277, kDt) -constraints = ExponentialProfileMeterVolts.Constraints.from_characteristics( - 12, 2.5629, 0.43277 -) +constraints = ExponentialProfile.Constraints.from_characteristics(12, 2.5629, 0.43277) def assert_near(val1, val2, eps): @@ -25,9 +23,9 @@ def assert_near_state(val1, val2, eps): def check_dynamics( - profile: ExponentialProfileMeterVolts, - current: ExponentialProfileMeterVolts.State, - goal: ExponentialProfileMeterVolts.State, + profile: ExponentialProfile, + current: ExponentialProfile.State, + goal: ExponentialProfile.State, ): next_state = profile.calculate(kDt, current, goal) @@ -40,12 +38,12 @@ def check_dynamics( @pytest.fixture def profile(): - return ExponentialProfileMeterVolts(constraints) + return ExponentialProfile(constraints) def test_reaches_goal(profile): - goal = ExponentialProfileMeterVolts.State(10, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(10, 0) + state = ExponentialProfile.State(0, 0) for _ in range(450): state = check_dynamics(profile, state, goal) @@ -54,13 +52,13 @@ def test_reaches_goal(profile): def test_pos_continuous_under_vel_change(profile): - goal = ExponentialProfileMeterVolts.State(10, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(10, 0) + state = ExponentialProfile.State(0, 0) for i in range(300): if i == 150: - profile = ExponentialProfileMeterVolts( - ExponentialProfileMeterVolts.Constraints.from_state_space( + profile = ExponentialProfile( + ExponentialProfile.Constraints.from_state_space( 9, constraints.a, constraints.b ) ) @@ -71,13 +69,13 @@ def test_pos_continuous_under_vel_change(profile): def test_pos_continuous_under_vel_change_backward(profile): - goal = ExponentialProfileMeterVolts.State(-10, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(-10, 0) + state = ExponentialProfile.State(0, 0) for i in range(300): if i == 150: - profile = ExponentialProfileMeterVolts( - ExponentialProfileMeterVolts.Constraints.from_state_space( + profile = ExponentialProfile( + ExponentialProfile.Constraints.from_state_space( 9, constraints.a, constraints.b ) ) @@ -88,8 +86,8 @@ def test_pos_continuous_under_vel_change_backward(profile): def test_backwards(profile): - goal = ExponentialProfileMeterVolts.State(-10, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(-10, 0) + state = ExponentialProfile.State(0, 0) for _ in range(400): state = check_dynamics(profile, state, goal) @@ -98,15 +96,15 @@ def test_backwards(profile): def test_switch_goal_in_middle(profile): - goal = ExponentialProfileMeterVolts.State(-10, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(-10, 0) + state = ExponentialProfile.State(0, 0) for _ in range(50): state = check_dynamics(profile, state, goal) assert state != goal - goal = ExponentialProfileMeterVolts.State(0.0, 0.0) + goal = ExponentialProfile.State(0.0, 0.0) for _ in range(100): state = check_dynamics(profile, state, goal) @@ -114,8 +112,8 @@ def test_switch_goal_in_middle(profile): def test_top_velocity(profile): - goal = ExponentialProfileMeterVolts.State(40, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(40, 0) + state = ExponentialProfile.State(0, 0) max_velocity = 0 for _ in range(900): @@ -127,8 +125,8 @@ def test_top_velocity(profile): def test_top_velocity_backward(profile): - goal = ExponentialProfileMeterVolts.State(-40, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(-40, 0) + state = ExponentialProfile.State(0, 0) max_velocity = 0 for _ in range(900): @@ -140,8 +138,8 @@ def test_top_velocity_backward(profile): def test_large_initial_velocity(profile): - goal = ExponentialProfileMeterVolts.State(40, 0) - state = ExponentialProfileMeterVolts.State(0, 8) + goal = ExponentialProfile.State(40, 0) + state = ExponentialProfile.State(0, 8) for _ in range(900): state = check_dynamics(profile, state, goal) @@ -150,8 +148,8 @@ def test_large_initial_velocity(profile): def test_large_negative_initial_velocity(profile): - goal = ExponentialProfileMeterVolts.State(-40, 0) - state = ExponentialProfileMeterVolts.State(0, -8) + goal = ExponentialProfile.State(-40, 0) + state = ExponentialProfile.State(0, -8) for _ in range(900): state = check_dynamics(profile, state, goal) @@ -169,104 +167,104 @@ def __init__(self, initial, goal, inflection_point): def test_heuristic(profile): test_cases = [ ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(0.75, -4), - ExponentialProfileMeterVolts.State(1.3758, 4.4304), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(0.75, -4), + ExponentialProfile.State(1.3758, 4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(1.4103, 4), - ExponentialProfileMeterVolts.State(1.3758, 4.4304), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(1.4103, 4), + ExponentialProfile.State(1.3758, 4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(0.75, -4), - ExponentialProfileMeterVolts.State(1.3758, 4.4304), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(0.75, -4), + ExponentialProfile.State(1.3758, 4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(1.4103, 4), - ExponentialProfileMeterVolts.State(1.3758, 4.4304), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(1.4103, 4), + ExponentialProfile.State(1.3758, 4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(0.5, -2), - ExponentialProfileMeterVolts.State(0.4367, 3.7217), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(0.5, -2), + ExponentialProfile.State(0.4367, 3.7217), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(0.546, 2), - ExponentialProfileMeterVolts.State(0.4367, 3.7217), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(0.546, 2), + ExponentialProfile.State(0.4367, 3.7217), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(0.5, -2), - ExponentialProfileMeterVolts.State(0.5560, -2.9616), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(0.5, -2), + ExponentialProfile.State(0.5560, -2.9616), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(0.546, 2), - ExponentialProfileMeterVolts.State(0.5560, -2.9616), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(0.546, 2), + ExponentialProfile.State(0.5560, -2.9616), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(-0.75, -4), - ExponentialProfileMeterVolts.State(-0.7156, -4.4304), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(-0.75, -4), + ExponentialProfile.State(-0.7156, -4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(-0.0897, 4), - ExponentialProfileMeterVolts.State(-0.7156, -4.4304), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(-0.0897, 4), + ExponentialProfile.State(-0.7156, -4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(-0.75, -4), - ExponentialProfileMeterVolts.State(-0.7156, -4.4304), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(-0.75, -4), + ExponentialProfile.State(-0.7156, -4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(-0.0897, 4), - ExponentialProfileMeterVolts.State(-0.7156, -4.4304), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(-0.0897, 4), + ExponentialProfile.State(-0.7156, -4.4304), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(-0.5, -4.5), - ExponentialProfileMeterVolts.State(1.095, 4.314), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(-0.5, -4.5), + ExponentialProfile.State(1.095, 4.314), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -4), - ExponentialProfileMeterVolts.State(1.0795, 4.5), - ExponentialProfileMeterVolts.State(-0.5122, -4.351), + ExponentialProfile.State(0.0, -4), + ExponentialProfile.State(1.0795, 4.5), + ExponentialProfile.State(-0.5122, -4.351), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(-0.5, -4.5), - ExponentialProfileMeterVolts.State(1.095, 4.314), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(-0.5, -4.5), + ExponentialProfile.State(1.095, 4.314), ), ETestCase( - ExponentialProfileMeterVolts.State(0.6603, 4), - ExponentialProfileMeterVolts.State(1.0795, 4.5), - ExponentialProfileMeterVolts.State(-0.5122, -4.351), + ExponentialProfile.State(0.6603, 4), + ExponentialProfile.State(1.0795, 4.5), + ExponentialProfile.State(-0.5122, -4.351), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -8), - ExponentialProfileMeterVolts.State(0, 0), - ExponentialProfileMeterVolts.State(-0.1384, 3.342), + ExponentialProfile.State(0.0, -8), + ExponentialProfile.State(0, 0), + ExponentialProfile.State(-0.1384, 3.342), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, -8), - ExponentialProfileMeterVolts.State(-1, 0), - ExponentialProfileMeterVolts.State(-0.562, -6.792), + ExponentialProfile.State(0.0, -8), + ExponentialProfile.State(-1, 0), + ExponentialProfile.State(-0.562, -6.792), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, 8), - ExponentialProfileMeterVolts.State(1, 0), - ExponentialProfileMeterVolts.State(0.562, 6.792), + ExponentialProfile.State(0.0, 8), + ExponentialProfile.State(1, 0), + ExponentialProfile.State(0.562, 6.792), ), ETestCase( - ExponentialProfileMeterVolts.State(0.0, 8), - ExponentialProfileMeterVolts.State(-1, 0), - ExponentialProfileMeterVolts.State(-0.785, -4.346), + ExponentialProfile.State(0.0, 8), + ExponentialProfile.State(-1, 0), + ExponentialProfile.State(-0.785, -4.346), ), ] @@ -276,8 +274,8 @@ def test_heuristic(profile): def test_timing_to_current(profile): - goal = ExponentialProfileMeterVolts.State(2, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(2, 0) + state = ExponentialProfile.State(0, 0) for _ in range(400): state = check_dynamics(profile, state, goal) @@ -285,8 +283,8 @@ def test_timing_to_current(profile): def test_timing_to_goal(profile): - goal = ExponentialProfileMeterVolts.State(2, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(2, 0) + state = ExponentialProfile.State(0, 0) predicted_time_left = profile.time_left_until(state, goal) reached_goal = False @@ -302,8 +300,8 @@ def test_timing_to_goal(profile): def test_timing_to_negative_goal(profile): - goal = ExponentialProfileMeterVolts.State(-2, 0) - state = ExponentialProfileMeterVolts.State(0, 0) + goal = ExponentialProfile.State(-2, 0) + state = ExponentialProfile.State(0, 0) predicted_time_left = profile.time_left_until(state, goal) reached_goal = False diff --git a/subprojects/robotpy-wpimath/tests/test_trapezoid_profile.py b/subprojects/robotpy-wpimath/tests/test_trapezoid_profile.py deleted file mode 100644 index 90c726f69..000000000 --- a/subprojects/robotpy-wpimath/tests/test_trapezoid_profile.py +++ /dev/null @@ -1,24 +0,0 @@ -import pytest - -import wpimath - -trapezoid_profile_types = [ - wpimath.TrapezoidProfile, - wpimath.TrapezoidProfileRadians, -] - - -@pytest.mark.parametrize("TrapezoidProfile", trapezoid_profile_types) -def test_constraints_repr(TrapezoidProfile): - expected_qualname = f"{TrapezoidProfile.__name__}.Constraints" - constraints = TrapezoidProfile.Constraints() - - assert repr(constraints).startswith(f"{expected_qualname}(max_velocity=0.") - - -@pytest.mark.parametrize("TrapezoidProfile", trapezoid_profile_types) -def test_state_repr(TrapezoidProfile): - expected_qualname = f"{TrapezoidProfile.__name__}.State" - constraints = TrapezoidProfile.State() - - assert repr(constraints).startswith(f"{expected_qualname}(position=0.") diff --git a/subprojects/robotpy-wpimath/tests/trajectory/test_trapezoid_profile.py b/subprojects/robotpy-wpimath/tests/trajectory/test_trapezoid_profile.py new file mode 100644 index 000000000..5b8e6982d --- /dev/null +++ b/subprojects/robotpy-wpimath/tests/trajectory/test_trapezoid_profile.py @@ -0,0 +1,312 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. + + +import math + +from wpimath import TrapezoidProfile + +DT = 0.01 # 10 ms + + +def assert_less_than_or_close(val_1, val_2, eps): + if val_1 <= val_2: + assert val_1 <= val_2 + else: + assert math.isclose(val_1, val_2, abs_tol=eps) + + +def assert_feasible(initial, final, max_accel): + delta_x = final.position - initial.position + delta_v = final.velocity - initial.velocity + max_pos_change = abs(initial.velocity) * DT + max_accel / 2.0 * DT * DT + max_vel_change = max_accel * DT + assert_less_than_or_close(abs(delta_x), max_pos_change, 1e-10) + assert_less_than_or_close(abs(delta_v), max_vel_change, 1e-10) + + +def test_timing(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(12.0, -1.0) + state = TrapezoidProfile.State(0.0, 1.0) + + profile = TrapezoidProfile(constraints) + profile.calculate(DT, state, goal) + profile_time = profile.duration() + + assert math.isclose(profile_time, 9.952380952380953, abs_tol=1e-10) + assert math.isclose( + profile_time, profile.time_left_until(state, goal), abs_tol=1e-10 + ) + profile.time_left_until(goal, goal) + assert math.isclose(profile_time, profile.duration(), abs_tol=1e-10) + + +def test_reaches_goal(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(3.0, 0.0) + state = TrapezoidProfile.State() + + profile = TrapezoidProfile(constraints) + for i in range(450): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert state == goal + + +def test_backwards(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(-2.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for i in range(400): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert state == goal + + +# Test the forwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_same_sign_as_peak(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(12.0, 0.0) + state = TrapezoidProfile.State(0.0, 3.0) + profile = TrapezoidProfile(constraints) + + plateau_count = 0 + # Profile lasts about 7.5s. + for i in range(1000): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + if new_state.velocity == constraints.max_velocity: + plateau_count += 1 + state = new_state + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateau_count > 5 + + assert state == goal + + +# Test the backwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_same_sign_as_peak_backwards(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(-12.0, 0.0) + state = TrapezoidProfile.State(0.0, -3.0) + profile = TrapezoidProfile(constraints) + + plateau_count = 0 + # Profile lasts about 7.5s. + for i in range(1000): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + if new_state.velocity == -constraints.max_velocity: + plateau_count += 1 + state = new_state + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateau_count > 5 + + assert state == goal + + +# Test the forwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_opposite_peak(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(12.0, 0.0) + state = TrapezoidProfile.State(0.0, -3.0) + profile = TrapezoidProfile(constraints) + + plateau_count = 0 + for i in range(1700): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + if new_state.velocity == constraints.max_velocity: + plateau_count += 1 + state = new_state + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateau_count > 5 + + assert state == goal + + +# Test the backwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_opposite_peak_backwards(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(-12.0, 0.0) + state = TrapezoidProfile.State(0.0, 3.0) + profile = TrapezoidProfile(constraints) + + plateau_count = 0 + for i in range(1700): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + if new_state.velocity == -constraints.max_velocity: + plateau_count += 1 + state = new_state + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateau_count > 5 + + assert state == goal + + +def test_sign_at_threshold(): + constraints = TrapezoidProfile.Constraints(4.0, 4.0) + goal = TrapezoidProfile.State(1, 1.0) + state = TrapezoidProfile.State(0.0, 3.0) + profile = TrapezoidProfile(constraints) + + # Normal profile is 0.5s, and an incorrect implementation might repeat. + for i in range(52): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + + # The "chattering" failure mode won't reach the goal. + assert state == goal + + +def test_sign_at_threshold_backwards(): + constraints = TrapezoidProfile.Constraints(4.0, 4.0) + goal = TrapezoidProfile.State(-1, -1.0) + state = TrapezoidProfile.State(0.0, -3.0) + profile = TrapezoidProfile(constraints) + + # Normal profile is 0.5s, and an incorrect implementation might repeat. + for i in range(52): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + + # The "chattering" failure mode won't reach the goal. + assert state == goal + + +# This is primary case that is broken in the old impl. +def test_large_velocity_and_small_position_delta(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(0.01, 0.0) + state = TrapezoidProfile.State(0.0, 1.0) + profile = TrapezoidProfile(constraints) + + for i in range(450): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + + assert state == goal + + +def test_large_velocity_and_small_position_delta_backwards(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(-0.01, 0.0) + state = TrapezoidProfile.State(0.0, -2.0) + profile = TrapezoidProfile(constraints) + + for i in range(700): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + + assert state == goal + + +def test_switch_goal_in_middle(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(-2.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for _ in range(200): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert state != goal + + goal = TrapezoidProfile.State(0.0, 0.0) + profile = TrapezoidProfile(constraints) + for _ in range(550): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert state == goal + + +def test_timing_to_current(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(2.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for _ in range(400): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert math.isclose(profile.time_left_until(state, state), 0.0, abs_tol=0.02) + + +def test_timing_to_goal(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(2.0, 0.0) + state = TrapezoidProfile.State(0.0, 0.0) + profile = TrapezoidProfile(constraints) + + predicted_time_left = profile.time_left_until(state, goal) + + reached_goal = False + for i in range(400): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + if not reached_goal and state == goal: + assert math.isclose(predicted_time_left, i * DT, abs_tol=0.25) + reached_goal = True + + +def test_timing_to_negative_goal(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(-2.0, 0.0) + state = TrapezoidProfile.State(0.0, 0.0) + profile = TrapezoidProfile(constraints) + + predicted_time_left = profile.time_left_until(state, goal) + + reached_goal = False + for i in range(400): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + if not reached_goal and state == goal: + assert math.isclose(predicted_time_left, i * DT, abs_tol=0.25) + reached_goal = True + + +def test_goal_velocity_constraints(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(10.0, 5.0) + state = TrapezoidProfile.State(0.0, 0.75) + profile = TrapezoidProfile(constraints) + + for i in range(1400): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert abs(state.velocity) <= constraints.max_velocity + + +def test_negative_goal_velocity_constraints(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(10.0, -5.0) + state = TrapezoidProfile.State(0.0, 0.75) + profile = TrapezoidProfile(constraints) + + for i in range(1600): + new_state = profile.calculate(DT, state, goal) + assert_feasible(state, new_state, constraints.max_acceleration) + state = new_state + assert abs(state.velocity) <= constraints.max_velocity diff --git a/subprojects/robotpy-wpimath/tests/trajectory/test_trapezoidal_profile.py b/subprojects/robotpy-wpimath/tests/trajectory/test_trapezoidal_profile.py deleted file mode 100644 index eba1b1242..000000000 --- a/subprojects/robotpy-wpimath/tests/trajectory/test_trapezoidal_profile.py +++ /dev/null @@ -1,176 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. - - -import math - -from wpimath import TrapezoidProfile - -kDt = 0.01 # 10 ms - - -def test_reaches_goal(): - constraints = TrapezoidProfile.Constraints(1.75, 0.75) - goal = TrapezoidProfile.State(3.0, 0.0) - state = TrapezoidProfile.State() - - profile = TrapezoidProfile(constraints) - for _ in range(450): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_pos_continuous_under_vel_change(): - constraints = TrapezoidProfile.Constraints(1.75, 0.75) - goal = TrapezoidProfile.State(12.0, 0.0) - profile = TrapezoidProfile(constraints) - state = profile.calculate(kDt, TrapezoidProfile.State(), goal) - - last_pos = state.position - for i in range(1600): - if i == 400: - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, state, goal) - estimated_vel = (state.position - last_pos) / kDt - - if i >= 400: - if estimated_vel <= constraints.max_velocity: - assert estimated_vel <= constraints.max_velocity - else: - assert math.isclose( - estimated_vel, constraints.max_velocity, abs_tol=1e-4 - ) - assert state.velocity <= constraints.max_velocity - - last_pos = state.position - - assert state == goal - - -def test_backwards(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(400): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_switch_goal_in_middle(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(200): - state = profile.calculate(kDt, state, goal) - assert state != goal - - goal = TrapezoidProfile.State(0.0, 0.0) - profile = TrapezoidProfile(constraints) - for _ in range(550): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_top_velocity(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(4.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(200): - state = profile.calculate(kDt, state, goal) - assert math.isclose(constraints.max_velocity, state.velocity, abs_tol=1e-4) - - profile = TrapezoidProfile(constraints) - for _ in range(2000): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_timing_to_current(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(2.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(400): - state = profile.calculate(kDt, state, goal) - assert math.isclose(profile.time_left_until(state.position), 0.0, abs_tol=0.02) - - -def test_timing_to_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.time_left_until(goal.position) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and state == goal: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25) - reached_goal = True - - -def test_timing_before_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.time_left_until(1.0) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and abs(state.velocity - 1.0) < 1e-4: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.02) - reached_goal = True - - -def test_timing_to_negative_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.time_left_until(goal.position) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and state == goal: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25) - reached_goal = True - - -def test_timing_before_negative_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.time_left_until(-1.0) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and abs(state.velocity + 1.0) < 1e-4: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.02) - reached_goal = True - - -def test_initialization_of_current_state(): - constraints = TrapezoidProfile.Constraints(1.0, 1.0) - profile = TrapezoidProfile(constraints) - assert math.isclose(profile.time_left_until(0.0), 0.0, abs_tol=1e-10) - assert math.isclose(profile.duration(), 0.0, abs_tol=1e-10) diff --git a/subprojects/robotpy-wpimath/wpimath/__init__.py b/subprojects/robotpy-wpimath/wpimath/__init__.py index 7f7ae00a6..348247aa3 100644 --- a/subprojects/robotpy-wpimath/wpimath/__init__.py +++ b/subprojects/robotpy-wpimath/wpimath/__init__.py @@ -41,11 +41,13 @@ DrivetrainSplineSample, DrivetrainSplineTrajectory, DrivetrainSplineTrajectoryBase, + DrivetrainSplineTrajectoryGenerator, + DrivetrainSplineTrajectoryParameterizer, EdgeCounterFilter, ElevatorFeedforward, Ellipse2d, EllipticalRegionConstraint, - ExponentialProfileMeterVolts, + ExponentialProfile, ExtendedKalmanFilter_1_1_1, ExtendedKalmanFilter_2_1_1, ExtendedKalmanFilter_2_1_2, @@ -183,8 +185,6 @@ TimeInterpolatableTranslation3dBuffer, TrajectoryConfig, TrajectoryConstraint, - TrajectoryGenerator, - TrajectoryParameterizer, TrajectorySample, Transform2d, Transform3d, @@ -245,11 +245,13 @@ "DrivetrainSplineSample", "DrivetrainSplineTrajectory", "DrivetrainSplineTrajectoryBase", + "DrivetrainSplineTrajectoryGenerator", + "DrivetrainSplineTrajectoryParameterizer", "EdgeCounterFilter", "ElevatorFeedforward", "Ellipse2d", "EllipticalRegionConstraint", - "ExponentialProfileMeterVolts", + "ExponentialProfile", "ExtendedKalmanFilter_1_1_1", "ExtendedKalmanFilter_2_1_1", "ExtendedKalmanFilter_2_1_2", @@ -387,8 +389,6 @@ "TimeInterpolatableTranslation3dBuffer", "TrajectoryConfig", "TrajectoryConstraint", - "TrajectoryGenerator", - "TrajectoryParameterizer", "TrajectorySample", "Transform2d", "Transform3d", diff --git a/subprojects/robotpy-xrp/pyproject.toml b/subprojects/robotpy-xrp/pyproject.toml index 538e48697..46cec0f9f 100644 --- a/subprojects/robotpy-xrp/pyproject.toml +++ b/subprojects/robotpy-xrp/pyproject.toml @@ -38,7 +38,7 @@ version_file = "xrp/version.py" artifact_id = "halsim_xrp" group_id = "org.wpilib.halsim" repo_url = "https://frcmaven.wpi.edu/artifactory/development-2027" -version = "2027.0.0-alpha-6-167-gdb3a9c428" +version = "2027.0.0-alpha-6-218-g3fa405550" use_headers = false extract_to = "xrp/extension"