HKUST-Aerial-Robotics / Fast-Planner

A Robust and Efficient Trajectory Planner for Quadrotors
GNU General Public License v3.0
2.4k stars 665 forks source link

reallocate time in bspline leads to discontinuous of vel and acc? #54

Closed WangChanglong17 closed 4 years ago

WangChanglong17 commented 4 years ago

I find that reallocate time in bspline leads to the change of delta_t, so the changing vel and acc are not equal to initialized vel and acc. Is this a problem?

ZbyLGsc commented 4 years ago

That is certainly not true. How did you find 'discontinuous' vel/acc? Did you really plot a curve of vel/acc before raising this issue?

A nice property of non-uniform B-spline is that it is always continuous. In our implementation we use 3 degree B-spline, the curves have C2 continuity. The change of time allocation adjusts the vel/acc locally to avoid them from exceeding the physical limits, but never leads to discontinuious problem.

WangChanglong17 commented 4 years ago

My question is that the start pos, vel and acc are changed after reallocating time, because the delta_t is change between knots. You use next code to gurantee the init vel and acc are equal to the start vel and acc. After reallocating time, ts is changed. But ctrlpts are not changed. So the init vel and acc of bspline are still equal to the real start vel and acc? // write A Eigen::Vector3d prow(3), vrow(3), arow(3); prow << 1, 4, 1; vrow << -1, 0, 1; arow << 1, -2, 1;

Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);

for (int i = 0; i < K; ++i) A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();

A.block(K, 0, 1, 3) = (1 / 2.0 / ts) vrow.transpose(); A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) vrow.transpose();

A.block(K + 2, 0, 1, 3) = (1 / ts / ts) arow.transpose(); A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) arow.transpose();

ZbyLGsc commented 4 years ago

The initial state remains the same. In time reallocation, not all the knots are changed, those that will lead to change of start vel/acc should not be modified. You can plot them to verify.