Code calculates Minimum Jerk instead of Minimum Snap

[infrontlight]

Hello,

I've been reviewing the trajectory optimization code and noticed a potential issue in the segment labeled as "minimum snap matrix." The code snippet is as follows:

/* ---------- minimum snap matrix ---------- */
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);

for (int k = 0; k < seg_num; k++)
{
  for (int i = 3; i < 6; i++)
  {
    for (int j = 3; j < 6; j++)
    {
      Q(k * 6 + i, k * 6 + j) =
          i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
    }
  }
}

From my understanding, this matrix setup seems to calculate the Q matrix for a minimum jerk problem (third derivatives of position), rather than the fourth derivatives required for a minimum snap calculation. The loop indices and the polynomial terms (i (i - 1) (i - 2) j (j - 1) * (j - 2)) specifically handle the third derivatives.

For minimum snap, we expect the matrix to involve fourth derivatives, which would typically include terms up to the fifth order (i (i - 1) (i - 2) (i - 3) j (j - 1) (j - 2) * (j - 3)). Could this be an oversight, or is there another part of the code that adjusts for this?

I would appreciate clarification on this, as the designation of the matrix as "minimum snap" might be misleading if it indeed calculates minimum jerk values.

Thank you for looking into this.

[bigsuperZZZX]

Thank you for pointing out this mistake. The current code is the implementation of minimum jerk rather than minimum snap indeed.