1.Fix a bug in the FootStepRegionConstraint in computing the bodyTransform
2.Change the visualizers to get better pictures