Implemented individual joinnt control of all five joints