Skip to content

Commit 27a3071

Browse files
TheTripleVlospugsvirtuald
authored
Adds basic SwerveControllerCommand (#45)
* Adds Swerve2/3/4/6ControllerCommand to Commands2 * Fixes for mypy errors * Fixes broken test run for incomplete pid * Experimenting with mypy assignment error * Ignoring mypy type error with explicit annotation * Re-formatted with black * Added Typing.overload to constructor definitions, forced kwargs for optional contructor arguments. * Removed superfluous comment * Updated implementation to use Overtake and Beartype. * Fixed test fixtures to supply tuple to odometry update * Fixed tests for odometry update calls to send Tuples * Fix argument * Bug fix: Added addRequirements call to contstructors * Pinned versions of experimental libraries * Add Simple SwerveControllerCommands * black --------- Co-authored-by: NewtonCrosby <lospugs@> Co-authored-by: lospugs <[email protected]> Co-authored-by: Dustin Spicuzza <[email protected]>
1 parent a0c3926 commit 27a3071

File tree

4 files changed

+257
-1
lines changed

4 files changed

+257
-1
lines changed

‎commands2/__init__.py

+2
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
from .sequentialcommandgroup import SequentialCommandGroup
4343
from .startendcommand import StartEndCommand
4444
from .subsystem import Subsystem
45+
from .swervecontrollercommand import SwerveControllerCommand
4546
from .timedcommandrobot import TimedCommandRobot
4647
from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
4748
from .waitcommand import WaitCommand
@@ -77,6 +78,7 @@
7778
"SequentialCommandGroup",
7879
"StartEndCommand",
7980
"Subsystem",
81+
"SwerveControllerCommand",
8082
"TimedCommandRobot",
8183
"TrapezoidProfileSubsystem",
8284
"WaitCommand",

‎commands2/swervecontrollercommand.py

+109
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
# Copyright (c) FIRST and other WPILib contributors.
2+
# Open Source Software; you can modify and/or share it under the terms of
3+
# the WPILib BSD license file in the root directory of this project.
4+
from __future__ import annotations
5+
from typing import Callable, Optional, Union, Tuple, Sequence
6+
from typing_extensions import overload
7+
from wpimath.controller import (
8+
HolonomicDriveController,
9+
)
10+
from wpimath.geometry import Pose2d, Rotation2d
11+
from wpimath.kinematics import (
12+
SwerveDrive2Kinematics,
13+
SwerveDrive3Kinematics,
14+
SwerveDrive4Kinematics,
15+
SwerveDrive6Kinematics,
16+
SwerveModuleState,
17+
)
18+
from wpimath.trajectory import Trajectory
19+
from wpilib import Timer
20+
21+
from .command import Command
22+
from .subsystem import Subsystem
23+
24+
25+
class SwerveControllerCommand(Command):
26+
"""
27+
A command that uses two PID controllers (PIDController) and a ProfiledPIDController
28+
(ProfiledPIDController) to follow a trajectory (Trajectory) with a swerve drive.
29+
30+
This command outputs the raw desired Swerve Module States (SwerveModuleState) in an
31+
array. The desired wheel and module rotation velocities should be taken from those and used in
32+
velocity PIDs.
33+
34+
The robot angle controller does not follow the angle given by the trajectory but rather goes
35+
to the angle given in the final state of the trajectory.
36+
"""
37+
38+
def __init__(
39+
self,
40+
trajectory: Trajectory,
41+
pose: Callable[[], Pose2d],
42+
kinematics: Union[
43+
SwerveDrive2Kinematics,
44+
SwerveDrive3Kinematics,
45+
SwerveDrive4Kinematics,
46+
SwerveDrive6Kinematics,
47+
],
48+
controller: HolonomicDriveController,
49+
outputModuleStates: Callable[[Sequence[SwerveModuleState]], None],
50+
requirements: Tuple[Subsystem],
51+
desiredRotation: Optional[Callable[[], Rotation2d]] = None,
52+
) -> None:
53+
"""
54+
Constructs a new SwerveControllerCommand that when executed will follow the
55+
provided trajectory. This command will not return output voltages but
56+
rather raw module states from the position controllers which need to be put
57+
into a velocity PID.
58+
59+
Note: The controllers will *not* set the outputVolts to zero upon
60+
completion of the path- this is left to the user, since it is not
61+
appropriate for paths with nonstationary endstates.
62+
63+
:param trajectory: The trajectory to follow.
64+
:param pose: A function that supplies the robot pose - use one of the odometry classes to
65+
provide this.
66+
:param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6
67+
SwerveKinematics.
68+
:param controller: The HolonomicDriveController for the drivetrain.
69+
If you have x, y, and theta controllers, pass them into
70+
HolonomicPIDController.
71+
:param outputModuleStates: The raw output module states from the position controllers.
72+
:param requirements: The subsystems to require.
73+
:param desiredRotation: (optional) The angle that the drivetrain should be
74+
facing. This is sampled at each time step. If not specified, that rotation of
75+
the final pose in the trajectory is used.
76+
"""
77+
super().__init__()
78+
self._trajectory = trajectory
79+
self._pose = pose
80+
self._kinematics = kinematics
81+
self._outputModuleStates = outputModuleStates
82+
self._controller = controller
83+
if desiredRotation is None:
84+
self._desiredRotation = trajectory.states()[-1].pose.rotation
85+
else:
86+
self._desiredRotation = desiredRotation
87+
88+
self._timer = Timer()
89+
self.addRequirements(*requirements)
90+
91+
def initialize(self):
92+
self._timer.restart()
93+
94+
def execute(self):
95+
curTime = self._timer.get()
96+
desiredState = self._trajectory.sample(curTime)
97+
98+
targetChassisSpeeds = self._controller.calculate(
99+
self._pose(), desiredState, self._desiredRotation()
100+
)
101+
targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds)
102+
103+
self._outputModuleStates(targetModuleStates)
104+
105+
def end(self, interrupted):
106+
self._timer.stop()
107+
108+
def isFinished(self):
109+
return self._timer.hasElapsed(self._trajectory.totalTime())

‎setup.py

+4-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,10 @@
1111
description="WPILib command framework v2",
1212
url="https://github.com/robotpy/robotpy-commands-v2",
1313
packages=["commands2"],
14-
install_requires=["wpilib<2025,>=2024.0.0b2", "typing_extensions>=4.1.0,<5"],
14+
install_requires=[
15+
"wpilib<2025,>=2024.0.0b2",
16+
"typing_extensions>=4.1.0,<5",
17+
],
1518
license="BSD-3-Clause",
1619
python_requires=">=3.8",
1720
include_package_data=True,

‎tests/test_swervecontrollercommand.py

+142
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
# Copyright (c) FIRST and other WPILib contributors.
2+
# Open Source Software; you can modify and/or share it under the terms of
3+
# the WPILib BSD license file in the root directory of this project.
4+
5+
from typing import TYPE_CHECKING, List, Tuple
6+
import math
7+
8+
from wpilib import Timer
9+
10+
from wpimath.geometry import Pose2d, Rotation2d, Translation2d
11+
from wpimath.kinematics import (
12+
SwerveModuleState,
13+
SwerveModulePosition,
14+
SwerveDrive4Kinematics,
15+
SwerveDrive4Odometry,
16+
)
17+
from wpimath.controller import (
18+
ProfiledPIDControllerRadians,
19+
PIDController,
20+
HolonomicDriveController,
21+
)
22+
from wpimath.trajectory import (
23+
TrapezoidProfileRadians,
24+
Trajectory,
25+
TrajectoryConfig,
26+
TrajectoryGenerator,
27+
)
28+
29+
30+
from util import * # type: ignore
31+
32+
if TYPE_CHECKING:
33+
from .util import *
34+
35+
import pytest
36+
37+
import commands2
38+
39+
40+
def test_swervecontrollercommand():
41+
timer = Timer()
42+
angle = Rotation2d(0)
43+
44+
swerve_module_states = (
45+
SwerveModuleState(0, Rotation2d(0)),
46+
SwerveModuleState(0, Rotation2d(0)),
47+
SwerveModuleState(0, Rotation2d(0)),
48+
SwerveModuleState(0, Rotation2d(0)),
49+
)
50+
51+
swerve_module_positions = (
52+
SwerveModulePosition(0, Rotation2d(0)),
53+
SwerveModulePosition(0, Rotation2d(0)),
54+
SwerveModulePosition(0, Rotation2d(0)),
55+
SwerveModulePosition(0, Rotation2d(0)),
56+
)
57+
58+
rot_controller = ProfiledPIDControllerRadians(
59+
1,
60+
0,
61+
0,
62+
TrapezoidProfileRadians.Constraints(3 * math.pi, math.pi),
63+
)
64+
65+
x_tolerance = 1 / 12.0
66+
y_tolerance = 1 / 12.0
67+
angular_tolerance = 1 / 12.0
68+
69+
wheel_base = 0.5
70+
track_width = 0.5
71+
72+
kinematics = SwerveDrive4Kinematics(
73+
Translation2d(wheel_base / 2, track_width / 2),
74+
Translation2d(wheel_base / 2, -track_width / 2),
75+
Translation2d(-wheel_base / 2, track_width / 2),
76+
Translation2d(-wheel_base / 2, -track_width / 2),
77+
)
78+
79+
odometry = SwerveDrive4Odometry(
80+
kinematics,
81+
Rotation2d(0),
82+
swerve_module_positions,
83+
Pose2d(0, 0, Rotation2d(0)),
84+
)
85+
86+
def set_module_states(states):
87+
nonlocal swerve_module_states
88+
swerve_module_states = states
89+
90+
def get_robot_pose() -> Pose2d:
91+
odometry.update(angle, swerve_module_positions)
92+
return odometry.getPose()
93+
94+
with ManualSimTime() as sim:
95+
subsystem = commands2.Subsystem()
96+
waypoints: List[Pose2d] = []
97+
waypoints.append(Pose2d(0, 0, Rotation2d(0)))
98+
waypoints.append(Pose2d(1, 5, Rotation2d(3)))
99+
config = TrajectoryConfig(8.8, 0.1)
100+
trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config)
101+
102+
end_state = trajectory.sample(trajectory.totalTime())
103+
104+
command = commands2.SwerveControllerCommand(
105+
trajectory=trajectory,
106+
pose=get_robot_pose,
107+
kinematics=kinematics,
108+
controller=HolonomicDriveController(
109+
PIDController(0.6, 0, 0),
110+
PIDController(0.6, 0, 0),
111+
rot_controller,
112+
),
113+
outputModuleStates=set_module_states,
114+
requirements=(subsystem,),
115+
)
116+
117+
timer.restart()
118+
119+
command.initialize()
120+
while not command.isFinished():
121+
command.execute()
122+
angle = trajectory.sample(timer.get()).pose.rotation()
123+
124+
for i in range(0, len(swerve_module_positions)):
125+
swerve_module_positions[i].distance += (
126+
swerve_module_states[i].speed * 0.005
127+
)
128+
swerve_module_positions[i].angle = swerve_module_states[i].angle
129+
130+
sim.step(0.005)
131+
132+
timer.stop()
133+
command.end(True)
134+
135+
assert end_state.pose.X() == pytest.approx(get_robot_pose().X(), x_tolerance)
136+
137+
assert end_state.pose.Y() == pytest.approx(get_robot_pose().Y(), y_tolerance)
138+
139+
assert end_state.pose.rotation().radians() == pytest.approx(
140+
get_robot_pose().rotation().radians(),
141+
angular_tolerance,
142+
)

0 commit comments

Comments
 (0)