Shared control for natural motion and safety in hands-on robotic surgery