[cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints #161

leonardoedgar · 2021-01-24T10:47:17Z

Extension for #157

Changes in this PRs:
Enable to construct spline with:

1st order derivatives at curve endpoints (incl. Clamped Spline)
2nd order derivatives at curve endpoints (incl. Natural Spline)
A mixed of 1st order and 2nd order derivatives at curve endpoints

Implementation is based on: https://jmahaffy.sdsu.edu/courses/s10/math541/lectures/pdf/week06/lecture.pdf

Checklists:

Update HISTORY.md with a single line describing this PR

cpp/tests/test_cubic_spline.cpp

jmirabel · 2021-02-01T21:41:23Z

cpp/src/toppra/geometric_path/piecewise_poly_path.cpp

+    // Validate boundary conditions
+    int expected_deriv_size = positions[0].size();
+    for (const BoundaryCond &bc: bc_type) {
+        if (bc.order != 1 and bc.order != 2) {


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)

Noted. Changes made in 9e71bff. Thanks

jmirabel · 2021-02-01T21:42:06Z

cpp/src/toppra/geometric_path/piecewise_poly_path.cpp

+        X[i].resize(times.rows());
+        X[i] << A.colPivHouseholderQr().solve(B[i]);


Cannot you use directly ?

Suggested change

X[i].resize(times.rows());

X[i] << A.colPivHouseholderQr().solve(B[i]);

X[i] = A.colPivHouseholderQr().solve(B[i]);

Noted. Changes made in 9e71bff. Thanks

jmirabel · 2021-02-01T21:44:26Z

cpp/src/toppra/geometric_path/piecewise_poly_path.cpp

+        coefficients[i].resize(4, coeff.cols());
+        coefficients[i] << coeff;


Suggested change

coefficients[i].resize(4, coeff.cols());

coefficients[i] << coeff;

coefficients[i] = coeff;

You may even remove the intermediate variable coeff.

Noted. Changes made in 9e71bff. Thanks

… the curve endpoints

…at curve endpoints

…d on comments

hungpham2511

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.

hungpham2511 · 2021-01-25T15:25:42Z

HISTORY.md

@@ -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


Suggested change

- [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

Noted. Changed in 9023de3. Thanks.

hungpham2511 · 2021-02-02T14:22:55Z

cpp/src/toppra/geometric_path/piecewise_poly_path.hpp

   * @param breakpoints Vector of breakpoints.
   */
  PiecewisePolyPath(const Matrices &coefficients, std::vector<value_type> breakpoints);

+
+  PiecewisePolyPath(const Vectors &positions, const Vector &times, const std::array<BoundaryCond, 2> &bc_type);


Please add docstring (compatible with doxygen) to describe this constructor.

Noted. Added in 9023de3. Thanks.

hungpham2511 · 2021-02-02T14:26:27Z

cpp/src/toppra/toppra.hpp

@@ -54,6 +54,12 @@ namespace toppra {
  /// Vector of Bound
  typedef std::vector<Bound, Eigen::aligned_allocator<Bound> > Bounds;

+  /// Boundary condition
+  typedef struct {


Since this boundary condition is required only for PieceWisePolynomial, I suggest putting it in the piecewise_poly_path.hpp header file.

Noted. Changed in 9023de3. Thanks.

hungpham2511 · 2021-02-02T14:31:35Z

cpp/src/toppra/geometric_path/piecewise_poly_path.cpp

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


I think this portion can be extracted to a separate function for readability. It will also make future extension easier.

Noted. Changed in 9023de3. Thanks.

hungpham2511

LGTM. Thanks!

leonardoedgar changed the title ~~Construct Spline By 1st and 2nd order derivatives at curve endpoints~~ Construct spline with 1st and 2nd order derivatives at curve endpoints Jan 24, 2021

leonardoedgar commented Jan 24, 2021

View reviewed changes

cpp/tests/test_cubic_spline.cpp Outdated Show resolved Hide resolved

leonardoedgar changed the title ~~Construct spline with 1st and 2nd order derivatives at curve endpoints~~ [cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints Jan 24, 2021

jmirabel reviewed Feb 1, 2021

View reviewed changes

leonardoedgar added 5 commits February 2, 2021 10:46

implement spline construction from 1st and 2nd order derivatives from…

87c6a4b

… the curve endpoints

add tests for constructing spline from 1st and 2nd order derivatives …

492f51f

…at curve endpoints

update history.md

18c0f4f

move duplicated function makeVectors into a separate file

8368fc2

add support for mixed 1st and 2nd order boundary conditions

888e439

leonardoedgar force-pushed the develop branch from 4b13d50 to 888e439 Compare February 2, 2021 02:47

remove unnecessary variables, copy constructors, and minor fixes base…

9e71bff

…d on comments

hungpham2511 reviewed Feb 2, 2021

View reviewed changes

add docstring and minor changes based on comments

9023de3

hungpham2511 approved these changes Feb 7, 2021

View reviewed changes

hungpham2511 merged commit facf380 into hungpham2511:develop Feb 7, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints #161

[cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints #161

leonardoedgar commented Jan 24, 2021 •

edited

Loading

jmirabel Feb 1, 2021

leonardoedgar Feb 2, 2021

jmirabel Feb 1, 2021

leonardoedgar Feb 2, 2021

jmirabel Feb 1, 2021

jmirabel Feb 1, 2021

leonardoedgar Feb 2, 2021

hungpham2511 left a comment

hungpham2511 Jan 25, 2021

leonardoedgar Feb 2, 2021

hungpham2511 Feb 2, 2021

leonardoedgar Feb 2, 2021

hungpham2511 Feb 2, 2021

leonardoedgar Feb 2, 2021

hungpham2511 Feb 2, 2021

leonardoedgar Feb 2, 2021

hungpham2511 left a comment

		X[i].resize(times.rows());
		X[i] << A.colPivHouseholderQr().solve(B[i]);

	X[i].resize(times.rows());
	X[i] << A.colPivHouseholderQr().solve(B[i]);
	X[i] = A.colPivHouseholderQr().solve(B[i]);

		coefficients[i].resize(4, coeff.cols());
		coefficients[i] << coeff;

	coefficients[i].resize(4, coeff.cols());
	coefficients[i] << coeff;
	coefficients[i] = coeff;

	- [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

[cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints #161

[cpp] Construct spline with 1st and 2nd order derivatives at curve endpoints #161

Conversation

leonardoedgar commented Jan 24, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hungpham2511 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hungpham2511 left a comment

Choose a reason for hiding this comment

leonardoedgar commented Jan 24, 2021 •

edited

Loading