improved setControlMode() methods. The functions are now blocking (timeout 100ms), guaranteeing that the joints have switched to the requested control modes