Polynomial class not interpolating across every dimension

[Woolfrey]

Noticed a problem testing joint trajectory generation & feedback control:

The error for joints 2 - 8 is 0 up until the end of the trajectory where it suddenly jumps.

I printed the desired joint values to the console:

Somehow the desired trajectory is not being interpolated properly for everything other than the 1st dimension (ノಠ益ಠ)ノ彡┻━┻

[Woolfrey]

For a cubic polynomial:

x(t) = \beta_0 + \beta_1 t + \beta_2 t^2 + \beta_3 t^3

For $t = 0$ then $x(0) = \beta_0$ so, and $\dot{x}(0) = \beta_1$ so the start positions and velocities are correct:

For the second row the coefficients are all zero, which is very wrong, which tells me there's a problem in the algorithm that computes the coefficients:

     Vector<DataType,Dynamic> supportPoints(this->_order+1); supportPoints.setZero();

     for(int i = 0; i < this->_dimensions; i++)
     {
            supportPoints(0) = startPoint(i);
            supportPoints(n) = endPoint(i);

            if(i == 0)
            {
                 supportPoints(1)   = startVelocity(i);
                 supportPoints(1+n) = endVelocity(i);
            }

            if(i == 1)
            {
                 supportPoints(2)   = startAcceleration(i);
                 supportPoints(2+n) = endAcceleration(i);
            }

           this->_coefficients.row(i) = (timeMatrixDecomp.solve(supportPoints)).transpose();
      }

[Woolfrey]

I fixed it! Here's the output from the kinematic_control_test executable: