A two-layer approach for shared control in semi-autonomous robotic surgery