Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51: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

12
wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
View File

@@ -27,10 +27,20 @@ CubicHermiteSpline::CubicHermiteSpline(
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 4; i++) {
// 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.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, 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.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (3 - i);
m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}

13
wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
View File

@@ -27,10 +27,19 @@ QuinticHermiteSpline::QuinticHermiteSpline(
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 6; i++) {
// 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.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (5 - i);
}
for (int i = 0; i < 5; i++) {
// 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.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (5 - i);
m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}

3
wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -48,7 +48,8 @@ class CubicHermiteSpline : public Spline<3> {
}
private:
Eigen::Matrix<double, 6, 4> m_coefficients;
Eigen::Matrix<double, 6, 4> m_coefficients =
Eigen::Matrix<double, 6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.

3
wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -48,7 +48,8 @@ class QuinticHermiteSpline : public Spline<5> {
}
private:
Eigen::Matrix<double, 6, 6> m_coefficients;
Eigen::Matrix<double, 6, 6> m_coefficients =
Eigen::Matrix<double, 6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.

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