fix(solver): Fix Hermite-to-power representation conversion

[flandyjohn]

Hello @kotakondo, thank you for the great work!

I was playing around with MIGHTY and I noticed that the quintic piecewise polynomial trajectories published are not $C²$ continuous at the segment boundaries.
This seems to be due to a bug in the conversion from the Hermite to power-basis representation in SolverLBFGS::getPieceWisePol, where the some time terms are missing.

In particular, these lines:

mighty/src/mighty/lbfgs_solver.cpp

Lines 1632 to 1634 in 03c54ae

	double c = (10.0 * c0v - 4.0 * c1v + 0.5 * c2v) / T3;
	double b = (-15.0 * c0v + 7.0 * c1v - c2v) / T4;
	double a = (6.0 * c0v - 3.0 * c1v + 0.5 * c2v) / T5;

Should be changed to:

        double c = (10.0 * c0v - 4.0 * c1v * T + 0.5 * c2v * T2) / T3;
        double b = (-15.0 * c0v + 7.0 * c1v * T - c2v * T2) / T4;
        double a = (6.0 * c0v - 3.0 * c1v * T + 0.5 * c2v * T2) / T5;

I've tested these changes locally and I can now get $C²$ polynomial trajectories at the output.

Let me know what you think.