Added multifinger planning with interactive marker