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

Merge branch 'main' into 2022

Browse Source
This commit is contained in:
Peter Johnson
2021-04-19 18:45:31 -07:00
parent 8d961dfd25 aaf24e2552
commit 05ebe93180
44 changed files with 1762 additions and 702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -13,6 +13,7 @@
#include "Eigen/QR"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "frc/trajectory/TrajectoryUtil.h"
#include "unsupported/Eigen/MatrixFunctions"
using namespace wpi::java;
@@ -43,6 +44,46 @@ bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
return true;
}
std::vector<double> GetElementsFromTrajectory(
const frc::Trajectory& trajectory) {
std::vector<double> elements;
elements.reserve(trajectory.States().size() * 7);
for (auto&& state : trajectory.States()) {
elements.push_back(state.t.to<double>());
elements.push_back(state.velocity.to<double>());
elements.push_back(state.acceleration.to<double>());
elements.push_back(state.pose.X().to<double>());
elements.push_back(state.pose.Y().to<double>());
elements.push_back(state.pose.Rotation().Radians().to<double>());
elements.push_back(state.curvature.to<double>());
}
return elements;
}
frc::Trajectory CreateTrajectoryFromElements(wpi::ArrayRef<double> elements) {
// Make sure that the elements have the correct length.
assert(elements.size() % 7 == 0);
// Create a vector of states from the elements.
std::vector<frc::Trajectory::State> states;
states.reserve(elements.size() / 7);
for (size_t i = 0; i < elements.size(); i += 7) {
states.emplace_back(frc::Trajectory::State{
units::second_t{elements[i]},
units::meters_per_second_t{elements[i + 1]},
units::meters_per_second_squared_t{elements[i + 2]},
frc::Pose2d{units::meter_t{elements[i + 3]},
units::meter_t{elements[i + 4]},
units::radian_t{elements[i + 5]}},
units::curvature_t{elements[i + 6]}});
}
return frc::Trajectory(states);
}
extern "C" {
/*
@@ -163,4 +204,99 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
return isStabilizable;
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: fromPathweaverJson
* Signature: (Ljava/lang/String;)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
(JNIEnv* env, jclass, jstring path)
{
try {
auto trajectory =
frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
std::vector<double> elements = GetElementsFromTrajectory(trajectory);
return MakeJDoubleArray(env, elements);
} catch (std::exception& e) {
jclass cls = env->FindClass("java/lang/IOException");
if (cls) {
env->ThrowNew(cls, e.what());
}
return nullptr;
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: toPathweaverJson
* Signature: ([DLjava/lang/String;)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
(JNIEnv* env, jclass, jdoubleArray elements, jstring path)
{
try {
auto trajectory =
CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
frc::TrajectoryUtil::ToPathweaverJson(trajectory,
JStringRef{env, path}.c_str());
} catch (std::exception& e) {
jclass cls = env->FindClass("java/lang/IOException");
if (cls) {
env->ThrowNew(cls, e.what());
}
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: deserializeTrajectory
* Signature: (Ljava/lang/String;)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
(JNIEnv* env, jclass, jstring json)
{
try {
auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
JStringRef{env, json}.c_str());
std::vector<double> elements = GetElementsFromTrajectory(trajectory);
return MakeJDoubleArray(env, elements);
} catch (std::exception& e) {
jclass cls = env->FindClass(
"edu/wpi/first/wpilibj/trajectory/TrajectoryUtil$"
"TrajectorySerializationException");
if (cls) {
env->ThrowNew(cls, e.what());
}
return nullptr;
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: serializeTrajectory
* Signature: ([D)Ljava/lang/String;
*/
JNIEXPORT jstring JNICALL
Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
(JNIEnv* env, jclass, jdoubleArray elements)
{
try {
auto trajectory =
CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
return MakeJString(env,
frc::TrajectoryUtil::SerializeTrajectory(trajectory));
} catch (std::exception& e) {
jclass cls = env->FindClass(
"edu/wpi/first/wpilibj/trajectory/TrajectoryUtil$"
"TrajectorySerializationException");
if (cls) {
env->ThrowNew(cls, e.what());
}
return nullptr;
}
}
} // extern "C"

6
wpimath/src/main/native/include/frc/LinearFilter.h
View File

@@ -104,6 +104,9 @@ class LinearFilter {
* y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
* above which the input starts to attenuate.
*
* This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
@@ -121,6 +124,9 @@ class LinearFilter {
* y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
* below which the input starts to attenuate.
*
* This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.

21
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -52,6 +52,27 @@ class CubicHermiteSpline : public Spline<3> {
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P(i+1) = P(1) = a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
//
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
// clang-format off
static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+2.0, +1.0, -2.0, +1.0,

30
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -52,10 +52,38 @@ class QuinticHermiteSpline : public Spline<5> {
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P''(i) = P''(0) = 2 * a2
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
//
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
// clang-format off
static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0,

16
wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
View File

@@ -36,19 +36,19 @@ class TrajectoryUtil {
static Trajectory FromPathweaverJson(const wpi::Twine& path);
/**
* Deserializes a Trajectory from PathWeaver-style JSON.
* @param json the string containing the serialized JSON
* @return the trajectory represented by the JSON
*/
* Deserializes a Trajectory from PathWeaver-style JSON.
*
* @param json the string containing the serialized JSON
*
* @return the trajectory represented by the JSON
*/
static std::string SerializeTrajectory(const Trajectory& trajectory);
/**
* Serializes a Trajectory to PathWeaver-style JSON.
*
* @param trajectory the trajectory to export
*
* @return the string containing the serialized JSON
*/
static Trajectory DeserializeTrajectory(wpi::StringRef json_str);