[tests/minimum-jerk-problem] Implement cost function