Modified task set to use the getDOFId
Cleaned up thread to have base cases for robot hardware testing