Skip to content

Commit 24fea60

Browse files
megarubbermegarubber
authored
Add & adapt State-Space examples (#101)
#49 Added the ```StateSpaceArm``` and ```StateSpaceElevator``` examples from Java. I did some adaptations in ```StateSpaceFlywheel```, it's considering the porting guide.
1 parent ef0d1c0 commit 24fea60

File tree

6 files changed

+324
-41
lines changed

6 files changed

+324
-41
lines changed

‎StateSpaceArm/robot.py

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) FIRST and other WPILib contributors.
4+
# Open Source Software; you can modify and/or share it under the terms of
5+
# the WPILib BSD license file in the root directory of this project.
6+
#
7+
8+
import math
9+
import wpilib
10+
import wpimath.controller
11+
import wpimath.estimator
12+
import wpimath.units
13+
import wpimath.trajectory
14+
import wpimath.system
15+
import wpimath.system.plant
16+
17+
kMotorPort = 0
18+
kEncoderAChannel = 0
19+
kEncoderBChannel = 1
20+
kJoystickPort = 0
21+
kRaisedPosition = wpimath.units.feetToMeters(90.0)
22+
kLoweredPosition = wpimath.units.feetToMeters(0.0)
23+
24+
# Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant
25+
# is difficult, LinearSystem.identifyPositionSystem may be better.
26+
kArmMOI = 1.2
27+
28+
# Reduction between motors and encoder, as output over input. If the arm spins slower than
29+
# the motors, this number should be greater than one.
30+
kArmGearing = 10.0
31+
32+
33+
class MyRobot(wpilib.TimedRobot):
34+
"""This is a sample program to demonstrate how to use a state-space controller to control an arm."""
35+
36+
def robotInit(self) -> None:
37+
self.profile = wpimath.trajectory.TrapezoidProfile(
38+
wpimath.trajectory.TrapezoidProfile.Constraints(
39+
wpimath.units.feetToMeters(45),
40+
wpimath.units.feetToMeters(90), # Max elevator speed and acceleration.
41+
)
42+
)
43+
44+
self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State()
45+
46+
# The plant holds a state-space model of our elevator. This system has the following properties:
47+
48+
# States: [position, velocity], in meters and meters per second.
49+
# Inputs (what we can "put in"): [voltage], in volts.
50+
# Outputs (what we can measure): [position], in meters.
51+
self.armPlant = wpimath.system.plant.LinearSystemId.singleJointedArmSystem(
52+
wpimath.system.plant.DCMotor.NEO(2),
53+
kArmMOI,
54+
kArmGearing,
55+
)
56+
57+
# The observer fuses our encoder data and voltage inputs to reject noise.
58+
self.observer = wpimath.estimator.KalmanFilter_2_1_1(
59+
self.armPlant,
60+
[
61+
0.015,
62+
0.17,
63+
], # How accurate we think our model is, in radians and radians/sec.
64+
[
65+
0.01
66+
], # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
67+
0.020,
68+
)
69+
70+
# A LQR uses feedback to create voltage commands.
71+
self.controller = wpimath.controller.LinearQuadraticRegulator_2_1(
72+
self.armPlant,
73+
[
74+
wpimath.units.degreesToRadians(1.0),
75+
wpimath.units.degreesToRadians(10.0),
76+
], # qelms.
77+
# Position and velocity error tolerances, in radians and radians per second. Decrease
78+
# this
79+
# to more heavily penalize state excursion, or make the controller behave more
80+
# aggressively. In this example we weight position much more highly than velocity, but
81+
# this
82+
# can be tuned to balance the two.
83+
[12.0], # relms. Control effort (voltage) tolerance. Decrease this to more
84+
# heavily penalize control effort, or make the controller less aggressive. 12 is a good
85+
# starting point because that is the (approximate) maximum voltage of a battery.
86+
0.020, # Nominal time between loops. 0.020 for TimedRobot, but can be
87+
# lower if using notifiers.
88+
)
89+
90+
# The state-space loop combines a controller, observer, feedforward and plant for easy control.
91+
self.loop = wpimath.system.LinearSystemLoop_2_1_1(
92+
self.armPlant, self.controller, self.observer, 12.0, 0.020
93+
)
94+
95+
# An encoder set up to measure flywheel velocity in radians per second.
96+
self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel)
97+
98+
self.motor = wpilib.PWMSparkMax(kMotorPort)
99+
100+
# A joystick to read the trigger from.
101+
self.joystick = wpilib.Joystick(kJoystickPort)
102+
103+
# We go 2 pi radians in 1 rotation, or 4096 counts.
104+
self.encoder.setDistancePerPulse(math.tau / 4096)
105+
106+
def teleopInit(self) -> None:
107+
# Reset our loop to make sure it's in a known state.
108+
self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()])
109+
110+
# Reset our last reference to the current state.
111+
self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State(
112+
self.encoder.getDistance(), self.encoder.getRate()
113+
)
114+
115+
def teleopPeriodic(self) -> None:
116+
# Sets the target position of our arm. This is similar to setting the setpoint of a
117+
# PID controller.
118+
119+
goal = wpimath.trajectory.TrapezoidProfile.State()
120+
121+
if self.joystick.getTrigger():
122+
# the trigger is pressed, so we go to the high goal.
123+
goal = wpimath.trajectory.TrapezoidProfile.State(kRaisedPosition, 0.0)
124+
125+
else:
126+
# Otherwise, we go to the low goal
127+
goal = wpimath.trajectory.TrapezoidProfile.State(kLoweredPosition, 0.0)
128+
129+
# Step our TrapezoidalProfile forward 20ms and set it as our next reference
130+
self.lastProfiledReference = self.profile.calculate(
131+
0.020, self.lastProfiledReference, goal
132+
)
133+
self.loop.setNextR(
134+
[self.lastProfiledReference.position, self.lastProfiledReference.velocity]
135+
)
136+
137+
# Correct our Kalman filter's state vector estimate with encoder data.
138+
self.loop.correct([self.encoder.getDistance()])
139+
140+
# Update our LQR to generate new voltage commands and use the voltages to predict the next
141+
# state with out Kalman filter.
142+
self.loop.predict(0.020)
143+
144+
# Send the new calculated voltage to the motors.
145+
# voltage = duty cycle * battery voltage, so
146+
# duty cycle = voltage / battery voltage
147+
nextVoltage = self.loop.U(0)
148+
self.motor.setVoltage(nextVoltage)

‎StateSpaceElevator/robot.py

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright (c) FIRST and other WPILib contributors.
4+
# Open Source Software; you can modify and/or share it under the terms of
5+
# the WPILib BSD license file in the root directory of this project.
6+
#
7+
8+
import math
9+
import wpilib
10+
import wpimath.controller
11+
import wpimath.estimator
12+
import wpimath.units
13+
import wpimath.trajectory
14+
import wpimath.system
15+
import wpimath.system.plant
16+
17+
kMotorPort = 0
18+
kEncoderAChannel = 0
19+
kEncoderBChannel = 1
20+
kJoystickPort = 0
21+
kHighGoalPosition = wpimath.units.feetToMeters(3)
22+
kLowGoalPosition = wpimath.units.feetToMeters(0)
23+
24+
kCarriageMass = 4.5
25+
# kilograms
26+
27+
# A 1.5in diameter drum has a radius of 0.75in, or 0.019in.
28+
kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0
29+
30+
# Reduction between motors and encoder, as output over input. If the elevator spins slower than
31+
# the motors, this number should be greater than one.
32+
kElevatorGearing = 6.0
33+
34+
35+
class MyRobot(wpilib.TimedRobot):
36+
"""This is a sample program to demonstrate how to use a state-space controller to control an
37+
elevator.
38+
"""
39+
40+
def robotInit(self) -> None:
41+
self.profile = wpimath.trajectory.TrapezoidProfile(
42+
wpimath.trajectory.TrapezoidProfile.Constraints(
43+
wpimath.units.feetToMeters(3.0),
44+
wpimath.units.feetToMeters(6.0), # Max elevator speed and acceleration.
45+
)
46+
)
47+
48+
self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State()
49+
50+
# The plant holds a state-space model of our elevator. This system has the following properties:
51+
52+
# States: [position, velocity], in meters and meters per second.
53+
# Inputs (what we can "put in"): [voltage], in volts.
54+
# Outputs (what we can measure): [position], in meters.
55+
56+
# This elevator is driven by two NEO motors.
57+
self.elevatorPlant = wpimath.system.plant.LinearSystemId.elevatorSystem(
58+
wpimath.system.plant.DCMotor.NEO(2),
59+
kCarriageMass,
60+
kDrumRadius,
61+
kElevatorGearing,
62+
)
63+
64+
# The observer fuses our encoder data and voltage inputs to reject noise.
65+
self.observer = wpimath.estimator.KalmanFilter_2_1_1(
66+
self.elevatorPlant,
67+
[
68+
wpimath.units.inchesToMeters(2),
69+
wpimath.units.inchesToMeters(40),
70+
], # How accurate we think our model is, in meters and meters/second.
71+
[
72+
0.001
73+
], # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
74+
0.020,
75+
)
76+
77+
# A LQR uses feedback to create voltage commands.
78+
self.controller = wpimath.controller.LinearQuadraticRegulator_2_1(
79+
self.elevatorPlant,
80+
[
81+
wpimath.units.inchesToMeters(1.0),
82+
wpimath.units.inchesToMeters(10.0),
83+
], # qelms. Position
84+
# and velocity error tolerances, in meters and meters per second. Decrease this to more
85+
# heavily penalize state excursion, or make the controller behave more aggressively. In
86+
# this example we weight position much more highly than velocity, but this can be
87+
# tuned to balance the two.
88+
[12.0], # relms. Control effort (voltage) tolerance. Decrease this to more
89+
# heavily penalize control effort, or make the controller less aggressive. 12 is a good
90+
# starting point because that is the (approximate) maximum voltage of a battery.
91+
0.020, # Nominal time between loops. 0.020 for TimedRobot, but can be
92+
# lower if using notifiers.
93+
)
94+
95+
# The state-space loop combines a controller, observer, feedforward and plant for easy control.
96+
self.loop = wpimath.system.LinearSystemLoop_2_1_1(
97+
self.elevatorPlant, self.controller, self.observer, 12.0, 0.020
98+
)
99+
100+
# An encoder set up to measure flywheel velocity in radians per second.
101+
self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel)
102+
103+
self.motor = wpilib.PWMSparkMax(kMotorPort)
104+
105+
# A joystick to read the trigger from.
106+
self.joystick = wpilib.Joystick(kJoystickPort)
107+
108+
# Circumference = pi * d, so distance per click = pi * d / counts
109+
self.encoder.setDistancePerPulse(math.tau * kDrumRadius / 4096)
110+
111+
def teleopInit(self) -> None:
112+
# Reset our loop to make sure it's in a known state.
113+
self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()])
114+
115+
# Reset our last reference to the current state.
116+
self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State(
117+
self.encoder.getDistance(), self.encoder.getRate()
118+
)
119+
120+
def teleopPeriodic(self) -> None:
121+
# Sets the target position of our arm. This is similar to setting the setpoint of a
122+
# PID controller.
123+
124+
goal = wpimath.trajectory.TrapezoidProfile.State()
125+
126+
if self.joystick.getTrigger():
127+
# the trigger is pressed, so we go to the high goal.
128+
goal = wpimath.trajectory.TrapezoidProfile.State(kHighGoalPosition, 0.0)
129+
130+
else:
131+
# Otherwise, we go to the low goal
132+
goal = wpimath.trajectory.TrapezoidProfile.State(kLowGoalPosition, 0.0)
133+
134+
# Step our TrapezoidalProfile forward 20ms and set it as our next reference
135+
self.lastProfiledReference = self.profile.calculate(
136+
0.020, self.lastProfiledReference, goal
137+
)
138+
self.loop.setNextR(
139+
[self.lastProfiledReference.position, self.lastProfiledReference.velocity]
140+
)
141+
142+
# Correct our Kalman filter's state vector estimate with encoder data.
143+
self.loop.correct([self.encoder.getDistance()])
144+
145+
# Update our LQR to generate new voltage commands and use the voltages to predict the next
146+
# state with out Kalman filter.
147+
self.loop.predict(0.020)
148+
149+
# Send the new calculated voltage to the motors.
150+
# voltage = duty cycle * battery voltage, so
151+
# duty cycle = voltage / battery voltage
152+
nextVoltage = self.loop.U(0)
153+
self.motor.setVoltage(nextVoltage)

0 commit comments

Comments
 (0)