A paper is accepted at both IEEE Robotics and Automation Letters (RAL) and IEEE International Conference on Robotics and Automation (ICRA)