added footstep object to display footsteps as vis markers
box mueller technique for normal dist
integrated foot step planner; obstacle inclusion still to go