Компонент odometry_legs

Одометрия на основе задающего воздействия, представленного без использования датчиков (акселерометр, гироскоп, датчики касания). Позволяет рассчитывать движения платформы при исполнении записанных движений. В случае, если задатчик отслеживает движение платформы сам, то не используется.

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

  1. limbs_sorted (RigidBodyState, EventPort) --- состояние конечностей робота в абслютной СК, полная упорядоченная поза (все кинематические цепочки) со скоростями.
  2. body (RigidBodyState) --- поза и скорость рассчитанная задатчиком (опционально, сбрасывает текущую позу по одометрии).
  3. support_sorted (SupportState) --- контакты по расчету задатчика (опционально, позволяют не выдвигать предположений о списке контактов).
  4. sync_step (SupportState) --- синхронизация цикла управления

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

  1. body (RigidBodyState) --- расчетная поза и скорость тела робота.
  2. tf (Transformation) --- поза тела робота для публикации в \tf.
  3. support (SupportState) --- предполагаемые контакты.

Параметры

  1. position_tolerance (double) --- допустимые отклонения по позиции при определнии контактов, метры
  2. angle_tolerance (double) --- допустимые отклонения по углу при определнии контактов, радианы.
  3. legs (srings) --- список ног робота (4 штуки).

Плагины

  1. Требует: robot_model --- URDF модель робота и его кинематических цепочек RobotModelURDF. Определяет порядок суставов в отсортированной JointState.

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

Предполагался, что при контакте копыто всегда ориентированно параллельно поверхности, поверхность представляет собой плоскость, ось Oy копыта направлена вверх, число опорных ног не меньше двух.

Компонент работает с СК, связанной с землей.

Основой цикл. При плучении сообщения joints происходит пересчет позы робота.

  1. Определение контактов. Перебираются всевозможные комбинации контактов (1 + 4 + 6), для каждого вычисляется опорная плоскость (нормаль --- средняя нормаль копыт, точка --- средння точка центров опорных площадок), потом проверяются следующие условия:

    • отклонение нормалей опорных копыт не больше angle_tolerance,
    • отклонение центров опорных копыт не больше position_tolerance,
    • свободные копыта находятся в положительном полупространстве.

    Если получено сообщение support, то используется его значение, вычисления не проводятся.

  2. Сохранение позиции нового контакта. Если появился новый контакт, то его позиция сохраняется.

  3. Расчет позиции. Если есть минимум пара контактов, сохранявшихся за цикл управления, то по их координатам и их нормалям с учетом сохраненных позиций рассчитывается поза платформы [Estimating 3-D rigid body transformations: a comparison of four major algorithms]. Если нужного числа контактов нет, то проводится интегрирование скорости.

    Если получено сообщение body, то используется его значение, рассчеты не проводятся.

  4. Расчет скорости. Путем решения СЛАУ для якобиана рассчитываются скорости.

    Если получено сообщение body, то используется его значение.

Замечание. При ручном управлении робота из rviz необходимо добвить в редактор траектории интерфейс для принудительного назначения текущих опорных ног робота.

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

Потребуется метод решения переопределенной СЛАУ и SVD разложения (для оценки позы).

Ошибки и исключения

Ошибка: 1. Некорректная limbs или supports.

Предупреждение: 1. Невозможно определить опорные ноги.