Динамика dynamics_inv

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

Использует вспомогательный класс RobotURDFModel для загрузки модели робота и задания цепочек.

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

  1. joints_sorted (JointState, EventPort) --- состояние робота в угловой СК: позиции, скорости. Полная поза робота.
  2. body (RigidBodyState) --- позиция и скорость платформы.
  3. contacts_sorted (SupportState) --- список контактов.

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

  1. joints (JointState) --- состояние робота в угловой СК: позиции, скорости, моменты приводов.
  2. contacts (SupportState) --- силы реакции опоры для контактов (y-компонента).
  3. contact_forces (WrenchStamped) --- силы реакции опоры для контактов (для визуализации).

Параметры

  1. dfilt_traтsient_time (double) --- постоянная времени дифференцирующего фильтра.

Операции

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

  1. Требует: RobotModelURDF.
  2. Требует загрузки: дифференцирующий фильтр (второй этап реализации).

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

При решении обратной задачи динамики предполагается, что при контакте копыто всегда ориентированно параллельно поверхности, его конструкция такова, что оси двух последних звеньев пересекаютсяю. Это позволяет сделать ряд упрощений:

  1. Считать, что контакт происходит в точке (0,0,0) в СК последнего звена.
  2. Рассчитывать якобиан только для поступательного движения точки контакта.

Конфигурация. Загрузка модели.

Исполнение. Решает обратную задачу динамики уравнением (3.99). Порядок ее решения выглядит так:

  1. Оценить ускорения путем вычисления численных производных входных скоростей.
  2. Вычислить совокупный якобиан контактов Jc(q).
  3. Рассчитать матрицу инерции H(q), используя библиотеку динамики.
  4. Вычислить проектор Nc.
  5. Вычислить моменты приводов по формуле (3.99).
  6. Вычислить силы реакции опоры (3.61). (Упрощение? Это по сути та же иерархическая задача, относительно (tau,N)).

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

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

Лучше использовать упрощенную URDF модель без глаз, ушей и хвоста. Лишние координаты во входной позе просто игнорируются.

Используется библиотека RDBL. Проблему может составить проверка совпадения нумерации приводов.

Расчеты ведутся в инерциальной СК, связанной с землей.

Для вычисления псевдообратной матрицы следует использовать SVD разложение.

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

Ошибки: 1. Некорректное имя цепочки. 2. Неполная поза, список контактов.

Предупреждения: 3. Нарушение условий касания: проекция одной из сил нормального давления меньше нуля.

Динамика dynamics_fwd

Решает прямую задачу кинематики для сил: пересчитывает усилия приводов во внешние силы, действующие на робота. Первая версия не учитывает инерцию робота, преобразование совершается только за счет кинематики.

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

  1. joints_sorted (JointState, EventPort) --- состояние робота в угловой СК: позиции, скорости. Полная поза робота.
  2. body (RigidBodyState) --- позиция и скорость платформы.

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

  1. contacts (SupportState) --- силы реакции опоры для выбранного списка точке (z-компонента силы в заданной точке).
  2. forces (WrenchStamped) --- 6-мерные внешние силы для указанного списка точек (опционально).

Параметры

  1. contact_list (strings) --- список точек, к которым приводятся внешние силы (задача неоднозначно, например, можно приводить к туловищу и голове, а можно к ногам и голове). Имена соответвуют кинематическим цепочкам, для обозначения туловища используется "body".

Операции

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

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

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

Выполняет пересчет моментов приводов к заданным точкам, используя псевдообращение якобиана. (Проверить корректность операции).

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

Задача сводится к решению переопределенной СЛАУ.

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

Ошибки: 1. Некорректное имя цепочки. 2. Неполная поза.