Added PD controller for swing legs