Fixed notation (foot positions, rotation matrices).
Plot activated/deactivated constraints. Added walk with changing reference velocity.