Relaxed torque limits
Changed lambda order to 1st, torque, then force
(to be compatible with other spatial notations)
Added controlOrientation function
(A function to regulate pelvis and feet orientation angles and angular velocities.
All compiled, but not debugged for the impact, set inactive by setting PD gains to zero)