Обращение модели приводов servo_inv_*

Компонент рассчитывает задание для приводов с дискретным позиционным управлением по параметрам желаемой траектории (позиция, скорость) и необходимому моменту.

Компонент servo_inv_lead

Рассчитывает целевую позицию привода исходя из предположение, что ПИД-регулятор привода обеспечивает точную отработку заданной позиции. Формирует упреждение на заданное число циклов управления.

Входные порты

  1. joints_fixed (JointState, EventPort) --- желаемое состояние для приводов, порядок приводов не меняется.

  2. sync_step (int) --- порт индикации начала нового цикла управления.

Выходные порты

  1. goals (ServoGoal) --- задания для приводов.

Параметры

  1. period (float) --- длительность цикла управления, секунды.

  2. lead (float) --- упреждение в секундах.

Операции

Отсутсвуют.

Семантика исполнения

Предполагается, что генератор траектории привода настроен в режиме прямоугольной скорости. (Нулевая длительность фазы ускорения по отношению длительности движения для приводов Herkulex). По получению нового желаемого состояния, происходит вычисление команды для приводов по формуле:

servo_goal_position = desired_position + (desired_position - previous_desired_position) * lead / period
servo_gaol_duration = laed + period

Исключения и ошибки

Предупреждения: 1. Изменение числа приводов.

Компонент servo_inv_7param

Использует статическую 7-параметрическую модель привода из проекта NimbRo-OP. Модель учитывает изменения напряжения питания, силу трения (вязкую, Кулона, Штрибека). При формировании команды для привода берется упреждение.

Входные порты

  1. joints_fixed (JointState, EventPort) --- желаемое состояние для приводов, порядок приводов не меняется.

  2. servo_model (ServoModel) --- порт для обновления списка моделей приводов. Используется для передачи модели от модуля идентификации к компоненту. Модель добавляется или заменяет соответвующую в списке моделей (параметр модуля).

  3. sync_step (int) --- порт индикации начала нового цикла управления.

  4. battery_state (sensor_msg::BatteryState) --- состояние батареи, в частности напряжение питания приводов.

Выходные порты

  1. goals (ServoGoal) --- задания для приводов.

Параметры

  1. period (float) --- длительность цикла управления, секунды.

  2. lead (float) --- упреждение в секундах.

  3. servo_models (ServoModel[]) --- список моделей приводов.

  4. battery_voltage (double) --- напряжение батареи по умолчанию.

Операции

Отсутствуют.

Семантика исполнения

Предполагается, что генератор траектории привода настроен в режиме прямоугольной скорости. (Нулевая длительность фазы ускорения по отношению длительности движения для приводов Herkulex).

Запуск или обработка первого сообщения: проверяется корректен ли список моделей (нет дублирующихся, возможно, диапазоны параметров) и устанавливается соответствие с полученной позой и элементами списка. Если модель привода в списке отсутствует, то соответствующее собщения не будет обрабатываться.

Исполнение. Получает задающее воздействие для каждого звена в виде тройки позиция, скорость, развиваемый момент. Используя эти значения рассчитывает команду для привода goals [Compliant Robot Behavior using Servo Actuator Models identified by Iterative Learning Control], перед посылкой команды берет упреждение по тому же алгоритму, что servo_inv_lead.

Сообщение servo_model. Предназначено для обновления моделей, когда компонент запущен. Находит соответствующую модель с списке моделей и заменяет ее. Все действия исполняются после обработки joints и формирования goals.

  • При необходимости переупорядочивается список моделей, чтобы он в соответствовал порядку сочленения в JointState. Установление соответствия между моделями
  • Если нужной модели нет в списке, то в соответствующую позицию добавляется "пустая" модель.

Детали реализации

Т.к. порт joints декларирован как fixed, то в процессе исполнения порядок звеньев не меняется, что можно использовать для выборки нужной модели. Соответственно возможны разные способы установления связи между списком моделей и приводами. 1. По имени. Создается map<string,ServoModel> или map<string,ServoModel&>, по которому находится соответствующая модель или номер ее в списке моделей. Минусы: медленно, дополнительные структуры данных. 2. По позиции в списке моделей. При инициализации список моделей переупорядочивается с соответствие с порядком сочленений в JointState. Добавлются "нулевые" модели, если модель в списке отсутвует. Минусы: операция обновления модели в активном состонии требует поиска по именам сложности N. 3. По позиции в JointState. Создание отдельного от параметра servo_models упорядоченного списка моделей. Минусы: дополнительная структура данных, длительность выборки по имени. Рекомендуемый вариант --- второй.

Исключения и ошибки

Предупреждения: 1. Изменение числа приводов. 1. Отсутствие модели для привода, лишние модели.