Merge in changes from SQ3 work

Quad: leg angles closer to Pypose
Command to adjust
Single Leg mode is optional can compile out