tests/minimum-jerk-problem.cc: add Torque constraint (close #6)