Динамика servo_ident

Компонет вычисляет невязку по силе моментами приводов, рассчитаными по задающему воздействию и по реальной траектории движения. Это данные также используются для идентификации 4-х параметров alpha из 7-параметрической модели привода ServoModel.

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

  1. joints_sorted (JointState) --- желаемое состояние робота в угловой СК: позиции, скорости, моменты приводов. Полный вектор состояния робота, отсортирован в соответвие с RobotModel.
  2. joints_measured (JointState, EventPort) --- измеренные позиции и скорости приводов. Может присутсвовать только часть приводов, моет прийти несколько сообщений за такт.

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

  1. servo_model (ServoModel[]) --- используется для пересылке модели к компоненту обращения приводов servo_inv_7param после ее идентификации.
  2. torque_error_sorted (JointState) --- ошибка предсказания модели приводов, пересчитанная в момент. Полный, отсортированный вектор всех динамически управляемых приводов.

Параметры

  1. control_delay (uint) --- задержка исполнения команд приводами в тактах системы управления.
  2. relaxaton_factor (double) --- фактор забывания при МНК идетификации.
  3. error_averaging_time (double) --- характерное время усреднения ошибки предсказания.
  4. treshhold (double) --- минимально допустимая точность по средней ошибке предсказания позиции, радианы.
  5. battery_voltage (double) --- напряжение батареи по умолчанию.
  6. servo_models (ServoModel[]) --- список моделей приводов. Могут быть переупорядочены или модифицированы компонентом.

Операции

Интерфейс операций: ручное использование идентификации.

  1. Предоставляет: bool startIdentification(strings servos), ` --- начать идентификацию заданного набора приводов. Возвращает false, если начать процедуру невозможно.
  2. Предоставляет: strings endIdentification() --- завершить идентификацию, если критерий качества treshhold достигнут, то отослать модели через порт servo_model, возвращает список успешно идентифицированных моделей.
  3. Предоставляет: bool abortIdentification() --- завершить идентификацию, если критерий качества treshhold достигнут, то отослать модели через порт servo_model.

Для управления из ROS потребубются аналоги этих операций. Вероятно, можно сделать одну операцию для запуска/получения статуса/прекращения/завершения. Альтернативный вариант --- интерфейс actionlib, но он не позволяет осуществлять операции abort/end.

Сервисы и плагины

  1. Требует: RobotModelURDF для получения списка приводов при инициализации.

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

Получение сообщения joints_measured. Используя соответвующую модель из параметра servo_models вычисляет невязку по силе tau_e и публикует ее. Для приводов, о которых нет информации --- невязка ноль, для остальных --- сохраняются предыдущие значения.

Если для привода начата процедура идентификации (была вызвана startIdentification), то проводит идентификацию МНК. Она продолжается до вызова endIdentification() или abortIdentification(). В первом случае, если усредненная по времени error_averaging_time ошибка пресказания позиции меньше treshhold, то результат идентификации попадает в сообщение servo_model. Во втором идентификация просто прерывается.

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

Из-за задержки в исполнительной системе придется хранить control_delay векторов с задающим воздействием. Это надо делать без копирования (циклический буффер).

Т.к. joints_measured не упорядочен, то для быстрого получения номера привода и доступа к структурам, мспользующимся при его идентификации предлагается создать unordered_map<string,ServoIdent>. ServoIdent содержит промежуточные параметры идентификации (матрицу ковариации, среднуюю ошибку предсказания и т.п.

Для ускорения доступа servo_models должен переупорядочиваться и дополняется в соответвие с порядком приводов в RobotModel. Результат идентификации можно сразу хранить в servo_models, либо загружать туда только, если вывана операция endIdentification().

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

Ошибки: 1. Некорректное имя привода. 1. Некоррекный joints_sorted.

Предупрежденрия: 1. Некоррекный joints_measured.

Информация: 1. Запуск, ход и остановка идентификации.