Adds TrapezoidProfileCommand and TrapezoidProfileCommandRadians to Commands2. robotpy/robbotpy-commands-v2#28

Author: NewtonCrosby

commands2/__init__.py

@@ -43,6 +43,7 @@
 from .startendcommand import StartEndCommand
 from .subsystem import Subsystem
 from .timedcommandrobot import TimedCommandRobot
+from .trapezoidprofilecommand import TrapezoidProfileCommand
 from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
 from .waitcommand import WaitCommand
 from .waituntilcommand import WaitUntilCommand
@@ -76,6 +77,7 @@
     "StartEndCommand",
     "Subsystem",
     "TimedCommandRobot",
+    "TrapezoidProfileCommand",
     "TrapezoidProfileSubsystem",
     "WaitCommand",
     "WaitUntilCommand",

commands2/trapezoidprofilecommand.py

@@ -0,0 +1,68 @@
+# 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.
+from __future__ import annotations
+
+from typing import TypeVar, Callable, Any, Generic
+
+from wpilib import Timer
+from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians
+
+from .command import Command
+from .subsystem import Subsystem
+
+# Defined two generic types for the Profile and ProfileState variables.
+# This allows an implementation for both dimensionless and Radians
+# instances of TrapezoidProfiles (or any future dimension as well)
+TP = TypeVar("TP")  # Generic[TrapezoidProfile]
+TS = TypeVar("TS")  # Generic[TrapezoidProfile.State]
+
+
+class TrapezoidProfileCommand(Command, Generic[TP, TS]):
+    """
+    A command that runs a :class:`.TrapezoidProfile`. Useful for smoothly controlling mechanism motion.
+
+    This class is provided by the NewCommands VendorDep
+    """
+
+    def __init__(
+        self,
+        profile: TP,
+        output: Callable[[TS], Any],
+        getGoal: Callable[[], TS],
+        getCurrent: Callable[[], TS],
+        *requirements: Subsystem,
+    ):
+        """Creates a new TrapezoidProfileCommand that will execute the given :class:`.TrapezoidProfile`.
+        Output will be piped to the provided consumer function.
+
+        :param profile:      The motion profile to execute.
+        :param output:       The consumer for the profile output.
+        :param getGoal:      The supplier for the desired state
+        :param getCurrent:   The supplier for the current state
+        :param requirements: The subsystems required by this command.
+        """
+        super().__init__()
+        self._profile = profile
+        self._output = output
+        self._getGoal = getGoal
+        self._getCurrent = getCurrent
+        self._timer = Timer()
+
+        self.addRequirements(*requirements)
+
+    def initialize(self) -> None:
+        self._timer.restart()
+
+    def execute(self) -> None:
+        self._output(
+            self._profile.calculate(  # type: ignore[attr-defined]
+                self._timer.get(), self._getGoal(), self._getCurrent()
+            )
+        )
+
+    def end(self, interrupted) -> None:
+        self._timer.stop()
+
+    def isFinished(self) -> bool:
+        return self._timer.hasElapsed(self._profile.totalTime())  # type: ignore[attr-defined]

tests/test_trapezoidprofilecommand.py

@@ -0,0 +1,146 @@
+# 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.
+
+from typing import TYPE_CHECKING, List, Tuple
+import math
+
+import wpimath.controller as controller
+import wpimath.trajectory as trajectory
+import wpimath.geometry as geometry
+import wpimath.kinematics as kinematics
+from wpimath.trajectory import TrapezoidProfile as DimensionlessProfile
+from wpimath.trajectory import TrapezoidProfileRadians as RadiansProfile
+
+from wpilib import Timer
+
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+import commands2
+
+
+class TrapezoidProfileRadiansFixture:
+    def __init__(self):
+        constraints: RadiansProfile.Constraints = RadiansProfile.Constraints(
+            3 * math.pi, math.pi
+        )
+        self._profile: RadiansProfile = RadiansProfile(constraints)
+        self._goal_state = RadiansProfile.State(3, 0)
+
+        self._state = self._profile.calculate(
+            0, self._goal_state, RadiansProfile.State(0, 0)
+        )
+
+        self._timer = Timer()
+
+    def profileOutput(self, state: RadiansProfile.State) -> None:
+        self._state = state
+
+    def currentState(self) -> RadiansProfile.State:
+        return self._state
+
+    def getGoal(self) -> RadiansProfile.State:
+        return self._goal_state
+
+
+@pytest.fixture()
+def get_trapezoid_profile_radians() -> TrapezoidProfileRadiansFixture:
+    return TrapezoidProfileRadiansFixture()
+
+
+class TrapezoidProfileFixture:
+    def __init__(self):
+        constraints: DimensionlessProfile.Constraints = (
+            DimensionlessProfile.Constraints(3 * math.pi, math.pi)
+        )
+        self._profile: DimensionlessProfile = DimensionlessProfile(constraints)
+        self._goal_state = DimensionlessProfile.State(3, 0)
+
+        self._state = self._profile.calculate(
+            0, self._goal_state, DimensionlessProfile.State(0, 0)
+        )
+
+        self._timer = Timer()
+
+    def profileOutput(self, state: DimensionlessProfile.State) -> None:
+        self._state = state
+
+    def currentState(self) -> DimensionlessProfile.State:
+        return self._state
+
+    def getGoal(self) -> DimensionlessProfile.State:
+        return self._goal_state
+
+
+@pytest.fixture()
+def get_trapezoid_profile_dimensionless() -> TrapezoidProfileFixture:
+    return TrapezoidProfileFixture()
+
+
+def test_trapezoidProfileDimensionless(
+    scheduler: commands2.CommandScheduler, get_trapezoid_profile_dimensionless
+):
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+
+        fixture_data = get_trapezoid_profile_dimensionless
+
+        command = commands2.TrapezoidProfileCommand[
+            DimensionlessProfile, DimensionlessProfile.State
+        ](
+            fixture_data._profile,
+            fixture_data.profileOutput,
+            fixture_data.getGoal,
+            fixture_data.currentState,
+            subsystem,
+        )
+
+        fixture_data._timer.restart()
+
+        command.initialize()
+
+        count = 0
+        while not command.isFinished():
+            command.execute()
+            count += 1
+            sim.step(0.005)
+
+        fixture_data._timer.stop()
+        command.end(True)
+
+
+def test_trapezoidProfileRadians(
+    scheduler: commands2.CommandScheduler, get_trapezoid_profile_radians
+):
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+
+        fixture_data = get_trapezoid_profile_radians
+
+        command = commands2.TrapezoidProfileCommand[
+            RadiansProfile, RadiansProfile.State
+        ](
+            fixture_data._profile,
+            fixture_data.profileOutput,
+            fixture_data.getGoal,
+            fixture_data.currentState,
+            subsystem,
+        )
+
+        fixture_data._timer.restart()
+
+        command.initialize()
+
+        count = 0
+        while not command.isFinished():
+            command.execute()
+            count += 1
+            sim.step(0.005)
+
+        fixture_data._timer.stop()
+        command.end(True)