minor changes to new update functions
this is the latest version that works with both weighting and prioritization. Uncomment constraint update and final sweep update functions to work with weighting approach. We also decomposed torque and beta inputs into torque control, torque constraint, beta control, beta posture, beta constraint