* Published in IROS2017 ()2017 IEEE/RSJ International Conference on
Intelligent Robots and Systems. arXiv admin note: text overlap with
arXiv:1803.02009 Access Paper or Ask Questions
* Accepted for publication in Robotics and Automation Letters. This
version contains extended proofs, which do not fit in the RA-L page limit Access Paper or Ask Questions
* Accepted for publication in Robotics and Automation Letters. This
version contains the full proofs which do not fit in the RA-L page limit Access Paper or Ask Questions