include init and final posture
properly scale Torque criteria
add discrete constraints