Added new thigh teleop package and finished thigh_arduino code