Replaced joint angles with Euler angles for ControlOrientation
(Replaced the three joint angles with euler angles for pelvis and feet.
The goal is to control the posture of these three end-effectors
on top of the low-level joint torque optimizer.
The used seq is spaceRotationSequence to get ZYX (euler angles))
Added checkContactState function
(removed the older one form WBOptimizer, to make a global & cleaner call
also is gonna used before optimization for setting the L/R load ratio
public int _contact_state variable added
A bit of clean-up in wboptimize
Added setFootCOMDistRatio
to distribute the foot load according to its distance to COM
sets the global variable _distRatio
Added RegulateCoMTransAccWithInvPendulum to saturate CoMAcc acc. to IP
(also added saturation function to helper,
effects still need to be tested and fixed)
Added function limitFeetCartesinAcc
to saturate the extracted feet cartesian acc
seems to slightly delay divergance/fall-over event