Skip to content

Commit 0faf2c0

Browse files
author
NewtonCrosby
committed
Added RamseteCommand to Commands 2. #28 
1 parent 9854bb2 commit 0faf2c0

File tree

3 files changed

+326
-0
lines changed

3 files changed

+326
-0
lines changed

‎commands2/__init__.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
from .printcommand import PrintCommand
3535
from .proxycommand import ProxyCommand
3636
from .proxyschedulecommand import ProxyScheduleCommand
37+
from .ramsetecommand import RamseteCommand
3738
from .repeatcommand import RepeatCommand
3839
from .runcommand import RunCommand
3940
from .schedulecommand import ScheduleCommand
@@ -65,6 +66,7 @@
6566
"PrintCommand",
6667
"ProxyCommand",
6768
"ProxyScheduleCommand",
69+
"RamseteCommand",
6870
"RepeatCommand",
6971
"RunCommand",
7072
"ScheduleCommand",

‎commands2/ramsetecommand.py

Lines changed: 182 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,182 @@
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+
6+
from typing import Callable, Union
7+
from wpimath.controller import PIDController, RamseteController, SimpleMotorFeedforwardMeters
8+
from wpimath.geometry import Pose2d
9+
from wpimath.kinematics import ChassisSpeeds, DifferentialDriveKinematics, DifferentialDriveWheelSpeeds
10+
from wpimath.trajectory import Trajectory
11+
from wpiutil import SendableBuilder
12+
from wpilib import Timer
13+
14+
15+
from .command import Command
16+
from .subsystem import Subsystem
17+
18+
class RamseteCommand(Command):
19+
"""
20+
A command that uses a RAMSETE controller (RamseteController) to follow a trajectory
21+
(Trajectory) with a differential drive.
22+
23+
The command handles trajectory-following, PID calculations, and feedforwards internally. This
24+
is intended to be a more-or-less "complete solution" that can be used by teams without a great
25+
deal of controls expertise.
26+
27+
Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
28+
functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
29+
and feedforward functionality, returning only the raw wheel speeds from the RAMSETE controller.
30+
31+
This class is provided by the NewCommands VendorDep
32+
"""
33+
34+
def __init__(
35+
self,
36+
trajectory: Trajectory,
37+
pose: Callable[[], Pose2d],
38+
controller: RamseteController,
39+
kinematics: DifferentialDriveKinematics,
40+
outputVolts: Callable[[float, float], None],
41+
*requirements: Subsystem,
42+
feedforward: SimpleMotorFeedforwardMeters | None = None,
43+
leftController: PIDController | None = None,
44+
rightController: PIDController | None = None,
45+
wheelSpeeds: Callable[[], DifferentialDriveWheelSpeeds] | None = None,
46+
):
47+
"""Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID
48+
control and feedforward are handled internally, and outputs are scaled -12 to 12 representing
49+
units of volts.
50+
51+
Note: The controller will *not* set the outputVolts to zero upon completion of the path -
52+
this is left to the user, since it is not appropriate for paths with nonstationary endstates.
53+
54+
:param trajectory: The trajectory to follow.
55+
:param pose: A function that supplies the robot pose - use one of the odometry classes to
56+
provide this.
57+
:param controller: The RAMSETE controller used to follow the trajectory.
58+
:param kinematics: The kinematics for the robot drivetrain.
59+
:param outputVolts: A function that consumes the computed left and right outputs (in volts) for
60+
the robot drive.
61+
:param requirements: The subsystems to require.
62+
63+
OPTIONAL PARAMETERS
64+
When the following optional parameters are provided, when executed, the RamseteCommand will follow
65+
provided trajectory. PID control and feedforward are handled internally, and the outputs are scaled
66+
from -12 to 12 representing units of volts. If any one of the optional parameters are provided, each
67+
other optional parameter becomes required.
68+
:param feedforward The feedforward to use for the drive
69+
:param leftController: The PIDController for the left side of the robot drive.
70+
:param rightController: The PIDController for the right side of the robot drive.
71+
:param wheelSpeeds: A function that supplies the speeds of the left and right sides of the robot
72+
drive.
73+
"""
74+
super().__init__()
75+
76+
self.timer = Timer()
77+
78+
# Store all the requirements that are required
79+
self.trajectory: Trajectory = trajectory
80+
self.pose = pose
81+
self.follower: RamseteController = controller
82+
self.kinematics = kinematics
83+
self.output: Callable[[float, float], None] = outputVolts
84+
self.leftController = None
85+
self.rightController = None
86+
self.feedforward = None
87+
self.wheelspeeds = None
88+
self.usePID = False
89+
# Optional requirements checks. If any one of the optionl parameters are not None, then all the optional
90+
# requirements become required.
91+
if feedforward or leftController or rightController or wheelSpeeds is not None:
92+
if feedforward or leftController or rightController or wheelSpeeds is None:
93+
raise RuntimeError(f'Failed to instantiate RamseteCommand, not all optional arguments were provided.\n \
94+
Feedforward - {feedforward}, LeftController - {leftController}, RightController - {rightController}, WheelSpeeds - {wheelSpeeds} '
95+
)
96+
97+
98+
self.leftController: PIDController = leftController
99+
self.rightController: PIDController = rightController
100+
self.wheelspeeds: Callable[[], DifferentialDriveWheelSpeeds] = wheelSpeeds
101+
self.feedforward: SimpleMotorFeedforwardMeters = feedforward
102+
self.usePID = True
103+
self.prevSpeeds = DifferentialDriveWheelSpeeds()
104+
self.prevTime = -1.0
105+
106+
self.addRequirements(*requirements)
107+
108+
def initialize(self):
109+
self.prevTime = -1
110+
initial_state = self.trajectory.sample(0)
111+
initial_speeds = self.kinematics.toWheelSpeeds(
112+
ChassisSpeeds( initial_state.velocity, 0, initial_state.curvature * initial_state.velocity )
113+
)
114+
self.prevSpeeds = initial_speeds
115+
self.timer.restart()
116+
if self.usePID:
117+
self.leftController.reset()
118+
self.rightController.reset()
119+
120+
def execute(self):
121+
cur_time = self.timer.get()
122+
dt = cur_time - self.prevTime
123+
124+
if self.prevTime < 0:
125+
self.output(0.0, 0.0)
126+
self.prevTime = cur_time
127+
return
128+
129+
target_wheel_speeds = self.kinematics.toWheelSpeeds(
130+
self.follower.calculate(self.pose(), self.trajectory.sample(cur_time))
131+
)
132+
133+
left_speed_setpoint = target_wheel_speeds.left
134+
right_speed_setpoint = target_wheel_speeds.right
135+
136+
if self.usePID:
137+
left_feedforward = self.feedforward.calculate(
138+
left_speed_setpoint,
139+
(left_speed_setpoint - self.prevSpeeds.left) / dt
140+
)
141+
142+
right_feedforward = self.feedforward.calculate(
143+
right_speed_setpoint,
144+
(right_speed_setpoint - self.prevSpeeds.right) / dt
145+
)
146+
147+
left_output = (
148+
left_feedforward
149+
+ self.leftController.calculate(
150+
self.wheelspeeds().left,
151+
left_speed_setpoint
152+
)
153+
)
154+
155+
right_output = (
156+
right_feedforward
157+
+ self.rightController.calculate(
158+
self.wheelspeeds().right,
159+
right_speed_setpoint
160+
)
161+
)
162+
else:
163+
left_output = left_speed_setpoint
164+
right_output = right_speed_setpoint
165+
166+
self.output(left_output, right_output)
167+
self.prevSpeeds = target_wheel_speeds
168+
self.prevTime = cur_time
169+
170+
def end(self, interrupted):
171+
self.timer.stop()
172+
173+
if interrupted:
174+
self.output(0.0, 0.0)
175+
176+
def isFinished(self):
177+
return self.timer.hasElapsed(self.trajectory.totalTime())
178+
179+
def initSendable(self, builder: SendableBuilder):
180+
super().initSendable(builder)
181+
builder.addDoubleProperty("leftVelocity", lambda: self.prevSpeeds.left, None)
182+
builder.addDoubleProperty("rightVelocity", lambda: self.prevSpeeds.right, None)

‎tests/test_ramsetecommand.py

Lines changed: 142 additions & 0 deletions
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
6+
import math
7+
8+
import wpimath.controller as controller
9+
import wpimath.trajectory as trajectory
10+
import wpimath.trajectory.constraint as constraints
11+
import wpimath.geometry as geometry
12+
import wpimath.kinematics as kinematics
13+
from wpilib import Timer
14+
15+
from util import * # type: ignore
16+
17+
if TYPE_CHECKING:
18+
from .util import *
19+
20+
import pytest
21+
22+
import commands2
23+
24+
25+
class RamseteCommandTestDataFixtures:
26+
def __init__(self):
27+
self.timer = Timer()
28+
self.angle: geometry.Rotation2d = geometry.Rotation2d(0)
29+
30+
# Track speeds and distances
31+
self.leftSpeed = 0
32+
self.leftDistance = 0
33+
self.rightSpeed = 0
34+
self.rightDistance = 0
35+
36+
# Chassis/Drivetrain constants
37+
self.kxTolerance = 1 / 12.0
38+
self.kyTolerance = 1 / 12.0
39+
self.kWheelBase = 0.5
40+
self.kTrackWidth = 0.5
41+
self.kWheelDiameterMeters = 0.1524
42+
self.kRamseteB = 2.0
43+
self.kRamseteZeta = 0.7
44+
self.ksVolts = 0.22
45+
self.kvVoltSecondsPerMeter = 1.98
46+
self.kaVoltSecondsSquaredPerMeter = 0.2
47+
self.kPDriveVel = 8.5
48+
self.kMaxMetersPerSecond = 3.0
49+
self.kMaxAccelerationMetersPerSecondSquared = 1.0
50+
self.kEncoderCPR = 1024
51+
self.kEncoderDistancePerPulse = (self.kWheelDiameterMeters * math.pi) / self.kEncoderCPR
52+
53+
self.command_kinematics: kinematics.DifferentialDriveKinematics = (
54+
kinematics.DifferentialDriveKinematics(self.kTrackWidth)
55+
)
56+
57+
self.command_voltage_constraint: constraints.DifferentialDriveVoltageConstraint = constraints.DifferentialDriveVoltageConstraint(
58+
controller.SimpleMotorFeedforwardMeters(
59+
self.ksVolts,
60+
self.kvVoltSecondsPerMeter,
61+
self.kaVoltSecondsSquaredPerMeter
62+
),
63+
self.command_kinematics,
64+
10
65+
)
66+
67+
self.command_odometry: kinematics.DifferentialDriveOdometry = kinematics.DifferentialDriveOdometry(
68+
self.angle,
69+
self.leftDistance,
70+
self.rightDistance,
71+
geometry.Pose2d(0, 0, geometry.Rotation2d(0))
72+
)
73+
74+
def setWheelSpeeds(self, leftspeed: float, rightspeed: float) -> None:
75+
self.leftSpeed = leftspeed
76+
self.rightSpeed = rightspeed
77+
78+
def getCurrentWheelDistances(self) -> kinematics.DifferentialDriveWheelPositions:
79+
positions = kinematics.DifferentialDriveWheelPositions()
80+
positions.right = self.rightDistance
81+
positions.left = self.leftDistance
82+
83+
return positions
84+
85+
def getRobotPose(self) -> geometry.Pose2d:
86+
positions = self.getCurrentWheelDistances()
87+
self.command_odometry.update(self.angle, positions.left,positions.right)
88+
return self.command_odometry.getPose()
89+
90+
91+
@pytest.fixture()
92+
def get_ramsete_command_data() -> RamseteCommandTestDataFixtures:
93+
return RamseteCommandTestDataFixtures()
94+
95+
96+
def test_ramseteCommand(
97+
scheduler: commands2.CommandScheduler, get_ramsete_command_data
98+
):
99+
with ManualSimTime() as sim:
100+
fixture_data = get_ramsete_command_data
101+
subsystem = commands2.Subsystem()
102+
waypoints: List[geometry.Pose2d] = []
103+
waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0)))
104+
waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0)))
105+
traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1)
106+
traj_config.setKinematics(fixture_data.command_kinematics)
107+
traj_config.addConstraint(fixture_data.command_voltage_constraint)
108+
new_trajectory: trajectory.Trajectory = (
109+
trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config)
110+
)
111+
end_state = new_trajectory.sample(new_trajectory.totalTime())
112+
113+
command = commands2.RamseteCommand(
114+
new_trajectory,
115+
fixture_data.getRobotPose,
116+
controller.RamseteController(fixture_data.kRamseteB, fixture_data.kRamseteZeta),
117+
fixture_data.command_kinematics,
118+
fixture_data.setWheelSpeeds,
119+
subsystem
120+
)
121+
122+
fixture_data.timer.restart()
123+
124+
command.initialize()
125+
126+
while not command.isFinished():
127+
command.execute()
128+
129+
fixture_data.leftDistance += fixture_data.leftSpeed * 0.005
130+
fixture_data.rightDistance += fixture_data.rightSpeed * 0.005
131+
132+
sim.step(0.005)
133+
134+
fixture_data.timer.stop()
135+
command.end(True)
136+
137+
assert end_state.pose.X() == pytest.approx(
138+
fixture_data.getRobotPose().X(), fixture_data.kxTolerance
139+
)
140+
assert end_state.pose.Y() == pytest.approx(
141+
fixture_data.getRobotPose().Y(), fixture_data.kyTolerance
142+
)

0 commit comments

Comments
 (0)