mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)
There were three options for where to put this function: 1. A free function in LinearQuadraticRegulator.h. Returning a K matrix means the user can't use the LinearQuadraticRegulator in a loop anymore. 2. A default argument added to ctors in LinearQuadraticRegulator for a time delay (default of 0). This has the smallest API footprint from the user perspective, but it bloats the already substantial constructor overload set of LinearQuadraticRegulator. 3. A member function in LinearQuadraticRegulator that modifies the internal K. This would still have to take in a LinearSystem or (A, B) pair because the ctor doesn't store it. Storing it internally feels like paying for what we don't use most of the time. I went with option 3. I verified the tests's expected values in Python with scipy.linalg.fractional_matrix_power(). Closes #2877.
This commit is contained in:
@@ -105,6 +105,28 @@ Java_edu_wpi_first_math_WPIMathJNI_exp
|
||||
env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_math_WPIMathJNI
|
||||
* Method: pow
|
||||
* Signature: ([DID[D)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_math_WPIMathJNI_pow
|
||||
(JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
|
||||
jdoubleArray dst)
|
||||
{
|
||||
jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
|
||||
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Amat{arrayBody, rows, rows};
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
|
||||
Amat.pow(exponent);
|
||||
|
||||
env->ReleaseDoubleArrayElements(src, arrayBody, 0);
|
||||
env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_math_WPIMathJNI
|
||||
* Method: isStabilizable
|
||||
|
||||
Reference in New Issue
Block a user