elbow shoulder working, tested on bioloid, needs filtering and constraints.