TrajOptIfoptDefaultMoveProfile resets Cartesian cost coeffs to 1

[rjoomen]

See here. Why should Cartesian coefficients be reset to 1? Seems wrong to me.

[Levi-Armstrong]

I do agree this needs some further investigation. Bellow is what was used before, but not sure which one is correct. There are difference with how/when the coeff is applied which needs to be revised to make sure this is correct and documented.

addCartesianPositionSquaredCost

[Levi-Armstrong]

We really need a good way to compare the two versions.

[Levi-Armstrong]

If you search in trajopt_qp_problem.cpp for weight, you will find a few todo that likely need to be addressed. I think the easiest way to figure a good way to compare the osqp matrices between Legacy TrajOpt and Ifopt to verify they align. I am not 100% sure these todo's are valid but need to be investigated.

[Levi-Armstrong]

The legacy trajopt handles the cartesian cost using a cost from error function here.

[rjoomen]

For reference, it was introduced in this PR.