One paper is accepted by IEEE/ASME Transactions on Mechatronics