Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2024-12-07 14:11:47 -08:00
parent c8900cadc3 f772bb141d
commit 52b33edcbd
44 changed files with 555 additions and 1044 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.github/workflows/comment-command.yml vendored
View File

@@ -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

7
.github/workflows/gradle.yml vendored
View File

@@ -12,16 +12,13 @@ jobs:
fail-fast: false
matrix:
include:
# - container: wpilib/roborio-cross-ubuntu:2025-22.04
# artifact-name: Athena
# build-options: "-Ponlylinuxathena"
- container: wpilib/systemcore-cross-ubuntu:2025-22.04
artifact-name: SystemCore
build-options: "-Ponlylinuxsystemcore"
- 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

4
.github/workflows/lint-format.yml vendored
View File

@@ -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

4
.github/workflows/sentinel-build.yml vendored
View File

@@ -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

2
buildSrc/build.gradle
View File

@@ -9,5 +9,5 @@ repositories {
}
}
dependencies {
implementation "edu.wpi.first:native-utils:2025.7.1"
implementation "edu.wpi.first:native-utils:2025.9.0"
}

7
developerRobot/README.md
View File

@@ -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
```

50
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java
View File

@@ -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<TypeElement> 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<? extends Element> 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<TypeElement> loggedTypes) {
var annotatedElements = roundEnv.getElementsAnnotatedWith(epilogueAnnotation);
List<String> 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;
}

6
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java
View File

@@ -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();

7
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java
View File

@@ -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

31
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java
View File

@@ -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<ElementHandler> m_handlers;
private final Logged m_defaultConfig =
new Logged() {
@Override
public Class<? extends Annotation> 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<ElementHandler> 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<Element> notSkipped = LoggerGenerator::isNotSkipped;

44
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java
View File

@@ -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<TypeMirror> m_sendableType;
private final Optional<TypeMirror> m_commandType;
private final Optional<TypeMirror> 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;
}

2
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java
View File

@@ -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(" ", "");

202
epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java
View File

@@ -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<Example> {
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<Example> {
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 =
@@ -141,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)));
}
}
}
@@ -150,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<Example> {
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 =
@@ -192,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<java.lang.String>) $chooser.get(object));
logSendable(backend.getNested("chooser"), ((edu.wpi.first.wpilibj.smartdashboard.SendableChooser<java.lang.String>) $chooser.get(object)));
}
}
}
@@ -1401,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 {
@@ -1416,6 +1571,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<Example> {
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 =

2
shared/config.gradle
View File

@@ -16,7 +16,7 @@ nativeUtils {
configureDependencies {
opencvYear = "frc2025"
niLibVersion = "2025.0.0"
opencvVersion = "4.8.0-1"
opencvVersion = "4.10.0-3"
}
}
}

2
shared/opencv.gradle
View File

@@ -1,4 +1,4 @@
def opencvVersion = '4.8.0-1'
def opencvVersion = '4.10.0-3'
if (project.hasProperty('useCpp') && project.useCpp) {
model {

638
upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch
View File

@@ -1,638 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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";

2
upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch → upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
View File

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 2/5] Use fmtlib
Subject: [PATCH 1/3] Use fmtlib
---
include/.styleguide | 1 +

36
upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch → upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
View File

@@ -1,11 +1,11 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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.

42
upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch
View File

@@ -1,42 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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

2
upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch → upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch
View File

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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 ++--

64
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -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<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
private final Consumer<MecanumDriveWheelSpeeds> 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));
}
}

24
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -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<Double, Double> 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

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -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);

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -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);

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -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());
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java
View File

@@ -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;
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
View File

@@ -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);
}
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
View File

@@ -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

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java
View File

@@ -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. */

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -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(

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -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(

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
View File

@@ -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));

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
View File

@@ -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));
}),

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -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 =

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -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);

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -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);

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java
View File

@@ -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(

35
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -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 <U> The velocity parameter either as distance or angle.
* @param setpoint The velocity setpoint.
* @return The computed feedforward.
*/
public <U extends Unit> Voltage calculate(Measure<? extends PerUnit<U, TimeUnit>> setpoint) {
return calculate(setpoint, setpoint);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* <p>Note this method is inaccurate when the velocity crosses 0.
*
* @param <U> 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 <U extends Unit> Voltage calculate(
Measure<? extends PerUnit<U, TimeUnit>> currentVelocity,
Measure<? extends PerUnit<U, TimeUnit>> 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);
}
}

11
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -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<Velocity> setpoint) const {
return Calculate(setpoint, setpoint);
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity) const {
return Calculate(velocity, velocity);
}
/**

161
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp vendored
View File

@@ -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));
},

22
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp vendored
View File

@@ -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";

11
wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View File

@@ -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);
}

38
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -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));
}
}

7
wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
View File

@@ -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;
}