Initial functionality transferred from hrp2jsknt_moveit_plugins