Added some obstacle avoidence and started implementing a full fingered kinematics inteface.