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

[wpiutil] Add Drake and Drake JNI (#2613)

Browse Source 
Drake is a collection of tools for analyzing robot dynamics and building control systems.
See https://drake.mit.edu/ for details.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2020-07-24 08:34:30 -07:00
committed by GitHub
parent 0c18abed33
commit e759dad019
39 changed files with 4387 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java Normal file
View File

@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import org.ejml.simple.SimpleMatrix;
public final class Drake {
private Drake() {
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A,
SimpleMatrix B,
SimpleMatrix Q,
SimpleMatrix R) {
var S = new SimpleMatrix(A.numRows(), A.numCols());
DrakeJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(),
Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(),
S.getDDRM().getData());
return S;
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings("ParameterName")
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
Matrix A,
Matrix B,
Matrix Q,
Matrix R) {
return discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(), Q.getStorage(),
R.getStorage());
}
}

79
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java Normal file
View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
import edu.wpi.first.wpiutil.RuntimeLoader;
public final class DrakeJNI {
static boolean libraryLoaded = false;
static RuntimeLoader<DrakeJNI> loader = null;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(),
DrakeJNI.class);
loader.loadLibrary();
} catch (IOException ex) {
ex.printStackTrace();
System.exit(1);
}
libraryLoaded = true;
}
}
/**
* Force load the library.
*
* @throws IOException If the library could not be loaded or found.
*/
public static synchronized void forceLoad() throws IOException {
if (libraryLoaded) {
return;
}
loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(),
DrakeJNI.class);
loader.loadLibrary();
libraryLoaded = true;
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
* @param Q Array containing elements of Q in row-major order.
* @param R Array containing elements of R in row-major order.
* @param states Number of states in A matrix.
* @param inputs Number of inputs in B matrix.
* @param S Array storage for DARE solution.
*/
public static native void discreteAlgebraicRiccatiEquation(
double[] A,
double[] B,
double[] Q,
double[] R,
int states,
int inputs,
double[] S);
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
}
}