Implemented working 6DOF controller and interactive shell!!

Fixed the control equations to include the position/rotation coupling
terms. Added joint torque limits. Added joint friction. Added lots
of helper functions to move operational point. Created a shell thread
and loop to take interactive input from the user.