Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-09-25 22:03:55 -07:00
parent cf4f6ce4b6 6b8be313c7
commit ef676aca70
93 changed files with 941 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.clang-tidy
View File

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

2
benchmark/src/main/native/cpp/Main.cpp
View File

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

18
build.gradle
View File

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

1
cscore/examples/usbcvstream/usbcvstream.cpp
View File

@@ -5,7 +5,6 @@
#include <opencv2/core/core.hpp>
#include <wpi/print.h>
#include "cscore.h"
#include "cscore_cv.h"
int main() {

4
cscore/src/main/native/cpp/cscore_c.cpp
View File

@@ -44,7 +44,7 @@ static O* ConvertToC(std::vector<I>&& 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<cs::RawEvent>;
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<T*>(buf)->~T();
std::free(arr);

3
cscore/src/main/native/windows/UsbCameraImpl.cpp
View File

@@ -707,8 +707,9 @@ void UsbCameraImpl::DeviceCacheProperty(
}
NotifyPropertyCreated(*rawIndex, *rawPropPtr);
if (perPropPtr && perIndex)
if (perPropPtr && perIndex) {
NotifyPropertyCreated(*perIndex, *perPropPtr);
}
}
CS_StatusValue UsbCameraImpl::DeviceProcessCommand(

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

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

102
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ProtobufHandler.java Normal file
View File

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

56
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java
View File

@@ -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<Rotation2d>` 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) {

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

@@ -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<ProtobufType, Message> proto = null; // value doesn't matter
static class Message extends ProtoMessage<Message> {
// 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<ProtobufType> 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<Example> {
public ExampleLogger() {
super(Example.class);
}
@Override
public void update(EpilogueBackend backend, Example object) {
if (Epilogue.shouldLog(Logged.Importance.DEBUG)) {
backend.log("x", object.x, edu.wpi.first.epilogue.ProtobufType.proto);
}
}
}
""";
assertLoggerGenerates(source, expectedGeneratedSource);
}
@Test
void lists() {
String source =

1
epilogue-runtime/BUILD.bazel
View File

@@ -20,5 +20,6 @@ wpilib_java_junit5_test(
deps = [
":epilogue-java",
"//wpiutil:wpiutil-java",
"@maven//:us_hebi_quickbuf_quickbuf_runtime",
],
)

3
epilogue-runtime/build.gradle
View File

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

13
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java
View File

@@ -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 <P> the protobuf-serializable type
* @param <M> the protobuf message type
*/
<P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> proto);
/**
* Logs a measurement's value in terms of its base unit.
*

19
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java
View File

@@ -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<String, DataLogEntry> m_entries = new HashMap<>();
private final Map<String, NestedBackend> m_subLoggers = new HashMap<>();
private final Set<Struct<?>> m_seenSchemas = new HashSet<>();
private final Set<Protobuf<?, ?>> m_seenProtos = new HashSet<>();
/**
* Creates a new file-based backend.
@@ -166,4 +170,19 @@ public class FileBackend implements EpilogueBackend {
((StructArrayLogEntry<S>) m_entries.get(identifier)).append(value);
}
@Override
@SuppressWarnings("unchecked")
public <P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> 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<P>) m_entries.get(identifier)).append(value);
}
}

15
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java
View File

@@ -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 <P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> 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);
}
}

9
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java
View File

@@ -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 <P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> proto) {
for (EpilogueBackend backend : m_backends) {
backend.log(identifier, value, proto);
}
}
}

21
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java
View File

@@ -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<String, Publisher> m_publishers = new HashMap<>();
private final Map<String, NestedBackend> m_nestedBackends = new HashMap<>();
private final Set<Struct<?>> m_seenSchemas = new HashSet<>();
private final Set<Protobuf<?, ?>> m_seenProtos = new HashSet<>();
private final Function<String, IntegerPublisher> m_createIntPublisher;
private final Function<String, FloatPublisher> m_createFloatPublisher;
private final Function<String, DoublePublisher> m_createDoublePublisher;
@@ -198,4 +202,21 @@ public class NTEpilogueBackend implements EpilogueBackend {
publisher.set(value);
}
}
@Override
@SuppressWarnings("unchecked")
public <P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> 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<P>) m_publishers.get(identifier)).set(value);
} else {
ProtobufPublisher<P> publisher = m_nt.getProtobufTopic(identifier, proto).publish();
m_publishers.put(identifier, publisher);
publisher.set(value);
}
}
}

7
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java
View File

@@ -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 <S> void log(String identifier, S[] value, Struct<S> struct) {
m_impl.log(withPrefix(identifier), value, struct);
}
@Override
public <P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> proto) {
m_impl.log(m_prefix + identifier, value, proto);
}
}

6
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullBackend.java
View File

@@ -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 <S> void log(String identifier, S[] value, Struct<S> struct) {}
@Override
public <P, M extends ProtoMessage<M>> void log(
String identifier, P value, Protobuf<P, M> proto) {}
}

14
epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java
View File

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

10
epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java
View File

@@ -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 <P, M extends ProtoMessage<M>> void log(String identifier, P value, Protobuf<P, M> proto) {
var msg = proto.createMessage();
proto.pack(msg, value);
var serialized = msg.toByteArray();
m_entries.add(new LogEntry<>(identifier, serialized));
}
}

4
glass/src/lib/native/cpp/other/PIDController.cpp
View File

@@ -31,8 +31,8 @@ void glass::DisplayPIDController(PIDControllerModel* m) {
[flag](const char* name, double* v,
std::function<void(double)> 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);
}
};

4
glass/src/lib/native/cpp/other/ProfiledPIDController.cpp
View File

@@ -31,8 +31,8 @@ void glass::DisplayProfiledPIDController(ProfiledPIDControllerModel* m) {
[flag](const char* name, double* v,
std::function<void(double)> 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);
}
};

4
msvcruntime/build.gradle
View File

@@ -68,8 +68,8 @@ if (OperatingSystem.current().isWindows()) {
artifact x64ZipTask
artifactId = "${baseArtifactId}"
groupId artifactGroupId
version wpilibVersioning.version.get()
groupId = artifactGroupId
version = wpilibVersioning.version.get()
}
}
}

1
ntcore/.clang-tidy
View File

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

1
ntcore/src/main/native/cpp/Value_internal.h
View File

@@ -177,6 +177,7 @@ static_assert(ValidType<uint8_t[]>);
static_assert(ValidType<std::vector<uint8_t>>);
template <ValidType T, NT_Type type>
// NOLINTNEXTLINE(google-readability-casting)
constexpr bool IsNTType = TypeInfo<std::remove_cvref_t<T>>::kType == type;
static_assert(IsNTType<bool, NT_BOOLEAN>);

2
ntcore/src/main/native/cpp/server/Functions.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <stdint.h>
#include <functional>
#include <string_view>

4
ntcore/src/main/native/include/networktables/ProtobufTopic.h
View File

@@ -7,9 +7,7 @@
#include <stdint.h>
#include <atomic>
#include <concepts>
#include <span>
#include <string_view>
#include <utility>
#include <vector>
@@ -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));

2
ntcore/src/test/native/cpp/StructTest.cpp
View File

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

5
processstarter/src/main/native/linux/main.cpp
View File

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

2
settings.gradle
View File

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

12
shared/java/javacommon.gradle
View File

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

6
shared/javacpp/publish.gradle
View File

@@ -50,8 +50,10 @@ task cppHeadersZip(type: Zip) {
}
artifacts {
archives cppHeadersZip
archives cppSourcesZip
tasks.named("assemble") {
dependsOn(cppHeadersZip)
dependsOn(cppSourcesZip)
}
}
addTaskToCopyAllOutputs(cppSourcesZip)

6
shared/jni/publish.gradle
View File

@@ -62,8 +62,10 @@ task cppHeadersZip(type: Zip) {
}
artifacts {
archives cppHeadersZip
archives cppSourcesZip
tasks.named("assemble") {
dependsOn(cppHeadersZip)
dependsOn(cppSourcesZip)
}
}
addTaskToCopyAllOutputs(cppSourcesZip)

2
sysid/publish.gradle
View File

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

5
upstream_utils/eigen.py
View File

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

4
upstream_utils/sleipnir.py
View File

@@ -52,8 +52,8 @@ using small_vector = wpi::SmallVector<T>;
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()

63
upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
View File

@@ -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 <format>
#include <source_location>
#include <stdexcept>
+
+#include <fmt/format.h>
/**
* 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 <Eigen/Core>
#include <Eigen/SparseCore>
+#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"

8
upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
View File

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

6
upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch
View File

@@ -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 <Eigen/Core>
#include <Eigen/SparseCore>
@@ -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<Variable>& constraints) {
std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) {

8
upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch
View File

@@ -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 <Eigen/Core>
#include <Eigen/SparseCore>
@@ -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<uint8_t>(constraint.type())];
}

4
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
View File

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

2
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch
View File

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

74
upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch
View File

@@ -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<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -512,11 +512,7 @@ gch::small_vector<Variable> 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<LHS> && ScalarLike<RHS>) {
@@ -518,11 +514,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -525,11 +521,7 @@ gch::small_vector<Variable> 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<LHS> && MatrixLike<RHS>) {
@@ -532,19 +524,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -539,19 +531,7 @@ gch::small_vector<Variable> 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 == rk4<const decltype(m_dynamics_function)&,
@@ -433,7 +433,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
@@ -415,7 +415,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);
@@ -771,9 +771,9 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20cb11a922 100644
index efde2006397fb7d8ca24651e9a84b47fc879ee15..c996b372311f708153f8c89ef15fa35a097a6171 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem {
@@ -797,7 +797,7 @@ index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20
}
diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp
index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac766fcad2a4 100644
index 6c3a040e08bdc5009885e762402a8b44434024c3..d9619a39d583e1a29c46602ba61e881531f57e09 100644
--- a/src/autodiff/variable_matrix.cpp
+++ b/src/autodiff/variable_matrix.cpp
@@ -12,17 +12,17 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
@@ -821,7 +821,7 @@ index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac76
+ 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;
@@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
//
@@ -905,7 +905,7 @@ index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac76
}
@@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& 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);

1
wpical/.styleguide
View File

@@ -31,7 +31,6 @@ includeOtherLibs {
^mrcal_wrapper\.h$
^opencv2\.h$
^portable-file-dialogs\.h$
^tagpose\.h$
^wpi/
^wpigui
}

2
wpical/Info.plist
View File

@@ -11,7 +11,7 @@
<key>CFBundleIdentifier</key>
<string>edu.wpi.first.tools.WPIcal</string>
<key>CFBundleIconFile</key>
<string>ov.icns</string>
<string>wpical.icns</string>
<key>CFBundlePackageType</key>
<string>APPL</string>
<key>CFBundleSupportedPlatforms</key>

5
wpical/src/main/native/cpp/WPIcal.cpp
View File

@@ -19,10 +19,11 @@
#include <fmt/format.h>
#include <imgui.h>
#include <portable-file-dialogs.h>
#include <tagpose.h>
#include <wpi/json.h>
#include <wpigui.h>
#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<std::string*>(payload->Data);
rv = true;
}
ImGui::EndDragDropTarget();

3
wpical/src/main/native/cpp/tagpose.cpp
View File

@@ -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 <tagpose.h>
#include "tagpose.h"
#include <wpi/deprecated.h>
WPI_IGNORE_DEPRECATED

3
wpical/src/main/native/include/fieldmap.h
View File

@@ -7,9 +7,10 @@
#include <cmath>
#include <map>
#include <tagpose.h>
#include <wpi/json.h>
#include "tagpose.h"
class Fieldmap {
public:
Fieldmap() = default;

6
wpical/src/main/native/include/fmap.h
View File

@@ -4,11 +4,11 @@
#pragma once
#include <fieldmap.h>
#include <tagpose.h>
#include <wpi/json.h>
#include "fieldmap.h"
#include "tagpose.h"
namespace fmap {
wpi::json singleTag(int tag, const tag::Pose& tagpose);
wpi::json convertfmap(const wpi::json& json);

2
wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
View File

@@ -53,7 +53,7 @@ class Config {
std::optional<units::volt_t> stepVoltage,
std::optional<units::second_t> timeout,
std::function<void(frc::sysid::State)> recordState)
: m_recordState{recordState} {
: m_recordState{std::move(recordState)} {
if (rampRate) {
m_rampRate = rampRate.value();
}

2
wpilibc/src/test/native/cpp/AlertTest.cpp
View File

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

6
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp
View File

@@ -4,14 +4,16 @@
#include "commands/TeleopArcadeDrive.h"
#include <utility>
#include "subsystems/Drivetrain.h"
TeleopArcadeDrive::TeleopArcadeDrive(
Drivetrain* subsystem, std::function<double()> xaxisSpeedSupplier,
std::function<double()> zaxisRotateSuppplier)
: m_drive{subsystem},
m_xaxisSpeedSupplier{xaxisSpeedSupplier},
m_zaxisRotateSupplier{zaxisRotateSuppplier} {
m_xaxisSpeedSupplier{std::move(xaxisSpeedSupplier)},
m_zaxisRotateSupplier{std::move(zaxisRotateSuppplier)} {
AddRequirements(subsystem);
}

10
wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
View File

@@ -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 <States extends Num> Matrix<States, N1> rk4(
UnaryOperator<Matrix<States, N1>> f, Matrix<States, N1> x, double dt) {

3
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core vendored
View File

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

14
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version vendored Normal file
View File

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

6
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h vendored
View File

@@ -235,8 +235,7 @@ DenseBase<Derived>::Constant(const Scalar& value) {
* \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&)
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<
Derived>::RandomAccessLinSpacedReturnType
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high) {
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low, high, size));
@@ -247,8 +246,7 @@ DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const
* \sa LinSpaced(const Scalar&, const Scalar&)
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<
Derived>::RandomAccessLinSpacedReturnType
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
DenseBase<Derived>::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high) {
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)

12
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h vendored
View File

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

50
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h vendored
View File

@@ -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<Packet>::run(a);
}
/** \internal \returns a <= b as a bit mask */
template <typename Packet>
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 <typename Packet>
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 <typename Packet>
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 <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) {
return a >= b ? pzero(a) : ptrue(a);
}
template <typename T>
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 <typename Packet>
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 <typename Packet>
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 <typename Packet>
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 <typename Packet>
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 <typename Packet, bool is_scalar = is_scalar<Packet>::value>
struct pselect_impl {

9
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h vendored
View File

@@ -373,12 +373,14 @@ class MatrixBase : public DenseBase<Derived> {
template <int Options = 0>
inline JacobiSVD<PlainObject, Options> jacobiSvd() const;
template <int Options = 0>
EIGEN_DEPRECATED inline JacobiSVD<PlainObject, Options> jacobiSvd(unsigned int computationOptions) const;
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using method's template parameter.")
inline JacobiSVD<PlainObject, Options> jacobiSvd(unsigned int computationOptions) const;
template <int Options = 0>
inline BDCSVD<PlainObject, Options> bdcSvd() const;
template <int Options = 0>
EIGEN_DEPRECATED inline BDCSVD<PlainObject, Options> bdcSvd(unsigned int computationOptions) const;
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using method's template parameter.")
inline BDCSVD<PlainObject, Options> bdcSvd(unsigned int computationOptions) const;
/////////// Geometry module ///////////
@@ -391,7 +393,8 @@ class MatrixBase : public DenseBase<Derived> {
EIGEN_DEVICE_FUNC inline PlainObject unitOrthogonal(void) const;
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> eulerAngles(Index a0, Index a1, Index a2) const;
EIGEN_DEPRECATED_WITH_REASON("Use .canonicalEulerAngles() instead.")
EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> eulerAngles(Index a0, Index a1, Index a2) const;
EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> canonicalEulerAngles(Index a0, Index a1, Index a2) const;

8
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h vendored
View File

@@ -1264,6 +1264,14 @@ struct generic_product_impl<Lhs, Rhs, SkewSymmetricShape, SkewSymmetricShape, Pr
}
};
template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
struct generic_product_impl<Lhs, Rhs, MatrixShape, HomogeneousShape, ProductTag>
: generic_product_impl<Lhs, typename Rhs::PlainObject, MatrixShape, DenseShape, ProductTag> {};
template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
struct generic_product_impl<Lhs, Rhs, HomogeneousShape, MatrixShape, ProductTag>
: generic_product_impl<typename Lhs::PlainObject, Rhs, DenseShape, MatrixShape, ProductTag> {};
} // end namespace internal
} // end namespace Eigen

15
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h vendored
View File

@@ -287,7 +287,7 @@ struct packet_traits<bool> : 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<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
return _mm_andnot_si128(b, a);
}
template <>
EIGEN_STRONG_INLINE Packet16b pandnot<Packet16b>(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) {

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h vendored
View File

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

20
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h vendored
View File

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

3
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h vendored
View File

@@ -28,7 +28,8 @@ class Serializer;
// Specialization for POD types.
template <typename T>
class Serializer<T, typename std::enable_if_t<std::is_trivial<T>::value && std::is_standard_layout<T>::value>> {
class Serializer<T,
typename std::enable_if_t<std::is_trivially_copyable<T>::value && std::is_standard_layout<T>::value>> {
public:
/**
* Determines the required size of the serialization buffer for a value.

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h vendored
View File

@@ -409,7 +409,7 @@ inline void RealSchur<MatrixType>::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);

4
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h vendored
View File

@@ -133,8 +133,8 @@ EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> Matr
* \sa class AngleAxis
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const {
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> MatrixBase<Derived>::eulerAngles(
Index a0, Index a1, Index a2) const {
/* Implemented from Graphics Gems IV */
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h vendored
View File

@@ -80,14 +80,12 @@ class Homogeneous : public MatrixBase<Homogeneous<MatrixType, Direction_> >, int
template <typename Rhs>
EIGEN_DEVICE_FUNC inline const Product<Homogeneous, Rhs> operator*(const MatrixBase<Rhs>& rhs) const {
eigen_assert(int(Direction) == Horizontal);
return Product<Homogeneous, Rhs>(*this, rhs.derived());
}
template <typename Lhs>
friend EIGEN_DEVICE_FUNC inline const Product<Lhs, Homogeneous> operator*(const MatrixBase<Lhs>& lhs,
const Homogeneous& rhs) {
eigen_assert(int(Direction) == Vertical);
return Product<Lhs, Homogeneous>(lhs.derived(), rhs);
}

10
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h vendored
View File

@@ -155,7 +155,8 @@ class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
* \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<MatrixType, Options>(computationOptions, rows, cols);
allocate(rows, cols, computationOptions);
}
@@ -183,8 +184,8 @@ class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
* be specified in the \a Options template parameter.
*/
template <typename Derived>
EIGEN_DEPRECATED BDCSVD(const MatrixBase<Derived>& 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<Derived>& matrix, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) {
internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
compute_impl(matrix, computationOptions);
}
@@ -211,7 +212,8 @@ class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
* be specified in the \a Options template parameter.
*/
template <typename Derived>
EIGEN_DEPRECATED BDCSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
BDCSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
return compute_impl(matrix, computationOptions);
}

6
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h vendored
View File

@@ -555,7 +555,8 @@ class JacobiSVD : public SVDBase<JacobiSVD<MatrixType_, Options_> > {
* \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<MatrixType, Options>(computationOptions, rows, cols);
allocate(rows, cols, computationOptions);
}
@@ -610,7 +611,8 @@ class JacobiSVD : public SVDBase<JacobiSVD<MatrixType_, Options_> > {
* be specified in the \a Options template parameter.
*/
template <typename Derived>
EIGEN_DEPRECATED JacobiSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
JacobiSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
internal::check_svd_options_assertions<MatrixBase<Derived>, Options>(m_computationOptions, matrix.rows(),
matrix.cols());
return compute_impl(matrix, computationOptions);

16
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h vendored
View File

@@ -354,40 +354,40 @@ class SparseVector : public SparseCompressedBase<SparseVector<Scalar_, Options_,
public:
/** \internal \deprecated use setZero() and reserve() */
EIGEN_DEPRECATED void startFill(Index reserve) {
EIGEN_DEPRECATED_WITH_REASON("Use .setZero() and .reserve() instead.") void startFill(Index reserve) {
setZero();
m_data.reserve(reserve);
}
/** \internal \deprecated use insertBack(Index,Index) */
EIGEN_DEPRECATED Scalar& fill(Index r, Index c) {
EIGEN_DEPRECATED_WITH_REASON("Use .insertBack() instead.") Scalar& fill(Index r, Index c) {
eigen_assert(r == 0 || c == 0);
return fill(IsColVector ? r : c);
}
/** \internal \deprecated use insertBack(Index) */
EIGEN_DEPRECATED Scalar& fill(Index i) {
EIGEN_DEPRECATED_WITH_REASON("Use .insertBack() instead.") Scalar& fill(Index i) {
m_data.append(0, i);
return m_data.value(m_data.size() - 1);
}
/** \internal \deprecated use insert(Index,Index) */
EIGEN_DEPRECATED Scalar& fillrand(Index r, Index c) {
EIGEN_DEPRECATED_WITH_REASON("Use .insert() instead.") Scalar& fillrand(Index r, Index c) {
eigen_assert(r == 0 || c == 0);
return fillrand(IsColVector ? r : c);
}
/** \internal \deprecated use insert(Index) */
EIGEN_DEPRECATED Scalar& fillrand(Index i) { return insert(i); }
EIGEN_DEPRECATED_WITH_REASON("Use .insert() instead.") Scalar& fillrand(Index i) { return insert(i); }
/** \internal \deprecated use finalize() */
EIGEN_DEPRECATED void endFill() {}
EIGEN_DEPRECATED_WITH_REASON("Use .finalize() instead.") void endFill() {}
// These two functions were here in the 3.1 release, so let's keep them in case some code rely on them.
/** \internal \deprecated use data() */
EIGEN_DEPRECATED Storage& _data() { return m_data; }
EIGEN_DEPRECATED_WITH_REASON("Use .data() instead.") Storage& _data() { return m_data; }
/** \internal \deprecated use data() */
EIGEN_DEPRECATED const Storage& _data() const { return m_data; }
EIGEN_DEPRECATED_WITH_REASON("Use .data() instead.") const Storage& _data() const { return m_data; }
#ifdef EIGEN_SPARSEVECTOR_PLUGIN
#include EIGEN_SPARSEVECTOR_PLUGIN

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp vendored
View File

@@ -11,6 +11,7 @@
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
namespace slp::detail {
@@ -50,6 +51,8 @@ class AdjointExpressionGraph {
* @return The variable's gradient tree.
*/
VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const {
slp_assert(wrt.cols() == 1);
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.

63
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp vendored
View File

@@ -422,6 +422,12 @@ struct Expression {
}
};
inline ExpressionPtr cbrt(const ExpressionPtr& x);
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/**
* Derived expression type for binary minus operator.
*
@@ -504,6 +510,58 @@ struct BinaryPlusExpression final : Expression {
}
};
/**
* Derived expression type for std::cbrt().
*/
struct CbrtExpression final : Expression {
/**
* Constructs an unary expression (an operator with one argument).
*
* @param lhs Unary operator's operand.
*/
explicit constexpr CbrtExpression(ExpressionPtr lhs)
: Expression{std::move(lhs)} {}
double value(double x, double) const override { return std::cbrt(x); }
ExpressionType type() const override { return ExpressionType::NONLINEAR; }
double grad_l(double x, double, double parent_adjoint) const override {
double c = std::cbrt(x);
return parent_adjoint / (3.0 * c * c);
}
ExpressionPtr grad_expr_l(
const ExpressionPtr& x, const ExpressionPtr&,
const ExpressionPtr& parent_adjoint) const override {
auto c = slp::detail::cbrt(x);
return parent_adjoint / (make_expression_ptr<ConstExpression>(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<ConstExpression>(std::cbrt(x->val));
}
}
return make_expression_ptr<CbrtExpression>(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.
*

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp vendored
View File

@@ -21,8 +21,7 @@ inline gch::small_vector<Expression*> topological_sort(
const ExpressionPtr& root) {
gch::small_vector<Expression*> 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;
}

10
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp vendored
View File

@@ -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<double>& value() {
m_g = m_jacobian.value();
m_g = m_jacobian.value().transpose();
return m_g;
}

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp vendored
View File

@@ -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<Eigen::Lower>();
}
} 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<Eigen::Lower>();
}
return m_H;

16
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp vendored
View File

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

35
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp vendored
View File

@@ -87,6 +87,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*/
Variable& operator=(double value) {
expr = detail::make_expression_ptr<detail::ConstExpression>(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<detail::ConstExpression>(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.
*

2
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp vendored
View File

@@ -1149,7 +1149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
const VariableMatrix& lhs, const VariableMatrix& rhs,
function_ref<Variable(const Variable& x, const Variable& y)> 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()};

189
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp vendored
View File

@@ -8,6 +8,9 @@
#include <utility>
#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 <typename F, typename State, typename Input, typename Time>
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<double> 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<double> 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<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
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 <typename F, typename State, typename Input, typename Time>
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<const decltype(m_dynamics_function)&,
VariableMatrix, VariableMatrix, Variable>(
m_dynamics_function, x_begin, u, time, dt));
subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
VariableMatrix, Variable>(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<const decltype(m_dynamics_function)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics_function, x_begin, u,
time, dt);
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
Variable>(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<double> m_dt;
int m_num_steps;
TranscriptionMethod m_transcription_method;
DynamicsType m_dynamics_type;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics_function;
TimestepMethod m_timestep_method;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
};
} // namespace slp

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp vendored Normal file
View File

@@ -0,0 +1,19 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
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

22
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp vendored Normal file
View File

@@ -0,0 +1,22 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
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

24
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp vendored Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
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

24
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp vendored
View File

@@ -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 <typename F>
requires requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_persistent_callback(F&& callback) {
m_persistent_iteration_callbacks.emplace_back(std::forward<F>(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<std::function<bool(const IterationInfo& info)>>
m_iteration_callbacks;
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_persistent_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis();

5
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp vendored
View File

@@ -3,9 +3,10 @@
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/**
* 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)); \
} \

1
wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide vendored
View File

@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
^fmt/
^gch/
}

18
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp vendored
View File

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

32
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp vendored
View File

@@ -9,6 +9,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#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<std::function<bool(const IterationInfo& info)>> 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<Spy>(
"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);
}
}

3
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp vendored
View File

@@ -327,8 +327,7 @@ ExitStatus interior_point(
Eigen::SparseMatrix<double> 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ₑ ]

1
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/newton.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>

5
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/sqp.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
@@ -232,8 +231,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
Eigen::SparseMatrix<double> 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(

1
wpiutil/src/main/native/include/wpi/jni_util.h
View File

@@ -322,6 +322,7 @@ class JSpanBase {
}
}
// NOLINTNEXTLINE(google-explicit-constructor)
operator std::span<T, Size>() const { return array(); }
std::span<T, Size> array() const {

3
wpiutil/src/test/native/cpp/StringMapTest.cpp
View File

@@ -335,6 +335,7 @@ TEST_F(StringMapTest, MoveConstruct) {
StringMap<int> A;
A["x"] = 42;
StringMap<int> 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);
}