-
-
Notifications
You must be signed in to change notification settings - Fork 167
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
[cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints #161
Conversation
// Validate boundary conditions | ||
int expected_deriv_size = positions[0].size(); | ||
for (const BoundaryCond &bc: bc_type) { | ||
if (bc.order != 1 and bc.order != 2) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Although more readable, and
can cause trouble for some compilers such as MSVC (see https://stackoverflow.com/questions/555505/when-were-the-and-and-or-alternative-tokens-introduced-in-c/555524#555524)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Changes made in 9e71bff. Thanks
X[i].resize(times.rows()); | ||
X[i] << A.colPivHouseholderQr().solve(B[i]); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Cannot you use directly ?
X[i].resize(times.rows()); | |
X[i] << A.colPivHouseholderQr().solve(B[i]); | |
X[i] = A.colPivHouseholderQr().solve(B[i]); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Changes made in 9e71bff. Thanks
coefficients[i].resize(4, coeff.cols()); | ||
coefficients[i] << coeff; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
coefficients[i].resize(4, coeff.cols()); | |
coefficients[i] << coeff; | |
coefficients[i] = coeff; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You may even remove the intermediate variable coeff
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Changes made in 9e71bff. Thanks
… the curve endpoints
…at curve endpoints
4b13d50
to
888e439
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Many thanks for the PR and sorry for the delay. I think it looks great, the test suite looks comprehensive. I only have a few comments, if you would address them I would be happy to merge.
HISTORY.md
Outdated
@@ -13,6 +13,7 @@ | |||
- [cpp] [#143][#143] Add PathParametrizationAlgorithm::setGridpoints | |||
- [cpp] [#146][#146] Add constraint::CartesianVelocityNorm | |||
- [cpp] [#158][#158] Add Seidel solver. | |||
- [cpp] [#157] Enable to construct spline from 1st and 2nd order derivatives at the curve endpoints |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
- [cpp] [#157] Enable to construct spline from 1st and 2nd order derivatives at the curve endpoints | |
- [cpp] [#161] Enable to construct spline from 1st and 2nd order derivatives at the curve endpoints |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Changed in 9023de3. Thanks.
* @param breakpoints Vector of breakpoints. | ||
*/ | ||
PiecewisePolyPath(const Matrices &coefficients, std::vector<value_type> breakpoints); | ||
|
||
|
||
PiecewisePolyPath(const Vectors &positions, const Vector ×, const std::array<BoundaryCond, 2> &bc_type); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please add docstring (compatible with doxygen) to describe this constructor.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Added in 9023de3. Thanks.
cpp/src/toppra/toppra.hpp
Outdated
@@ -54,6 +54,12 @@ namespace toppra { | |||
/// Vector of Bound | |||
typedef std::vector<Bound, Eigen::aligned_allocator<Bound> > Bounds; | |||
|
|||
/// Boundary condition | |||
typedef struct { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Since this boundary condition is required only for PieceWisePolynomial, I suggest putting it in the piecewise_poly_path.hpp
header file.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Changed in 9023de3. Thanks.
// h(i) = t(i+1) - t(i) | ||
Vector h (times.rows() - 1); | ||
for (size_t i = 0; i < h.rows(); i++){ | ||
h(i) = times(i + 1) - times(i); | ||
} | ||
|
||
// Construct the tri-diagonal matrix A based on spline continuity criteria | ||
Matrix A = Matrix::Zero(times.rows(), times.rows()); | ||
for (size_t i = 1; i < A.rows() - 1; i++) { | ||
A.row(i).segment(i - 1, 3) << h(i - 1), 2 * (h(i - 1) + h(i)), h(i); | ||
} | ||
|
||
// Construct B based on spline continuity criteria | ||
Vectors B (positions.at(0).rows()); | ||
for (size_t i = 0; i < B.size(); i++) { | ||
B[i].resize(times.rows()); | ||
for (size_t j = 1; j < A.rows() - 1; j++) { | ||
B[i](j) = 3 * (positions[j + 1](i) - positions[j](i)) / h(j) - | ||
3 * (positions[j](i) - positions[j - 1](i)) / h(j - 1); | ||
} | ||
} | ||
|
||
// Insert boundary conditions to A and B | ||
if (bc_type[0].order == 1) { | ||
A.row(0).segment(0, 2) << 2 * h(0), h(0); | ||
for (size_t i = 0; i < B.size(); i++) { | ||
B[i](0) = 3 * (positions[1](i) - positions[0](i)) / h(0) - 3 * bc_type[0].values(i); | ||
} | ||
} | ||
else if (bc_type[0].order == 2) { | ||
A(0, 0) = 2; | ||
for (size_t i = 0; i < B.size(); i++) { | ||
B[i](0) = bc_type[0].values(i); | ||
} | ||
} | ||
|
||
if (bc_type[1].order == 1) { | ||
A.row(A.rows() - 1).segment(A.cols() - 2, 2) << h(h.rows() - 1), 2 * h(h.rows() - 1); | ||
for (size_t i = 0; i < B.size(); i++) { | ||
B[i](B[i].rows() - 1) = | ||
3 * bc_type[1].values(i) - | ||
3 * (positions[positions.size() - 1](i) - positions[positions.size() - 2](i)) / h(h.rows() - 1); | ||
} | ||
} | ||
else if (bc_type[1].order == 2) { | ||
A(A.rows() - 1, A.cols() - 1) = 2; | ||
for (size_t i = 0; i < B.size(); i++) { | ||
B[i](B[i].rows() - 1) = bc_type[1].values(i); | ||
} | ||
} | ||
|
||
// Solve AX = B | ||
Vectors X (positions[0].rows()); | ||
for (size_t i = 0; i < X.size(); i++) { | ||
X[i].resize(times.rows()); | ||
X[i] = A.colPivHouseholderQr().solve(B[i]); | ||
} | ||
|
||
// Insert spline coefficients | ||
Matrices coefficients(times.size() - 1); | ||
for (size_t i = 0; i < coefficients.size() ; i++) { | ||
coefficients[i].resize(4, positions[0].rows()); | ||
for (size_t j = 0; j < coefficients[i].cols(); j++) { | ||
coefficients[i](0, j) = (X[j](i + 1) - X[j](i)) / (3 * h(i)); | ||
coefficients[i](1, j) = X[j](i); | ||
coefficients[i](2, j) = (positions[i + 1](j) - positions[i](j)) / h(i) - | ||
h(i) / 3 * (2 * X[j](i) + X[j](i + 1)); | ||
coefficients[i](3, j) = positions[i](j); | ||
} | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think this portion can be extracted to a separate function for readability. It will also make future extension easier.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Noted. Changed in 9023de3. Thanks.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
LGTM. Thanks!
Extension for #157
Changes in this PRs:
Enable to construct spline with:
Implementation is based on: https://jmahaffy.sdsu.edu/courses/s10/math541/lectures/pdf/week06/lecture.pdf
Checklists: