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

Fix bug in cubic and quintic hermetic spline generation (#2139)

Browse Source 
Add documentation for spline derivatives and explicitly zero matrices.
This commit is contained in:
Matt
2019-12-01 21:29:52 -08:00
committed by Peter Johnson
parent e37ecd33ae
commit 6c8f6cf479
6 changed files with 54 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
View File

@@ -50,10 +50,21 @@ public class CubicHermiteSpline extends Spline {
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
// Here, we are multiplying by (3 - i) to manually take the derivative. The
// power of the term in index 0 is 3, index 1 is 2 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
m_coefficients.set(4, i, m_coefficients.get(2, i) * (3 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (3 - i));
}
for (int i = 0; i < 3; i++) {
// Here, we are multiplying by (2 - i) to manually take the derivative. The
// power of the term in index 0 is 2, index 1 is 1 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
View File

@@ -47,13 +47,24 @@ public class QuinticHermiteSpline extends Spline {
for (int i = 0; i < 6; i++) {
m_coefficients.set(0, i, xCoeffs.get(0, i));
m_coefficients.set(1, i, yCoeffs.get(0, i));
}
for (int i = 0; i < 6; i++) {
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
// Here, we are multiplying by (5 - i) to manually take the derivative. The
// power of the term in index 0 is 5, index 1 is 4 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
m_coefficients.set(4, i, m_coefficients.get(2, i) * (5 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (5 - i));
}
for (int i = 0; i < 5; i++) {
// Then populate row 4 and 5 with the second derivatives.
// Here, we are multiplying by (4 - i) to manually take the derivative. The
// power of the term in index 0 is 4, index 1 is 3 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
}
}