From 54f0e11fc06373e0e8b495f744d54a8656ada1a1 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 3 Dec 2024 23:30:03 -0500 Subject: [PATCH 01/10] [build] Update OpenCV to 2025-4.8.0-2 (#7476) --- shared/config.gradle | 2 +- shared/opencv.gradle | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/config.gradle b/shared/config.gradle index 6e68b00f93..829ead7058 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -15,7 +15,7 @@ nativeUtils { configureDependencies { opencvYear = "frc2025" niLibVersion = "2025.0.0" - opencvVersion = "4.8.0-1" + opencvVersion = "4.8.0-2" } } } diff --git a/shared/opencv.gradle b/shared/opencv.gradle index 432a0658f2..71ab8a1c75 100644 --- a/shared/opencv.gradle +++ b/shared/opencv.gradle @@ -1,4 +1,4 @@ -def opencvVersion = '4.8.0-1' +def opencvVersion = '4.8.0-2' if (project.hasProperty('useCpp') && project.useCpp) { model { From 60198c0becffddabd43064aa971069d704007fbc Mon Sep 17 00:00:00 2001 From: Daniel Chen <108989218+Daniel1464@users.noreply.github.com> Date: Tue, 3 Dec 2024 23:30:55 -0500 Subject: [PATCH 02/10] [epilogue] Improved opt-in logging support (#7437) --- .../processor/AnnotationProcessor.java | 50 ++++-- .../epilogue/processor/LoggableHandler.java | 7 +- .../epilogue/processor/LoggerGenerator.java | 31 ++++ .../first/epilogue/processor/StringUtils.java | 2 +- .../processor/AnnotationProcessorTest.java | 143 ++++++++++++++++++ 5 files changed, 215 insertions(+), 18 deletions(-) diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java index 22f0e89459..2781c055c8 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java @@ -11,9 +11,12 @@ import java.io.IOException; import java.util.ArrayList; import java.util.Comparator; import java.util.HashMap; +import java.util.LinkedHashSet; import java.util.List; import java.util.Map; import java.util.Set; +import java.util.stream.Collectors; +import java.util.stream.Stream; import javax.annotation.processing.AbstractProcessor; import javax.annotation.processing.RoundEnvironment; import javax.annotation.processing.SupportedAnnotationTypes; @@ -90,15 +93,15 @@ public class AnnotationProcessor extends AbstractProcessor { e); }); + var loggedTypes = getLoggedTypes(roundEnv); + // Handlers are declared in order of priority. If an element could be logged in more than one // way (eg a class implements both Sendable and StructSerializable), the order of the handlers // in this list will determine how it gets logged. m_handlers = List.of( new LoggableHandler( - processingEnv, - roundEnv.getElementsAnnotatedWith( - Logged.class)), // prioritize epilogue logging over Sendable + processingEnv, loggedTypes), // prioritize epilogue logging over Sendable new ConfiguredLoggerHandler( processingEnv, customLoggers), // then customized logging configs new ArrayHandler(processingEnv), @@ -118,12 +121,39 @@ public class AnnotationProcessor extends AbstractProcessor { .findAny() .ifPresent( epilogue -> { - processEpilogue(roundEnv, epilogue); + processEpilogue(roundEnv, epilogue, loggedTypes); }); return false; } + /** + * Gets the set of all loggable types in the compilation unit. A type is considered loggable if it + * is directly annotated with {@code @Logged} or contains a field or method with a {@code @Logged} + * annotation. + * + * @param roundEnv the compilation round environment + * @return the set of all loggable types + */ + private Set getLoggedTypes(RoundEnvironment roundEnv) { + // Fetches everything annotated with @Logged; classes, methods, values, etc. + var annotatedElements = roundEnv.getElementsAnnotatedWith(Logged.class); + return Stream.concat( + // 1. All type elements (classes, interfaces, or enums) with the @Logged annotation + annotatedElements.stream() + .filter(e -> e instanceof TypeElement) + .map(e -> (TypeElement) e), + // 2. All type elements containing a field or method with the @Logged annotation + annotatedElements.stream() + .filter(e -> e instanceof VariableElement || e instanceof ExecutableElement) + .map(Element::getEnclosingElement) + .filter(e -> e instanceof TypeElement) + .map(e -> (TypeElement) e)) + .sorted(Comparator.comparing(e -> e.getSimpleName().toString())) + .collect( + Collectors.toCollection(LinkedHashSet::new)); // Collect to a set to avoid duplicates + } + private boolean validateFields(Set annotatedElements) { var fields = annotatedElements.stream() @@ -340,7 +370,8 @@ public class AnnotationProcessor extends AbstractProcessor { return customLoggers; } - private void processEpilogue(RoundEnvironment roundEnv, TypeElement epilogueAnnotation) { + private void processEpilogue( + RoundEnvironment roundEnv, TypeElement epilogueAnnotation, Set loggedTypes) { var annotatedElements = roundEnv.getElementsAnnotatedWith(epilogueAnnotation); List loggerClassNames = new ArrayList<>(); @@ -358,12 +389,7 @@ public class AnnotationProcessor extends AbstractProcessor { return; } - var classes = - annotatedElements.stream() - .filter(e -> e instanceof TypeElement) - .map(e -> (TypeElement) e) - .toList(); - for (TypeElement clazz : classes) { + for (TypeElement clazz : loggedTypes) { try { warnOfNonLoggableElements(clazz); m_loggerGenerator.writeLoggerFile(clazz); @@ -391,7 +417,7 @@ public class AnnotationProcessor extends AbstractProcessor { private void warnOfNonLoggableElements(TypeElement clazz) { var config = clazz.getAnnotation(Logged.class); - if (config.strategy() == Logged.Strategy.OPT_IN) { + if (config == null || config.strategy() == Logged.Strategy.OPT_IN) { // field and method validations will have already checked everything return; } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java index 8dea822c4a..ba14022a2c 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java @@ -15,7 +15,6 @@ import javax.lang.model.element.ElementKind; import javax.lang.model.element.ExecutableElement; import javax.lang.model.element.TypeElement; import javax.lang.model.element.VariableElement; -import javax.lang.model.type.DeclaredType; import javax.lang.model.type.TypeKind; import javax.lang.model.type.TypeMirror; @@ -35,10 +34,8 @@ public class LoggableHandler extends ElementHandler { @Override public boolean isLoggable(Element element) { - var dataType = dataType(element); - return dataType.getAnnotation(Logged.class) != null - || dataType instanceof DeclaredType decl - && decl.asElement().getAnnotation(Logged.class) != null; + return m_processingEnv.getTypeUtils().asElement(dataType(element)) instanceof TypeElement t + && m_loggedTypes.contains(t); } @Override diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java index 15fa2104dc..499d9912a7 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java @@ -15,6 +15,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import java.io.IOException; import java.io.PrintWriter; +import java.lang.annotation.Annotation; import java.util.ArrayDeque; import java.util.ArrayList; import java.util.Deque; @@ -42,6 +43,33 @@ public class LoggerGenerator { LoggerGenerator::isBuiltInJavaMethod; private final ProcessingEnvironment m_processingEnv; private final List m_handlers; + private final Logged m_defaultConfig = + new Logged() { + @Override + public Class annotationType() { + return Logged.class; + } + + @Override + public String name() { + return ""; + } + + @Override + public Strategy strategy() { + return Strategy.OPT_IN; + } + + @Override + public Importance importance() { + return Importance.DEBUG; + } + + @Override + public Naming defaultNaming() { + return Naming.USE_CODE_NAME; + } + }; public LoggerGenerator(ProcessingEnvironment processingEnv, List handlers) { this.m_processingEnv = processingEnv; @@ -76,6 +104,9 @@ public class LoggerGenerator { */ public void writeLoggerFile(TypeElement clazz) throws IOException { var config = clazz.getAnnotation(Logged.class); + if (config == null) { + config = m_defaultConfig; + } boolean requireExplicitOptIn = config.strategy() == Logged.Strategy.OPT_IN; Predicate notSkipped = LoggerGenerator::isNotSkipped; diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java index 23fa2964ba..44063eed65 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java @@ -123,7 +123,7 @@ public final class StringUtils { String packageName = p.getQualifiedName().toString(); String className; - if (config.name().isEmpty()) { + if (config == null || config.name().isEmpty()) { className = String.join("$", nesting); } else { className = capitalize(config.name()).replaceAll(" ", ""); diff --git a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java index cdfb99e082..f211999bfe 100644 --- a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java +++ b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java @@ -9,6 +9,7 @@ import static com.google.testing.compile.Compiler.javac; import static edu.wpi.first.epilogue.processor.CompileTestOptions.kJavaVersionOptions; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; import com.google.testing.compile.Compilation; import com.google.testing.compile.JavaFileObjects; @@ -59,6 +60,107 @@ class AnnotationProcessorTest { assertLoggerGenerates(source, expectedGeneratedSource); } + @Test + void optInFields() { + String source = + """ + package edu.wpi.first.epilogue; + + class Example { + @Logged double x; + @Logged int y; + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("x", object.x); + backend.log("y", object.y); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + + @Test + void optInMethods() { + String source = + """ + package edu.wpi.first.epilogue; + + class Example { + @Logged public double getValue() { return 2.0; } + @Logged public String getName() { return "Example"; } + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("getValue", object.getValue()); + backend.log("getName", object.getName()); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + + @Test + void shouldNotLog() { + String source = + """ + class Example { + public double getValue() { return 2.0; } + public String getName() { return "Example"; } + } + """; + + Compilation compilation = + javac() + .withOptions(kJavaVersionOptions) + .withProcessors(new AnnotationProcessor()) + .compile(JavaFileObjects.forSourceString("edu.wpi.first.epilogue.Example", source)); + + assertThat(compilation).succeeded(); + // nothing is annotated with @Logged; so, no logger file should be generated + assertTrue( + compilation.generatedSourceFiles().stream() + .noneMatch(jfo -> jfo.getName().contains("Example"))); + } + @Test void multiple() { String source = @@ -1416,6 +1518,47 @@ class AnnotationProcessorTest { assertLoggerGenerates(source, expectedRootLogger); } + @Test + void nestedOptIn() { + String source = + """ + package edu.wpi.first.epilogue; + + class Implicit { + @Logged double x; + } + + class Example { + @Logged Implicit i; + } + """; + + String expectedRootLogger = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + Epilogue.implicitLogger.tryUpdate(backend.getNested("i"), object.i, Epilogue.getConfig().errorHandler); + } + } + } + """; + + assertLoggerGenerates(source, expectedRootLogger); + } + @Test void customLogger() { String source = From 4ce8930342f5133fdde5a4a53919c08955632a23 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Fri, 6 Dec 2024 03:24:26 -0500 Subject: [PATCH 03/10] [developerRobot] Add instructions for developerRobot java sim (NFC) (#7498) --- developerRobot/README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/developerRobot/README.md b/developerRobot/README.md index 8e01131440..2b332f8afe 100644 --- a/developerRobot/README.md +++ b/developerRobot/README.md @@ -9,7 +9,12 @@ This command builds everything. ## Simulation -This command runs the C++ subproject on desktop. +This command runs the Java project on desktop. +```bash +./gradlew developerRobot:run +``` + +This command runs the C++ project on desktop. ```bash ./gradlew developerRobot:runCpp ``` From c3fc7c829d2251bb65f29cbed452df1e8883c07d Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 6 Dec 2024 15:41:40 -0800 Subject: [PATCH 04/10] [build] Upgrade OpenCV to 4.10.0 (#7500) --- shared/config.gradle | 2 +- shared/opencv.gradle | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/config.gradle b/shared/config.gradle index 829ead7058..e15a80c35a 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -15,7 +15,7 @@ nativeUtils { configureDependencies { opencvYear = "frc2025" niLibVersion = "2025.0.0" - opencvVersion = "4.8.0-2" + opencvVersion = "4.10.0-2" } } } diff --git a/shared/opencv.gradle b/shared/opencv.gradle index 71ab8a1c75..56c84bf48d 100644 --- a/shared/opencv.gradle +++ b/shared/opencv.gradle @@ -1,4 +1,4 @@ -def opencvVersion = '4.8.0-2' +def opencvVersion = '4.10.0-2' if (project.hasProperty('useCpp') && project.useCpp) { model { From 1921d019b70b101eb6cd7c93c5ccd62d6c77a261 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 6 Dec 2024 22:19:00 -0800 Subject: [PATCH 05/10] [build] Update arm builds to bookworm (#7501) Also bumps native-utils to 2025.9.0. And upgrades OpenCV to -3; fixes some enum conversion deprecation warnings. --- .github/workflows/gradle.yml | 4 ++-- .github/workflows/sentinel-build.yml | 4 ++-- buildSrc/build.gradle | 2 +- shared/config.gradle | 2 +- shared/opencv.gradle | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index e6935cfba9..5ee9430ac0 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -15,10 +15,10 @@ jobs: - container: wpilib/roborio-cross-ubuntu:2025-22.04 artifact-name: Athena build-options: "-Ponlylinuxathena" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 artifact-name: Arm64 build-options: "-Ponlylinuxarm64" - container: wpilib/ubuntu-base:22.04 diff --git a/.github/workflows/sentinel-build.yml b/.github/workflows/sentinel-build.yml index 9a8eb85184..18adcf7675 100644 --- a/.github/workflows/sentinel-build.yml +++ b/.github/workflows/sentinel-build.yml @@ -19,10 +19,10 @@ jobs: - container: wpilib/roborio-cross-ubuntu:2025-22.04 artifact-name: Athena build-options: "-Ponlylinuxathena" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 artifact-name: Arm64 build-options: "-Ponlylinuxarm64" - container: wpilib/ubuntu-base:22.04 diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle index 5c555c1c53..8a0ad0c1a9 100644 --- a/buildSrc/build.gradle +++ b/buildSrc/build.gradle @@ -9,5 +9,5 @@ repositories { } } dependencies { - implementation "edu.wpi.first:native-utils:2025.7.1" + implementation "edu.wpi.first:native-utils:2025.9.0" } diff --git a/shared/config.gradle b/shared/config.gradle index e15a80c35a..7eebdb0a77 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -15,7 +15,7 @@ nativeUtils { configureDependencies { opencvYear = "frc2025" niLibVersion = "2025.0.0" - opencvVersion = "4.10.0-2" + opencvVersion = "4.10.0-3" } } } diff --git a/shared/opencv.gradle b/shared/opencv.gradle index 56c84bf48d..32b4598932 100644 --- a/shared/opencv.gradle +++ b/shared/opencv.gradle @@ -1,4 +1,4 @@ -def opencvVersion = '4.10.0-2' +def opencvVersion = '4.10.0-3' if (project.hasProperty('useCpp') && project.useCpp) { model { From 882233bede9a1af6f411d3f62864732a87d8e194 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 6 Dec 2024 22:32:40 -0800 Subject: [PATCH 06/10] [wpimath] Improve SimpleMotorFeedforward argument names and deprecation message (#7489) Also roll back SimpleMotorFeedforward unit API until 2027. --- .../command/MecanumControllerCommand.java | 64 ++++++++----------- .../wpilibj2/command/RamseteCommand.java | 24 +++---- .../differentialdrivebot/Drivetrain.java | 9 +-- .../Drivetrain.java | 9 +-- .../subsystems/DriveSubsystem.java | 13 +--- .../elevatorexponentialprofile/Robot.java | 5 +- .../elevatortrapezoidprofile/Robot.java | 5 +- .../wpilibj/examples/eventloop/Robot.java | 5 +- .../flywheelbangbangcontroller/Robot.java | 6 +- .../examples/mecanumbot/Drivetrain.java | 15 ++--- .../mecanumdriveposeestimator/Drivetrain.java | 15 ++--- .../subsystems/Drive.java | 7 +- .../subsystems/Shooter.java | 6 +- .../Drivetrain.java | 9 +-- .../examples/swervebot/SwerveModule.java | 13 +--- .../SwerveModule.java | 13 +--- .../sysidroutine/subsystems/Shooter.java | 4 +- .../controller/SimpleMotorFeedforward.java | 35 ++-------- .../frc/controller/SimpleMotorFeedforward.h | 11 ++-- .../SimpleMotorFeedforwardTest.java | 11 ++-- ...ifferentialDriveVoltageConstraintTest.java | 38 ++++------- .../trajectory/ExponentialProfileTest.java | 7 +- 22 files changed, 91 insertions(+), 233 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index bb47e58a37..70978c63f1 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.controller.HolonomicDriveController; @@ -19,7 +17,6 @@ import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; import java.util.function.Supplier; @@ -58,14 +55,10 @@ public class MecanumControllerCommand extends Command { private final Supplier m_currentWheelSpeeds; private final Consumer m_outputDriveVoltages; private final Consumer m_outputWheelSpeeds; - private final MutLinearVelocity m_prevFrontLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRearLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevFrontRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRearRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_frontLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rearLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_frontRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rearRightSpeedSetpoint = MetersPerSecond.mutable(0); + private double m_prevFrontLeftSpeedSetpoint; // m/s + private double m_prevRearLeftSpeedSetpoint; // m/s + private double m_prevFrontRightSpeedSetpoint; // m/s + private double m_prevRearRightSpeedSetpoint; // m/s /** * Constructs a new MecanumControllerCommand that when executed will follow the provided @@ -343,10 +336,10 @@ public class MecanumControllerCommand extends Command { MecanumDriveWheelSpeeds prevSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); - m_prevFrontLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontLeftMetersPerSecond); - m_prevRearLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearLeftMetersPerSecond); - m_prevFrontRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontRightMetersPerSecond); - m_prevRearRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearRightMetersPerSecond); + m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeftMetersPerSecond; + m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeftMetersPerSecond; + m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRightMetersPerSecond; + m_prevRearRightSpeedSetpoint = prevSpeeds.rearRightMetersPerSecond; m_timer.restart(); } @@ -363,10 +356,10 @@ public class MecanumControllerCommand extends Command { targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond); - m_frontLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontLeftMetersPerSecond); - m_rearLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearLeftMetersPerSecond); - m_frontRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontRightMetersPerSecond); - m_rearRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearRightMetersPerSecond); + double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; + double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; + double frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; + double rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; double frontLeftOutput; double rearLeftOutput; @@ -375,42 +368,39 @@ public class MecanumControllerCommand extends Command { if (m_usePID) { final double frontLeftFeedforward = - m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, m_frontLeftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities( + m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint); final double rearLeftFeedforward = - m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, m_rearLeftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint); final double frontRightFeedforward = - m_feedforward - .calculate(m_prevFrontRightSpeedSetpoint, m_frontRightSpeedSetpoint) - .in(Volts); + m_feedforward.calculateWithVelocities( + m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint); final double rearRightFeedforward = - m_feedforward.calculate(m_prevRearRightSpeedSetpoint, m_rearRightSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities( + m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint); frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate( - m_currentWheelSpeeds.get().frontLeftMetersPerSecond, - m_frontLeftSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint); rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate( - m_currentWheelSpeeds.get().rearLeftMetersPerSecond, - m_rearLeftSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint); frontRightOutput = frontRightFeedforward + m_frontRightController.calculate( - m_currentWheelSpeeds.get().frontRightMetersPerSecond, - m_frontRightSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint); rearRightOutput = rearRightFeedforward + m_rearRightController.calculate( - m_currentWheelSpeeds.get().rearRightMetersPerSecond, - m_rearRightSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint); m_outputDriveVoltages.accept( new MecanumDriveMotorVoltages( @@ -419,10 +409,10 @@ public class MecanumControllerCommand extends Command { } else { m_outputWheelSpeeds.accept( new MecanumDriveWheelSpeeds( - m_frontLeftSpeedSetpoint.in(MetersPerSecond), - m_frontRightSpeedSetpoint.in(MetersPerSecond), - m_rearLeftSpeedSetpoint.in(MetersPerSecond), - m_rearRightSpeedSetpoint.in(MetersPerSecond))); + frontLeftSpeedSetpoint, + frontRightSpeedSetpoint, + rearLeftSpeedSetpoint, + rearRightSpeedSetpoint)); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 533db5b039..88aae9cca1 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.controller.PIDController; @@ -16,7 +14,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Timer; import java.util.function.BiConsumer; @@ -49,10 +46,8 @@ public class RamseteCommand extends Command { private final PIDController m_rightController; private final BiConsumer m_output; private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds(); - private final MutLinearVelocity m_prevLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_leftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rightSpeedSetpoint = MetersPerSecond.mutable(0); + private double m_prevLeftSpeedSetpoint; // m/s + private double m_prevRightSpeedSetpoint; // m/s private double m_prevTime; /** @@ -156,8 +151,8 @@ public class RamseteCommand extends Command { initialState.velocityMetersPerSecond, 0, initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond)); - m_prevLeftSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.leftMetersPerSecond); - m_prevRightSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.rightMetersPerSecond); + m_prevLeftSpeedSetpoint = m_prevSpeeds.leftMetersPerSecond; + m_prevRightSpeedSetpoint = m_prevSpeeds.rightMetersPerSecond; m_timer.restart(); if (m_usePID) { m_leftController.reset(); @@ -179,21 +174,18 @@ public class RamseteCommand extends Command { m_kinematics.toWheelSpeeds( m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime))); - var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; - var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; - - m_leftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.leftMetersPerSecond); - m_rightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rightMetersPerSecond); + double leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; + double rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; double leftOutput; double rightOutput; if (m_usePID) { double leftFeedforward = - m_feedforward.calculate(m_prevLeftSpeedSetpoint, m_leftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevLeftSpeedSetpoint, leftSpeedSetpoint); double rightFeedforward = - m_feedforward.calculate(m_prevRightSpeedSetpoint, m_rightSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevRightSpeedSetpoint, rightSpeedSetpoint); leftOutput = leftFeedforward diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index b351d169db..adc52e2ef6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.differentialdrivebot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -82,10 +79,8 @@ public class Drivetrain { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 7df9f3b449..fbf6e90b9c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.ComputerVisionUtil; @@ -142,10 +139,8 @@ public class Drivetrain { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index 67fea4c8ae..b8947ce6da 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; @@ -99,18 +96,12 @@ public class DriveSubsystem extends SubsystemBase { m_leftLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentLeft.position, - m_feedforward - .calculate( - MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) - .in(Volts) + m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); m_rightLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentRight.position, - m_feedforward - .calculate( - MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) - .in(Volts) + m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java index 83958230ec..373485923c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.ExponentialProfile; import edu.wpi.first.wpilibj.Joystick; @@ -48,7 +45,7 @@ public class Robot extends TimedRobot { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, - m_feedforward.calculate(RadiansPerSecond.of(next.velocity)).in(Volts) / 12.0); + m_feedforward.calculate(next.velocity) / 12.0); m_setpoint = next; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java index e1ae800421..2ab9560dcb 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Joystick; @@ -48,6 +45,6 @@ public class Robot extends TimedRobot { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, - m_feedforward.calculate(MetersPerSecond.of(m_setpoint.velocity)).in(Volts) / 12.0); + m_feedforward.calculate(m_setpoint.velocity) / 12.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java index e75edfe3d1..f9646e282a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.eventloop; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.Encoder; @@ -67,7 +64,7 @@ public class Robot extends TimedRobot { () -> { m_shooter.setVoltage( m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY) - + m_ff.calculate(RPM.of(SHOT_VELOCITY)).in(Volts)); + + m_ff.calculate(SHOT_VELOCITY)); }); // if not, stop diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index abba8a7365..76fc431759 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; @@ -89,8 +86,7 @@ public class Robot extends TimedRobot { // Controls a motor with the output of the BangBang controller and a // feedforward. The feedforward is reduced slightly to avoid overspeeding // the shooter. - m_flywheelMotor.setVoltage( - bangOutput + 0.9 * m_feedforward.calculate(RadiansPerSecond.of(setpoint)).in(Volts)); + m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint)); } /** Update our simulation. This should be run every robot loop in simulation. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index addedc0a9a..61cfce638e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.mecanumbot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Translation2d; @@ -98,14 +95,10 @@ public class Drivetrain { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts); - final double frontRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts); - final double backLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts); - final double backRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); final double frontLeftOutput = m_frontLeftPIDController.calculate( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index ada2b6d83e..de48109448 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -110,14 +107,10 @@ public class Drivetrain { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts); - final double frontRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts); - final double backLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts); - final double backRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); final double frontLeftOutput = m_frontLeftPIDController.calculate( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index 7e9e8e0370..dbb234a8d0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -143,9 +140,7 @@ public class Drive extends SubsystemBase { 0, m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg) // Divide feedforward voltage by battery voltage to normalize it to [-1, 1] - + m_feedforward - .calculate(DegreesPerSecond.of(m_controller.getSetpoint().velocity)) - .in(Volts) + + m_feedforward.calculate(m_controller.getSetpoint().velocity) / RobotController.getBatteryVoltage())) .until(m_controller::atGoal) .finallyDo(() -> m_drive.arcadeDrive(0, 0)); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java index a10f6f9143..39023c2e8e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.wpilibj2.command.Commands.parallel; import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; @@ -60,9 +58,7 @@ public class Shooter extends SubsystemBase { run( () -> { m_shooterMotor.set( - m_shooterFeedforward - .calculate(RotationsPerSecond.of(setpointRotationsPerSecond)) - .in(Volts) + m_shooterFeedforward.calculate(setpointRotationsPerSecond) + m_shooterFeedback.calculate( m_shooterEncoder.getRate(), setpointRotationsPerSecond)); }), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index eb911054a5..d0d1ed4dcd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; @@ -97,10 +94,8 @@ public class Drivetrain { /** Sets speeds to the drivetrain motors. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); double rightOutput = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index eb95c4c52c..b57ba6e5ea 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -4,10 +4,6 @@ package edu.wpi.first.wpilibj.examples.swervebot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -127,10 +123,7 @@ public class SwerveModule { final double driveOutput = m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); - final double driveFeedforward = - m_driveFeedforward - .calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)) - .in(Volts); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); // Calculate the turning motor output from the turning PID controller. final double turnOutput = @@ -138,9 +131,7 @@ public class SwerveModule { m_turningEncoder.getDistance(), desiredState.angle.getRadians()); final double turnFeedforward = - m_turnFeedforward - .calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity)) - .in(Volts); + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); m_driveMotor.setVoltage(driveOutput + driveFeedforward); m_turningMotor.setVoltage(turnOutput + turnFeedforward); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index f6a9b46bdd..e8af157d37 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -4,10 +4,6 @@ package edu.wpi.first.wpilibj.examples.swervedriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -126,10 +122,7 @@ public class SwerveModule { final double driveOutput = m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); - final double driveFeedforward = - m_driveFeedforward - .calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)) - .in(Volts); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); // Calculate the turning motor output from the turning PID controller. final double turnOutput = @@ -137,9 +130,7 @@ public class SwerveModule { m_turningEncoder.getDistance(), desiredState.angle.getRadians()); final double turnFeedforward = - m_turnFeedforward - .calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity)) - .in(Volts); + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); m_driveMotor.setVoltage(driveOutput + driveFeedforward); m_turningMotor.setVoltage(turnOutput + turnFeedforward); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java index cc7fbac2fe..affceda144 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java @@ -95,9 +95,7 @@ public class Shooter extends SubsystemBase { return run(() -> { m_shooterMotor.setVoltage( m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble()) - + m_shooterFeedforward - .calculate(RotationsPerSecond.of(shooterSpeed.getAsDouble())) - .in(Volts)); + + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble())); m_feederMotor.set(ShooterConstants.kFeederSpeed); }) .finallyDo( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 9356481ffa..b7603a6bfb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -4,15 +4,8 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.proto.SimpleMotorFeedforwardProto; import edu.wpi.first.math.controller.struct.SimpleMotorFeedforwardStruct; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.PerUnit; -import edu.wpi.first.units.TimeUnit; -import edu.wpi.first.units.Unit; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -136,7 +129,7 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria * @param velocity The velocity setpoint. * @param acceleration The acceleration setpoint. * @return The computed feedforward. - * @deprecated Use the current/next velocity overload instead. + * @deprecated Use {@link #calculateWithVelocities(double, double)} instead. */ @SuppressWarnings("removal") @Deprecated(forRemoval = true, since = "2025") @@ -150,50 +143,30 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria * * @param velocity The velocity setpoint. * @return The computed feedforward. - * @deprecated Use the current/next velocity overload instead. */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") public double calculate(double velocity) { return calculate(velocity, 0); } - /** - * Calculates the feedforward from the gains and setpoints assuming discrete control when the - * setpoint does not change. - * - * @param The velocity parameter either as distance or angle. - * @param setpoint The velocity setpoint. - * @return The computed feedforward. - */ - public Voltage calculate(Measure> setpoint) { - return calculate(setpoint, setpoint); - } - /** * Calculates the feedforward from the gains and setpoints assuming discrete control. * *

Note this method is inaccurate when the velocity crosses 0. * - * @param The velocity parameter either as distance or angle. * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. * @return The computed feedforward. */ - public Voltage calculate( - Measure> currentVelocity, - Measure> nextVelocity) { + public double calculateWithVelocities(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Simple_motor_feedforward for derivation if (ka == 0.0) { - return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude()); + return ks * Math.signum(nextVelocity) + kv * nextVelocity; } else { double A = -kv / ka; double B = 1.0 / ka; double A_d = Math.exp(A * m_dt); double B_d = 1.0 / A * (A_d - 1.0) * B; - return Volts.of( - ks * Math.signum(currentVelocity.magnitude()) - + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude())); + return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); } } diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 932172810d..87099047f5 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -84,14 +84,15 @@ class SimpleMotorFeedforward { } /** - * Calculates the feedforward from the gains and setpoint assuming discrete - * control. Use this method when the setpoint does not change. + * Calculates the feedforward from the gains and velocity setpoint assuming + * discrete control. Use this method when the velocity setpoint does not + * change. * - * @param setpoint The velocity setpoint. + * @param velocity The velocity setpoint. * @return The computed feedforward, in volts. */ - constexpr units::volt_t Calculate(units::unit_t setpoint) const { - return Calculate(setpoint, setpoint); + constexpr units::volt_t Calculate(units::unit_t velocity) const { + return Calculate(velocity, velocity); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index 1fd6f58d25..3c0a74835a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -31,23 +30,23 @@ class SimpleMotorFeedforwardTest { var r = VecBuilder.fill(2.0); var nextR = VecBuilder.fill(3.0); - var currentVelocity = RadiansPerSecond.mutable(2.0); - var nextVelocity = RadiansPerSecond.mutable(3.0); + double currentVelocity = 2.0; // rad/s + double nextVelocity = 3.0; // rad/s assertEquals( 37.52499583432516 + 0.5, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 0.002); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 0.002); // These won't match exactly. It's just an approximation to make sure they're // in the same ballpark. assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 2.0); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 1cedcae1de..52af40e89b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -4,8 +4,6 @@ package edu.wpi.first.math.trajectory; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -53,39 +51,27 @@ class DifferentialDriveVoltageConstraintTest { assertAll( () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.leftMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.leftMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.leftMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.leftMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.rightMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.rightMetersPerSecond, + wheelSpeeds.rightMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.rightMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.rightMetersPerSecond, + wheelSpeeds.rightMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java index 80d6abb41c..2f4ffac2f2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.trajectory; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; @@ -44,11 +43,9 @@ class ExponentialProfileTest { private static ExponentialProfile.State checkDynamics( ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) { var next = profile.calculate(kDt, current, goal); - var currentVelocity = RadiansPerSecond.mutable(current.velocity); - var nextVelocity = RadiansPerSecond.mutable(next.velocity); - var signal = feedforward.calculate(currentVelocity, nextVelocity); + var signal = feedforward.calculateWithVelocities(current.velocity, next.velocity); - assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9); + assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9); return next; } From 5c95966d44f29140772b913936be8943ebcd4a0d Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sat, 7 Dec 2024 13:56:05 -0500 Subject: [PATCH 07/10] Fix epilogue loading optional types from the newcommands vendordep (#7505) --- .../epilogue/processor/SendableHandler.java | 44 +++++++++++++------ 1 file changed, 30 insertions(+), 14 deletions(-) diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java index 3e9af00801..d25ce09d31 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java @@ -4,24 +4,32 @@ package edu.wpi.first.epilogue.processor; +import java.util.Optional; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; public class SendableHandler extends ElementHandler { - private final TypeMirror m_sendableType; - private final TypeMirror m_commandType; - private final TypeMirror m_subsystemType; + private final Optional m_sendableType; + private final Optional m_commandType; + private final Optional m_subsystemType; protected SendableHandler(ProcessingEnvironment processingEnv) { super(processingEnv); m_sendableType = - lookupTypeElement(processingEnv, "edu.wpi.first.util.sendable.Sendable").asType(); + Optional.ofNullable( + lookupTypeElement(processingEnv, "edu.wpi.first.util.sendable.Sendable")) + .map(TypeElement::asType); m_commandType = - lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.Command").asType(); + Optional.ofNullable( + lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.Command")) + .map(TypeElement::asType); m_subsystemType = - lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.SubsystemBase").asType(); + Optional.ofNullable( + lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.SubsystemBase")) + .map(TypeElement::asType); } @Override @@ -30,20 +38,28 @@ public class SendableHandler extends ElementHandler { // Accept any sendable type. However, the log invocation will return null // for sendable types that should not be logged (commands, subsystems) - return m_processingEnv.getTypeUtils().isAssignable(dataType, m_sendableType); + return m_sendableType + .map(t -> m_processingEnv.getTypeUtils().isAssignable(dataType, t)) + .orElse(false); } @Override public String logInvocation(Element element) { var dataType = dataType(element); - if (m_processingEnv.getTypeUtils().isAssignable(dataType, m_commandType) - || m_processingEnv.getTypeUtils().isAssignable(dataType, m_subsystemType)) { - // Do not log commands or subsystems via their sendable implementations - // We accept all sendable objects to prevent them from being reported as not loggable, - // but their sendable implementations do not include helpful information. - // Users are free to provide custom logging implementations for commands, and tag their - // subsystems with @Logged to log their contents automatically + // Do not log commands or subsystems via their sendable implementations + // We accept all sendable objects to prevent them from being reported as not loggable, + // but their sendable implementations do not include helpful information. + // Users are free to provide custom logging implementations for commands, and tag their + // subsystems with @Logged to log their contents automatically + if (m_commandType + .map(t -> m_processingEnv.getTypeUtils().isAssignable(dataType, t)) + .orElse(false)) { + return null; + } + if (m_subsystemType + .map(t -> m_processingEnv.getTypeUtils().isAssignable(dataType, t)) + .orElse(false)) { return null; } From e876452967eb8df8b8a950d9170ffe66aed13fef Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 11:40:24 -0800 Subject: [PATCH 08/10] [ci] Upgrade to wpiformat 2024.50 (#7506) --- .github/workflows/comment-command.yml | 2 +- .github/workflows/lint-format.yml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/comment-command.yml b/.github/workflows/comment-command.yml index 4e73337487..09c2428dec 100644 --- a/.github/workflows/comment-command.yml +++ b/.github/workflows/comment-command.yml @@ -43,7 +43,7 @@ jobs: distribution: 'temurin' java-version: 17 - name: Install wpiformat - run: pip3 install wpiformat==2024.45 + run: pip3 install wpiformat==2024.50 - name: Run wpiformat run: wpiformat - name: Run spotlessApply diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index 8de7bd9c20..9b75d4ff89 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -27,7 +27,7 @@ jobs: with: python-version: '3.12' - name: Install wpiformat - run: pip3 install wpiformat==2024.45 + run: pip3 install wpiformat==2024.50 - name: Run run: wpiformat - name: Check output @@ -66,7 +66,7 @@ jobs: with: python-version: '3.12' - name: Install wpiformat - run: pip3 install wpiformat==2024.45 + run: pip3 install wpiformat==2024.50 - name: Create compile_commands.json run: | ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio From 278efa63843f155c0332ac79e85b5b5fcb9d97fa Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 12:36:25 -0800 Subject: [PATCH 09/10] [upstream_utils] Remove Sleipnir patches no longer needed with GCC 11 (#7491) --- .../0001-Remove-using-enum-declarations.patch | 638 ------------------ ...Use-fmtlib.patch => 0001-Use-fmtlib.patch} | 2 +- ...r.patch => 0002-Use-wpi-SmallVector.patch} | 36 +- .../0003-Remove-unsupported-constexpr.patch | 42 -- ...Suppress-clang-tidy-false-positives.patch} | 2 +- .../include/sleipnir/autodiff/Expression.hpp | 161 +++-- .../optimization/SolverExitCondition.hpp | 22 +- 7 files changed, 143 insertions(+), 760 deletions(-) delete mode 100644 upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch rename upstream_utils/sleipnir_patches/{0002-Use-fmtlib.patch => 0001-Use-fmtlib.patch} (98%) rename upstream_utils/sleipnir_patches/{0004-Use-wpi-SmallVector.patch => 0002-Use-wpi-SmallVector.patch} (93%) delete mode 100644 upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch rename upstream_utils/sleipnir_patches/{0005-Suppress-clang-tidy-false-positives.patch => 0003-Suppress-clang-tidy-false-positives.patch} (96%) diff --git a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch deleted file mode 100644 index 2a74fa6a84..0000000000 --- a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch +++ /dev/null @@ -1,638 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 24 Apr 2024 15:56:06 -0700 -Subject: [PATCH 1/5] Remove "using enum" declarations - ---- - include/sleipnir/autodiff/Expression.hpp | 161 +++++++----------- - .../optimization/SolverExitCondition.hpp | 22 ++- - 2 files changed, 73 insertions(+), 110 deletions(-) - -diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..51070613e82cdf5e4105519f39632deb5d2bf19e 100644 ---- a/include/sleipnir/autodiff/Expression.hpp -+++ b/include/sleipnir/autodiff/Expression.hpp -@@ -203,8 +203,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero -@@ -219,20 +217,22 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value * rhs->value); - } - - // Evaluate expression type - ExpressionType type; -- if (lhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant) { - type = rhs->type; -- } else if (rhs->type == kConstant) { -+ } else if (rhs->type == ExpressionType::kConstant) { - type = lhs->type; -- } else if (lhs->type == kLinear && rhs->type == kLinear) { -- type = kQuadratic; -+ } else if (lhs->type == ExpressionType::kLinear && -+ rhs->type == ExpressionType::kLinear) { -+ type = ExpressionType::kQuadratic; - } else { -- type = kNonlinear; -+ type = ExpressionType::kNonlinear; - } - - return MakeExpressionPtr( -@@ -258,8 +258,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero -@@ -269,16 +267,17 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value / rhs->value); - } - - // Evaluate expression type - ExpressionType type; -- if (rhs->type == kConstant) { -+ if (rhs->type == ExpressionType::kConstant) { - type = lhs->type; - } else { -- type = kNonlinear; -+ type = ExpressionType::kNonlinear; - } - - return MakeExpressionPtr( -@@ -306,8 +305,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs == nullptr || lhs->IsConstant(0.0)) { - return rhs; -@@ -316,7 +313,8 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value + rhs->value); - } - -@@ -340,8 +338,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - if (rhs->IsConstant(0.0)) { -@@ -355,7 +351,8 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value - rhs->value); - } - -@@ -377,8 +374,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - * @param lhs Operand of unary minus. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero -@@ -386,7 +381,7 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(-lhs->value); - } - -@@ -469,8 +464,6 @@ inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -478,12 +471,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::abs(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::abs(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::abs(x); }, - [](double x, double, double parentAdjoint) { - if (x < 0.0) { - return -parentAdjoint; -@@ -514,20 +507,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(std::numbers::pi / 2.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::acos(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::acos(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::acos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint / std::sqrt(1.0 - x * x); - }, -@@ -546,8 +537,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -555,12 +544,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::asin(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::asin(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::asin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / std::sqrt(1.0 - x * x); - }, -@@ -579,8 +568,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -588,12 +575,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::atan(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::atan(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::atan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (1.0 + x * x); - }, -@@ -612,8 +599,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - const ExpressionPtr& y, const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (y->IsConstant(0.0)) { - // Return zero -@@ -623,12 +608,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - } - - // Evaluate constant -- if (y->type == kConstant && x->type == kConstant) { -+ if (y->type == ExpressionType::kConstant && -+ x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::atan2(y->value, x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double y, double x) { return std::atan2(y, x); }, -+ ExpressionType::kNonlinear, -+ [](double y, double x) { return std::atan2(y, x); }, - [](double y, double x, double parentAdjoint) { - return parentAdjoint * x / (y * y + x * x); - }, -@@ -653,20 +640,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::cos(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::cos(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::cos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint * std::sin(x); - }, -@@ -684,20 +669,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::cosh(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::cosh(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::cosh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::sinh(x); - }, -@@ -715,8 +698,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -724,12 +705,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::erf(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::erf(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::erf(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * - std::exp(-x * x); -@@ -750,20 +731,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::exp(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::exp(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::exp(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::exp(x); - }, -@@ -782,8 +761,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - const ExpressionPtr& x, const ExpressionPtr& y) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return y; -@@ -792,12 +769,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant && y->type == kConstant) { -+ if (x->type == ExpressionType::kConstant && -+ y->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::hypot(x->value, y->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double y) { return std::hypot(x, y); }, -+ ExpressionType::kNonlinear, -+ [](double x, double y) { return std::hypot(x, y); }, - [](double x, double y, double parentAdjoint) { - return parentAdjoint * x / std::hypot(x, y); - }, -@@ -822,8 +801,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -831,12 +808,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::log(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::log(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::log(x); }, - [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, -@@ -850,8 +827,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -859,12 +834,13 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::log10(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::log10(x); }, -+ ExpressionType::kNonlinear, -+ [](double x, double) { return std::log10(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::numbers::ln10 * x); - }, -@@ -883,8 +859,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - const ExpressionPtr& base, const ExpressionPtr& power) { -- using enum ExpressionType; -- - // Prune expression - if (base->IsConstant(0.0)) { - // Return zero -@@ -899,12 +873,15 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - } - - // Evaluate constant -- if (base->type == kConstant && power->type == kConstant) { -+ if (base->type == ExpressionType::kConstant && -+ power->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::pow(base->value, power->value)); - } - - return MakeExpressionPtr( -- base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, -+ base->type == ExpressionType::kLinear && power->IsConstant(2.0) -+ ? ExpressionType::kQuadratic -+ : ExpressionType::kNonlinear, - [](double base, double power) { return std::pow(base, power); }, - [](double base, double power, double parentAdjoint) { - return parentAdjoint * std::pow(base, power - 1) * power; -@@ -945,10 +922,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - * @param x The argument. - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - if (x->value < 0.0) { - return MakeExpressionPtr(-1.0); - } else if (x->value == 0.0) { -@@ -960,7 +935,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { - } - - return MakeExpressionPtr( -- kNonlinear, -+ ExpressionType::kNonlinear, - [](double x, double) { - if (x < 0.0) { - return -1.0; -@@ -985,8 +960,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -994,12 +967,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::sin(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::sin(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::sin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cos(x); - }, -@@ -1016,8 +989,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - * @param x The argument. - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -1025,12 +996,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::sinh(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::sinh(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::sinh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cosh(x); - }, -@@ -1048,10 +1019,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - if (x->value == 0.0) { - // Return zero - return x; -@@ -1063,7 +1032,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::sqrt(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::sqrt(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (2.0 * std::sqrt(x)); - }, -@@ -1082,8 +1051,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -1091,12 +1058,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::tan(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::tan(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::tan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cos(x) * std::cos(x)); - }, -@@ -1114,8 +1081,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - * @param x The argument. - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -1123,12 +1088,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::tanh(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::tanh(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::tanh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cosh(x) * std::cosh(x)); - }, -diff --git a/include/sleipnir/optimization/SolverExitCondition.hpp b/include/sleipnir/optimization/SolverExitCondition.hpp -index 7d1445297e33e3c62bcdf9d03eebeaad20af9a1c..734cd3d127327e8ce01e1a42fe74ccc81fea1f90 100644 ---- a/include/sleipnir/optimization/SolverExitCondition.hpp -+++ b/include/sleipnir/optimization/SolverExitCondition.hpp -@@ -46,31 +46,29 @@ enum class SolverExitCondition : int8_t { - */ - SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( - const SolverExitCondition& exitCondition) { -- using enum SolverExitCondition; -- - switch (exitCondition) { -- case kSuccess: -+ case SolverExitCondition::kSuccess: - return "solved to desired tolerance"; -- case kSolvedToAcceptableTolerance: -+ case SolverExitCondition::kSolvedToAcceptableTolerance: - return "solved to acceptable tolerance"; -- case kCallbackRequestedStop: -+ case SolverExitCondition::kCallbackRequestedStop: - return "callback requested stop"; -- case kTooFewDOFs: -+ case SolverExitCondition::kTooFewDOFs: - return "problem has too few degrees of freedom"; -- case kLocallyInfeasible: -+ case SolverExitCondition::kLocallyInfeasible: - return "problem is locally infeasible"; -- case kFeasibilityRestorationFailed: -+ case SolverExitCondition::kFeasibilityRestorationFailed: - return "solver failed to reach the desired tolerance, and feasibility " - "restoration failed to converge"; -- case kNonfiniteInitialCostOrConstraints: -+ case SolverExitCondition::kNonfiniteInitialCostOrConstraints: - return "solver encountered nonfinite initial cost or constraints and " - "gave up"; -- case kDivergingIterates: -+ case SolverExitCondition::kDivergingIterates: - return "solver encountered diverging primal iterates xₖ and/or sₖ and " - "gave up"; -- case kMaxIterationsExceeded: -+ case SolverExitCondition::kMaxIterationsExceeded: - return "solution returned after maximum iterations exceeded"; -- case kTimeout: -+ case SolverExitCondition::kTimeout: - return "solution returned after maximum wall clock time exceeded"; - default: - return "unknown"; diff --git a/upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch similarity index 98% rename from upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch rename to upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index e5779dc71f..deaaf7b5cb 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 2/5] Use fmtlib +Subject: [PATCH 1/3] Use fmtlib --- include/.styleguide | 1 + diff --git a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch similarity index 93% rename from upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch rename to upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 550983549d..61e887791c 100644 --- a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -1,11 +1,11 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 16 Jun 2024 12:08:49 -0700 -Subject: [PATCH 4/5] Use wpi::SmallVector +Subject: [PATCH 2/3] Use wpi::SmallVector --- include/.styleguide | 1 + - include/sleipnir/autodiff/Expression.hpp | 5 +++-- + include/sleipnir/autodiff/Expression.hpp | 13 +++++++------ include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- include/sleipnir/autodiff/Hessian.hpp | 4 ++-- include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- @@ -19,7 +19,7 @@ Subject: [PATCH 4/5] Use wpi::SmallVector src/optimization/solver/InteriorPoint.cpp | 4 ++-- .../solver/util/FeasibilityRestoration.hpp | 10 +++++----- src/optimization/solver/util/Filter.hpp | 4 ++-- - 15 files changed, 50 insertions(+), 44 deletions(-) + 15 files changed, 54 insertions(+), 48 deletions(-) diff --git a/include/.styleguide b/include/.styleguide index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644 @@ -32,7 +32,7 @@ index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f185 + ^wpi/ } diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index dff8e2a6ef24413e3e6356bf0ec57286e50654cf..bdbeb4730223f88cfdc0c424a8f7b852b34742f7 100644 +index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp @@ -11,11 +11,12 @@ @@ -49,7 +49,33 @@ index dff8e2a6ef24413e3e6356bf0ec57286e50654cf..bdbeb4730223f88cfdc0c424a8f7b852 namespace sleipnir::detail { -@@ -427,7 +428,7 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { +@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true; + + struct SLEIPNIR_DLLEXPORT Expression; + +-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); +-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr); ++inline void IntrusiveSharedPtrIncRefCount(Expression* expr); ++inline void IntrusiveSharedPtrDecRefCount(Expression* expr); + + /** + * Typedef for intrusive shared pointer to Expression. +@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); + * + * @param expr The shared pointer's managed object. + */ +-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { ++inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { + ++expr->refCount; + } + +@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { + * + * @param expr The shared pointer's managed object. + */ +-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { ++inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { + // If a deeply nested tree is being deallocated all at once, calling the // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their // refcounts and deallocate them. diff --git a/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch b/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch deleted file mode 100644 index 853c4d2988..0000000000 --- a/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch +++ /dev/null @@ -1,42 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Mon, 20 May 2024 09:01:54 -0700 -Subject: [PATCH 3/5] Remove unsupported constexpr - ---- - include/sleipnir/autodiff/Expression.hpp | 8 ++++---- - 1 file changed, 4 insertions(+), 4 deletions(-) - -diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index 51070613e82cdf5e4105519f39632deb5d2bf19e..dff8e2a6ef24413e3e6356bf0ec57286e50654cf 100644 ---- a/include/sleipnir/autodiff/Expression.hpp -+++ b/include/sleipnir/autodiff/Expression.hpp -@@ -29,8 +29,8 @@ inline constexpr bool kUsePoolAllocator = true; - - struct SLEIPNIR_DLLEXPORT Expression; - --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr); -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr); -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr); - - /** - * Typedef for intrusive shared pointer to Expression. -@@ -413,7 +413,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); - * - * @param expr The shared pointer's managed object. - */ --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { - ++expr->refCount; - } - -@@ -422,7 +422,7 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { - * - * @param expr The shared pointer's managed object. - */ --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { - // If a deeply nested tree is being deallocated all at once, calling the - // Expression destructor when expr's refcount reaches zero can cause a stack - // overflow. Instead, we iterate over its children to decrement their diff --git a/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch similarity index 96% rename from upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch rename to upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch index a7f3de198e..90ae6e261b 100644 --- a/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 26 Jun 2024 12:13:33 -0700 -Subject: [PATCH 5/5] Suppress clang-tidy false positives +Subject: [PATCH 3/3] Suppress clang-tidy false positives --- include/sleipnir/autodiff/Variable.hpp | 4 ++-- diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp index bdbeb47302..ef9a15bf69 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp @@ -204,6 +204,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { // Return zero @@ -218,22 +220,20 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value * rhs->value); } // Evaluate expression type ExpressionType type; - if (lhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant) { type = rhs->type; - } else if (rhs->type == ExpressionType::kConstant) { + } else if (rhs->type == kConstant) { type = lhs->type; - } else if (lhs->type == ExpressionType::kLinear && - rhs->type == ExpressionType::kLinear) { - type = ExpressionType::kQuadratic; + } else if (lhs->type == kLinear && rhs->type == kLinear) { + type = kQuadratic; } else { - type = ExpressionType::kNonlinear; + type = kNonlinear; } return MakeExpressionPtr( @@ -259,6 +259,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { // Return zero @@ -268,17 +270,16 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value / rhs->value); } // Evaluate expression type ExpressionType type; - if (rhs->type == ExpressionType::kConstant) { + if (rhs->type == kConstant) { type = lhs->type; } else { - type = ExpressionType::kNonlinear; + type = kNonlinear; } return MakeExpressionPtr( @@ -306,6 +307,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs == nullptr || lhs->IsConstant(0.0)) { return rhs; @@ -314,8 +317,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value + rhs->value); } @@ -339,6 +341,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { if (rhs->IsConstant(0.0)) { @@ -352,8 +356,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value - rhs->value); } @@ -375,6 +378,8 @@ struct SLEIPNIR_DLLEXPORT Expression { * @param lhs Operand of unary minus. */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { // Return zero @@ -382,7 +387,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant) { return MakeExpressionPtr(-lhs->value); } @@ -465,6 +470,8 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -472,12 +479,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::abs(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::abs(x); }, + kNonlinear, [](double x, double) { return std::abs(x); }, [](double x, double, double parentAdjoint) { if (x < 0.0) { return -parentAdjoint; @@ -508,18 +515,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(std::numbers::pi / 2.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::acos(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::acos(x); }, + kNonlinear, [](double x, double) { return std::acos(x); }, [](double x, double, double parentAdjoint) { return -parentAdjoint / std::sqrt(1.0 - x * x); }, @@ -538,6 +547,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -545,12 +556,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::asin(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::asin(x); }, + kNonlinear, [](double x, double) { return std::asin(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / std::sqrt(1.0 - x * x); }, @@ -569,6 +580,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -576,12 +589,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::atan(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::atan(x); }, + kNonlinear, [](double x, double) { return std::atan(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (1.0 + x * x); }, @@ -600,6 +613,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT const ExpressionPtr& y, const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (y->IsConstant(0.0)) { // Return zero @@ -609,14 +624,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT } // Evaluate constant - if (y->type == ExpressionType::kConstant && - x->type == ExpressionType::kConstant) { + if (y->type == kConstant && x->type == kConstant) { return MakeExpressionPtr(std::atan2(y->value, x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, - [](double y, double x) { return std::atan2(y, x); }, + kNonlinear, [](double y, double x) { return std::atan2(y, x); }, [](double y, double x, double parentAdjoint) { return parentAdjoint * x / (y * y + x * x); }, @@ -641,18 +654,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(1.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::cos(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::cos(x); }, + kNonlinear, [](double x, double) { return std::cos(x); }, [](double x, double, double parentAdjoint) { return -parentAdjoint * std::sin(x); }, @@ -670,18 +685,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(1.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::cosh(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::cosh(x); }, + kNonlinear, [](double x, double) { return std::cosh(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::sinh(x); }, @@ -699,6 +716,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -706,12 +725,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::erf(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::erf(x); }, + kNonlinear, [](double x, double) { return std::erf(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * std::exp(-x * x); @@ -732,18 +751,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(1.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::exp(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::exp(x); }, + kNonlinear, [](double x, double) { return std::exp(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::exp(x); }, @@ -762,6 +783,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT const ExpressionPtr& x, const ExpressionPtr& y) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return y; @@ -770,14 +793,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant && - y->type == ExpressionType::kConstant) { + if (x->type == kConstant && y->type == kConstant) { return MakeExpressionPtr(std::hypot(x->value, y->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, - [](double x, double y) { return std::hypot(x, y); }, + kNonlinear, [](double x, double y) { return std::hypot(x, y); }, [](double x, double y, double parentAdjoint) { return parentAdjoint * x / std::hypot(x, y); }, @@ -802,6 +823,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -809,12 +832,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::log(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::log(x); }, + kNonlinear, [](double x, double) { return std::log(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, [](const ExpressionPtr& x, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, @@ -828,6 +851,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -835,13 +860,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::log10(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, - [](double x, double) { return std::log10(x); }, + kNonlinear, [](double x, double) { return std::log10(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (std::numbers::ln10 * x); }, @@ -860,6 +884,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT const ExpressionPtr& base, const ExpressionPtr& power) { + using enum ExpressionType; + // Prune expression if (base->IsConstant(0.0)) { // Return zero @@ -874,15 +900,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT } // Evaluate constant - if (base->type == ExpressionType::kConstant && - power->type == ExpressionType::kConstant) { + if (base->type == kConstant && power->type == kConstant) { return MakeExpressionPtr(std::pow(base->value, power->value)); } return MakeExpressionPtr( - base->type == ExpressionType::kLinear && power->IsConstant(2.0) - ? ExpressionType::kQuadratic - : ExpressionType::kNonlinear, + base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, [](double base, double power) { return std::pow(base, power); }, [](double base, double power, double parentAdjoint) { return parentAdjoint * std::pow(base, power - 1) * power; @@ -923,8 +946,10 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { + using enum ExpressionType; + // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { if (x->value < 0.0) { return MakeExpressionPtr(-1.0); } else if (x->value == 0.0) { @@ -936,7 +961,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { } return MakeExpressionPtr( - ExpressionType::kNonlinear, + kNonlinear, [](double x, double) { if (x < 0.0) { return -1.0; @@ -961,6 +986,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -968,12 +995,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::sin(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::sin(x); }, + kNonlinear, [](double x, double) { return std::sin(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::cos(x); }, @@ -990,6 +1017,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -997,12 +1026,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::sinh(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::sinh(x); }, + kNonlinear, [](double x, double) { return std::sinh(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::cosh(x); }, @@ -1020,8 +1049,10 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { if (x->value == 0.0) { // Return zero return x; @@ -1033,7 +1064,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::sqrt(x); }, + kNonlinear, [](double x, double) { return std::sqrt(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (2.0 * std::sqrt(x)); }, @@ -1052,6 +1083,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -1059,12 +1092,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::tan(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::tan(x); }, + kNonlinear, [](double x, double) { return std::tan(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (std::cos(x) * std::cos(x)); }, @@ -1082,6 +1115,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -1089,12 +1124,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::tanh(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::tanh(x); }, + kNonlinear, [](double x, double) { return std::tanh(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (std::cosh(x) * std::cosh(x)); }, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp index 734cd3d127..7d1445297e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp @@ -46,29 +46,31 @@ enum class SolverExitCondition : int8_t { */ SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( const SolverExitCondition& exitCondition) { + using enum SolverExitCondition; + switch (exitCondition) { - case SolverExitCondition::kSuccess: + case kSuccess: return "solved to desired tolerance"; - case SolverExitCondition::kSolvedToAcceptableTolerance: + case kSolvedToAcceptableTolerance: return "solved to acceptable tolerance"; - case SolverExitCondition::kCallbackRequestedStop: + case kCallbackRequestedStop: return "callback requested stop"; - case SolverExitCondition::kTooFewDOFs: + case kTooFewDOFs: return "problem has too few degrees of freedom"; - case SolverExitCondition::kLocallyInfeasible: + case kLocallyInfeasible: return "problem is locally infeasible"; - case SolverExitCondition::kFeasibilityRestorationFailed: + case kFeasibilityRestorationFailed: return "solver failed to reach the desired tolerance, and feasibility " "restoration failed to converge"; - case SolverExitCondition::kNonfiniteInitialCostOrConstraints: + case kNonfiniteInitialCostOrConstraints: return "solver encountered nonfinite initial cost or constraints and " "gave up"; - case SolverExitCondition::kDivergingIterates: + case kDivergingIterates: return "solver encountered diverging primal iterates xₖ and/or sₖ and " "gave up"; - case SolverExitCondition::kMaxIterationsExceeded: + case kMaxIterationsExceeded: return "solution returned after maximum iterations exceeded"; - case SolverExitCondition::kTimeout: + case kTimeout: return "solution returned after maximum wall clock time exceeded"; default: return "unknown"; From f772bb141de661d48ee76c87db2716c34d9942d9 Mon Sep 17 00:00:00 2001 From: shueja <32416547+shueja@users.noreply.github.com> Date: Sat, 7 Dec 2024 13:09:35 -0800 Subject: [PATCH 10/10] [epilogue] Add extra parentheses around cast when using varhandle (#7428) * [epilogue] Add extra parentheses around cast when using varhandle * Fixed tests and added one for private suppliers * Fix tests * Formatting fixes * Update epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com> --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com> Co-authored-by: Sam Carlberg --- .../epilogue/processor/ElementHandler.java | 6 +- .../processor/AnnotationProcessorTest.java | 59 ++++++++++++++++++- 2 files changed, 60 insertions(+), 5 deletions(-) diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java index 37d2ce2460..c097d59137 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java @@ -127,8 +127,10 @@ public abstract class ElementHandler { private static String fieldAccess(VariableElement field) { if (field.getModifiers().contains(Modifier.PRIVATE)) { - // (com.example.Foo) $fooField.get(object) - return "(" + field.asType() + ") $" + field.getSimpleName() + ".get(object)"; + // ((com.example.Foo) $fooField.get(object)) + // Extra parentheses so cast evaluates before appended methods + // (e.g. when appending .getAsDouble()) + return "((" + field.asType() + ") $" + field.getSimpleName() + ".get(object))"; } else { // object.fooField return "object." + field.getSimpleName(); diff --git a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java index f211999bfe..ec4e52d5f4 100644 --- a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java +++ b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java @@ -243,7 +243,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", (double) $x.get(object)); + backend.log("x", ((double) $x.get(object))); } } } @@ -252,6 +252,59 @@ class AnnotationProcessorTest { assertLoggerGenerates(source, expectedGeneratedSource); } + @Test + void privateSuppliers() { + String source = + """ + package edu.wpi.first.epilogue; + + import java.util.function.DoubleSupplier; + + @Logged + class Example { + private DoubleSupplier x; + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + import java.lang.invoke.MethodHandles; + import java.lang.invoke.VarHandle; + + public class ExampleLogger extends ClassSpecificLogger { + private static final VarHandle $x; + + static { + try { + var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); + $x = lookup.findVarHandle(Example.class, "x", java.util.function.DoubleSupplier.class); + } catch (ReflectiveOperationException e) { + throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); + } + } + + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("x", ((java.util.function.DoubleSupplier) $x.get(object)).getAsDouble()); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + @Test void privateWithGenerics() { String source = @@ -294,7 +347,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - logSendable(backend.getNested("chooser"), (edu.wpi.first.wpilibj.smartdashboard.SendableChooser) $chooser.get(object)); + logSendable(backend.getNested("chooser"), ((edu.wpi.first.wpilibj.smartdashboard.SendableChooser) $chooser.get(object))); } } } @@ -1503,7 +1556,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - var $$theField = (edu.wpi.first.epilogue.I) $theField.get(object); + var $$theField = ((edu.wpi.first.epilogue.I) $theField.get(object)); if ($$theField instanceof edu.wpi.first.epilogue.Base edu_wpi_first_epilogue_Base) { Epilogue.baseLogger.tryUpdate(backend.getNested("theField"), edu_wpi_first_epilogue_Base, Epilogue.getConfig().errorHandler); } else {