progressssss but bad on committing, we removed first implementation of lewan...

progressssss but bad on committing, we removed first implementation of lewan servo and added basic support for third argument for joint_goto - position after reaching goal, and some bugs removal