Add constraints for first frame freeze and joint speed, not yet working (solve partially #22, #23)