Добавьте состояния в конец пути
append( добавляет последовательность состояний, средств управления, длительности и целей в конец пути. Если путь пуст, pathObj,states,controls,durations,targets)states введите должен поссориться, чем другие входные векторы и матрицы. Если путь уже содержит точки, функция применяет первое управление к последнему состоянию в текущем пути.