Skip to content

Commit 9c7ed12

Browse files
author
NewtonCrosby
committed
Adds MecanumControllerCommand to Commands2. #28 
1 parent 9854bb2 commit 9c7ed12

File tree

3 files changed

+380
-0
lines changed

3 files changed

+380
-0
lines changed

‎commands2/__init__.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
from .conditionalcommand import ConditionalCommand
2626
from .functionalcommand import FunctionalCommand
2727
from .instantcommand import InstantCommand
28+
from .mecanumcontrollercommand import MecanumControllerCommand
2829
from .notifiercommand import NotifierCommand
2930
from .parallelcommandgroup import ParallelCommandGroup
3031
from .paralleldeadlinegroup import ParallelDeadlineGroup
@@ -56,6 +57,7 @@
5657
"IllegalCommandUse",
5758
"InstantCommand",
5859
"InterruptionBehavior",
60+
"MecanumControllerCommand",
5961
"NotifierCommand",
6062
"ParallelCommandGroup",
6163
"ParallelDeadlineGroup",

‎commands2/mecanumcontrollercommand.py

Lines changed: 222 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,222 @@
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 Set, Callable, Union, List
6+
7+
from wpimath.controller import (
8+
HolonomicDriveController,
9+
PIDController,
10+
ProfiledPIDControllerRadians,
11+
SimpleMotorFeedforwardMeters,
12+
)
13+
from wpimath.geometry import Pose2d, Rotation2d
14+
from wpimath.kinematics import (
15+
ChassisSpeeds,
16+
MecanumDriveKinematics,
17+
MecanumDriveWheelSpeeds,
18+
)
19+
from wpimath.trajectory import Trajectory
20+
from wpilib import Timer
21+
22+
from .subsystem import Subsystem
23+
from .command import Command
24+
25+
26+
class MecanumControllerCommand(Command):
27+
"""
28+
A command that uses two PID controllers (PIDController) and a ProfiledPIDController
29+
(ProfiledPIDController) to follow a trajectory Trajectory with a mecanum drive.
30+
31+
The command handles trajectory-following, Velocity PID calculations, and feedforwards
32+
internally. This is intended to be a more-or-less "complete solution" that can be used by teams
33+
without a great deal of controls expertise.
34+
35+
Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
36+
functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
37+
and feedforward functionality, returning only the raw wheel speeds from the PID controllers.
38+
39+
The robot angle controller does not follow the angle given by the trajectory but rather goes
40+
to the angle given in the final state of the trajectory.
41+
"""
42+
43+
__FRONT_LEFT_INDEX = 0
44+
__REAR_LEFT_INDEX = 1
45+
__FRONT_RIGHT_INDEX = 2
46+
__REAR_RIGHT_INDEX = 3
47+
48+
def __init__(
49+
self,
50+
trajectory: Trajectory,
51+
pose: Callable[[], Pose2d],
52+
kinematics: MecanumDriveKinematics,
53+
xController: PIDController,
54+
yController: PIDController,
55+
thetaController: ProfiledPIDControllerRadians,
56+
# rotationSupplier: Callable[[], Rotation2d],
57+
maxWheelVelocityMetersPerSecond: float,
58+
outputConsumer: Callable[[Union[List[float], MecanumDriveWheelSpeeds]], None],
59+
*requirements: Subsystem,
60+
feedforward: SimpleMotorFeedforwardMeters | None = None,
61+
chassisWheelPIDControllers: List[PIDController] | None = None,
62+
currentWheelSpeedsSupplier: Callable[[], MecanumDriveWheelSpeeds] | None = None,
63+
):
64+
super().__init__()
65+
66+
self.trajectory: Trajectory = trajectory
67+
self.pose = pose
68+
self.kinematics = kinematics
69+
self.controller = HolonomicDriveController(
70+
xController, yController, thetaController
71+
)
72+
# self.rotationSupplier = rotationSupplier
73+
self.maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond
74+
self.outputConsumer = outputConsumer
75+
76+
# Member to track time of last loop execution
77+
self.prevTime = 0
78+
self.usePID = False
79+
80+
# Optional requirements checks. If any one of the feedforward, pidcontrollers, or wheelspeedsuppliers
81+
# are not None, then all should be non-null. Raise RuntimeError if they are. The List of PID controllers
82+
# should be equal to each corner of the robot being commanded.
83+
if (
84+
feedforward
85+
or chassisWheelPIDControllers
86+
or currentWheelSpeedsSupplier is not None
87+
):
88+
if (
89+
feedforward
90+
or chassisWheelPIDControllers
91+
or currentWheelSpeedsSupplier is None
92+
):
93+
raise RuntimeError(
94+
f"Failed to instantiate MecanumControllerCommand, one of the arguments passed in was None: \n \
95+
\t Feedforward: {feedforward} - chassisWheelPIDControllers: {chassisWheelPIDControllers} - wheelSpeedSupplier: {currentWheelSpeedsSupplier} "
96+
)
97+
98+
# check the length of the PID controllers
99+
if len(chassisWheelPIDControllers != 4):
100+
raise RuntimeError(
101+
f"Failed to instantiate MecanumControllerCommand, not enough PID controllers provided.\n \
102+
\t Required: 4 - Provided: {len(chassisWheelPIDControllers)}"
103+
)
104+
105+
self.frontLeftController = chassisWheelPIDControllers[
106+
self.__FRONT_LEFT_INDEX
107+
]
108+
self.rearLeftController = chassisWheelPIDControllers[self.__REAR_LEFT_INDEX]
109+
self.frontRightController = chassisWheelPIDControllers[
110+
self.__FRONT_RIGHT_INDEX
111+
]
112+
self.rearRightController = chassisWheelPIDControllers[
113+
self.__REAR_RIGHT_INDEX
114+
]
115+
116+
if currentWheelSpeedsSupplier is None:
117+
raise RuntimeError(
118+
f"Failed to instantiate MecanumControllerCommand, no WheelSpeedSupplierProvided."
119+
)
120+
121+
self.currentWheelSpeeds = currentWheelSpeedsSupplier
122+
123+
if feedforward is None:
124+
raise RuntimeError(
125+
f"Failed to instantiate MecanumControllerCommand, no SimpleMotorFeedforwardMeters supplied."
126+
)
127+
128+
self.feedforward: SimpleMotorFeedforwardMeters = feedforward
129+
130+
# All the optional requirements verify, set usePid flag to True
131+
self.usePID = True
132+
133+
# Set the requirements for the command
134+
self.addRequirements(*requirements)
135+
136+
self.timer = Timer()
137+
138+
def initialize(self):
139+
initialState: Trajectory.State = self.trajectory.sample(0)
140+
initialXVelocity = initialState.velocity * initialState.pose.rotation().cos()
141+
initialYVelocity = initialState.velocity * initialState.pose.rotation().sin()
142+
self.m_prevSpeeds = self.kinematics.toWheelSpeeds(
143+
ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)
144+
)
145+
self.timer.restart()
146+
self.prevTime = 0
147+
148+
def execute(self):
149+
curTime = self.timer.get()
150+
dt = curTime - self.prevTime
151+
desiredState: Trajectory.State = self.trajectory.sample(curTime)
152+
targetChassisSpeeds = self.controller.calculate(
153+
self.pose(),
154+
desiredState,
155+
desiredState.pose.rotation()
156+
# self.pose.get(), desiredState, self.rotationSupplier.get()
157+
)
158+
targetWheelSpeeds = self.kinematics.toWheelSpeeds(targetChassisSpeeds)
159+
targetWheelSpeeds.desaturate(self.maxWheelVelocityMetersPerSecond)
160+
frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft
161+
rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft
162+
frontRightSpeedSetpoint = targetWheelSpeeds.frontRight
163+
rearRightSpeedSetpoint = targetWheelSpeeds.rearRight
164+
165+
if not self.usePID:
166+
self.outputConsumer(
167+
MecanumDriveWheelSpeeds(
168+
frontLeftSpeedSetpoint,
169+
frontRightSpeedSetpoint,
170+
rearLeftSpeedSetpoint,
171+
rearRightSpeedSetpoint,
172+
)
173+
)
174+
else:
175+
frontLeftFeedforward = self.feedforward.calculate(
176+
frontLeftSpeedSetpoint,
177+
(frontLeftSpeedSetpoint - self.m_prevSpeeds.frontLeft) / dt,
178+
)
179+
rearLeftFeedforward = self.feedforward.calculate(
180+
rearLeftSpeedSetpoint,
181+
(rearLeftSpeedSetpoint - self.m_prevSpeeds.rearLeft) / dt,
182+
)
183+
frontRightFeedforward = self.feedforward.calculate(
184+
frontRightSpeedSetpoint,
185+
(frontRightSpeedSetpoint - self.m_prevSpeeds.frontRight) / dt,
186+
)
187+
rearRightFeedforward = self.feedforward.calculate(
188+
rearRightSpeedSetpoint,
189+
(rearRightSpeedSetpoint - self.m_prevSpeeds.rearRight) / dt,
190+
)
191+
frontLeftOutput = frontLeftFeedforward + self.frontLeftController.calculate(
192+
self.currentWheelSpeeds().frontLeft,
193+
frontLeftSpeedSetpoint,
194+
)
195+
rearLeftOutput = rearLeftFeedforward + self.rearLeftController.calculate(
196+
self.currentWheelSpeeds().rearLeft,
197+
rearLeftSpeedSetpoint,
198+
)
199+
frontRightOutput = (
200+
frontRightFeedforward
201+
+ self.frontRightController.calculate(
202+
self.currentWheelSpeeds().frontRight,
203+
frontRightSpeedSetpoint,
204+
)
205+
)
206+
rearRightOutput = rearRightFeedforward + self.rearRightController.calculate(
207+
self.currentWheelSpeeds().rearRight,
208+
rearRightSpeedSetpoint,
209+
)
210+
self.outputConsumer(
211+
frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput
212+
)
213+
214+
# Store current call information for next call
215+
self.prevTime = curTime
216+
self.m_prevSpeeds = targetWheelSpeeds
217+
218+
def end(self, interrupted):
219+
self.timer.stop()
220+
221+
def isFinished(self):
222+
return self.timer.hasElapsed(self.trajectory.totalTime())

‎tests/test_mecanumcontrollercommand.py

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,156 @@
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
6+
import math
7+
8+
import wpimath.controller as controller
9+
import wpimath.trajectory as trajectory
10+
import wpimath.geometry as geometry
11+
import wpimath.kinematics as kinematics
12+
from wpilib import Timer
13+
14+
from util import * # type: ignore
15+
16+
if TYPE_CHECKING:
17+
from .util import *
18+
19+
import pytest
20+
21+
import commands2
22+
23+
24+
class MecanumControllerCommandTestDataFixtures:
25+
def __init__(self):
26+
self.timer = Timer()
27+
self.angle: geometry.Rotation2d = geometry.Rotation2d(0)
28+
29+
# Track speeds and distances
30+
self.frontLeftSpeed = 0
31+
self.frontLeftDistance = 0
32+
self.rearLeftSpeed = 0
33+
self.rearLeftDistance = 0
34+
self.frontRightSpeed = 0
35+
self.frontRightDistance = 0
36+
self.rearRightSpeed = 0
37+
self.rearRightDistance = 0
38+
39+
# Profile Controller and constraints
40+
trapProfileConstraints: trajectory.TrapezoidProfileRadians.Constraints = (
41+
trajectory.TrapezoidProfileRadians.Constraints(3 * math.pi, math.pi)
42+
)
43+
self.rotationController: controller.ProfiledPIDControllerRadians = (
44+
controller.ProfiledPIDControllerRadians(
45+
1.0, 0.0, 0.0, trapProfileConstraints
46+
)
47+
)
48+
49+
# Chassis/Drivetrain constants
50+
self.kxTolerance = 1 / 12.0
51+
self.kyTolerance = 1 / 12.0
52+
self.kAngularTolerance = 1 / 12.0
53+
self.kWheelBase = 0.5
54+
self.kTrackWidth = 0.5
55+
56+
self.command_kinematics: kinematics.MecanumDriveKinematics = (
57+
kinematics.MecanumDriveKinematics(
58+
geometry.Translation2d(self.kWheelBase / 2, self.kTrackWidth / 2),
59+
geometry.Translation2d(self.kWheelBase / 2, -self.kTrackWidth / 2),
60+
geometry.Translation2d(self.kWheelBase / 2, self.kTrackWidth / 2),
61+
geometry.Translation2d(self.kWheelBase / 2, -self.kTrackWidth / 2),
62+
)
63+
)
64+
65+
self.command_odometry: kinematics.MecanumDriveOdometry = (
66+
kinematics.MecanumDriveOdometry(
67+
self.command_kinematics,
68+
geometry.Rotation2d(0),
69+
kinematics.MecanumDriveWheelPositions(),
70+
geometry.Pose2d(0, 0, geometry.Rotation2d(0)),
71+
)
72+
)
73+
74+
def setWheelSpeeds(self, wheelSpeeds: kinematics.MecanumDriveWheelSpeeds) -> None:
75+
self.frontLeftSpeed = wheelSpeeds.frontLeft
76+
self.rearLeftSpeed = wheelSpeeds.rearLeft
77+
self.frontRightSpeed = wheelSpeeds.frontRight
78+
self.rearRightSpeed = wheelSpeeds.rearRight
79+
80+
def getCurrentWheelDistances(self) -> kinematics.MecanumDriveWheelPositions:
81+
positions = kinematics.MecanumDriveWheelPositions()
82+
positions.frontLeft = self.frontLeftDistance
83+
positions.frontRight = self.frontRightDistance
84+
positions.rearLeft = self.rearLeftDistance
85+
positions.rearRight = self.rearRightDistance
86+
87+
return positions
88+
89+
def getRobotPose(self) -> geometry.Pose2d:
90+
self.command_odometry.update(self.angle, self.getCurrentWheelDistances())
91+
return self.command_odometry.getPose()
92+
93+
94+
@pytest.fixture()
95+
def get_mec_controller_data() -> MecanumControllerCommandTestDataFixtures:
96+
return MecanumControllerCommandTestDataFixtures()
97+
98+
99+
def test_mecanumControllerCommand(
100+
scheduler: commands2.CommandScheduler, get_mec_controller_data
101+
):
102+
with ManualSimTime() as sim:
103+
subsystem = commands2.Subsystem()
104+
waypoints: List[geometry.Pose2d] = []
105+
waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0)))
106+
waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3)))
107+
traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1)
108+
new_trajectory: trajectory.Trajectory = (
109+
trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config)
110+
)
111+
end_state = new_trajectory.sample(new_trajectory.totalTime())
112+
113+
fixture_data = get_mec_controller_data
114+
115+
mecContCommand = commands2.MecanumControllerCommand(
116+
new_trajectory,
117+
fixture_data.getRobotPose,
118+
fixture_data.command_kinematics,
119+
controller.PIDController(0.6, 0, 0),
120+
controller.PIDController(0.6, 0, 0),
121+
fixture_data.rotationController,
122+
8.8,
123+
fixture_data.setWheelSpeeds,
124+
subsystem,
125+
)
126+
127+
fixture_data.timer.restart()
128+
129+
mecContCommand.initialize()
130+
131+
while not mecContCommand.isFinished():
132+
mecContCommand.execute()
133+
fixture_data.angle = new_trajectory.sample(
134+
fixture_data.timer.get()
135+
).pose.rotation()
136+
137+
fixture_data.frontLeftDistance += fixture_data.frontLeftSpeed * 0.005
138+
fixture_data.rearLeftDistance += fixture_data.rearLeftSpeed * 0.005
139+
fixture_data.frontRightDistance += fixture_data.frontRightSpeed * 0.005
140+
fixture_data.rearRightDistance += fixture_data.rearRightSpeed * 0.005
141+
142+
sim.step(0.005)
143+
144+
fixture_data.timer.stop()
145+
mecContCommand.end(True)
146+
147+
assert end_state.pose.X() == pytest.approx(
148+
fixture_data.getRobotPose().X(), fixture_data.kxTolerance
149+
)
150+
assert end_state.pose.Y() == pytest.approx(
151+
fixture_data.getRobotPose().Y(), fixture_data.kyTolerance
152+
)
153+
assert end_state.pose.rotation().radians() == pytest.approx(
154+
fixture_data.getRobotPose().rotation().radians(),
155+
fixture_data.kAngularTolerance,
156+
)

0 commit comments

Comments
 (0)