Different cost values returned by the cost collocation functions for same input
In cost_collocation.hpp line 148-150 (experimental branch):
The computed cost is not the same using the operator() and the value_gradient() function. The computation are not similar due to different conditional statement regarding the cost that needs to be doubled between segments and at the end.
The indices defined with modulo are not correct.
cost_value += t_scale * m_weights[it % NUM_NODES] * Lagrange;
if( ((it % NUM_NODES) == 0) && (it != 0))
cost_value += t_scale * m_weights[it % NUM_NODES] * Lagrange; // add twice at the border points
versus
coeff = t_scale * m_weights[it % POLY_ORDER];
cost_value += coeff * ad_value.value();
/** extract gradient */
cost_gradient. template segment<NX>(k) = coeff * ad_value.derivatives(). template head<NX>();
cost_gradient. template segment<NU>(n + VARX_SIZE) = coeff * ad_value.derivatives(). template tail<NU>();
/** @note: better way to identify junction points? */
if( ((it % POLY_ORDER) == 0) && (it != 0) && (it < NumSegments * POLY_ORDER))
{
cost_value += coeff * ad_value.value(); // add twice at the border points
cost_gradient. template segment<NX>(k) *= Scalar(2);
cost_gradient. template segment<NU>(n + VARX_SIZE) *= Scalar(2);
}