Avoid hardcoding matrix size from robot number of joints plus 6
Tune IPOPT parameters