Added RAW_POSITION_MODE option for cart_teleop
Started alternative approach using PyKDL