diff --git a/.clang-tidy b/.clang-tidy index e8fe57b952..33d48c6044 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -45,6 +45,8 @@ Checks: -clang-diagnostic-#warnings, -clang-diagnostic-pedantic, clang-analyzer-*, + -clang-analyzer-optin.cplusplus.UninitializedObject, + -clang-analyzer-security.FloatLoopCounter, cppcoreguidelines-slicing, google-build-namespaces, google-explicit-constructor, diff --git a/benchmark/src/main/native/cpp/Main.cpp b/benchmark/src/main/native/cpp/Main.cpp index 1a9c3262a4..eb97dd941e 100644 --- a/benchmark/src/main/native/cpp/Main.cpp +++ b/benchmark/src/main/native/cpp/Main.cpp @@ -22,6 +22,7 @@ void BM_Transform(benchmark::State& state) { auto transform = pose2 - pose1; return units::math::hypot(transform.X(), transform.Y()).value(); }}; + // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) for (auto _ : state) { traveler.Solve(poses, iterations); } @@ -33,6 +34,7 @@ void BM_Twist(benchmark::State& state) { auto twist = (pose2 - pose1).Log(); return units::math::hypot(twist.dx, twist.dy).value(); }}; + // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) for (auto _ : state) { traveler.Solve(poses, iterations); } diff --git a/build.gradle b/build.gradle index 09ed228753..468ff3e137 100644 --- a/build.gradle +++ b/build.gradle @@ -13,14 +13,14 @@ plugins { id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '2023.0.1' id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2025.0' id 'edu.wpi.first.NativeUtils' apply false - id 'edu.wpi.first.GradleJni' version '1.1.0' + id 'edu.wpi.first.GradleJni' version '1.2.0' id 'edu.wpi.first.GradleVsCode' id 'idea' id 'visual-studio' id 'net.ltgt.errorprone' version '4.3.0' apply false - id 'com.gradleup.shadow' version '9.0.0' apply false - id 'com.diffplug.spotless' version '7.2.1' apply false - id 'com.github.spotbugs' version '6.2.3' apply false + id 'com.gradleup.shadow' version '9.1.0' apply false + id 'com.diffplug.spotless' version '8.0.0' apply false + id 'com.github.spotbugs' version '6.4.2' apply false } wpilibVersioning.buildServerMode = project.hasProperty('buildServer') @@ -40,11 +40,11 @@ allprojects { } } -buildScan { - termsOfServiceUrl = 'https://gradle.com/terms-of-service' - termsOfServiceAgree = 'yes' - - publishAlways() +develocity { + buildScan { + termsOfUseUrl = "https://gradle.com/help/legal-terms-of-use" + termsOfUseAgree = "yes" + } } import com.github.spotbugs.snom.Effort diff --git a/cscore/examples/usbcvstream/usbcvstream.cpp b/cscore/examples/usbcvstream/usbcvstream.cpp index ae37d3726c..7a438e2c86 100644 --- a/cscore/examples/usbcvstream/usbcvstream.cpp +++ b/cscore/examples/usbcvstream/usbcvstream.cpp @@ -5,7 +5,6 @@ #include #include -#include "cscore.h" #include "cscore_cv.h" int main() { diff --git a/cscore/src/main/native/cpp/cscore_c.cpp b/cscore/src/main/native/cpp/cscore_c.cpp index b55d697995..9f4a2c38c2 100644 --- a/cscore/src/main/native/cpp/cscore_c.cpp +++ b/cscore/src/main/native/cpp/cscore_c.cpp @@ -44,7 +44,7 @@ static O* ConvertToC(std::vector&& in, int* count) { // retain vector at end of returned array alignas(T) unsigned char buf[sizeof(T)]; new (buf) T(std::move(in)); - std::memcpy(out + size * sizeof(O), buf, sizeof(T)); + std::memcpy(out + size, buf, sizeof(T)); return out; } @@ -392,7 +392,7 @@ void CS_FreeEvents(CS_Event* arr, int count) { // destroy vector saved at end of array using T = std::vector; alignas(T) unsigned char buf[sizeof(T)]; - std::memcpy(buf, arr + count * sizeof(CS_Event), sizeof(T)); + std::memcpy(buf, arr + count, sizeof(T)); reinterpret_cast(buf)->~T(); std::free(arr); diff --git a/cscore/src/main/native/windows/UsbCameraImpl.cpp b/cscore/src/main/native/windows/UsbCameraImpl.cpp index 1f180a142c..516e33baaa 100644 --- a/cscore/src/main/native/windows/UsbCameraImpl.cpp +++ b/cscore/src/main/native/windows/UsbCameraImpl.cpp @@ -707,8 +707,9 @@ void UsbCameraImpl::DeviceCacheProperty( } NotifyPropertyCreated(*rawIndex, *rawPropPtr); - if (perPropPtr && perIndex) + if (perPropPtr && perIndex) { NotifyPropertyCreated(*perIndex, *perPropPtr); + } } CS_StatusValue UsbCameraImpl::DeviceProcessCommand( diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java index 5dc6ad5b69..15602cd31e 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java @@ -112,7 +112,8 @@ public class AnnotationProcessor extends AbstractProcessor { new MeasureHandler(processingEnv), new PrimitiveHandler(processingEnv), new SupplierHandler(processingEnv), - new StructHandler(processingEnv), // prioritize struct over sendable + new StructHandler(processingEnv), // prioritize struct over sendable and protobuf + new ProtobufHandler(processingEnv), // then protobuf new SendableHandler(processingEnv)); m_epiloguerGenerator = new EpilogueGenerator(processingEnv, customLoggers); diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ProtobufHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ProtobufHandler.java new file mode 100644 index 0000000000..63186b1640 --- /dev/null +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ProtobufHandler.java @@ -0,0 +1,102 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import java.util.Set; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.element.Modifier; +import javax.lang.model.element.TypeElement; +import javax.lang.model.element.VariableElement; +import javax.lang.model.type.TypeMirror; +import javax.lang.model.util.Elements; +import javax.lang.model.util.Types; + +/** + * Supports protobuf serializable types. Protobuf-serializable types are loggable if they have a + * public static final {@code proto} field of a type that inherits from {@code Protobuf}. + */ +public class ProtobufHandler extends ElementHandler { + private final TypeMirror m_serializable; + private final TypeElement m_protobufType; + private final Types m_typeUtils; + private final Elements m_elementUtils; + + protected ProtobufHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + + m_serializable = + processingEnv + .getElementUtils() + .getTypeElement("edu.wpi.first.util.protobuf.ProtobufSerializable") + .asType(); + m_protobufType = + processingEnv.getElementUtils().getTypeElement("edu.wpi.first.util.protobuf.Protobuf"); + m_typeUtils = processingEnv.getTypeUtils(); + m_elementUtils = processingEnv.getElementUtils(); + } + + @Override + public boolean isLoggable(Element element) { + return isLoggableType(dataType(element)); + } + + /** + * Checks if a type is protobuf-serializable: implements the ProtobufSerializable marker interface + * and has a `public static final proto` field of a type that inherits from Protobuf with a + * compatible generic type bound. + * + * @param type The type to check + * @return true if the type is protobuf-serializable, false otherwise + */ + public boolean isLoggableType(TypeMirror type) { + var serializableType = m_typeUtils.erasure(type); + var typeElement = m_elementUtils.getTypeElement(serializableType.toString()); + if (typeElement == null) { + return false; + } + + // eg `Protobuf` instead of the raw `Protobuf` type. The message type doesn't + // really matter here; we can leave it as a wildcard. + var sharpProtobufType = + m_typeUtils.getDeclaredType( + m_protobufType, + typeElement.asType(), // the serializable type + m_typeUtils.getWildcardType( + m_elementUtils.getTypeElement("us.hebi.quickbuf.ProtoMessage").asType(), null)); + + boolean hasProto = + typeElement.getEnclosedElements().stream() + .filter(e -> e instanceof VariableElement) + .map(e -> (VariableElement) e) + .anyMatch( + field -> { + var nameMatch = field.getSimpleName().contentEquals("proto"); + var modifiersMatch = + field + .getModifiers() + .containsAll(Set.of(Modifier.PUBLIC, Modifier.STATIC, Modifier.FINAL)); + var typeMatch = + m_typeUtils.isAssignable( + m_typeUtils.erasure(field.asType()), sharpProtobufType); + return nameMatch && modifiersMatch && typeMatch; + }); + return m_typeUtils.isAssignable(type, m_serializable) && hasProto; + } + + public String protoAccess(TypeMirror serializableType) { + var className = m_typeUtils.erasure(serializableType).toString(); + return className + ".proto"; + } + + @Override + public String logInvocation(Element element, TypeElement loggedClass) { + return "backend.log(\"%s\", %s, %s)" + .formatted( + loggedName(element), + elementAccess(element, loggedClass), + protoAccess(dataType(element))); + } +} diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java index 1c658818e6..a6063227d5 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java @@ -4,15 +4,26 @@ package edu.wpi.first.epilogue.processor; +import java.util.Set; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.Modifier; import javax.lang.model.element.TypeElement; +import javax.lang.model.element.VariableElement; +import javax.lang.model.type.ArrayType; import javax.lang.model.type.TypeMirror; +import javax.lang.model.util.Elements; import javax.lang.model.util.Types; +/** + * Supports struct serializable types. Struct-serializable types are loggable if they have a public + * static final {@code struct} field of a type that inherits from {@code Struct}. + */ public class StructHandler extends ElementHandler { private final TypeMirror m_serializable; + private final TypeElement m_structType; private final Types m_typeUtils; + private final Elements m_elementUtils; protected StructHandler(ProcessingEnvironment processingEnv) { super(processingEnv); @@ -21,16 +32,57 @@ public class StructHandler extends ElementHandler { .getElementUtils() .getTypeElement("edu.wpi.first.util.struct.StructSerializable") .asType(); + m_structType = + processingEnv.getElementUtils().getTypeElement("edu.wpi.first.util.struct.Struct"); m_typeUtils = processingEnv.getTypeUtils(); + m_elementUtils = processingEnv.getElementUtils(); } @Override public boolean isLoggable(Element element) { - return m_typeUtils.isAssignable(dataType(element), m_serializable); + return isLoggableType(dataType(element)); } + /** + * Checks if a type is struct-serializable: implements the StructSerializable marker interface and + * has a `public static final struct` field of a type that inherits from Struct with a compatible + * generic type bound. + * + * @param type The type to check + * @return true if the type is struct-serializable, false otherwise + */ public boolean isLoggableType(TypeMirror type) { - return m_typeUtils.isAssignable(type, m_serializable); + TypeMirror serializableType; + if (type instanceof ArrayType arr) { + serializableType = arr.getComponentType(); + } else { + serializableType = m_typeUtils.erasure(type); + } + var typeElement = m_elementUtils.getTypeElement(serializableType.toString()); + if (typeElement == null) { + return false; + } + + // eg `Struct` instead of the raw `Struct` type + var sharpStructType = m_typeUtils.getDeclaredType(m_structType, typeElement.asType()); + + boolean hasStruct = + typeElement.getEnclosedElements().stream() + .filter(e -> e instanceof VariableElement) + .map(e -> (VariableElement) e) + .anyMatch( + field -> { + var nameMatch = field.getSimpleName().contentEquals("struct"); + var modifiersMatch = + field + .getModifiers() + .containsAll(Set.of(Modifier.PUBLIC, Modifier.STATIC, Modifier.FINAL)); + var typeMatch = + m_typeUtils.isAssignable( + m_typeUtils.erasure(field.asType()), sharpStructType); + return nameMatch && modifiersMatch && typeMatch; + }); + return m_typeUtils.isAssignable(type, m_serializable) && hasStruct; } public String structAccess(TypeMirror serializableType) { diff --git a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java index 65a7a2e736..6b30239fab 100644 --- a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java +++ b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java @@ -1141,6 +1141,76 @@ class AnnotationProcessorTest { assertLoggerGenerates(source, expectedGeneratedSource); } + @Test + void protobuf() { + String source = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.util.protobuf.Protobuf; + import edu.wpi.first.util.protobuf.ProtobufSerializable; + import java.util.List; + import us.hebi.quickbuf.*; + + class ProtobufType implements ProtobufSerializable { + // Message type is necessary - Epilogue can't log with a wildcard message type in the proto + public static final Protobuf proto = null; // value doesn't matter + + static class Message extends ProtoMessage { + // Implement stubs for the abstract base class. + // This code never runs so actual implementations are unnecessary. + @Override + public Message copyFrom(Message other) { return null; } + @Override + public Message clear() { return null; } + @Override + public int computeSerializedSize() { return 0; } + @Override + public void writeTo(ProtoSink output) {} + @Override + public Message mergeFrom(ProtoSource input) { return null; } + @Override + public boolean equals(Object obj) { return false; } + @Override + public Message clone() { return null; } + } + } + + @Logged + class Example { + ProtobufType x; // Should be logged + ProtobufType[] arr1; // Should not be logged + ProtobufType[][] arr2; // Should not be logged + List list; // Should not be logged + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("x", object.x, edu.wpi.first.epilogue.ProtobufType.proto); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + @Test void lists() { String source = diff --git a/epilogue-runtime/BUILD.bazel b/epilogue-runtime/BUILD.bazel index 81dea97630..71398ae89e 100644 --- a/epilogue-runtime/BUILD.bazel +++ b/epilogue-runtime/BUILD.bazel @@ -20,5 +20,6 @@ wpilib_java_junit5_test( deps = [ ":epilogue-java", "//wpiutil:wpiutil-java", + "@maven//:us_hebi_quickbuf_quickbuf_runtime", ], ) diff --git a/epilogue-runtime/build.gradle b/epilogue-runtime/build.gradle index ffcf6c096f..020c0d86b8 100644 --- a/epilogue-runtime/build.gradle +++ b/epilogue-runtime/build.gradle @@ -10,8 +10,9 @@ ext { apply from: "${rootDir}/shared/java/javacommon.gradle" dependencies { + api(project(':datalog')) api(project(':ntcore')) api(project(':wpiutil')) api(project(':wpiunits')) - api(project(':datalog')) + testImplementation(project(':wpimath')) // for convenient protobuf types } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java index f006a319dc..f941b7ca6f 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java @@ -6,8 +6,10 @@ package edu.wpi.first.epilogue.logging; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Unit; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.util.Collection; +import us.hebi.quickbuf.ProtoMessage; /** A backend is a generic interface for Epilogue to log discrete data points. */ public interface EpilogueBackend { @@ -193,6 +195,17 @@ public interface EpilogueBackend { log(identifier, array, struct); } + /** + * Logs a protobuf-serializable object. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + * @param proto the protobuf to use to serialize the data + * @param

the protobuf-serializable type + * @param the protobuf message type + */ + > void log(String identifier, P value, Protobuf proto); + /** * Logs a measurement's value in terms of its base unit. * diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java index c575cd5b35..cc3a1e4544 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java @@ -16,17 +16,20 @@ import edu.wpi.first.datalog.FloatArrayLogEntry; import edu.wpi.first.datalog.FloatLogEntry; import edu.wpi.first.datalog.IntegerArrayLogEntry; import edu.wpi.first.datalog.IntegerLogEntry; +import edu.wpi.first.datalog.ProtobufLogEntry; import edu.wpi.first.datalog.RawLogEntry; import edu.wpi.first.datalog.StringArrayLogEntry; import edu.wpi.first.datalog.StringLogEntry; import edu.wpi.first.datalog.StructArrayLogEntry; import edu.wpi.first.datalog.StructLogEntry; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.util.HashMap; import java.util.HashSet; import java.util.Map; import java.util.Set; import java.util.function.BiFunction; +import us.hebi.quickbuf.ProtoMessage; /** A backend implementation that saves information to a WPILib {@link DataLog} file on disk. */ public class FileBackend implements EpilogueBackend { @@ -34,6 +37,7 @@ public class FileBackend implements EpilogueBackend { private final Map m_entries = new HashMap<>(); private final Map m_subLoggers = new HashMap<>(); private final Set> m_seenSchemas = new HashSet<>(); + private final Set> m_seenProtos = new HashSet<>(); /** * Creates a new file-based backend. @@ -166,4 +170,19 @@ public class FileBackend implements EpilogueBackend { ((StructArrayLogEntry) m_entries.get(identifier)).append(value); } + + @Override + @SuppressWarnings("unchecked") + public > void log(String identifier, P value, Protobuf proto) { + // DataLog.addSchema has checks that we're able to skip, avoiding allocations + if (m_seenProtos.add(proto)) { + m_dataLog.addSchema(proto); + } + + if (!m_entries.containsKey(identifier)) { + m_entries.put(identifier, ProtobufLogEntry.create(m_dataLog, identifier, proto)); + } + + ((ProtobufLogEntry

) m_entries.get(identifier)).append(value); + } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java index bd925165e0..34938e2d98 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java @@ -4,11 +4,13 @@ package edu.wpi.first.epilogue.logging; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.util.Arrays; import java.util.HashMap; import java.util.Map; import java.util.Objects; +import us.hebi.quickbuf.ProtoMessage; /** * A backend implementation that only logs data when it changes. Useful for keeping bandwidth and @@ -243,4 +245,17 @@ public class LazyBackend implements EpilogueBackend { m_previousValues.put(identifier, value.clone()); m_backend.log(identifier, value, struct); } + + @Override + public > void log(String identifier, P value, Protobuf proto) { + var previous = m_previousValues.get(identifier); + + if (Objects.equals(previous, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_backend.log(identifier, value, proto); + } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java index 575fde05b2..c6710a53eb 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java @@ -4,10 +4,12 @@ package edu.wpi.first.epilogue.logging; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.util.HashMap; import java.util.List; import java.util.Map; +import us.hebi.quickbuf.ProtoMessage; /** * A backend implementation that delegates to other backends. Helpful for simultaneous logging to @@ -137,4 +139,11 @@ public class MultiBackend implements EpilogueBackend { backend.log(identifier, value, struct); } } + + @Override + public > void log(String identifier, P value, Protobuf proto) { + for (EpilogueBackend backend : m_backends) { + backend.log(identifier, value, proto); + } + } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java index e398172e77..0af1b82440 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java @@ -13,18 +13,21 @@ import edu.wpi.first.networktables.FloatPublisher; import edu.wpi.first.networktables.IntegerArrayPublisher; import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.Publisher; import edu.wpi.first.networktables.RawPublisher; import edu.wpi.first.networktables.StringArrayPublisher; import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.util.HashMap; import java.util.HashSet; import java.util.Map; import java.util.Set; import java.util.function.Function; +import us.hebi.quickbuf.ProtoMessage; /** * A backend implementation that sends data over network tables. Be careful when using this, since @@ -36,6 +39,7 @@ public class NTEpilogueBackend implements EpilogueBackend { private final Map m_publishers = new HashMap<>(); private final Map m_nestedBackends = new HashMap<>(); private final Set> m_seenSchemas = new HashSet<>(); + private final Set> m_seenProtos = new HashSet<>(); private final Function m_createIntPublisher; private final Function m_createFloatPublisher; private final Function m_createDoublePublisher; @@ -198,4 +202,21 @@ public class NTEpilogueBackend implements EpilogueBackend { publisher.set(value); } } + + @Override + @SuppressWarnings("unchecked") + public > void log(String identifier, P value, Protobuf proto) { + // NetworkTableInstance.addSchema has checks that we're able to skip, avoiding allocations + if (m_seenProtos.add(proto)) { + m_nt.addSchema(proto); + } + + if (m_publishers.containsKey(identifier)) { + ((ProtobufPublisher

) m_publishers.get(identifier)).set(value); + } else { + ProtobufPublisher

publisher = m_nt.getProtobufTopic(identifier, proto).publish(); + m_publishers.put(identifier, publisher); + publisher.set(value); + } + } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java index f288566085..e256b81172 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java @@ -4,9 +4,11 @@ package edu.wpi.first.epilogue.logging; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.util.HashMap; import java.util.Map; +import us.hebi.quickbuf.ProtoMessage; /** * A backend that logs to an underlying backend, prepending all logged data with a specific prefix. @@ -147,4 +149,9 @@ public class NestedBackend implements EpilogueBackend { public void log(String identifier, S[] value, Struct struct) { m_impl.log(withPrefix(identifier), value, struct); } + + @Override + public > void log(String identifier, P value, Protobuf proto) { + m_impl.log(m_prefix + identifier, value, proto); + } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullBackend.java index e78ca0191d..8a2c52f2b6 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullBackend.java @@ -4,7 +4,9 @@ package edu.wpi.first.epilogue.logging; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; +import us.hebi.quickbuf.ProtoMessage; /** Null backend implementation that logs nothing. */ public class NullBackend implements EpilogueBackend { @@ -62,4 +64,8 @@ public class NullBackend implements EpilogueBackend { @Override public void log(String identifier, S[] value, Struct struct) {} + + @Override + public > void log( + String identifier, P value, Protobuf proto) {} } diff --git a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java index d0b394330c..39525d8a9f 100644 --- a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java +++ b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java @@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertArrayEquals; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertSame; +import edu.wpi.first.math.geometry.Rotation2d; import java.util.List; import org.junit.jupiter.api.Test; @@ -185,4 +186,17 @@ class LazyBackendTest { assertArrayEquals( new byte[] {0x01, 0x00, 0x00, 0x00}, (byte[]) backend.getEntries().get(1).value()); } + + @Test + void lazyProtobuf() { + var backend = new TestBackend(); + var lazy = new LazyBackend(backend); + + var rotation = Rotation2d.kZero; + lazy.log("rotation", rotation, Rotation2d.proto); + assertEquals(1, backend.getEntries().size()); + var entry = backend.getEntries().get(0); + assertEquals("rotation", entry.identifier()); + assertArrayEquals(new byte[] {9, 0, 0, 0, 0, 0, 0, 0, 0}, (byte[]) entry.value()); + } } diff --git a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java index 1372921002..90c06b465a 100644 --- a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java +++ b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java @@ -4,12 +4,14 @@ package edu.wpi.first.epilogue.logging; +import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import edu.wpi.first.util.struct.StructBuffer; import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; +import us.hebi.quickbuf.ProtoMessage; @SuppressWarnings("PMD.TestClassWithoutTestCases") // This is not a test class! public class TestBackend implements EpilogueBackend { @@ -114,4 +116,12 @@ public class TestBackend implements EpilogueBackend { m_entries.add(new LogEntry<>(identifier, serialized)); } + + @Override + public > void log(String identifier, P value, Protobuf proto) { + var msg = proto.createMessage(); + proto.pack(msg, value); + var serialized = msg.toByteArray(); + m_entries.add(new LogEntry<>(identifier, serialized)); + } } diff --git a/glass/src/lib/native/cpp/other/PIDController.cpp b/glass/src/lib/native/cpp/other/PIDController.cpp index ae42dee0ca..bfcac82f5a 100644 --- a/glass/src/lib/native/cpp/other/PIDController.cpp +++ b/glass/src/lib/native/cpp/other/PIDController.cpp @@ -31,8 +31,8 @@ void glass::DisplayPIDController(PIDControllerModel* m) { [flag](const char* name, double* v, std::function callback) { ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); - if (ImGui::InputScalar(name, ImGuiDataType_Double, v, NULL, NULL, - "%.3f", flag)) { + if (ImGui::InputScalar(name, ImGuiDataType_Double, v, nullptr, + nullptr, "%.3f", flag)) { callback(*v); } }; diff --git a/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp b/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp index 3c9b75b16b..49e414b85d 100644 --- a/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp +++ b/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp @@ -31,8 +31,8 @@ void glass::DisplayProfiledPIDController(ProfiledPIDControllerModel* m) { [flag](const char* name, double* v, std::function callback) { ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); - if (ImGui::InputScalar(name, ImGuiDataType_Double, v, NULL, NULL, - "%.3f", flag)) { + if (ImGui::InputScalar(name, ImGuiDataType_Double, v, nullptr, + nullptr, "%.3f", flag)) { callback(*v); } }; diff --git a/msvcruntime/build.gradle b/msvcruntime/build.gradle index b8cb6b880e..732652db66 100644 --- a/msvcruntime/build.gradle +++ b/msvcruntime/build.gradle @@ -68,8 +68,8 @@ if (OperatingSystem.current().isWindows()) { artifact x64ZipTask artifactId = "${baseArtifactId}" - groupId artifactGroupId - version wpilibVersioning.version.get() + groupId = artifactGroupId + version = wpilibVersioning.version.get() } } } diff --git a/ntcore/.clang-tidy b/ntcore/.clang-tidy index 41c15bba7d..59a5681cb7 100644 --- a/ntcore/.clang-tidy +++ b/ntcore/.clang-tidy @@ -44,6 +44,7 @@ Checks: -clang-diagnostic-#warnings, -clang-diagnostic-pedantic, clang-analyzer-*, + -clang-analyzer-optin.cplusplus.UninitializedObject, cppcoreguidelines-slicing, google-build-namespaces, google-explicit-constructor, diff --git a/ntcore/src/main/native/cpp/Value_internal.h b/ntcore/src/main/native/cpp/Value_internal.h index fd633f1300..768b9034ce 100644 --- a/ntcore/src/main/native/cpp/Value_internal.h +++ b/ntcore/src/main/native/cpp/Value_internal.h @@ -177,6 +177,7 @@ static_assert(ValidType); static_assert(ValidType>); template +// NOLINTNEXTLINE(google-readability-casting) constexpr bool IsNTType = TypeInfo>::kType == type; static_assert(IsNTType); diff --git a/ntcore/src/main/native/cpp/server/Functions.h b/ntcore/src/main/native/cpp/server/Functions.h index a1b4ac65db..89b7756863 100644 --- a/ntcore/src/main/native/cpp/server/Functions.h +++ b/ntcore/src/main/native/cpp/server/Functions.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include diff --git a/ntcore/src/main/native/include/networktables/ProtobufTopic.h b/ntcore/src/main/native/include/networktables/ProtobufTopic.h index 7a56759f67..43baa87927 100644 --- a/ntcore/src/main/native/include/networktables/ProtobufTopic.h +++ b/ntcore/src/main/native/include/networktables/ProtobufTopic.h @@ -7,9 +7,7 @@ #include #include -#include #include -#include #include #include @@ -207,7 +205,7 @@ class ProtobufPublisher : public Publisher { ProtobufPublisher(ProtobufPublisher&& rhs) : Publisher{std::move(rhs)}, m_msg{std::move(rhs.m_msg)}, - m_schemaPublished{rhs.m_schemaPublished} {} + m_schemaPublished{rhs.m_schemaPublished.load()} {} ProtobufPublisher& operator=(ProtobufPublisher&& rhs) { Publisher::operator=(std::move(rhs)); diff --git a/ntcore/src/test/native/cpp/StructTest.cpp b/ntcore/src/test/native/cpp/StructTest.cpp index 6842edb480..3c66482822 100644 --- a/ntcore/src/test/native/cpp/StructTest.cpp +++ b/ntcore/src/test/native/cpp/StructTest.cpp @@ -157,7 +157,7 @@ namespace nt { class StructTest : public ::testing::Test { public: StructTest() { inst = nt::NetworkTableInstance::Create(); } - ~StructTest() { nt::NetworkTableInstance::Destroy(inst); } + ~StructTest() override { nt::NetworkTableInstance::Destroy(inst); } nt::NetworkTableInstance inst; }; diff --git a/processstarter/src/main/native/linux/main.cpp b/processstarter/src/main/native/linux/main.cpp index 1d2847967e..0ee1bc6b4e 100644 --- a/processstarter/src/main/native/linux/main.cpp +++ b/processstarter/src/main/native/linux/main.cpp @@ -72,8 +72,9 @@ int StartJavaTool(std::filesystem::path& exePath) { std::string data = jarPath; std::string jarArg = "-jar"; - char* const arguments[] = {Java.generic_string().data(), jarArg.data(), - data.data(), nullptr}; + auto javaGenericStr = Java.generic_string(); + char* const arguments[] = {javaGenericStr.data(), jarArg.data(), data.data(), + nullptr}; int status = posix_spawn(&pid, Java.c_str(), nullptr, nullptr, arguments, environ); diff --git a/settings.gradle b/settings.gradle index 57fbacab70..3904a04479 100644 --- a/settings.gradle +++ b/settings.gradle @@ -10,7 +10,7 @@ pluginManagement { } plugins { - id "com.gradle.enterprise" version "3.15.1" + id "com.gradle.develocity" version "4.2" } // Set the flag to tell gradle to ignore unresolved headers diff --git a/shared/java/javacommon.gradle b/shared/java/javacommon.gradle index a66eb99228..fcb0fb24ea 100644 --- a/shared/java/javacommon.gradle +++ b/shared/java/javacommon.gradle @@ -39,11 +39,13 @@ task outputJavadocJar(type: Jar, dependsOn: javadoc) { } artifacts { - archives sourcesJar - archives javadocJar - archives outputJar - archives outputSourcesJar - archives outputJavadocJar + tasks.named("assemble") { + dependsOn(sourcesJar) + dependsOn(javadocJar) + dependsOn(outputJar) + dependsOn(outputSourcesJar) + dependsOn(outputJavadocJar) + } } addTaskToCopyAllOutputs(outputSourcesJar) diff --git a/shared/javacpp/publish.gradle b/shared/javacpp/publish.gradle index 7546388def..5a83be582a 100644 --- a/shared/javacpp/publish.gradle +++ b/shared/javacpp/publish.gradle @@ -50,8 +50,10 @@ task cppHeadersZip(type: Zip) { } artifacts { - archives cppHeadersZip - archives cppSourcesZip + tasks.named("assemble") { + dependsOn(cppHeadersZip) + dependsOn(cppSourcesZip) + } } addTaskToCopyAllOutputs(cppSourcesZip) diff --git a/shared/jni/publish.gradle b/shared/jni/publish.gradle index 9bfe4d0282..0705fa2a98 100644 --- a/shared/jni/publish.gradle +++ b/shared/jni/publish.gradle @@ -62,8 +62,10 @@ task cppHeadersZip(type: Zip) { } artifacts { - archives cppHeadersZip - archives cppSourcesZip + tasks.named("assemble") { + dependsOn(cppHeadersZip) + dependsOn(cppSourcesZip) + } } addTaskToCopyAllOutputs(cppSourcesZip) diff --git a/sysid/publish.gradle b/sysid/publish.gradle index c998accd30..a891b138d9 100644 --- a/sysid/publish.gradle +++ b/sysid/publish.gradle @@ -27,7 +27,7 @@ model { // We are now in the binary that we want. // This is the default application path for the ZIP task. def applicationPath = binary.executable.file - def icon = file("$project.projectDir/src/main/native/mac/ov.icns") + def icon = file("$project.projectDir/src/main/native/mac/sysid.icns") // Create the macOS bundle. def bundleTask = project.tasks.create("bundleSysIdOsxApp" + binary.targetPlatform.architecture.name, Copy) { diff --git a/upstream_utils/eigen.py b/upstream_utils/eigen.py index 6cb3facfa9..222b45e5f2 100755 --- a/upstream_utils/eigen.py +++ b/upstream_utils/eigen.py @@ -77,6 +77,7 @@ def eigen_inclusions(dp: Path, f: str): "SparseLU", "SparseQR", "StdVector", + "Version", "misc", "plugins", ] @@ -144,8 +145,8 @@ def copy_upstream_src(wpilib_root: Path): def main(): name = "eigen" url = "https://gitlab.com/libeigen/eigen.git" - # master on 2025-09-08 - tag = "e0a59e5a66e6d16fa93ab4f5e48bf539205e837f" + # 5.0.0 release as of 2025-09-23 + tag = "d65cda87c1a673047b59b20a9f9e165a452f91e9" eigen = Lib(name, url, tag, copy_upstream_src) eigen.main() diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index c1b0de9a87..8600d0be13 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -52,8 +52,8 @@ using small_vector = wpi::SmallVector; def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2025-05-18 - tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e" + # main on 2025-09-19 + tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index 2baae9b2ab..3677503197 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -4,10 +4,12 @@ Date: Wed, 29 May 2024 16:29:55 -0700 Subject: [PATCH 1/8] Use fmtlib --- - include/.styleguide | 1 + - include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- - src/optimization/problem.cpp | 2 +- - 3 files changed, 20 insertions(+), 14 deletions(-) + include/.styleguide | 1 + + include/sleipnir/util/assert.hpp | 5 +++-- + include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- + src/.styleguide | 1 + + src/optimization/problem.cpp | 1 + + 5 files changed, 24 insertions(+), 15 deletions(-) diff --git a/include/.styleguide b/include/.styleguide index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644 @@ -20,6 +22,31 @@ index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb58 + ^fmt/ ^gch/ } +diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp +index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a928bdc5a9 100644 +--- a/include/sleipnir/util/assert.hpp ++++ b/include/sleipnir/util/assert.hpp +@@ -3,9 +3,10 @@ + #pragma once + + #ifdef JORMUNGANDR +-#include + #include + #include ++ ++#include + /** + * Throw an exception in Python. + */ +@@ -13,7 +14,7 @@ + do { \ + if (!(condition)) { \ + auto location = std::source_location::current(); \ +- throw std::invalid_argument(std::format( \ ++ throw std::invalid_argument(fmt::format( \ + "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ + location.line(), location.function_name(), #condition)); \ + } \ diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644 --- a/include/sleipnir/util/print.hpp @@ -99,16 +126,26 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115 } catch (const std::system_error&) { } } +diff --git a/src/.styleguide b/src/.styleguide +index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644 +--- a/src/.styleguide ++++ b/src/.styleguide +@@ -8,5 +8,6 @@ cppSrcFileInclude { + + includeOtherLibs { + ^Eigen/ ++ ^fmt/ + ^gch/ + } diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp -index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644 +index c3331197e2365934273f57422b79fa18c2b78a5b..09828cdb6d7cddff692b9d17603dc0c11cd5a3ec 100644 --- a/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp -@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { - slp::println(" ↳ executed {} iterations", options.max_iterations); - } - if (std::isfinite(options.timeout.count())) { -- slp::println(" ↳ {} elapsed", options.timeout); -+ slp::println(" ↳ {} elapsed", options.timeout.count()); - } - } +@@ -11,6 +11,7 @@ + #include + #include ++#include + #include + + #include "optimization/bounds.hpp" diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index df21b64a10..63dde99b18 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -10,7 +10,7 @@ Subject: [PATCH 2/8] Use wpi::SmallVector 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp -index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644 +index bb4d8c5641a5b3d633d372674e0a35f857889cd4..53a5f6d68d3153537840c4ff45fe5e5d8b0076b7 100644 --- a/include/sleipnir/autodiff/expression.hpp +++ b/include/sleipnir/autodiff/expression.hpp @@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true; @@ -22,7 +22,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782 /** * Typedef for intrusive shared pointer to Expression. -@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) { +@@ -733,7 +733,7 @@ inline constexpr void inc_ref_count(Expression* expr) { * * @param expr The shared pointer's managed object. */ @@ -32,7 +32,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782 // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644 +index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c050e40e2 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp @@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable { @@ -55,7 +55,7 @@ index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d /** * Assignment operator for double. diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644 +index e1a419ca5356660b3c1c27230d1cb2a86977fb65..349a1550235516f9853609b61feded834ef2894b 100644 --- a/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch index 1b5f998cf8..f4d84bae89 100644 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -9,7 +9,7 @@ Subject: [PATCH 4/8] Replace std::to_underlying() 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp -index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644 +index 09828cdb6d7cddff692b9d17603dc0c11cd5a3ec..886de24cc0532d31f1e186150da79e925f212556 100644 --- a/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp @@ -7,7 +7,6 @@ @@ -20,7 +20,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff #include #include -@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() { +@@ -350,11 +349,11 @@ void Problem::print_problem_analysis() { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", @@ -35,7 +35,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff if (m_decision_variables.size() == 1) { slp::print("\n1 decision variable\n"); -@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() { +@@ -366,7 +365,7 @@ void Problem::print_problem_analysis() { [](const gch::small_vector& constraints) { std::array counts{}; for (const auto& constraint : constraints) { diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch index 2885d850de..f1fd7a7c59 100644 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -9,10 +9,10 @@ Subject: [PATCH 5/8] Replace std::views::zip() 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp -index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644 +index 33b6eee615141a1d6472f116842d62052ef54dd9..b333aebd3e59fa23eed6046c13d736c3d2eccac7 100644 --- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp +++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp -@@ -155,7 +155,10 @@ class AdjointExpressionGraph { +@@ -158,7 +158,10 @@ class AdjointExpressionGraph { } } } else { @@ -25,7 +25,7 @@ index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d if (col != -1 && node->adjoint != 0.0) { triplets.emplace_back(row, col, node->adjoint); diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp -index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644 +index 886de24cc0532d31f1e186150da79e925f212556..e32481e9314c9ef472843adb5bedbd993627d5d9 100644 --- a/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp @@ -6,7 +6,6 @@ @@ -36,7 +36,7 @@ index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba #include #include -@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() { +@@ -367,9 +366,11 @@ void Problem::print_problem_analysis() { for (const auto& constraint : constraints) { ++counts[static_cast(constraint.type())]; } diff --git a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch index c71e612919..109126b697 100644 --- a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch @@ -8,10 +8,10 @@ Subject: [PATCH 6/8] Suppress clang-tidy false positives 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644 +index 264f0950f293c67d6e6c7e729887090c050e40e2..62135a5539308ae69f6b45a64d9337c4c3e96d7b 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp -@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { +@@ -633,7 +633,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param inequality_constraints The list of InequalityConstraints to * concatenate. */ diff --git a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch index a4841664ed..aed8ccc605 100644 --- a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch +++ b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch @@ -8,7 +8,7 @@ Subject: [PATCH 7/8] Suppress GCC 12 warning false positive 1 file changed, 7 insertions(+) diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644 +index 349a1550235516f9853609b61feded834ef2894b..70bccf4fc078a49e22b6699db1228c765430a121 100644 --- a/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { diff --git a/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch index d31ba4bcc7..b04d79f5bc 100644 --- a/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch +++ b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch @@ -11,16 +11,16 @@ This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0. include/sleipnir/autodiff/variable.hpp | 26 +---- include/sleipnir/autodiff/variable_block.hpp | 70 +++++------ include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------ - include/sleipnir/control/ocp.hpp | 14 +-- + include/sleipnir/optimization/ocp.hpp | 14 +-- include/sleipnir/optimization/problem.hpp | 6 +- src/autodiff/variable_matrix.cpp | 66 +++++------ 8 files changed, 118 insertions(+), 182 deletions(-) diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp -index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644 +index fa6d8af0843eca8b674744f02551584dd8d79c21..4f093b7b39ea84e56c4a12ae1b6f645c4f84a1f0 100644 --- a/include/sleipnir/autodiff/hessian.hpp +++ b/include/sleipnir/autodiff/hessian.hpp -@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian { +@@ -106,9 +106,9 @@ class SLEIPNIR_DLLEXPORT Hessian { auto grad = m_graphs[row].generate_gradient_tree(m_wrt); for (int col = 0; col < m_wrt.rows(); ++col) { if (grad[col].expr != nullptr) { @@ -33,10 +33,10 @@ index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67 } } diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp -index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644 +index 4515076cde12a2112e1b5711acc3092bd807e250..3662b5e49b93f63b5ccac0e732149bd9178f1aae 100644 --- a/include/sleipnir/autodiff/jacobian.hpp +++ b/include/sleipnir/autodiff/jacobian.hpp -@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian { +@@ -99,9 +99,9 @@ class SLEIPNIR_DLLEXPORT Jacobian { auto grad = m_graphs[row].generate_gradient_tree(m_wrt); for (int col = 0; col < m_wrt.rows(); ++col) { if (grad[col].expr != nullptr) { @@ -49,10 +49,10 @@ index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a7 } } diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644 +index 62135a5539308ae69f6b45a64d9337c4c3e96d7b..2fc2119d2dedaa5b4c941ce449b7fb113c641635 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp -@@ -505,11 +505,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -512,11 +512,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { for (int row = 0; row < rhs.rows(); ++row) { for (int col = 0; col < rhs.cols(); ++col) { // Make right-hand side zero @@ -65,7 +65,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2 } } } else if constexpr (MatrixLike && ScalarLike) { -@@ -518,11 +514,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -525,11 +521,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { for (int row = 0; row < lhs.rows(); ++row) { for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero @@ -78,7 +78,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2 } } } else if constexpr (MatrixLike && MatrixLike) { -@@ -532,19 +524,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -539,19 +531,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { for (int row = 0; row < lhs.rows(); ++row) { for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero @@ -376,7 +376,7 @@ index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fe } diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644 +index 70bccf4fc078a49e22b6699db1228c765430a121..2ed997819e70c584ce413f639826b6da506e382b 100644 --- a/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { @@ -708,35 +708,35 @@ index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be35621 } } -diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp -index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644 ---- a/include/sleipnir/control/ocp.hpp -+++ b/include/sleipnir/control/ocp.hpp -@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { - if (m_timestep_method == TimestepMethod::FIXED) { +diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp +index 124224cf5ba6e54c141086e3a21389530198449f..74492a0d756a9d587df6158c7e2ef8548ae22be4 100644 +--- a/include/sleipnir/optimization/ocp.hpp ++++ b/include/sleipnir/optimization/ocp.hpp +@@ -122,7 +122,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + if (timestep_method == TimestepMethod::FIXED) { m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i] = m_dt.count(); -+ m_DT(0, i) = m_dt.count(); +- m_DT[0, i] = dt.count(); ++ m_DT(0, i) = dt.count(); } - } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { - Variable dt = decision_variable(); -@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable single_dt = decision_variable(); +@@ -131,12 +131,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { // Set the member variable matrix to track the decision variable m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i] = dt; -+ m_DT(0, i) = dt; +- m_DT[0, i] = single_dt; ++ m_DT(0, i) = single_dt; } - } else if (m_timestep_method == TimestepMethod::VARIABLE) { + } else if (timestep_method == TimestepMethod::VARIABLE) { m_DT = decision_variable(1, m_num_steps + 1); for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i].set_value(m_dt.count()); -+ m_DT(0, i).set_value(m_dt.count()); +- m_DT[0, i].set_value(dt.count()); ++ m_DT(0, i).set_value(dt.count()); } } -@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { +@@ -212,7 +212,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { for (int i = 0; i < m_num_steps + 1; ++i) { auto x = X().col(i); auto u = U().col(i); @@ -745,16 +745,16 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc callback(time, x, u, dt); time += dt; -@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { +@@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ for (int i = 0; i < m_num_steps; ++i) { - Variable h = dt()[0, i]; + Variable h = dt()(0, i); - auto& f = m_dynamics_function; + auto& f = m_dynamics; -@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { +@@ -391,7 +391,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { auto x_begin = X().col(i); auto x_end = X().col(i + 1); auto u = U().col(i); @@ -762,8 +762,8 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc + Variable dt = this->dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - subject_to(x_end == rk4dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - x_end = rk4CFBundleIdentifier edu.wpi.first.tools.WPIcal CFBundleIconFile - ov.icns + wpical.icns CFBundlePackageType APPL CFBundleSupportedPlatforms diff --git a/wpical/src/main/native/cpp/WPIcal.cpp b/wpical/src/main/native/cpp/WPIcal.cpp index f73105a50d..4f26625bba 100644 --- a/wpical/src/main/native/cpp/WPIcal.cpp +++ b/wpical/src/main/native/cpp/WPIcal.cpp @@ -19,10 +19,11 @@ #include #include #include -#include #include #include +#include "tagpose.h" + namespace gui = wpi::gui; const char* GetWPILibVersion(); @@ -140,7 +141,7 @@ static bool EmitEntryTarget(int tag_id, std::string& file) { if (ImGui::BeginDragDropTarget()) { if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload("FieldCalibration")) { - file = *(std::string*)payload->Data; + file = *static_cast(payload->Data); rv = true; } ImGui::EndDragDropTarget(); diff --git a/wpical/src/main/native/cpp/tagpose.cpp b/wpical/src/main/native/cpp/tagpose.cpp index ec871d5651..9a78479655 100644 --- a/wpical/src/main/native/cpp/tagpose.cpp +++ b/wpical/src/main/native/cpp/tagpose.cpp @@ -2,7 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include +#include "tagpose.h" + #include WPI_IGNORE_DEPRECATED diff --git a/wpical/src/main/native/include/fieldmap.h b/wpical/src/main/native/include/fieldmap.h index 5e581c7322..4008bb0f96 100644 --- a/wpical/src/main/native/include/fieldmap.h +++ b/wpical/src/main/native/include/fieldmap.h @@ -7,9 +7,10 @@ #include #include -#include #include +#include "tagpose.h" + class Fieldmap { public: Fieldmap() = default; diff --git a/wpical/src/main/native/include/fmap.h b/wpical/src/main/native/include/fmap.h index 6757b7cada..38a3127e9a 100644 --- a/wpical/src/main/native/include/fmap.h +++ b/wpical/src/main/native/include/fmap.h @@ -4,11 +4,11 @@ #pragma once -#include - -#include #include +#include "fieldmap.h" +#include "tagpose.h" + namespace fmap { wpi::json singleTag(int tag, const tag::Pose& tagpose); wpi::json convertfmap(const wpi::json& json); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h index 4ccfd41328..3315f73d31 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h @@ -53,7 +53,7 @@ class Config { std::optional stepVoltage, std::optional timeout, std::function recordState) - : m_recordState{recordState} { + : m_recordState{std::move(recordState)} { if (rampRate) { m_rampRate = rampRate.value(); } diff --git a/wpilibc/src/test/native/cpp/AlertTest.cpp b/wpilibc/src/test/native/cpp/AlertTest.cpp index b19f0dd0c4..5f233e3342 100644 --- a/wpilibc/src/test/native/cpp/AlertTest.cpp +++ b/wpilibc/src/test/native/cpp/AlertTest.cpp @@ -22,7 +22,7 @@ using namespace frc; using enum Alert::AlertType; class AlertsTest : public ::testing::Test { public: - ~AlertsTest() { + ~AlertsTest() override { // test all destructors Update(); EXPECT_EQ(GetSubscriberForType(kError).Get().size(), 0ul); diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp index e457af9c43..f444d2918b 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp @@ -4,14 +4,16 @@ #include "commands/TeleopArcadeDrive.h" +#include + #include "subsystems/Drivetrain.h" TeleopArcadeDrive::TeleopArcadeDrive( Drivetrain* subsystem, std::function xaxisSpeedSupplier, std::function zaxisRotateSuppplier) : m_drive{subsystem}, - m_xaxisSpeedSupplier{xaxisSpeedSupplier}, - m_zaxisRotateSupplier{zaxisRotateSuppplier} { + m_xaxisSpeedSupplier{std::move(xaxisSpeedSupplier)}, + m_zaxisRotateSupplier{std::move(zaxisRotateSuppplier)} { AddRequirements(subsystem); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java index 3eb6dda871..8a9396df85 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java @@ -19,9 +19,9 @@ public final class NumericalIntegration { } /** - * Performs Runge Kutta integration (4th order). + * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt. * - * @param f The function to integrate, which takes one argument x. + * @param f The function to integrate. It must take one argument x. * @param x The initial value of x. * @param dt The time over which to integrate in seconds. * @return the integration of dx/dt = f(x) for dt. @@ -37,13 +37,13 @@ public final class NumericalIntegration { } /** - * Performs Runge Kutta integration (4th order). + * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt. * * @param f The function to integrate. It must take two arguments x and u. * @param x The initial value of x. * @param u The value u held constant over the integration period. * @param dt The time over which to integrate in seconds. - * @return The result of Runge Kutta integration (4th order). + * @return the integration of dx/dt = f(x, u) for dt. */ public static double rk4(DoubleBinaryOperator f, double x, double u, double dt) { final var h = dt; @@ -89,7 +89,7 @@ public final class NumericalIntegration { * @param f The function to integrate. It must take one argument x. * @param x The initial value of x. * @param dt The time over which to integrate in seconds. - * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt. + * @return the integration of dx/dt = f(x) for dt. */ public static Matrix rk4( UnaryOperator> f, Matrix x, double dt) { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core index 2fedfd3b80..546625bb1f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core @@ -11,6 +11,9 @@ #ifndef EIGEN_CORE_MODULE_H #define EIGEN_CORE_MODULE_H +// Eigen version information. +#include "Version" + // first thing Eigen does: stop the compiler from reporting useless warnings. #include "src/Core/util/DisableStupidWarnings.h" diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version new file mode 100644 index 0000000000..db0f3bf2aa --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version @@ -0,0 +1,14 @@ +#ifndef EIGEN_VERSION_H +#define EIGEN_VERSION_H + +// The "WORLD" version will forever remain "3" for the "Eigen3" library. +#define EIGEN_WORLD_VERSION 3 +// As of Eigen3 5.0.0, we have moved to Semantic Versioning (semver.org). +#define EIGEN_MAJOR_VERSION 5 +#define EIGEN_MINOR_VERSION 0 +#define EIGEN_PATCH_VERSION 0 +#define EIGEN_PRERELEASE_VERSION +#define EIGEN_BUILD_VERSION +#define EIGEN_VERSION_STRING "5.0.0" + +#endif // EIGEN_VERSION_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h index 13a542a023..e4c5fedc3f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h @@ -235,8 +235,7 @@ DenseBase::Constant(const Scalar& value) { * \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&) */ template -EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase< - Derived>::RandomAccessLinSpacedReturnType +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) return DenseBase::NullaryExpr(size, internal::linspaced_op(low, high, size)); @@ -247,8 +246,7 @@ DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const * \sa LinSpaced(const Scalar&, const Scalar&) */ template -EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase< - Derived>::RandomAccessLinSpacedReturnType +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType DenseBase::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h index 0333ad167a..c81e1d109a 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h @@ -306,12 +306,12 @@ class DenseBase EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index size, const Scalar& value); EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(const Scalar& value); - EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, Index size, - const Scalar& low, - const Scalar& high); - EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, - const Scalar& low, - const Scalar& high); + EIGEN_DEPRECATED_WITH_REASON("The method may result in accuracy loss. Use .EqualSpaced() instead.") + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, Index size, const Scalar& low, + const Scalar& high); + EIGEN_DEPRECATED_WITH_REASON("The method may result in accuracy loss. Use .EqualSpaced() instead.") + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, const Scalar& low, + const Scalar& high); EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Index size, const Scalar& low, const Scalar& high); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h index 139b10e8a4..64e11231ed 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h @@ -65,7 +65,7 @@ struct default_packet_traits { HasAbsDiff = 0, HasBlend = 0, // This flag is used to indicate whether packet comparison is supported. - // pcmp_eq, pcmp_lt and pcmp_le should be defined for it to be true. + // pcmp_eq and pcmp_lt should be defined for it to be true. HasCmp = 0, HasDiv = 0, @@ -432,30 +432,6 @@ EIGEN_DEVICE_FUNC inline Packet pzero(const Packet& a) { return pzero_impl::run(a); } -/** \internal \returns a <= b as a bit mask */ -template -EIGEN_DEVICE_FUNC inline Packet pcmp_le(const Packet& a, const Packet& b) { - return a <= b ? ptrue(a) : pzero(a); -} - -/** \internal \returns a < b as a bit mask */ -template -EIGEN_DEVICE_FUNC inline Packet pcmp_lt(const Packet& a, const Packet& b) { - return a < b ? ptrue(a) : pzero(a); -} - -/** \internal \returns a == b as a bit mask */ -template -EIGEN_DEVICE_FUNC inline Packet pcmp_eq(const Packet& a, const Packet& b) { - return a == b ? ptrue(a) : pzero(a); -} - -/** \internal \returns a < b or a==NaN or b==NaN as a bit mask */ -template -EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) { - return a >= b ? pzero(a) : ptrue(a); -} - template struct bit_and { EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a & b; } @@ -582,6 +558,30 @@ EIGEN_DEVICE_FUNC inline Packet pandnot(const Packet& a, const Packet& b) { return pand(a, pnot(b)); } +/** \internal \returns a < b as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_lt(const Packet& a, const Packet& b) { + return a < b ? ptrue(a) : pzero(a); +} + +/** \internal \returns a == b as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_eq(const Packet& a, const Packet& b) { + return a == b ? ptrue(a) : pzero(a); +} + +/** \internal \returns a <= b as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_le(const Packet& a, const Packet& b) { + return por(pcmp_eq(a, b), pcmp_lt(a, b)); +} + +/** \internal \returns a < b or a==NaN or b==NaN as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) { + return a >= b ? pzero(a) : ptrue(a); +} + // In the general case, use bitwise select. template ::value> struct pselect_impl { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h index 8d5c47e472..045993d466 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h @@ -373,12 +373,14 @@ class MatrixBase : public DenseBase { template inline JacobiSVD jacobiSvd() const; template - EIGEN_DEPRECATED inline JacobiSVD jacobiSvd(unsigned int computationOptions) const; + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using method's template parameter.") + inline JacobiSVD jacobiSvd(unsigned int computationOptions) const; template inline BDCSVD bdcSvd() const; template - EIGEN_DEPRECATED inline BDCSVD bdcSvd(unsigned int computationOptions) const; + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using method's template parameter.") + inline BDCSVD bdcSvd(unsigned int computationOptions) const; /////////// Geometry module /////////// @@ -391,7 +393,8 @@ class MatrixBase : public DenseBase { EIGEN_DEVICE_FUNC inline PlainObject unitOrthogonal(void) const; - EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix eulerAngles(Index a0, Index a1, Index a2) const; + EIGEN_DEPRECATED_WITH_REASON("Use .canonicalEulerAngles() instead.") + EIGEN_DEVICE_FUNC inline Matrix eulerAngles(Index a0, Index a1, Index a2) const; EIGEN_DEVICE_FUNC inline Matrix canonicalEulerAngles(Index a0, Index a1, Index a2) const; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h index a230044583..5955e496f6 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h @@ -1264,6 +1264,14 @@ struct generic_product_impl +struct generic_product_impl + : generic_product_impl {}; + +template +struct generic_product_impl + : generic_product_impl {}; + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h index b66a4db7c3..e0119dd760 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h @@ -287,7 +287,7 @@ struct packet_traits : default_packet_traits { AlignedOnScalar = 1, size = 16, - HasCmp = 1, // note -- only pcmp_eq is defined + HasCmp = 1, HasShift = 0, HasAbs = 0, HasAbs2 = 0, @@ -883,7 +883,14 @@ template <> EIGEN_STRONG_INLINE Packet4ui pandnot(const Packet4ui& a, const Packet4ui& b) { return _mm_andnot_si128(b, a); } - +template <> +EIGEN_STRONG_INLINE Packet16b pandnot(const Packet16b& a, const Packet16b& b) { + return _mm_andnot_si128(b, a); +} +template <> +EIGEN_STRONG_INLINE Packet16b pcmp_lt(const Packet16b& a, const Packet16b& b) { + return _mm_andnot_si128(a, b); +} template <> EIGEN_STRONG_INLINE Packet4f pcmp_le(const Packet4f& a, const Packet4f& b) { return _mm_cmple_ps(a, b); @@ -927,7 +934,11 @@ EIGEN_STRONG_INLINE Packet4i pcmp_eq(const Packet4i& a, const Packet4i& b) { } template <> EIGEN_STRONG_INLINE Packet4i pcmp_le(const Packet4i& a, const Packet4i& b) { +#ifdef EIGEN_VECTORIZE_SSE4_1 + return _mm_cmpeq_epi32(a, _mm_min_epi32(a, b)); +#else return por(pcmp_lt(a, b), pcmp_eq(a, b)); +#endif } template <> EIGEN_STRONG_INLINE Packet2l pcmp_lt(const Packet2l& a, const Packet2l& b) { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h index 4f3668944f..b1b89ef971 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h @@ -47,7 +47,7 @@ inline void manage_multi_threading(Action action, int* v); // Public APIs. /** Must be call first when calling Eigen from multiple threads */ -EIGEN_DEPRECATED inline void initParallel() {} +EIGEN_DEPRECATED_WITH_REASON("Initialization is no longer needed.") inline void initParallel() {} /** \returns the max number of threads reserved for Eigen * \sa setNbThreads */ diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h index 9198555973..db4a63089b 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h @@ -17,13 +17,9 @@ // Eigen version and basic defaults //------------------------------------------------------------------------------------------ -#define EIGEN_WORLD_VERSION 3 -#define EIGEN_MAJOR_VERSION 4 -#define EIGEN_MINOR_VERSION 90 - #define EIGEN_VERSION_AT_LEAST(x, y, z) \ - (EIGEN_WORLD_VERSION > x || \ - (EIGEN_WORLD_VERSION >= x && (EIGEN_MAJOR_VERSION > y || (EIGEN_MAJOR_VERSION >= y && EIGEN_MINOR_VERSION >= z)))) + (EIGEN_MAJOR_VERSION > x || \ + (EIGEN_MAJOR_VERSION >= x && (EIGEN_MINOR_VERSION > y || (EIGEN_MINOR_VERSION >= y && EIGEN_PATCH_VERSION >= z)))) #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR #define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor @@ -944,6 +940,18 @@ #define EIGEN_DEPRECATED #endif +#ifndef EIGEN_NO_DEPRECATED_WARNING +#if EIGEN_COMP_GNUC +#define EIGEN_DEPRECATED_WITH_REASON(message) __attribute__((deprecated(message))) +#elif EIGEN_COMP_MSVC +#define EIGEN_DEPRECATED_WITH_REASON(message) __declspec(deprecated(message)) +#else +#define EIGEN_DEPRECATED_WITH_REASON(message) +#endif +#else +#define EIGEN_DEPRECATED_WITH_REASON(message) +#endif + #if EIGEN_COMP_GNUC #define EIGEN_UNUSED __attribute__((unused)) #else diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h index 1e12820008..dc3bd13083 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h @@ -28,7 +28,8 @@ class Serializer; // Specialization for POD types. template -class Serializer::value && std::is_standard_layout::value>> { +class Serializer::value && std::is_standard_layout::value>> { public: /** * Determines the required size of the serialization buffer for a value. diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h index 54a74e2f59..94bc34dd8d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h @@ -409,7 +409,7 @@ inline void RealSchur::computeShift(Index iu, Index iter, Scalar& ex shiftInfo.coeffRef(2) = m_matT.coeff(iu, iu - 1) * m_matT.coeff(iu - 1, iu); // Alternate exceptional shifting strategy every 16 iterations. - if (iter % 16 == 0) { + if (iter > 0 && iter % 16 == 0) { // Wilkinson's original ad hoc shift if (iter % 32 != 0) { exshift += shiftInfo.coeff(0); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h index ad6b821bea..366a32ce42 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h @@ -133,8 +133,8 @@ EIGEN_DEVICE_FUNC inline Matrix::Scalar, 3, 1> Matr * \sa class AngleAxis */ template -EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix::Scalar, 3, 1> -MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const { +EIGEN_DEVICE_FUNC inline Matrix::Scalar, 3, 1> MatrixBase::eulerAngles( + Index a0, Index a1, Index a2) const { /* Implemented from Graphics Gems IV */ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h index 795af0d8d6..4159dc6d0c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h @@ -80,14 +80,12 @@ class Homogeneous : public MatrixBase >, int template EIGEN_DEVICE_FUNC inline const Product operator*(const MatrixBase& rhs) const { - eigen_assert(int(Direction) == Horizontal); return Product(*this, rhs.derived()); } template friend EIGEN_DEVICE_FUNC inline const Product operator*(const MatrixBase& lhs, const Homogeneous& rhs) { - eigen_assert(int(Direction) == Vertical); return Product(lhs.derived(), rhs); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h index 6fab905e54..db1e4a2640 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h @@ -155,7 +155,8 @@ class BDCSVD : public SVDBase > { * \deprecated Will be removed in the next major Eigen version. Options should * be specified in the \a Options template parameter. */ - EIGEN_DEPRECATED BDCSVD(Index rows, Index cols, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) { + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.") + BDCSVD(Index rows, Index cols, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) { internal::check_svd_options_assertions(computationOptions, rows, cols); allocate(rows, cols, computationOptions); } @@ -183,8 +184,8 @@ class BDCSVD : public SVDBase > { * be specified in the \a Options template parameter. */ template - EIGEN_DEPRECATED BDCSVD(const MatrixBase& matrix, unsigned int computationOptions) - : m_algoswap(16), m_numIters(0) { + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.") + BDCSVD(const MatrixBase& matrix, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) { internal::check_svd_options_assertions(computationOptions, matrix.rows(), matrix.cols()); compute_impl(matrix, computationOptions); } @@ -211,7 +212,8 @@ class BDCSVD : public SVDBase > { * be specified in the \a Options template parameter. */ template - EIGEN_DEPRECATED BDCSVD& compute(const MatrixBase& matrix, unsigned int computationOptions) { + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.") + BDCSVD& compute(const MatrixBase& matrix, unsigned int computationOptions) { internal::check_svd_options_assertions(computationOptions, matrix.rows(), matrix.cols()); return compute_impl(matrix, computationOptions); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h index 1abde17fd9..da2f295930 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h @@ -555,7 +555,8 @@ class JacobiSVD : public SVDBase > { * \deprecated Will be removed in the next major Eigen version. Options should * be specified in the \a Options template parameter. */ - EIGEN_DEPRECATED JacobiSVD(Index rows, Index cols, unsigned int computationOptions) { + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.") + JacobiSVD(Index rows, Index cols, unsigned int computationOptions) { internal::check_svd_options_assertions(computationOptions, rows, cols); allocate(rows, cols, computationOptions); } @@ -610,7 +611,8 @@ class JacobiSVD : public SVDBase > { * be specified in the \a Options template parameter. */ template - EIGEN_DEPRECATED JacobiSVD& compute(const MatrixBase& matrix, unsigned int computationOptions) { + EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.") + JacobiSVD& compute(const MatrixBase& matrix, unsigned int computationOptions) { internal::check_svd_options_assertions, Options>(m_computationOptions, matrix.rows(), matrix.cols()); return compute_impl(matrix, computationOptions); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h index 3f72a34dab..d19a00dd9f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h @@ -354,40 +354,40 @@ class SparseVector : public SparseCompressedBase(3.0) * c * c); + } +}; + +/** + * std::cbrt() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr cbrt(const ExpressionPtr& x) { + using enum ExpressionType; + + // Evaluate constant + if (x->type() == CONSTANT) { + if (x->val == 0.0) { + // Return zero + return x; + } else if (x->val == -1.0 || x->val == 1.0) { + return x; + } else { + return make_expression_ptr(std::cbrt(x->val)); + } + } + + return make_expression_ptr(x); +} + /** * Derived expression type for constant. */ @@ -661,11 +719,6 @@ struct UnaryMinusExpression final : Expression { } }; -inline ExpressionPtr exp(const ExpressionPtr& x); -inline ExpressionPtr sin(const ExpressionPtr& x); -inline ExpressionPtr sinh(const ExpressionPtr& x); -inline ExpressionPtr sqrt(const ExpressionPtr& x); - /** * Refcount increment for intrusive shared pointer. * diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp index a1d9c33539..1e32cb07c2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp @@ -21,8 +21,7 @@ inline gch::small_vector topological_sort( const ExpressionPtr& root) { gch::small_vector list; - // If the root type is a constant, Update() is a no-op, so there's no work - // to do + // If the root type is constant, updates are a no-op, so return an empty list if (root == nullptr || root->type() == ExpressionType::CONSTANT) { return list; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp index 728259d3f5..e9944652d8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp @@ -15,8 +15,8 @@ namespace slp { /** - * This class calculates the gradient of a a variable with respect to a vector - * of variables. + * This class calculates the gradient of a variable with respect to a vector of + * variables. * * The gradient is only recomputed if the variable expression is quadratic or * higher order. @@ -29,7 +29,7 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param variable Variable of which to compute the gradient. * @param wrt Variable with respect to which to compute the gradient. */ - Gradient(Variable variable, Variable wrt) noexcept + Gradient(Variable variable, Variable wrt) : m_jacobian{std::move(variable), std::move(wrt)} {} /** @@ -39,7 +39,7 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param wrt Vector of variables with respect to which to compute the * gradient. */ - Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept + Gradient(Variable variable, SleipnirMatrixLike auto wrt) : m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {} /** @@ -58,7 +58,7 @@ class SLEIPNIR_DLLEXPORT Gradient { * @return The gradient at wrt's value. */ const Eigen::SparseVector& value() { - m_g = m_jacobian.value(); + m_g = m_jacobian.value().transpose(); return m_g; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp index 8b048ab3ba..4f093b7b39 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp @@ -10,6 +10,7 @@ #include "sleipnir/autodiff/adjoint_expression_graph.hpp" #include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/assert.hpp" #include "sleipnir/util/concepts.hpp" #include "sleipnir/util/symbol_exports.hpp" @@ -34,7 +35,7 @@ class SLEIPNIR_DLLEXPORT Hessian { * @param variable Variable of which to compute the Hessian. * @param wrt Variable with respect to which to compute the Hessian. */ - Hessian(Variable variable, Variable wrt) noexcept + Hessian(Variable variable, Variable wrt) : Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {} /** @@ -44,10 +45,12 @@ class SLEIPNIR_DLLEXPORT Hessian { * @param wrt Vector of variables with respect to which to compute the * Hessian. */ - Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept + Hessian(Variable variable, SleipnirMatrixLike auto wrt) : m_variables{detail::AdjointExpressionGraph{variable} .generate_gradient_tree(wrt)}, m_wrt{wrt} { + slp_assert(m_wrt.cols() == 1); + // Initialize column each expression's adjoint occupies in the Jacobian for (size_t col = 0; col < m_wrt.size(); ++col) { m_wrt[col].expr->col = col; @@ -136,15 +139,9 @@ class SLEIPNIR_DLLEXPORT Hessian { m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); } - if (!triplets.empty()) { - m_H.setFromTriplets(triplets.begin(), triplets.end()); - if constexpr (UpLo == Eigen::Lower) { - m_H = m_H.triangularView(); - } - } else { - // setFromTriplets() is a no-op on empty triplets, so explicitly zero out - // the storage - m_H.setZero(); + m_H.setFromTriplets(triplets.begin(), triplets.end()); + if constexpr (UpLo == Eigen::Lower) { + m_H = m_H.triangularView(); } return m_H; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp index 7e7e1340d0..3662b5e49b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp @@ -10,6 +10,7 @@ #include "sleipnir/autodiff/adjoint_expression_graph.hpp" #include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/assert.hpp" #include "sleipnir/util/concepts.hpp" #include "sleipnir/util/symbol_exports.hpp" @@ -30,7 +31,7 @@ class SLEIPNIR_DLLEXPORT Jacobian { * @param variable Variable of which to compute the Jacobian. * @param wrt Variable with respect to which to compute the Jacobian. */ - Jacobian(Variable variable, Variable wrt) noexcept + Jacobian(Variable variable, Variable wrt) : Jacobian{VariableMatrix{std::move(variable)}, VariableMatrix{std::move(wrt)}} {} @@ -41,8 +42,11 @@ class SLEIPNIR_DLLEXPORT Jacobian { * @param wrt Vector of variables with respect to which to compute the * Jacobian. */ - Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept + Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} { + slp_assert(m_variables.cols() == 1); + slp_assert(m_wrt.cols() == 1); + // Initialize column each expression's adjoint occupies in the Jacobian for (size_t col = 0; col < m_wrt.size(); ++col) { m_wrt[col].expr->col = col; @@ -128,13 +132,7 @@ class SLEIPNIR_DLLEXPORT Jacobian { m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); } - if (!triplets.empty()) { - m_J.setFromTriplets(triplets.begin(), triplets.end()); - } else { - // setFromTriplets() is a no-op on empty triplets, so explicitly zero out - // the storage - m_J.setZero(); - } + m_J.setFromTriplets(triplets.begin(), triplets.end()); return m_J; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp index 03b929c778..2fc2119d2d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp @@ -87,6 +87,7 @@ class SLEIPNIR_DLLEXPORT Variable { */ Variable& operator=(double value) { expr = detail::make_expression_ptr(value); + m_graph_initialized = false; return *this; } @@ -97,22 +98,18 @@ class SLEIPNIR_DLLEXPORT Variable { * @param value The value of the Variable. */ void set_value(double value) { - if (expr->is_constant(0.0)) { - expr = detail::make_expression_ptr(value); - } else { #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - // We only need to check the first argument since unary and binary - // operators both use it - if (expr->args[0] != nullptr) { - auto location = std::source_location::current(); - slp::println( - stderr, - "WARNING: {}:{}: {}: Modified the value of a dependent variable", - location.file_name(), location.line(), location.function_name()); - } -#endif - expr->val = value; + // We only need to check the first argument since unary and binary operators + // both use it + if (expr->args[0] != nullptr) { + auto location = std::source_location::current(); + slp::println( + stderr, + "WARNING: {}:{}: {}: Modified the value of a dependent variable", + location.file_name(), location.line(), location.function_name()); } +#endif + expr->val = value; } /** @@ -266,6 +263,7 @@ class SLEIPNIR_DLLEXPORT Variable { friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y, const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable cbrt(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable cosh(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable erf(const Variable& x); @@ -338,6 +336,15 @@ SLEIPNIR_DLLEXPORT inline Variable atan2(const Variable& y, const Variable& x) { return Variable{detail::atan2(y.expr, x.expr)}; } +/** + * std::cbrt() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable cbrt(const Variable& x) { + return Variable{detail::cbrt(x.expr)}; +} + /** * std::cos() for Variables. * diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp index 4dc2cea00c..2ed997819e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp @@ -1149,7 +1149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( const VariableMatrix& lhs, const VariableMatrix& rhs, function_ref binary_op) { - slp_assert(lhs.rows() == rhs.rows() && lhs.rows() == rhs.rows()); + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp similarity index 74% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp index d917442666..74492a0d75 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp @@ -8,6 +8,9 @@ #include #include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/ocp/dynamics_type.hpp" +#include "sleipnir/optimization/ocp/timestep_method.hpp" +#include "sleipnir/optimization/ocp/transcription_method.hpp" #include "sleipnir/optimization/problem.hpp" #include "sleipnir/util/assert.hpp" #include "sleipnir/util/concepts.hpp" @@ -16,64 +19,6 @@ namespace slp { -/** - * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt. - * - * @param f The function to integrate. It must take two arguments x and u. - * @param x The initial value of x. - * @param u The value u held constant over the integration period. - * @param t0 The initial time. - * @param dt The time over which to integrate. - */ -template -State rk4(F&& f, State x, Input u, Time t0, Time dt) { - auto halfdt = dt * 0.5; - State k1 = f(t0, x, u, dt); - State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt); - State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt); - State k4 = f(t0 + dt, x + k3 * dt, u, dt); - - return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0); -} - -/** - * Enum describing an OCP transcription method. - */ -enum class TranscriptionMethod : uint8_t { - /// Each state is a decision variable constrained to the integrated dynamics - /// of the previous state. - DIRECT_TRANSCRIPTION, - /// The trajectory is modeled as a series of cubic polynomials where the - /// centerpoint slope is constrained. - DIRECT_COLLOCATION, - /// States depend explicitly as a function of all previous states and all - /// previous inputs. - SINGLE_SHOOTING -}; - -/** - * Enum describing a type of system dynamics constraints. - */ -enum class DynamicsType : uint8_t { - /// The dynamics are a function in the form dx/dt = f(t, x, u). - EXPLICIT_ODE, - /// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ). - DISCRETE -}; - -/** - * Enum describing the type of system timestep. - */ -enum class TimestepMethod : uint8_t { - /// The timestep is a fixed constant. - FIXED, - /// The timesteps are allowed to vary as independent decision variables. - VARIABLE, - /// The timesteps are equal length but allowed to vary as a single decision - /// variable. - VARIABLE_SINGLE -}; - /** * This class allows the user to pose and solve a constrained optimal control * problem (OCP) in a variety of ways. @@ -117,7 +62,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { * - State transition: xₖ₊₁ = f(xₖ, uₖ) * @param dynamics_type The type of system evolution function. * @param timestep_method The timestep method. - * @param method The transcription method. + * @param transcription_method The transcription method. */ OCP(int num_states, int num_inputs, std::chrono::duration dt, int num_steps, @@ -126,7 +71,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { dynamics, DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, TimestepMethod timestep_method = TimestepMethod::FIXED, - TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) + TranscriptionMethod transcription_method = + TranscriptionMethod::DIRECT_TRANSCRIPTION) : OCP{num_states, num_inputs, dt, @@ -139,7 +85,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { }, dynamics_type, timestep_method, - method} {} + transcription_method} {} /** * Build an optimization problem using a system evolution function (explicit @@ -156,7 +102,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt) * @param dynamics_type The type of system evolution function. * @param timestep_method The timestep method. - * @param method The transcription method. + * @param transcription_method The transcription method. */ OCP(int num_states, int num_inputs, std::chrono::duration dt, int num_steps, @@ -165,50 +111,46 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { dynamics, DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, TimestepMethod timestep_method = TimestepMethod::FIXED, - TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) - : m_num_states{num_states}, - m_num_inputs{num_inputs}, - m_dt{dt}, - m_num_steps{num_steps}, - m_transcription_method{method}, - m_dynamics_type{dynamics_type}, - m_dynamics_function{std::move(dynamics)}, - m_timestep_method{timestep_method} { + TranscriptionMethod transcription_method = + TranscriptionMethod::DIRECT_TRANSCRIPTION) + : m_num_steps{num_steps}, + m_dynamics{std::move(dynamics)}, + m_dynamics_type{dynamics_type} { // u is num_steps + 1 so that the final constraint function evaluation works - m_U = decision_variable(m_num_inputs, m_num_steps + 1); + m_U = decision_variable(num_inputs, m_num_steps + 1); - if (m_timestep_method == TimestepMethod::FIXED) { + if (timestep_method == TimestepMethod::FIXED) { m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { - m_DT(0, i) = m_dt.count(); + m_DT(0, i) = dt.count(); } - } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { - Variable dt = decision_variable(); - dt.set_value(m_dt.count()); + } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable single_dt = decision_variable(); + single_dt.set_value(dt.count()); // Set the member variable matrix to track the decision variable m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { - m_DT(0, i) = dt; + m_DT(0, i) = single_dt; } - } else if (m_timestep_method == TimestepMethod::VARIABLE) { + } else if (timestep_method == TimestepMethod::VARIABLE) { m_DT = decision_variable(1, m_num_steps + 1); for (int i = 0; i < num_steps + 1; ++i) { - m_DT(0, i).set_value(m_dt.count()); + m_DT(0, i).set_value(dt.count()); } } - if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) { - m_X = decision_variable(m_num_states, m_num_steps + 1); + if (transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) { + m_X = decision_variable(num_states, m_num_steps + 1); constrain_direct_transcription(); - } else if (m_transcription_method == + } else if (transcription_method == TranscriptionMethod::DIRECT_COLLOCATION) { - m_X = decision_variable(m_num_states, m_num_steps + 1); + m_X = decision_variable(num_states, m_num_steps + 1); constrain_direct_collocation(); - } else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) { + } else if (transcription_method == TranscriptionMethod::SINGLE_SHOOTING) { // In single-shooting the states aren't decision variables, but instead // depend on the input and previous states - m_X = VariableMatrix{m_num_states, m_num_steps + 1}; + m_X = VariableMatrix{num_states, m_num_steps + 1}; constrain_single_shooting(); } } @@ -370,6 +312,40 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { VariableMatrix final_state() { return m_X.col(m_num_steps); } private: + int m_num_steps; + + function_ref + m_dynamics; + DynamicsType m_dynamics_type; + + VariableMatrix m_X; + VariableMatrix m_U; + VariableMatrix m_DT; + + /** + * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt. + * + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param t0 The initial time. + * @param dt The time over which to integrate. + */ + template + State rk4(F&& f, State x, Input u, Time t0, Time dt) { + auto halfdt = dt * 0.5; + State k1 = f(t0, x, u, dt); + State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt); + State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt); + State k4 = f(t0 + dt, x + k3 * dt, u, dt); + + return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0); + } + + /** + * Apply direct collocation dynamics constraints. + */ void constrain_direct_collocation() { slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE); @@ -379,7 +355,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { for (int i = 0; i < m_num_steps; ++i) { Variable h = dt()(0, i); - auto& f = m_dynamics_function; + auto& f = m_dynamics; auto t_begin = time; auto t_end = t_begin + h; @@ -405,6 +381,9 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { } } + /** + * Apply direct transcription dynamics constraints. + */ void constrain_direct_transcription() { Variable time = 0.0; @@ -415,17 +394,20 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { Variable dt = this->dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - subject_to(x_end == rk4( - m_dynamics_function, x_begin, u, time, dt)); + subject_to(x_end == rk4(m_dynamics, x_begin, + u, time, dt)); } else if (m_dynamics_type == DynamicsType::DISCRETE) { - subject_to(x_end == m_dynamics_function(time, x_begin, u, dt)); + subject_to(x_end == m_dynamics(time, x_begin, u, dt)); } time += dt; } } + /** + * Apply single shooting dynamics constraints. + */ void constrain_single_shooting() { Variable time = 0.0; @@ -436,34 +418,15 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { Variable dt = this->dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - x_end = rk4(m_dynamics_function, x_begin, u, - time, dt); + x_end = rk4(m_dynamics, x_begin, u, time, dt); } else if (m_dynamics_type == DynamicsType::DISCRETE) { - x_end = m_dynamics_function(time, x_begin, u, dt); + x_end = m_dynamics(time, x_begin, u, dt); } time += dt; } } - - int m_num_states; - int m_num_inputs; - std::chrono::duration m_dt; - int m_num_steps; - TranscriptionMethod m_transcription_method; - - DynamicsType m_dynamics_type; - - function_ref - m_dynamics_function; - - TimestepMethod m_timestep_method; - - VariableMatrix m_X; - VariableMatrix m_U; - VariableMatrix m_DT; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp new file mode 100644 index 0000000000..835a12eef8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp @@ -0,0 +1,19 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace slp { + +/** + * Enum describing a type of system dynamics constraints. + */ +enum class DynamicsType : uint8_t { + /// The dynamics are a function in the form dx/dt = f(t, x, u). + EXPLICIT_ODE, + /// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ). + DISCRETE +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp new file mode 100644 index 0000000000..ef1cf88a08 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp @@ -0,0 +1,22 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace slp { + +/** + * Enum describing the type of system timestep. + */ +enum class TimestepMethod : uint8_t { + /// The timestep is a fixed constant. + FIXED, + /// The timesteps are allowed to vary as independent decision variables. + VARIABLE, + /// The timesteps are equal length but allowed to vary as a single decision + /// variable. + VARIABLE_SINGLE +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp new file mode 100644 index 0000000000..e0513eef89 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp @@ -0,0 +1,24 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace slp { + +/** + * Enum describing an OCP transcription method. + */ +enum class TranscriptionMethod : uint8_t { + /// Each state is a decision variable constrained to the integrated dynamics + /// of the previous state. + DIRECT_TRANSCRIPTION, + /// The trajectory is modeled as a series of cubic polynomials where the + /// centerpoint slope is constrained. + DIRECT_COLLOCATION, + /// States depend explicitly as a function of all previous states and all + /// previous inputs. + SINGLE_SHOOTING +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp index b484ec08d6..c996b37231 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -73,7 +73,7 @@ class SLEIPNIR_DLLEXPORT Problem { VariableMatrix decision_variable(int rows, int cols = 1) { m_decision_variables.reserve(m_decision_variables.size() + rows * cols); - VariableMatrix vars{rows, cols}; + VariableMatrix vars{VariableMatrix::empty, rows, cols}; for (int row = 0; row < rows; ++row) { for (int col = 0; col < cols; ++col) { @@ -108,7 +108,7 @@ class SLEIPNIR_DLLEXPORT Problem { m_decision_variables.reserve(m_decision_variables.size() + (rows * rows + rows) / 2); - VariableMatrix vars{rows, rows}; + VariableMatrix vars{VariableMatrix::empty, rows, rows}; for (int row = 0; row < rows; ++row) { for (int col = 0; col <= row; ++col) { @@ -317,6 +317,24 @@ class SLEIPNIR_DLLEXPORT Problem { */ void clear_callbacks() { m_iteration_callbacks.clear(); } + /** + * Adds a callback to be called at the beginning of each solver iteration. + * + * Language bindings should call this in the Problem constructor to register + * callbacks that shouldn't be removed by clear_callbacks(). Persistent + * callbacks run after non-persistent callbacks. + * + * @param callback The callback. Returning true from the callback causes the + * solver to exit early with the solution it has so far. + */ + template + requires requires(F callback, const IterationInfo& info) { + { callback(info) } -> std::same_as; + } + void add_persistent_callback(F&& callback) { + m_persistent_iteration_callbacks.emplace_back(std::forward(callback)); + } + private: // The list of decision variables, which are the root of the problem's // expression tree @@ -334,6 +352,8 @@ class SLEIPNIR_DLLEXPORT Problem { // The iteration callbacks gch::small_vector> m_iteration_callbacks; + gch::small_vector> + m_persistent_iteration_callbacks; void print_exit_conditions([[maybe_unused]] const Options& options); void print_problem_analysis(); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp index 75d8ffca32..53de01928b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp @@ -3,9 +3,10 @@ #pragma once #ifdef JORMUNGANDR -#include #include #include + +#include /** * Throw an exception in Python. */ @@ -13,7 +14,7 @@ do { \ if (!(condition)) { \ auto location = std::source_location::current(); \ - throw std::invalid_argument(std::format( \ + throw std::invalid_argument(fmt::format( \ "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ location.line(), location.function_name(), #condition)); \ } \ diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide index 1b6652d3d5..4f4c762040 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide @@ -8,5 +8,6 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ + ^fmt/ ^gch/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp index 71f8153d34..d9619a39d5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp @@ -24,7 +24,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { const auto& c = A(1, 0); const auto& d = A(1, 1); - slp::VariableMatrix adj_A{{d, -b}, {-c, a}}; + VariableMatrix adj_A{{d, -b}, {-c, a}}; auto det_A = a * d - b * c; return adj_A / det_A * B; } else if (A.rows() == 3 && A.cols() == 3) { @@ -72,9 +72,9 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { auto adj_A10 = fg - di; auto adj_A20 = dh - eg; - slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, - {adj_A10, ai - cg, cd - af}, - {adj_A20, bg - ah, ae - bd}}; + VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, + {adj_A10, ai - cg, cd - af}, + {adj_A20, bg - ah, ae - bd}}; auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20; return adj_A / det_A * B; } else if (A.rows() == 4 && A.cols() == 4) { @@ -220,10 +220,10 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { auto adj_A32 = -afo + agn + beo - bgm - cen + cfm; auto adj_A33 = afk - agj - bek + bgi + cej - cfi; - slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, - {adj_A10, adj_A11, adj_A12, adj_A13}, - {adj_A20, adj_A21, adj_A22, adj_A23}, - {adj_A30, adj_A31, adj_A32, adj_A33}}; + VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, + {adj_A10, adj_A11, adj_A12, adj_A13}, + {adj_A20, adj_A21, adj_A22, adj_A23}, + {adj_A30, adj_A31, adj_A32, adj_A33}}; auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30; return adj_A / det_A * B; } else { @@ -245,7 +245,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B); - VariableMatrix X{A.cols(), B.cols()}; + VariableMatrix X{VariableMatrix::empty, A.cols(), B.cols()}; for (int row = 0; row < X.rows(); ++row) { for (int col = 0; col < X.cols(); ++col) { X(row, col) = eigen_X(row, col); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp index 5532b39624..e32481e931 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include "optimization/bounds.hpp" @@ -79,6 +80,14 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { [[maybe_unused]] int num_inequality_constraints = m_inequality_constraints.size(); + gch::small_vector> callbacks; + for (const auto& callback : m_iteration_callbacks) { + callbacks.emplace_back(callback); + } + for (const auto& callback : m_persistent_iteration_callbacks) { + callbacks.emplace_back(callback); + } + // Solve the optimization problem ExitStatus status; if (m_equality_constraints.empty() && m_inequality_constraints.empty()) { @@ -101,7 +110,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { H_spy = std::make_unique( "H.spy", "Hessian", "Decision variables", "Decision variables", num_decision_variables, num_decision_variables); - m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + callbacks.push_back([&](const IterationInfo& info) -> bool { H_spy->add(info.H); return false; }); @@ -123,7 +132,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { x_ad.set_value(x); return H.value(); }}, - m_iteration_callbacks, options, x); + callbacks, options, x); } else if (m_inequality_constraints.empty()) { if (options.diagnostics) { slp::println("\nInvoking SQP solver\n"); @@ -160,7 +169,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { "Constraints", "Decision variables", num_equality_constraints, num_decision_variables); - m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + callbacks.push_back([&](const IterationInfo& info) -> bool { H_spy->add(info.H); A_e_spy->add(info.A_e); return false; @@ -193,7 +202,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { x_ad.set_value(x); return A_e.value(); }}, - m_iteration_callbacks, options, x); + callbacks, options, x); } else { if (options.diagnostics) { slp::println("\nInvoking IPM solver...\n"); @@ -242,7 +251,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { "A_i.spy", "Inequality constraint Jacobian", "Constraints", "Decision variables", num_inequality_constraints, num_decision_variables); - m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + callbacks.push_back([&](const IterationInfo& info) -> bool { H_spy->add(info.H); A_e_spy->add(info.A_e); A_i_spy->add(info.A_i); @@ -298,19 +307,13 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { x_ad.set_value(x); return A_i.value(); }}, - m_iteration_callbacks, options, + callbacks, options, #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION bound_constraint_mask, #endif x); } -#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - if (spy) { - m_iteration_callbacks.pop_back(); - } -#endif - if (options.diagnostics) { print_autodiff_diagnostics(ad_setup_profilers); slp::println("\nExit: {}", to_message(status)); @@ -326,14 +329,15 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { // Print possible exit conditions slp::println("User-configured exit conditions:"); slp::println(" ↳ error below {}", options.tolerance); - if (!m_iteration_callbacks.empty()) { + if (!m_iteration_callbacks.empty() || + !m_persistent_iteration_callbacks.empty()) { slp::println(" ↳ iteration callback requested stop"); } if (std::isfinite(options.max_iterations)) { slp::println(" ↳ executed {} iterations", options.max_iterations); } if (std::isfinite(options.timeout.count())) { - slp::println(" ↳ {} elapsed", options.timeout.count()); + slp::println(" ↳ {} elapsed", options.timeout); } } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp index b1421bf961..f85b765875 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp @@ -327,8 +327,7 @@ ExitStatus interior_point( Eigen::SparseMatrix lhs( num_decision_variables + num_equality_constraints, num_decision_variables + num_equality_constraints); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end()); // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] // [ cₑ ] diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp index 98ed6c941f..535ca1f57b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp @@ -2,7 +2,6 @@ #include "sleipnir/optimization/solver/newton.hpp" -#include #include #include #include diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp index 04447f8892..3b90ca7e9d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp @@ -2,7 +2,6 @@ #include "sleipnir/optimization/solver/sqp.hpp" -#include #include #include #include @@ -232,8 +231,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, Eigen::SparseMatrix lhs( num_decision_variables + num_equality_constraints, num_decision_variables + num_equality_constraints); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end()); // rhs = −[∇f − Aₑᵀy] // [ cₑ ] @@ -407,7 +405,6 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, trial_x = x + α_max * step.p_x; trial_y = y + α_max * step.p_y; - trial_f = matrices.f(trial_x); trial_c_e = matrices.c_e(trial_x); double next_kkt_error = kkt_error( diff --git a/wpiutil/src/main/native/include/wpi/jni_util.h b/wpiutil/src/main/native/include/wpi/jni_util.h index 4e451a6367..cd3eeedd64 100644 --- a/wpiutil/src/main/native/include/wpi/jni_util.h +++ b/wpiutil/src/main/native/include/wpi/jni_util.h @@ -322,6 +322,7 @@ class JSpanBase { } } + // NOLINTNEXTLINE(google-explicit-constructor) operator std::span() const { return array(); } std::span array() const { diff --git a/wpiutil/src/test/native/cpp/StringMapTest.cpp b/wpiutil/src/test/native/cpp/StringMapTest.cpp index 0086660923..69da187c5a 100644 --- a/wpiutil/src/test/native/cpp/StringMapTest.cpp +++ b/wpiutil/src/test/native/cpp/StringMapTest.cpp @@ -335,6 +335,7 @@ TEST_F(StringMapTest, MoveConstruct) { StringMap A; A["x"] = 42; StringMap B = std::move(A); + // NOLINTNEXTLINE(clang-analyzer-cplusplus.Move) ASSERT_EQ(A.size(), 0u); ASSERT_EQ(B.size(), 1u); ASSERT_EQ(B["x"], 42); @@ -348,8 +349,10 @@ TEST_F(StringMapTest, MoveAssignment) { B["y"] = 117; A = std::move(B); ASSERT_EQ(A.size(), 1u); + // NOLINTNEXTLINE(clang-analyzer-cplusplus.Move) ASSERT_EQ(B.size(), 0u); ASSERT_EQ(A["y"], 117); + // NOLINTNEXTLINE(clang-analyzer-cplusplus.Move) ASSERT_EQ(B.count("x"), 0u); }