ICRA Paper Accepted