From 24fea60accf47859e731a22da624d00ca2d893cb Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Wed, 24 Jan 2024 06:19:57 -0300 Subject: [PATCH] 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. --- StateSpaceArm/robot.py | 148 ++++++++++++++++++++++++++++++ StateSpaceElevator/robot.py | 153 +++++++++++++++++++++++++++++++ StateSpaceFlywheel/robot.py | 47 ++++------ StateSpaceFlywheel/util/units.py | 13 --- StateSpaceFlywheelSysId/robot.py | 2 +- run_tests.sh | 2 + 6 files changed, 324 insertions(+), 41 deletions(-) create mode 100644 StateSpaceArm/robot.py create mode 100644 StateSpaceElevator/robot.py delete mode 100644 StateSpaceFlywheel/util/units.py diff --git a/StateSpaceArm/robot.py b/StateSpaceArm/robot.py new file mode 100644 index 00000000..e8e25dfd --- /dev/null +++ b/StateSpaceArm/robot.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +# +# 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. +# + +import math +import wpilib +import wpimath.controller +import wpimath.estimator +import wpimath.units +import wpimath.trajectory +import wpimath.system +import wpimath.system.plant + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 +kRaisedPosition = wpimath.units.feetToMeters(90.0) +kLoweredPosition = wpimath.units.feetToMeters(0.0) + +# Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant +# is difficult, LinearSystem.identifyPositionSystem may be better. +kArmMOI = 1.2 + +# Reduction between motors and encoder, as output over input. If the arm spins slower than +# the motors, this number should be greater than one. +kArmGearing = 10.0 + + +class MyRobot(wpilib.TimedRobot): + """This is a sample program to demonstrate how to use a state-space controller to control an arm.""" + + def robotInit(self) -> None: + self.profile = wpimath.trajectory.TrapezoidProfile( + wpimath.trajectory.TrapezoidProfile.Constraints( + wpimath.units.feetToMeters(45), + wpimath.units.feetToMeters(90), # Max elevator speed and acceleration. + ) + ) + + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State() + + # The plant holds a state-space model of our elevator. This system has the following properties: + + # States: [position, velocity], in meters and meters per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [position], in meters. + self.armPlant = wpimath.system.plant.LinearSystemId.singleJointedArmSystem( + wpimath.system.plant.DCMotor.NEO(2), + kArmMOI, + kArmGearing, + ) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.estimator.KalmanFilter_2_1_1( + self.armPlant, + [ + 0.015, + 0.17, + ], # How accurate we think our model is, in radians and radians/sec. + [ + 0.01 + ], # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading. + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.controller.LinearQuadraticRegulator_2_1( + self.armPlant, + [ + wpimath.units.degreesToRadians(1.0), + wpimath.units.degreesToRadians(10.0), + ], # qelms. + # Position and velocity error tolerances, in radians and radians per second. Decrease + # this + # to more heavily penalize state excursion, or make the controller behave more + # aggressively. In this example we weight position much more highly than velocity, but + # this + # can be tuned to balance the two. + [12.0], # relms. Control effort (voltage) tolerance. Decrease this to more + # heavily penalize control effort, or make the controller less aggressive. 12 is a good + # starting point because that is the (approximate) maximum voltage of a battery. + 0.020, # Nominal time between loops. 0.020 for TimedRobot, but can be + # lower if using notifiers. + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.system.LinearSystemLoop_2_1_1( + self.armPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure flywheel velocity in radians per second. + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(kMotorPort) + + # A joystick to read the trigger from. + self.joystick = wpilib.Joystick(kJoystickPort) + + # We go 2 pi radians in 1 rotation, or 4096 counts. + self.encoder.setDistancePerPulse(math.tau / 4096) + + def teleopInit(self) -> None: + # Reset our loop to make sure it's in a known state. + self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()]) + + # Reset our last reference to the current state. + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State( + self.encoder.getDistance(), self.encoder.getRate() + ) + + def teleopPeriodic(self) -> None: + # Sets the target position of our arm. This is similar to setting the setpoint of a + # PID controller. + + goal = wpimath.trajectory.TrapezoidProfile.State() + + if self.joystick.getTrigger(): + # the trigger is pressed, so we go to the high goal. + goal = wpimath.trajectory.TrapezoidProfile.State(kRaisedPosition, 0.0) + + else: + # Otherwise, we go to the low goal + goal = wpimath.trajectory.TrapezoidProfile.State(kLoweredPosition, 0.0) + + # Step our TrapezoidalProfile forward 20ms and set it as our next reference + self.lastProfiledReference = self.profile.calculate( + 0.020, self.lastProfiledReference, goal + ) + self.loop.setNextR( + [self.lastProfiledReference.position, self.lastProfiledReference.velocity] + ) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct([self.encoder.getDistance()]) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next + # state with out Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) diff --git a/StateSpaceElevator/robot.py b/StateSpaceElevator/robot.py new file mode 100644 index 00000000..6e3279a7 --- /dev/null +++ b/StateSpaceElevator/robot.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +# +# 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. +# + +import math +import wpilib +import wpimath.controller +import wpimath.estimator +import wpimath.units +import wpimath.trajectory +import wpimath.system +import wpimath.system.plant + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 +kHighGoalPosition = wpimath.units.feetToMeters(3) +kLowGoalPosition = wpimath.units.feetToMeters(0) + +kCarriageMass = 4.5 +# kilograms + +# A 1.5in diameter drum has a radius of 0.75in, or 0.019in. +kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0 + +# Reduction between motors and encoder, as output over input. If the elevator spins slower than +# the motors, this number should be greater than one. +kElevatorGearing = 6.0 + + +class MyRobot(wpilib.TimedRobot): + """This is a sample program to demonstrate how to use a state-space controller to control an + elevator. + """ + + def robotInit(self) -> None: + self.profile = wpimath.trajectory.TrapezoidProfile( + wpimath.trajectory.TrapezoidProfile.Constraints( + wpimath.units.feetToMeters(3.0), + wpimath.units.feetToMeters(6.0), # Max elevator speed and acceleration. + ) + ) + + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State() + + # The plant holds a state-space model of our elevator. This system has the following properties: + + # States: [position, velocity], in meters and meters per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [position], in meters. + + # This elevator is driven by two NEO motors. + self.elevatorPlant = wpimath.system.plant.LinearSystemId.elevatorSystem( + wpimath.system.plant.DCMotor.NEO(2), + kCarriageMass, + kDrumRadius, + kElevatorGearing, + ) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.estimator.KalmanFilter_2_1_1( + self.elevatorPlant, + [ + wpimath.units.inchesToMeters(2), + wpimath.units.inchesToMeters(40), + ], # How accurate we think our model is, in meters and meters/second. + [ + 0.001 + ], # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading. + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.controller.LinearQuadraticRegulator_2_1( + self.elevatorPlant, + [ + wpimath.units.inchesToMeters(1.0), + wpimath.units.inchesToMeters(10.0), + ], # qelms. Position + # and velocity error tolerances, in meters and meters per second. Decrease this to more + # heavily penalize state excursion, or make the controller behave more aggressively. In + # this example we weight position much more highly than velocity, but this can be + # tuned to balance the two. + [12.0], # relms. Control effort (voltage) tolerance. Decrease this to more + # heavily penalize control effort, or make the controller less aggressive. 12 is a good + # starting point because that is the (approximate) maximum voltage of a battery. + 0.020, # Nominal time between loops. 0.020 for TimedRobot, but can be + # lower if using notifiers. + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.system.LinearSystemLoop_2_1_1( + self.elevatorPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure flywheel velocity in radians per second. + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(kMotorPort) + + # A joystick to read the trigger from. + self.joystick = wpilib.Joystick(kJoystickPort) + + # Circumference = pi * d, so distance per click = pi * d / counts + self.encoder.setDistancePerPulse(math.tau * kDrumRadius / 4096) + + def teleopInit(self) -> None: + # Reset our loop to make sure it's in a known state. + self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()]) + + # Reset our last reference to the current state. + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State( + self.encoder.getDistance(), self.encoder.getRate() + ) + + def teleopPeriodic(self) -> None: + # Sets the target position of our arm. This is similar to setting the setpoint of a + # PID controller. + + goal = wpimath.trajectory.TrapezoidProfile.State() + + if self.joystick.getTrigger(): + # the trigger is pressed, so we go to the high goal. + goal = wpimath.trajectory.TrapezoidProfile.State(kHighGoalPosition, 0.0) + + else: + # Otherwise, we go to the low goal + goal = wpimath.trajectory.TrapezoidProfile.State(kLowGoalPosition, 0.0) + + # Step our TrapezoidalProfile forward 20ms and set it as our next reference + self.lastProfiledReference = self.profile.calculate( + 0.020, self.lastProfiledReference, goal + ) + self.loop.setNextR( + [self.lastProfiledReference.position, self.lastProfiledReference.velocity] + ) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct([self.encoder.getDistance()]) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next + # state with out Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) diff --git a/StateSpaceFlywheel/robot.py b/StateSpaceFlywheel/robot.py index b5ca017d..ebba3b10 100644 --- a/StateSpaceFlywheel/robot.py +++ b/StateSpaceFlywheel/robot.py @@ -5,18 +5,24 @@ # the WPILib BSD license file in the root directory of this project. # +import math import wpilib -import wpimath +import wpimath.units import wpimath.controller import wpimath.system import wpimath.system.plant import wpimath.estimator -import math -import numpy +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 -# A simple utility class for converting rpm to radians, as robotPY does not currently have a wpimath.util class. -import util.units +kFlywheelMomentOfInertia = 0.00032 # kg/m^2 + +# Reduction between motors and encoder, as output over input. If the flywheel spins slower than +# the motors, this number should be greater than one. +kFlywheelGearing = 1 class MyRobot(wpilib.TimedRobot): @@ -25,21 +31,8 @@ class MyRobot(wpilib.TimedRobot): flywheel. """ - kMotorPort = 0 - kEncoderAChannel = 0 - kEncoderBChannel = 1 - kJoystickPort = 0 - - kFlywheelMomentOfInertia = 0.00032 # kg/m^2 - - # Reduction between motors and encoder, as output over input. If the flywheel spins slower than - # the motors, this number should be greater than one. - kFlywheelGearing = 1 - def robotInit(self) -> None: - self.kSpinUpRadPerSec = util.units.Units.rotationsPerMinuteToRadiansPerSecond( - 500 - ) + self.kSpinUpRadPerSec = wpimath.units.rotationsPerMinuteToRadiansPerSecond(500) # The plant holds a state-space model of our flywheel. This system has the following properties: # @@ -48,8 +41,8 @@ def robotInit(self) -> None: # Outputs (what we can measure): [velocity], in radians per second. self.flywheelPlant = wpimath.system.plant.LinearSystemId.flywheelSystem( wpimath.system.plant.DCMotor.NEO(2), - self.kFlywheelMomentOfInertia, - self.kFlywheelGearing, + kFlywheelMomentOfInertia, + kFlywheelGearing, ) # The observer fuses our encoder data and voltage inputs to reject noise. @@ -78,15 +71,15 @@ def robotInit(self) -> None: ) # An encoder set up to measure flywheel velocity in radians per second. - self.encoder = wpilib.Encoder(self.kEncoderAChannel, self.kEncoderBChannel) + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) - self.motor = wpilib.PWMSparkMax(self.kMotorPort) + self.motor = wpilib.PWMSparkMax(kMotorPort) # A joystick to read the trigger from. - self.joystick = wpilib.Joystick(self.kJoystickPort) + self.joystick = wpilib.Joystick(kJoystickPort) # We go 2 pi radians per 4096 clicks. - self.encoder.setDistancePerPulse(2 * math.pi / 4096) + self.encoder.setDistancePerPulse(math.tau / 4096) def teleopInit(self) -> None: self.loop.reset([self.encoder.getRate()]) @@ -96,11 +89,11 @@ def teleopPeriodic(self) -> None: # PID controller. if self.joystick.getTriggerPressed(): # We just pressed the trigger, so let's set our next reference - self.loop.setNextR(numpy.array([self.kSpinUpRadPerSec])) + self.loop.setNextR([kSpinUpRadPerSec]) elif self.joystick.getTriggerReleased(): # We just released the trigger, so let's spin down - self.loop.setNextR(numpy.array([self.kSpinUpRadPerSec])) + self.loop.setNextR([0.0]) # Correct our Kalman filter's state vector estimate with encoder data. self.loop.correct([self.encoder.getRate()]) diff --git a/StateSpaceFlywheel/util/units.py b/StateSpaceFlywheel/util/units.py deleted file mode 100644 index 1f99d879..00000000 --- a/StateSpaceFlywheel/util/units.py +++ /dev/null @@ -1,13 +0,0 @@ -# -# 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. -# - - -class Units: - def __init__(self) -> None: - raise Exception("This is a utility class!") - - def rotationsPerMinuteToRadiansPerSecond(rpm: float) -> float: - return rpm * 0.10472 diff --git a/StateSpaceFlywheelSysId/robot.py b/StateSpaceFlywheelSysId/robot.py index c6aeeedc..78085300 100644 --- a/StateSpaceFlywheelSysId/robot.py +++ b/StateSpaceFlywheelSysId/robot.py @@ -77,7 +77,7 @@ def robotInit(self) -> None: self.joystick = wpilib.Joystick(kJoystickPort) # We go 2 pi radians per 4096 clicks. - self.encoder.setDistancePerPulse(2 * math.pi / 4096) + self.encoder.setDistancePerPulse(math.tau / 4096) def teleopInit(self) -> None: self.loop.reset([self.encoder.getRate()]) diff --git a/run_tests.sh b/run_tests.sh index c8d40cbf..9d7481f3 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -53,6 +53,8 @@ BASE_TESTS=" ShuffleBoard Solenoid StatefulAutonomous + StateSpaceArm + StateSpaceElevator StateSpaceFlywheel StateSpaceFlywheelSysId SwerveBot